命令挤在一条队列里，上一条没返回后面的就点不了；软拖拽又被位置环顶回去。相机、夹爪、触觉分开走，停止只杀本界面登记的进程组。左指右指挪到画面外面，标题条改成实底，图像不再把字盖住。

验收：test_command_lanes、test_listed_arms、test_camera_preview 共 26 项通过。

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

config/site.example.yaml

@@ -812,11 +812,9 @@ hole_detect:
     max_depth_m: 0.60
     linger_sec: 8.0          # 完成后继续发布结果/叠图的时长
 
-  # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
+  # ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
-  # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
+  # 搜孔轻压已取消：不带 --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  # 螺纹规旋入
   stage3:
     # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
     #    节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
@@ -843,9 +841,12 @@ hole_detect:
     drop_frames: 2           # 跌落需持续的帧数（防毛刺）
     center_scan: false       # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
     center_scan_mm: 0.3
-    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
+    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入。旋转装配不读这项
     insert_retry: 3
-    screw_pitch_mm: 0.0      # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
+    screw_pitch_mm: 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   # 触觉话题断流多久中止
@@ -1128,7 +1129,8 @@ 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

@@ -796,11 +796,9 @@ hole_detect:
     max_depth_m: 0.60
     linger_sec: 8.0          # 完成后继续发布结果/叠图的时长
 
-  # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
+  # ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
-  # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
+  # 搜孔轻压已取消：不带 --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  # 螺纹规旋入
   stage3:
     # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
     #    节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
@@ -827,9 +825,12 @@ hole_detect:
     drop_frames: 2           # 跌落需持续的帧数（防毛刺）
     center_scan: false       # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
     center_scan_mm: 0.3
-    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
+    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入。旋转装配不读这项
     insert_retry: 3
-    screw_pitch_mm: 0.0      # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
+    screw_pitch_mm: 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   # 触觉话题断流多久中止
@@ -1112,7 +1113,8 @@ 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/ENVIRONMENT.md

@@ -38,16 +38,16 @@
 
 ## 2. 新机器先对齐的路径
 
-下面这些路径被脚本写死为用户 `cc` 的家目录。新机器用**同一个用户名** `cc`（家目录 `/home/cc`）时，后面的命令可以原样复制。
+脚本跟**当前用户的家目录**走，不写死 `/home/cc`。用户名可以不是 `cc`。只支持 Linux。
 
 | 路径 | 谁在用 |
 |------|--------|
-| `/home/cc/anaconda3` | `scripts/run_real.sh`、`scripts/run_mock_demo.sh` 直接 `source` 这个 `conda.sh`。`scripts/env.sh` 用的是 `$HOME/anaconda3`，两边要是同一个目录 |
+| `$HOME/anaconda3` 或 `$HOME/miniconda3` | `scripts/lib/dev_tools.sh`。环境名默认 `ros_flexiv`，可用 `REAL_ENV_NAME` 改 |
-| `/home/cc/rdk_py310/bin/python` | `run_real.sh`、`test_rdk_python.sh`、`rdk_switch_tool.sh`、`scripts/lib/real_preflight.py` 的默认 `RDK_PYTHON` |
+| `$HOME/rdk_py310/bin/python` | 默认的 RDK 解释器。已经在别的目录时，启动前 `export RDK_PYTHON=...` |
-| `/home/cc/flexiv_install` | `run_real.sh` 用 `$HOME/flexiv_install`（随用户走） |
+| `$HOME/flexiv_install` | `run_real.sh` / `env.sh` 的 `LD_LIBRARY_PATH` |
-| `/home/cc/ros2` | 工作区。`colcon build` 必须在这条 ASCII 路径下，不要在中文目录里编 |
+| 仓库目录 | 克隆到哪就从哪启动。`colcon build` 必须在 ASCII 路径下，不要在中文目录里编 |
 
-用户名不是 `cc` 时，至少要改 `scripts/run_real.sh` 里的 conda 路径，并在启动前 `export RDK_PYTHON=$HOME/rdk_py310/bin/python`。否则真机入口会去找 `/home/cc`。
+机械臂、相机、夹爪、网卡和 IP 只写在 `config/site.yaml`。臂号是序列号末两位，换一台臂改 SN，不要改程序里的 89/91。工位窗口的显示器跟当前登录会话（`who` 里的 `:0` / `:1`），不写死某一块屏。
 
 建议磁盘空闲 **40GB 以上**（两个 conda 环境加下载缓存）。
 
@@ -82,7 +82,7 @@ ACTION=="add|bind", SUBSYSTEM=="usb", ATTR{idVendor}=="1bcf", ATTR{idProduct}=="
 
 ## 4. Anaconda
 
-装到 **`$HOME/anaconda3`**（安装器里选 Just Me、路径填这个）。不要装成 `~/miniconda3` 或其他目录：`run_real.sh` 写死了 `/home/cc/anaconda3/etc/profile.d/conda.sh`。
+装到 **`$HOME/anaconda3`**（安装器里选 Just Me、路径填这个）。`$HOME/miniconda3` 也能被 `scripts/lib/dev_tools.sh` 找到。不要装到这两个目录以外的路径，除非自己改 `dev_source_conda`。
 
 本机 conda 版本是 25.11.1。装完新开一个终端：
 

docs/GRIPPER.md

@@ -30,6 +30,8 @@ bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
 
 PGEA 力/速：`gripper.force_pct`（最低 20）`speed_pct` `open_position` `close_position`。RGI 不要 `auto_init: true`（`robots[]` 里已关）。
 
+平行爪的 `--cli` 在驱动没跑时会先把节点拉起来，再把命令发到 `/gripper/ctrl`。`auto_init` 仍用这一臂在 `site.yaml` 里的值（现场 `false`，不写 `0xA5`，不回零）。串口被别的进程占着时，报的是占用。
+
 ## ROS 节点
 
 单爪：

docs/HANDBOOK.md

@@ -34,7 +34,7 @@
 
 ### 1.3 一次完整任务分几步
 
-代码里用 **stage1 / stage2 / stage3** 三段串起来，一条命令跑完：
+代码里跟踪和对孔串成一条命令。搜孔轻压已取消，`run_all.sh` 不会再进入那一步，也不能用参数打开。旋入是单独的 `run_stage3_search.sh --screw`。
 
 ```
 run_all.sh
@@ -46,18 +46,13 @@ run_all.sh
   │                  YOLO 找工件外框 → Hough 找左右两个孔心 →
   │                  深度 + 内参 + 手眼 → 换算出孔的基座坐标 →
   │                  移动夹爪轴线到目标孔正上方 → 复拍验证 → 锁定
-  │
-  └─ stage3_search   柔顺搜孔：力闭环下的螺旋搜索，直到规的尖端"掉进"孔里，
-                     再十字微扫定心，最后在力限内下压（可选按螺距旋入）
 ```
 
-三段之间**在同一个进程里交接**，所以相机、触觉、手眼 TF 只初始化一次。
+跟踪和对孔在同一个进程里交接。盯日志：
-盯日志就能知道跑到哪了 —— 三行关键输出：
 
 ```
 [pipeline] === stage1_track ===      → 在等跟踪锁定
 [pipeline] === stage2_locate ===     → 在拍照对孔
-[pipeline] === stage3_search ===     → 触面 → ★ 入孔 → 轻压到位
 ```
 
 日常入口（三个终端，**必须同一个 `--arm`**）：
@@ -65,7 +60,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
 ```
 
@@ -112,8 +107,8 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
 
 | 能力 | 状态 | 怎么自己确认 |
 |------|------|--------------|
-| 三段串联（跟踪 → 对孔 → 搜孔轻压） | ✅ 真机跑通 | `run_all.sh --arm 89` |
+| 跟踪 → 对孔 | ✅ 真机跑通过。搜孔轻压已取消，这条命令不再下压搜孔 | `run_all.sh --arm 89` |
-| 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
+| 按螺距旋入（螺纹规） | ✅ 单独阶段。不带 `--screw` 的旧搜孔命令会直接拒绝 | `run_stage3_search.sh --arm 89 --screw` |
 | Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
 | Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
 | D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
@@ -510,7 +505,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/3** | 跟踪 / 对孔 / 搜孔，见 1.3 |
+| **stage1/2** | 跟踪 / 对孔，见 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,8 +57,7 @@ hole_detect:
 
 ## 三阶段串联（推荐日常入口）
 
-一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入**
+一条命令跑完：**视觉跟踪锁定 → 拍照对孔**。搜孔轻压已取消，这条命令不再下压，也不能用参数重新打开。旋入是后面的单独命令。
-（拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。
 
 ```bash
 # 终端 1 —— 控制栈（一直开着）
@@ -67,12 +66,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_all.sh --arm 89 --depth 6 --pitch 1.0
+bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
 ```
 
 `run_pipeline.sh` 是 `run_all.sh` 的别名。
@@ -85,9 +84,9 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
 1. `[pipeline] === stage1_track ===` → 等跟踪锁定
 2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
 3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
-4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入）
+对孔结束后臂停在当前高度。搜孔轻压不再接着跑。
 
-急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看
+急停：终端 3 Ctrl-C。失败时臂保持当前位姿，看
 `/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。
 
 高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、
@@ -149,7 +148,10 @@ 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` 直接退出，不会触面，也不会螺旋搜孔。
+下面是这个节点在旋入时仍会走的力控步骤，不是一条还能单独打开的搜孔命令。
 
 前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程：
 
@@ -167,12 +169,9 @@ 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；对孔已完成（臂保持不动）
-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 --screw
-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 读数是形变任意单位且

install/arm_bringup/share/arm_bringup/config/bringup.yaml

@@ -4,7 +4,7 @@ rizon_type: Rizon4
 # Flexiv Studio 夹爪（GN01）—— 保持 false。DH PGEA 使用 run_gripper.sh。
 load_gripper: false
 use_fake_hardware: false
-start_rviz: true
+start_rviz: false
 
 # Flexiv 官方关节命名：Rizon4-063989_joint1 ... joint7
 joint_prefix: Rizon4-063989_

install/arm_bringup/share/arm_bringup/launch/real.launch.py

@@ -74,6 +74,6 @@ def generate_launch_description():
         DeclareLaunchArgument('use_fake_hardware', default_value='false'),
         # 仅适用于 Flexiv-GN01。DH PGEA 用: bash ~/ros2/scripts/run_gripper.sh
         DeclareLaunchArgument('load_gripper', default_value='false'),
-        DeclareLaunchArgument('start_rviz', default_value='true'),
+        DeclareLaunchArgument('start_rviz', default_value='false'),
         OpaqueFunction(function=_launch_setup),
     ])

install/arm_bringup/share/arm_bringup/launch/sim.launch.py

@@ -35,7 +35,7 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('use_rviz', default_value='false'),
         DeclareLaunchArgument('use_moveit', default_value='false'),
         # 仿真**故意**不用真机的 SN 前缀：两边同时跑时，同名关节会让 TF 与
         # 控制器互相打架（症状是 RViz 里模型乱跳）。见 devices/sim/gazebo/README.md。

install/arm_bringup/share/arm_bringup/launch/stack.launch.py

@@ -201,7 +201,7 @@ def generate_launch_description():
             default_value='mock',
             description='mock | moveit | real | isaac',
         ),
-        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('use_rviz', default_value='false'),
         DeclareLaunchArgument('robot_sn', default_value=SITE_SN),
         DeclareLaunchArgument('load_gripper', default_value='false'),
         OpaqueFunction(function=_setup),

install/arm_control/share/arm_control/launch/mock_control.launch.py

@@ -47,7 +47,7 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('use_rviz', default_value='false'),
         DeclareLaunchArgument(
             'robot_sn',
             default_value=SITE_ROBOT_SN,

install/arm_description/share/arm_description/launch/display.launch.py

@@ -34,7 +34,7 @@ def generate_launch_description():
 
     return LaunchDescription([
         DeclareLaunchArgument('use_gui', default_value='true'),
-        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('use_rviz', default_value='false'),
         # 默认取当前臂（scripts/env.sh 已导出）；不再写死某个 SN。
         DeclareLaunchArgument(
             'robot_sn',

install/arm_moveit_config/share/arm_moveit_config/launch/move_group.launch.py

@@ -69,12 +69,21 @@ def _launch_setup(context, *args, **kwargs):
         ],
     )
 
-    return [move_group, rviz]
+    # 模型在工位程序里看。只有显式 start_rviz:=true 才再开 RViz。
+    nodes = [move_group]
+    if LaunchConfiguration('start_rviz').perform(context).strip().lower() in ('1', 'true', 'yes'):
+        nodes.append(rviz)
+    return nodes
 
 
 def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument('use_fake_hardware', default_value='true'),
+        DeclareLaunchArgument(
+            'start_rviz',
+            default_value='false',
+            description='机械臂模型在工位程序里看，默认不再弹出 RViz',
+        ),
         # 从环境取当前臂 SN，避免靠 xacro 里的写死默认值"蒙对"。
         DeclareLaunchArgument(
             'robot_sn',

scripts/env.sh

@@ -11,23 +11,24 @@ elif [[ -f /opt/ros/jazzy/setup.bash ]]; then
   source /opt/ros/jazzy/setup.bash
 fi
 
-if [[ -f "${HOME}/anaconda3/etc/profile.d/conda.sh" ]]; then
+# conda 可能在 ~/anaconda3 或 ~/miniconda3。不写死用户名。
-  # shellcheck disable=SC1091
+# shellcheck disable=SC1091
-  source "${HOME}/anaconda3/etc/profile.d/conda.sh"
+source "${ROOT}/scripts/lib/dev_tools.sh"
+dev_source_conda || true
+if command -v conda >/dev/null 2>&1; then
   # 干净切换：先把堆叠的激活退到 base，再激活真机环境。
-  # 同一 shell 里从仿真环境(ros_gz_sim)切过来时，不清干净会残留 PATH/PYTHONPATH，
+  # 同一 shell 里从仿真环境切过来时，不清干净会残留 PATH/PYTHONPATH。
-  # 导致 python/pip 解析到另一个环境的包，构建报 "--editable not recognized"
-  # 之类与真实原因无关的错。详见 scripts/lib/conda_switch.sh。
   _env_helper="${ROOT}/scripts/lib/conda_switch.sh"
+  _real_env="${REAL_ENV_NAME:-ros_flexiv}"
   if [[ -f "${_env_helper}" ]]; then
     # shellcheck disable=SC1090
     source "${_env_helper}"
-    conda_switch_clean "ros_flexiv" "${SIM_ENV_NAME:-ros_gz_sim}" || \
+    conda_switch_clean "${_real_env}" "${SIM_ENV_NAME:-ros_gz_sim}" || \
-      conda activate ros_flexiv 2>/dev/null || true
+      conda activate "${_real_env}" 2>/dev/null || true
   else
-    conda activate ros_flexiv 2>/dev/null || true
+    conda activate "${_real_env}" 2>/dev/null || true
   fi
-  unset _env_helper
+  unset _env_helper _real_env
 fi
 
 if [[ -f "${ROOT}/install/setup.bash" ]]; then
@@ -38,13 +39,14 @@ else
 fi
 export LD_LIBRARY_PATH="${HOME}/flexiv_install/lib:${LD_LIBRARY_PATH:-}"
 # pip 版 TensorRT（tensorrt-cu12）—— 加载 YOLO `.engine` 需要它
-_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/ros_flexiv}/lib/python3.11/site-packages"
+_pyver="$(python3 -c 'import sys; print(f"{sys.version_info[0]}.{sys.version_info[1]}")' 2>/dev/null || echo 3.11)"
+_TRT_SITE="${CONDA_PREFIX:-${HOME}/anaconda3/envs/${REAL_ENV_NAME:-ros_flexiv}}/lib/python${_pyver}/site-packages"
 for _d in "${_TRT_SITE}/tensorrt" "${_TRT_SITE}/tensorrt_libs" "${_TRT_SITE}/nvidia/tensorrt/lib"; do
   if [[ -d "${_d}" ]]; then
     export LD_LIBRARY_PATH="${_d}:${LD_LIBRARY_PATH}"
   fi
 done
-unset _d _TRT_SITE
+unset _d _TRT_SITE _pyver
 export YOLO_AUTOINSTALL=false
 export ROS2_WS="${ROOT}"
 

scripts/fetch_flexiv.sh

@@ -106,11 +106,11 @@ Done. Next steps (Ubuntu 22.04 / Humble + Rizon):
        source install/setup.bash
 
   4) Real robot (site SN, no gripper yet):
-       ros2 launch arm_bringup real.launch.py
+       ros2 launch arm_bringup real.launch.py start_rviz:=false
        # or explicitly:
-       ros2 launch arm_bringup real.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> load_gripper:=false
+       ros2 launch arm_bringup real.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> load_gripper:=false start_rviz:=false
 
   Official fake hardware:
-       ros2 launch flexiv_bringup rizon.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> use_fake_hardware:=true load_gripper:=false
+       ros2 launch flexiv_bringup rizon.launch.py robot_sn:=<site.yaml 里该臂的 robot.sn> use_fake_hardware:=true load_gripper:=false start_rviz:=false
 
 EOF

scripts/lib/rdk_status.py

@@ -117,10 +117,48 @@ def classify_state(
     )
 
 
-def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]:
+def pack_status(state: Dict[str, object]) -> Dict[str, object]:
-    """连一次机器人并读状态。返回给 classify_state 的关键字参数。"""
+    """把探针结果收成给界面的 JSON。
-    import flexivrdk  # 延迟导入：让本模块在没有 flexivrdk 的环境里也能被单测导入
 
+    ``q`` 不能传进 ``classify_state``：那个函数只认使能字段。
+    曾经用 ``classify_state(**state)``，关节角一读到就抛 TypeError，
+    JSON 根本没打出来，界面就永远停在“还没连上”。
+    """
+    fields = dict(state)
+    q = fields.pop("q", None)
+    level, summary, actions = classify_state(**fields)  # type: ignore[arg-type]
+    payload: Dict[str, object] = {
+        **fields,
+        "level": level,
+        "summary": summary,
+        "actions": actions,
+    }
+    if isinstance(q, list) and len(q) == 7:
+        payload["q"] = q
+    return payload
+
+
+def _open_robot(sn: str, iface: str, pc_ip: str):
+    """按 IPv4 白名单连机器人。网卡名不能直接交给 RDK。"""
+    import flexivrdk
+    from rdk_helpers import rdk_whitelist_lists
+
+    # 空名单才退回不指定网卡。指定了 eth0 这种名字时，FastDDS 会把真地址滤掉，
+    # 表现是 ping 得通、这里一直搜到超时。
+    candidates = rdk_whitelist_lists(iface, pc_ip) or [[]]
+    last = ""
+    for listen in candidates:
+        try:
+            if listen:
+                return flexivrdk.Robot(sn, list(listen), True, False), ""
+            return flexivrdk.Robot(sn), ""
+        except Exception as exc:  # noqa: BLE001
+            last = str(exc)
+    return None, last or "连不上 RDK"
+
+
+def _probe(sn: str, iface: str, enable: bool, pc_ip: str = "") -> Dict[str, object]:
+    """连一次机器人并读状态。返回给 classify_state 的关键字参数，另加关节角 ``q``。"""
     out: Dict[str, object] = {
         "connected": False,
         "operational": False,
@@ -130,13 +168,9 @@ def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]:
         "estop_released": True,
         "error": "",
     }
-    try:
+    robot, error = _open_robot(sn, iface, pc_ip)
-        if iface:
+    if robot is None:
-            robot = flexivrdk.Robot(sn, [iface], True, False)
+        out["error"] = error
-        else:
-            robot = flexivrdk.Robot(sn)
-    except Exception as exc:  # noqa: BLE001
-        out["error"] = str(exc)
         return out
 
     out["connected"] = True
@@ -157,6 +191,13 @@ def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]:
     out["op_status"] = str(_safe(robot.operational_status, ""))
     out["fault"] = bool(_safe(robot.fault, False))
     out["estop_released"] = bool(_safe(robot.estop_released, True))
+    # 只读关节角，给界面画姿态。读不到就空着，绝不在这里 Enable。
+    try:
+        q = [float(v) for v in list(robot.states().q)[:7]]
+    except Exception:  # noqa: BLE001
+        q = []
+    if len(q) == 7 and all(v == v for v in q):
+        out["q"] = q
     return out
 
 
@@ -164,18 +205,20 @@ def main(argv: Optional[Sequence[str]] = None) -> int:
     ap = argparse.ArgumentParser(description="读一次机械臂状态并给结论")
     ap.add_argument("--sn", required=True)
     ap.add_argument("--iface", default="")
+    ap.add_argument("--pc-ip", default="", help="本机机器人网卡的 IPv4，用来做 RDK 白名单")
     ap.add_argument("--enable", action="store_true", help="顺手尝试 Enable（会真的使能）")
     ap.add_argument("--out", default="", help="把 KEY=VALUE 写到这个文件")
     ap.add_argument("--json", action="store_true", help="把结果打到 stdout（JSON）")
     args = ap.parse_args(argv)
 
-    state = _probe(args.sn, args.iface, args.enable)
+    state = _probe(args.sn, args.iface, args.enable, args.pc_ip)
-    level, summary, actions = classify_state(**state)  # type: ignore[arg-type]
+    payload = pack_status(state)
-    payload = {**state, "level": level, "summary": summary, "actions": actions}
+    level = str(payload["level"])
+    actions = list(payload["actions"])
 
     if args.out:
         lines = [f"{k}={v}" for k, v in payload.items() if k != "actions"]
-        lines.append("actions=" + " | ".join(actions))
+        lines.append("actions=" + " | ".join(str(item) for item in actions))
         Path(args.out).write_text("\n".join(lines) + "\n", encoding="utf-8")
     if args.json:
         print(json.dumps(payload, ensure_ascii=False, indent=2))

scripts/run_all.sh

@@ -1,5 +1,5 @@
 #!/usr/bin/env bash
-# 三阶段装配流程的完整入口：一阶段跟踪 → 二阶段对孔 → 三阶段搜孔轻压。
+# 跟踪 → 对孔。搜孔轻压已取消，本脚本不再进入那一步，也不能用参数打开。
 #
 # 为什么要单独有这个脚本：
 #   pipeline.launch.py 只管两件事 —— 发布手眼静态 TF、起 assembly_pipeline 串联节点。
@@ -13,10 +13,10 @@
 #   4) exec pipeline.launch.py，把下面的 CLI 参数映射成它的 launch 参数
 #
 # 用法：
-#   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_all.sh --arm 89 --depth 6 --pitch 1.0
+#   旋入不在这里。用 run_stage3_search.sh --arm 89 --screw
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview     # 相机带 Qt 预览窗
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera          # 相机已在别处拉起了
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile         # 不起触觉服务
@@ -37,7 +37,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 # ---- 默认值：全部沿用 site.yaml / launch 默认，CLI 只做覆盖 ----
 SKIP_STAGE1=false
 SKIP_STAGE2=false
-SKIP_STAGE3=false
+SKIP_STAGE3=true  # 搜孔已取消。--skip-stage3 仍然接受，但没有开关能把它重新打开。
 HOLE=""
 DEPTH=""
 PITCH=""
@@ -83,6 +83,12 @@ while [[ $# -gt 0 ]]; do
   esac
 done
 
+if [[ -n "${DEPTH}" || -n "${PITCH}" ]]; then
+  echo "ERROR: 搜孔轻压已取消，整体流程不再搜孔，也不在这里旋入。" >&2
+  echo "       旋入请用: bash ~/ros2/scripts/run_stage3_search.sh --arm ${SITE_ROBOT_ARM:-89} --screw" >&2
+  exit 1
+fi
+
 if [[ -n "${FINAL_GAP}" && "${NO_DESCENT}" == true ]]; then
   echo "ERROR: 同时写了 --final-gap 和 --no-descent，不知道听谁的。" >&2
   exit 1
@@ -241,7 +247,7 @@ fi
 # 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。
 _SKIP_TACTILE_REASON=""
 if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then
-  echo "=== --skip-stage3：一/二阶段不用触觉，跳过拉起 ==="
+  echo "=== 搜孔已取消：跟踪和对孔不用触觉，跳过拉起 ==="
   WITH_TACTILE=false
   _SKIP_TACTILE_REASON="stage3"
 fi

scripts/run_camera_d405.sh

@@ -31,6 +31,8 @@ done
 
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/env.sh"
+# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
+export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/lib/load_site_config.sh"
 
@@ -127,12 +129,14 @@ DO_PREVIEW="${SITE_CAM_PREVIEW:-1}"
 if [[ "${PREVIEW}" -ge 0 ]]; then
   DO_PREVIEW="${PREVIEW}"
 fi
-if [[ "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
+# 程序界面只看彩色和深度。红外是旧的四窗格查看器才要的。
+# 彩色要等红外齐帧时，场景一动，整组帧对不齐，彩色就被丢掉。
+if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
   CAM_ARGS+=(-p enable_infra:=true)
 else
   CAM_ARGS+=(-p enable_infra:=false)
 fi
-if [[ "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
+if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
   CAM_ARGS+=(-p preview_stereo:=true)
 else
   CAM_ARGS+=(-p preview_stereo:=false)

scripts/run_camera_d435.sh

@@ -17,6 +17,8 @@ while [[ $# -gt 0 ]]; do
 done
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/env.sh"
+# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
+export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/lib/load_site_config.sh"
 SN="${SITE_D435_SERIAL:-}"

scripts/run_console.sh

@@ -8,10 +8,45 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/env.sh"
 export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
-if [[ -z "${DISPLAY:-}" && -S /tmp/.X11-unix/X1 ]]; then
+# 屏幕跟当前登录会话走。这台工控机是 :1，别的 Linux 桌面常常是 :0。
-  export DISPLAY=:1
+if [[ -n "${DISPLAY:-}" ]]; then
+  _disp="${DISPLAY#:}"
+  _disp="${_disp%%.*}"
+  if [[ ! -S "/tmp/.X11-unix/X${_disp}" ]]; then
+    unset DISPLAY
+  fi
+  unset _disp
 fi
-if [[ -z "${XAUTHORITY:-}" && -r "/run/user/$(id -u)/gdm/Xauthority" ]]; then
+if [[ -z "${DISPLAY:-}" ]]; then
+  _seat="$(who | awk -v u="$(id -un)" '$1==u && $2 ~ /^:/ {print $2; exit}')"
+  _seat="${_seat%%.*}"
+  _n="${_seat#:}"
+  if [[ -n "${_n}" && -S "/tmp/.X11-unix/X${_n}" ]]; then
+    export DISPLAY="${_seat}"
+  else
+    for _sock in /tmp/.X11-unix/X*; do
+      if [[ -S "${_sock}" ]]; then
+        export DISPLAY=":${_sock##*/X}"
+        break
+      fi
+    done
+  fi
+  unset _seat _n _sock
+fi
+# 有真实屏幕时不要留在 offscreen，否则 OpenGL 窗口起不来，模型也不会画到显卡上。
+if [[ "${QT_QPA_PLATFORM:-}" == "offscreen" && -n "${DISPLAY:-}" ]]; then
+  _disp="${DISPLAY#:}"
+  _disp="${_disp%%.*}"
+  if [[ -S "/tmp/.X11-unix/X${_disp}" ]]; then
+    unset QT_QPA_PLATFORM
+  fi
+  unset _disp
+fi
+if [[ -z "${XAUTHORITY:-}" ]]; then
+  if [[ -r "${HOME}/.Xauthority" ]]; then
+    export XAUTHORITY="${HOME}/.Xauthority"
+  elif [[ -r "/run/user/$(id -u)/gdm/Xauthority" ]]; then
     export XAUTHORITY="/run/user/$(id -u)/gdm/Xauthority"
+  fi
 fi
 exec python3 -m arm_application.operator_app

scripts/run_display.sh

@@ -1,7 +1,7 @@
 #!/usr/bin/env bash
-# 只看 URDF（joint_state_publisher_gui + RViz）
+# 发布 URDF。模型在工位程序里看，不再弹出 RViz 和关节滑条窗。
 set -euo pipefail
 ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 # shellcheck disable=SC1091
 source "${ROOT}/install/setup.bash"
-ros2 launch arm_description display.launch.py
+ros2 launch arm_description display.launch.py use_rviz:=false use_gui:=false

scripts/run_gripper.sh

@@ -61,6 +61,107 @@ fi
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/env.sh"
 
+# 平行爪节点起来之后串口是它的。再开一次会报占用，开合就失效。
+# 这时把命令交给节点已经在听的 /gripper/ctrl。
+pgea_node_up() {
+  local line
+  while IFS= read -r line; do
+    [[ "${line}" == *pgrep* ]] && continue
+    [[ -n "${line}" ]] && return 0
+  done < <(pgrep -af 'dh_pgea_node.py|lib/arm_application/dh_pgea' 2>/dev/null || true)
+  return 1
+}
+
+# 控制前把平行爪驱动拉起来。不 pkill 已经在跑的节点，也不写 0xA5。
+# auto_init 用 site 里这一臂的值（现场是 false，回零会把夹着的件丢掉）。
+# setsid：命令脚本退出后驱动还在，不会跟着会话一起被挂断。
+ensure_pgea_node() {
+  if pgea_node_up; then
+    return 0
+  fi
+  local port="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
+  if [[ ! -e "${port}" ]]; then
+    echo "ERROR: ${port} 不在。USB–RS485 没插上，夹爪驱动起不来。" >&2
+    return 1
+  fi
+  local node_py="${ROOT}/src/arm_application/arm_application/dh_pgea_node.py"
+  local short="${SITE_ROBOT_SHORT_ID:-89}"
+  local log_dir="/tmp/orisys_console"
+  local log="${log_dir}/gripper_node_${short}.log"
+  local auto="${SITE_GRIP_AUTO_INIT:-true}"
+  mkdir -p "${log_dir}"
+  echo "=== 平行爪驱动没在跑，先拉起来（auto_init=${auto}，不另写回零） ${port} ==="
+  setsid python3 "${node_py}" --ros-args \
+    -p port:="${port}" \
+    -p baud:="${SITE_GRIP_BAUD:-115200}" \
+    -p slave_id:="${SITE_GRIP_SLAVE:-1}" \
+    -p stroke_mm:="${SITE_GRIP_STROKE_MM:-20.0}" \
+    -p force_pct:="${SITE_GRIP_FORCE:-50}" \
+    -p speed_pct:="${SITE_GRIP_SPEED:-50}" \
+    -p auto_init:="${auto}" \
+    -p model:="${MODEL}" \
+    -p "serial_number:='${SITE_GRIP_SN:-}'" \
+    -p "rs485_rts:='${SITE_GRIP_RS485_RTS:-}'" \
+    >>"${log}" 2>&1 < /dev/null &
+  local pid=$!
+  local i
+  for i in $(seq 1 24); do
+    if ! kill -0 "${pid}" 2>/dev/null; then
+      echo "ERROR: 夹爪驱动起来就退出了。" >&2
+      tail -n 30 "${log}" >&2 || true
+      return 1
+    fi
+    if timeout 2 ros2 topic echo /gripper/states --once >/dev/null 2>&1; then
+      echo "=== 夹爪驱动已在发 /gripper/states（pid ${pid}）==="
+      return 0
+    fi
+  done
+  echo "ERROR: 夹爪驱动没有发出 /gripper/states。" >&2
+  tail -n 30 "${log}" >&2 || true
+  kill "${pid}" 2>/dev/null || true
+  return 1
+}
+
+if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then
+  ensure_pgea_node || exit 1
+fi
+
+if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]] && pgea_node_up; then
+  export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
+  export GRIP_CMD="${CLI[0]}"
+  export GRIP_VAL="${CLI[1]:-}"
+  export GRIP_FORCE="${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}"
+  export GRIP_SPEED="${GRIP_SPEED:-${SITE_GRIP_SPEED:-50}}"
+  echo "=== 平行爪节点已占 ${SITE_GRIP_PORT:-/dev/ttyUSB0}，命令走 /gripper/ctrl ==="
+  CTRL="$(python3 - <<'PY'
+import os
+from arm_application.dh_pgea import ctrl_values
+vals = ctrl_values(
+    os.environ.get("GRIP_CMD", ""),
+    os.environ.get("GRIP_VAL", ""),
+    int(os.environ.get("GRIP_FORCE") or "50"),
+    int(os.environ.get("GRIP_SPEED") or "50"),
+)
+if vals is None:
+    raise SystemExit(2)
+print(", ".join(str(v) for v in vals))
+PY
+)" && rc=0 || rc=$?
+  if [[ "${rc}" -eq 2 ]]; then
+    exec timeout 4 ros2 topic echo /gripper/states --once
+  fi
+  if [[ "${rc}" -ne 0 ]]; then
+    echo "ERROR: 无法把夹爪命令转成 /gripper/ctrl" >&2
+    exit 1
+  fi
+  exec timeout 8 ros2 topic pub --once /gripper/ctrl std_msgs/msg/Float32MultiArray "{data: [${CTRL}]}"
+fi
+
+if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then
+  echo "ERROR: 平行爪命令没有送到 /gripper/ctrl。" >&2
+  exit 1
+fi
+
 if [[ ${#CLI[@]} -gt 0 ]]; then
   if [[ "${IS_ROTARY}" -eq 1 ]]; then
     CMD="${CLI[0]}"

scripts/run_real.sh

@@ -19,7 +19,8 @@ while [[ $# -gt 0 ]]; do
     --list|-l) LIST_ONLY=true; shift ;;
     -h|--help)
       sed -n '2,8p' "$0"
-      echo "  Extra args are passed to ros2 launch (e.g. use_rviz:=false)."
+      echo "  机械臂模型在工位程序里看，这里默认不开 RViz。"
+      echo "  其余参数传给 ros2 launch。要临时打开 RViz：use_rviz:=true"
       exit 0
       ;;
     *) break ;;
@@ -127,4 +128,4 @@ if ! "${RDK_PYTHON}" "${ROOT}/scripts/rdk_switch_tool.py" "${RDK_TOOL_ARGS[@]}";
   exit 1
 fi
 
-exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" "$@"
+exec ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:="${ROBOT_SN}" use_rviz:=false "$@"

scripts/run_sim.sh

@@ -1,5 +1,5 @@
 #!/usr/bin/env bash
-# 快速启动：mock 机械臂 + RViz
+# 快速启动 mock 机械臂。模型在工位程序里看，不再弹出 RViz。
 set -euo pipefail
 ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 # shellcheck disable=SC1091
@@ -16,4 +16,4 @@ source "${ROOT}/install/setup.bash"
 # shellcheck source=lib/ros_domain.sh
 source "${ROOT}/scripts/lib/ros_domain.sh"
 ros_domain_force_real
-ros2 launch arm_bringup sim.launch.py
+ros2 launch arm_bringup sim.launch.py use_rviz:=false

scripts/run_sim_gz.sh

@@ -13,7 +13,7 @@
 #     bash scripts/run_sim_gz.sh --no-gripper --no-sensors   # 最省资源的运动学验证
 #
 # 与 scripts/run_sim.sh 的区别（两个名字容易混，这里说清）：
-#     run_sim.sh     ← 走 arm_bringup 的 **mock** 驱动：假数据 + RViz，无物理
+#     run_sim.sh     ← 走 arm_bringup 的 **mock** 驱动：假数据，无物理，模型在工位程序里看
 #     run_sim_gz.sh  ← 走 Gazebo：真物理、真接触
 #   两者都不需要真机。要验证力控/接触类逻辑只能用后者。
 #

scripts/run_stage3_search.sh

@@ -1,18 +1,16 @@
 #!/usr/bin/env bash
-# 三阶段柔顺搜孔：触面 → 力闭环螺旋搜孔 → 十字定心 → 力限插入/旋入 → 锁定。
+# 旋转装配入口。不带 --screw 会直接拒绝，不再单独做搜孔轻压。
 # 前置（各占一个终端）：
 #   T1  bash ~/ros2/scripts/run_real.sh --arm 89          # 控制器
 #   ——  二阶段已完成（run_stage2_locate.sh --arm 89），臂保持悬停不动
 # 本脚本会自动拉起触觉传感器服务（仅左臂 89 装了 Orisys 传感器）。
 #
 # 用法：
-#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89                 # 搜孔+落座（site 默认深度）
+#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw                 # 深度和螺距只读 site
-#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --hole k2       # 对 k2 孔
+#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --hole k2 --screw
-#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0       # 只搜孔定心，不插入
+#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 --screw       # 临时改搜索半径
-#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0   # 螺纹规旋入(M6=1.0)
+#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap --screw        # 旋入前用 D405 复拍
-#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5       # 临时改搜索半径
+#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --no-tactile --screw    # 不起触觉服务（调试用）
-#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap        # 搜前用 D405 复拍对准
-#   bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --no-tactile    # 不起触觉服务（调试用）
 #
 # 参数固定接口：config/site.yaml → hole_detect.stage3（力阈值/螺旋/插入全部可调）
 # 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看 Orisys 读数量级
@@ -22,6 +20,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 HOLE=""
 DEPTH=""
 PITCH=""
+SCREW=false
 RMAX=""
 RESNAP=false
 WITH_TACTILE=true
@@ -39,7 +38,8 @@ while [[ $# -gt 0 ]]; do
     --r-max) RMAX="${2:?}"; shift 2 ;;
     --resnap) RESNAP=true; shift ;;
     --no-tactile) WITH_TACTILE=false; shift ;;
-    -h|--help) sed -n '2,24p' "$0"; exit 0 ;;
+    --screw) SCREW=true; shift ;;
+    -h|--help) sed -n '2,19p' "$0"; exit 0 ;;
     *) echo "Unknown: $1" >&2; exit 1 ;;
   esac
 done
@@ -49,6 +49,44 @@ source "${ROOT}/scripts/env.sh"
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/lib/load_site_config.sh"
 
+if [[ "${SCREW}" != true ]]; then
+  echo "ERROR: 搜孔轻压已取消。这一步不再单独跑，也不再跟在跟踪和对孔后面。" >&2
+  echo "       要旋入请加 --screw，深度和螺距只读 site.yaml。" >&2
+  exit 1
+fi
+
+if [[ "${SCREW}" == true ]]; then
+  if [[ -n "${DEPTH}" || -n "${PITCH}" ]]; then
+    echo "ERROR: --screw 不要再写 --depth 或 --pitch。深度和螺距只在 site.yaml。" >&2
+    exit 1
+  fi
+  _screw_line="$(python3 - "${SITE_CONFIG}" << 'PY'
+import sys
+import yaml
+
+path = sys.argv[1]
+with open(path, encoding="utf-8") as handle:
+    data = yaml.safe_load(handle) or {}
+stage3 = ((data.get("hole_detect") or {}).get("stage3") or {})
+depth = stage3.get("screw_stage_depth_mm")
+pitch = stage3.get("screw_stage_pitch_mm")
+if not isinstance(depth, (int, float)) or not isinstance(pitch, (int, float)):
+    sys.stderr.write(
+        "ERROR: site.yaml 的 hole_detect.stage3 缺少 screw_stage_depth_mm 或 screw_stage_pitch_mm。\n"
+    )
+    sys.exit(1)
+if float(depth) <= 0 or float(pitch) == 0:
+    sys.stderr.write(
+        f"ERROR: 旋转装配深度必须大于 0、螺距不能为 0，现在是 depth={depth} pitch={pitch}，这样旋不进去。\n"
+    )
+    sys.exit(1)
+print(f"{float(depth)} {float(pitch)}")
+PY
+)"
+  DEPTH="${_screw_line%% *}"
+  PITCH="${_screw_line#* }"
+fi
+
 if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
   export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
   export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"

src/arm_application/arm_application/dh_pgea.py

@@ -213,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 客户端（官方寄存器映射）。"""
 
@@ -257,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:
@@ -562,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(

src/arm_application/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])

src/arm_application/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

src/arm_application/arm_application/operator_camera_view.py

@@ -1,6 +1,12 @@
 """工位程序里的相机预览。
 
 只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
+
+回调里先丢掉当前页没在看的流。彩色按相机原分辨率收进来，不按时钟抽帧，
+也不在收图的线程里做颜色转换。另一条线只画最新的一张，所以帧率跟相机走，
+848 和 1280 都不会为了帧率被缩成 480。深度是伪彩色，仍然抽稀，
+而且没打开深度时不订阅，避免大图挤占彩色。
+手眼页要 JPEG 时也按原分辨率编码，只编正在看的那一路。
 """
 
 from __future__ import annotations
@@ -11,33 +17,121 @@ 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 targets() -> list[dict]:
+def preview_targets(site: dict) -> list[dict]:
-    """真机腕部相机和手外相机。仿真相机不进预览。"""
+    """主界面只要左臂腕部相机和共用的手外相机。
-    from arm_application.operator_console import _arms, _site
+
-    from arm_application.site_io import overhead_camera
+    右臂腕部不进这一屏。仿真相机名字以 ``sim`` 开头，也不进。
+    推理框的角色仍是 ``wrist`` / ``overhead``，跟话题对得上。
+    """
+    from arm_application.site_io import listed_arms, overhead_camera
 
     found: list[dict] = []
     seen: set[str] = set()
-    for arm in _arms():
+    left = next((row for row in listed_arms(site) if row.get("side") == "left"), None)
-        name = str(arm.get("camera") or "").strip().strip("/")
+    name = str((left or {}).get("camera") or "").strip().strip("/")
-        if not name or name.startswith("sim") or name in seen:
+    if name and not name.startswith("sim"):
-            continue
+        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"{arm.get('label') or name} 腕部"})
+        found.append({"id": name, "label": f"左腕 {tag}", "role": "wrist"})
-    cam, _others = overhead_camera(_site(), None)
+    cam, _others = overhead_camera(site or {}, None)
-    name = str((cam or {}).get("name") or "").strip().strip("/")
+    oname = str((cam or {}).get("name") or "").strip().strip("/")
-    if name and not name.startswith("sim") and name not in seen:
+    if oname and not oname.startswith("sim") and oname not in seen:
-        found.append({"id": name, "label": str((cam or {}).get("label") or "手外相机")})
+        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 {
@@ -53,141 +147,492 @@ def frame(name: str, kind: str) -> bytes | None:
         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 _jpeg(img: np.ndarray) -> bytes | None:
+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 img.size == 0:
+    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
-    view = img
+    ok, buf = cv2.imencode(
-    if view.shape[1] > 640:
+        ".jpg",
-        scale = 640.0 / float(view.shape[1])
+        cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR),
-        view = cv2.resize(view, (640, max(1, int(view.shape[0] * scale))))
+        [int(cv2.IMWRITE_JPEG_QUALITY), 70],
-    ok, buf = cv2.imencode(".jpg", view, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
+    )
     if not ok:
         return None
     return buf.tobytes()
 
 
-def _depth_bgr(depth: np.ndarray) -> np.ndarray:
+def _as_array(data, dtype, count: int) -> np.ndarray:
-    import cv2
+    """优先直接看消息缓冲。缓冲不支持时才拷一份。"""
-
+    try:
-    vis = np.zeros((depth.shape[0], depth.shape[1], 3), np.uint8)
+        arr = np.frombuffer(data, dtype=dtype, count=count)
-    valid = depth > 0
+    except (TypeError, BufferError, ValueError):
-    if not bool(valid.any()):
+        arr = np.frombuffer(bytes(data), dtype=dtype, count=count)
-        return vis
+    if arr.size < count:
-    values = depth.astype(np.float32)
+        raise ValueError("图像字节数不够")
-    lo, hi = np.percentile(values[valid], [5, 95])
+    return arr
-    hi = max(float(hi), float(lo) + 1.0)
-    gray = (np.clip((values - lo) / (hi - lo), 0.0, 1.0) * 255.0).astype(np.uint8)
-    gray[~valid] = 0
-    colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
-    colored[~valid] = 0
-    return colored
 
 
-def _color_array(msg) -> np.ndarray | None:
+def _color_view(msg) -> np.ndarray | None:
     if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
         return None
-    arr = np.frombuffer(bytes(msg.data), dtype=np.uint8)
     need = int(msg.height) * int(msg.width) * 3
-    if arr.size < need:
+    arr = _as_array(msg.data, np.uint8, need)
-        return None
+    return arr.reshape(int(msg.height), int(msg.width), 3)
-    arr = arr[:need].reshape(int(msg.height), int(msg.width), 3)
-    if msg.encoding == "rgb8":
-        arr = arr[:, :, ::-1]
-    return np.ascontiguousarray(arr)
 
 
-def _depth_array(msg) -> np.ndarray | None:
+def _depth_view(msg) -> np.ndarray | None:
     if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
         return None
-    arr = np.frombuffer(bytes(msg.data), dtype=np.uint16)
     need = int(msg.height) * int(msg.width)
-    if arr.size < need:
+    arr = _as_array(msg.data, np.uint16, need)
-        return None
+    return arr.reshape(int(msg.height), int(msg.width))
-    return np.ascontiguousarray(arr[:need].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 qos_profile_sensor_data
+    from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data
     from sensor_msgs.msg import Image
-
+    from std_msgs.msg import Float32MultiArray
-    latest: dict[str, np.ndarray] = {}
-    lock = threading.Lock()
 
     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),
+                    self._color(name, role),
                     qos_profile_sensor_data,
                 )
-                self.create_subscription(
+
+        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 _color(self, name: str):
             def _cb(msg: Image) -> None:
-                arr = _color_array(msg)
+                # 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。
-                if arr is None:
+                try:
+                    view = _color_view(msg)
+                    if view is None:
                         return
-                with lock:
+                    offer_color(key, view, bgr=(msg.encoding == "bgr8"), role=role)
-                    latest[f"{name}_color"] = arr
+                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:
-                arr = _depth_array(msg)
+                try:
-                if arr is None:
+                    view = _depth_view(msg)
+                    if view is None:
                         return
-                with lock:
+                    offer_depth(key, view)
-                    latest[f"{name}_depth"] = arr
+                except Exception as exc:  # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
+                    with _LOCK:
+                        _STATE["error"] = str(exc)
             return _cb
 
     rclpy.init()
     node = _Node()
-    next_encode = 0.0
     try:
         while not _STOP.is_set():
-            rclpy.spin_once(node, timeout_sec=0.05)
+            node.sync_depth()
-            now = time.monotonic()
+            rclpy.spin_once(node, timeout_sec=0.01)
-            if now < next_encode:
-                continue
-            next_encode = now + 0.12
-            with lock:
-                snap = dict(latest)
-                latest.clear()
-            frames = {}
-            blobs = {}
-            for key, arr in snap.items():
-                name, _, kind = key.rpartition("_")
-                img = _depth_bgr(arr) if kind == "depth" else arr
-                blob = _jpeg(img)
-                if not blob:
-                    continue
-                blobs[key] = blob
-                frames[key] = True
-            if blobs:
-                with _LOCK:
-                    _JPEG.update(blobs)
-                    _STATE["frames"].update(frames)
-                    _STATE["stamp"] = time.time()
-                    _STATE["error"] = ""
     except Exception as exc:
         with _LOCK:
             _STATE["error"] = str(exc)
@@ -200,8 +645,8 @@ def _loop(cams: list[dict]) -> None:
 
 
 def start() -> str:
-    global _THREAD
+    global _THREAD, _PAINT_THREAD
-    cams = targets()
+    cams = stream_targets()
     if not cams:
         with _LOCK:
             _STATE["error"] = "site 里没有可预览的真机相机"
@@ -213,14 +658,18 @@ def start() -> str:
         _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} 的彩色和深度，不含左右红外。"
+    return f"预览订了 {names} 的彩色原图，深度点开才订，不含左右红外。"
 
 
 def stop() -> None:
     _STOP.set()
-    thread = _THREAD
+    _WAKE.set()
+    for thread in (_THREAD, _PAINT_THREAD):
         if thread is not None:
             thread.join(timeout=1.5)

src/arm_application/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,22 +698,60 @@ 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:
-            proc = subprocess.run(
+        if spec.get("sync"):
+            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,
             )
+            _remember(action, proc)
+            try:
+                detail, _ = proc.communicate(timeout=float(spec.get("timeout") or 40))
             except subprocess.TimeoutExpired:
+                try:
+                    os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
+                except OSError:
+                    pass
                 return {"ok": False, "message": "命令超时还没返回", "detail": shown}
-        detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+            finally:
+                _forget(action)
+            detail = (detail or "").strip()
             log_path.write_text(detail)
             ok = proc.returncode == 0
             return {
@@ -569,8 +768,16 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
             stderr=subprocess.STDOUT,
             start_new_session=True,
         )
-    _PROCS[action] = proc.pid
+        _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:
+            detail, _ = proc.communicate(timeout=90)
         except subprocess.TimeoutExpired:
+            try:
+                os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
+            except OSError:
+                pass
             return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
-    detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+    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)

src/arm_application/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

src/arm_application/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))

src/arm_application/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}

src/arm_application/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
 
+    release_for_control()
+    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()
+            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()
-    return {
+            message = str(result.get("error") or result)
-        "ok": True,
+            if restore:
-        "message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。",
+                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": "软拖拽已关闭，臂保持当前位置。"}
 
 

src/arm_application/arm_application/pipeline_node.py

@@ -1,11 +1,11 @@
 #!/usr/bin/env python3
-"""三阶段串联：视觉跟踪锁定 → 孔定位 → 搜孔并轻轻按压（默认不拧入）。
+"""跟踪锁定 → 孔定位。搜孔轻压已取消，本节点不再下压搜孔。
 
 流畅性（相对 bash 杀进程再起节点）：
   - 相机 / 触觉 / 手眼 TF 全程只起一次
   - 一阶段锁定后 SIGTERM 跟踪节点，立刻 pin 当前关节，不回零、不抬臂
-  - 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完立刻下压搜孔
+  - 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完就停，不再下压搜孔
-  - 找到孔位后力闭环轻压保持；拧入是后续阶段（--depth 才插入）
+  - 旋入不在这个节点里
 """
 
 from __future__ import annotations
@@ -125,7 +125,7 @@ class AssemblyPipeline(Stage3Search):
         super().__init__(node_name=node_name)
         self.declare_parameter("skip_stage1", False)
         self.declare_parameter("skip_stage2", False)
-        self.declare_parameter("skip_stage3", False)  # 默认搜孔+轻压；拧入靠 --depth
+        self.declare_parameter("skip_stage3", True)  # 搜孔轻压已取消，写成 false 也不再下压
         self.declare_parameter("d435_move_only", False)
         self.declare_parameter("init_z_m", -1.0)  # >0：启动后先到这个法兰高度
         self.declare_parameter("stage1_timeout_sec", 150.0)
@@ -333,7 +333,9 @@ class AssemblyPipeline(Stage3Search):
     def run(self) -> int:
         skip1 = bool(self.get_parameter("skip_stage1").value)
         skip2 = bool(self.get_parameter("skip_stage2").value)
-        skip3 = bool(self.get_parameter("skip_stage3").value)
+        if not bool(self.get_parameter("skip_stage3").value):
+            self.get_logger().info("搜孔轻压已取消，忽略要求重新打开它的参数")
+        skip3 = True
         if self.d435_move_only:
             skip2 = True
             skip3 = True
@@ -365,11 +367,7 @@ class AssemblyPipeline(Stage3Search):
                     return code
                 self.get_logger().info(
                     f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
-                    + (
+                    " — 搜孔已取消，停在这里。旋入请另跑。"
-                        " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
-                        if skip3
-                        else " — 立刻下压搜孔并轻压（不拧入）"
-                    )
                 )
             else:
                 self._state("skip_stage2")
@@ -383,7 +381,7 @@ class AssemblyPipeline(Stage3Search):
                     )
                 else:
                     self.get_logger().info(
-                        "跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
+                        "搜孔已取消 —— 臂停在当前高度。"
                         "15s 后本进程退出，臂保持不动"
                     )
                 if final:

src/arm_application/arm_application/realsense_color_pub.py

@@ -176,6 +176,10 @@ class RealsenseColorPub(TolerantNode):
         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()
@@ -309,9 +313,12 @@ class RealsenseColorPub(TolerantNode):
         self._info_pub.publish(info)
 
         if depth_u16 is not None:
+            self._publish_depth(depth_u16, stamp)
+
+    def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None:
         dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
         dmsg = Image()
-            dmsg.header.stamp = stamp
+        dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
         dmsg.header.frame_id = self._frame_id
         dmsg.height = dh
         dmsg.width = dw
@@ -457,7 +464,9 @@ class RealsenseColorPub(TolerantNode):
                         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
@@ -508,6 +517,13 @@ class RealsenseColorPub(TolerantNode):
 
         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
@@ -609,7 +625,7 @@ class RealsenseColorPub(TolerantNode):
 
         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:
@@ -702,18 +718,60 @@ class RealsenseColorPub(TolerantNode):
             )
             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')
@@ -724,44 +782,45 @@ class RealsenseColorPub(TolerantNode):
                     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()
+                else:
+                    self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
                 continue
-                if misses == 1:
-                    self.get_logger().warn(f'wait_for_frames：{exc}')
-                continue
-            if not color:
-                misses += 1
-                continue
-            misses = 0
-            bgr = np.asanyarray(color.get_data())
-            depth = frames.get_depth_frame()
             depth_u16 = np.asanyarray(depth.get_data()) if depth else None
             if depth_u16 is not None and self._depth_to_mm != 1.0:
                 # 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
                 depth_u16 = np.clip(
                     depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
                 ).astype(np.uint16)
+            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 帧才做一次
@@ -794,7 +853,6 @@ class RealsenseColorPub(TolerantNode):
                     )
             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:

src/arm_application/arm_application/site_io.py

@@ -94,6 +94,80 @@ def site_path() -> Path:
     return ros2_root() / "config" / "site.yaml"
 
 
+def arm_short_id(sn: str) -> str:
+    """序列号末两位就是臂号（``Rizon4-063989`` → ``89``）。
+
+    和 ``scripts/lib/export_site_env.arm_short_id`` 同一条规则。换一台臂
+    只改 ``site.yaml`` 的 SN，不要在界面里再写一张本机对照表。
+    """
+    tail = str(sn or "").strip().rsplit("-", 1)[-1]
+    return tail[-2:] if tail.isdigit() and len(tail) >= 2 else ""
+
+
+def listed_arms(data: dict[str, Any] | None) -> list[dict[str, Any]]:
+    """``robots[]`` 里的真机臂。``sn: sim`` 不进控制。
+
+    旋转爪只看夹爪型号里有没有 ``RGI``。右臂也可以是平行爪，不能靠臂号猜。
+    """
+    robots = ((data or {}).get("robot") or {}).get("robots") or []
+    out: list[dict[str, Any]] = []
+    seen: set[str] = set()
+    for robot in robots:
+        if not isinstance(robot, dict):
+            continue
+        sn = str(robot.get("sn") or "").strip()
+        if not sn or sn.lower() == "sim":
+            continue
+        arm_no = arm_short_id(sn)
+        if not arm_no:
+            continue
+        if arm_no in seen:
+            raise ValueError(f"两台臂的序列号末两位都是 {arm_no}，--arm 分不清")
+        seen.add(arm_no)
+        side = str(robot.get("arm") or "").strip().lower()
+        grip = robot.get("gripper") if isinstance(robot.get("gripper"), dict) else {}
+        model = str(grip.get("model") or "")
+        shared = (data or {}).get("gripper") if isinstance((data or {}).get("gripper"), dict) else {}
+        stroke = grip.get("stroke_mm")
+        if stroke is None:
+            stroke = shared.get("stroke_mm") or 20.0
+        out.append({
+            "id": arm_no,
+            "label": str(robot.get("label") or "").strip() or f"臂{arm_no}",
+            "sn": sn,
+            "side": side if side in ("left", "right") else "",
+            "tactile": bool(robot.get("tactile")),
+            "camera": str((robot.get("camera") or {}).get("name") or "") if isinstance(robot.get("camera"), dict) else "",
+            "gripper": model,
+            "rotary": "RGI" in model.upper(),
+            "iface": str(robot.get("iface") or ""),
+            "pc_ip": str(robot.get("pc_ip") or ""),
+            "port": str(grip.get("port") or ""),
+            "stroke_mm": float(stroke),
+        })
+    return out
+
+
+def usb_expectations(data: dict[str, Any] | None) -> dict[str, int]:
+    """设备页上「应有几台」跟 ``site.yaml`` 走，不写死本机的 2+1+2。"""
+    cameras = (data or {}).get("cameras") or {}
+    d405 = 0
+    d435 = 0
+    if isinstance(cameras, dict):
+        for key, val in cameras.items():
+            model = str((val or {}).get("model") or "") if isinstance(val, dict) else ""
+            token = f"{key} {model}".lower()
+            if "d405" in token:
+                d405 += 1
+            elif "d435" in token:
+                d435 += 1
+    tactile = (data or {}).get("tactile") or {}
+    orisys = 0
+    if isinstance(tactile, dict):
+        orisys = sum(1 for key in ("left_video", "right_video") if tactile.get(key) is not None)
+    return {"d405": d405, "d435": d435, "orisys": orisys}
+
+
 def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
     p = Path(path).expanduser() if path else site_path()
     if not p.is_file():

src/arm_application/launch/pipeline.launch.py

@@ -159,7 +159,7 @@ def generate_launch_description() -> LaunchDescription:
         [
             DeclareLaunchArgument("skip_stage1", default_value="false"),
             DeclareLaunchArgument("skip_stage2", default_value="false"),
-            DeclareLaunchArgument("skip_stage3", default_value="false"),
+            DeclareLaunchArgument("skip_stage3", default_value="true"),
             DeclareLaunchArgument("target_hole", default_value=""),
             DeclareLaunchArgument("insert_depth_mm", default_value=""),
             DeclareLaunchArgument("screw_pitch_mm", default_value=""),

src/arm_application/test/test_camera_preview.py

@@ -0,0 +1,273 @@
+"""相机预览必须在抽稀后的小图上做，并且贴图不能走 setPixmap。
+
+以前每一帧都在整幅深度上算百分位再编码 JPEG，界面线程再解码，
+六路一起刷的时候整页布局跟着重算。这里的时间预算按那个旧路径会红。
+"""
+
+import time
+
+import numpy as np
+import pytest
+
+from arm_application import operator_camera_view as camera_view
+
+
+def test_unwatched_frames_are_dropped_and_the_first_watched_frame_is_taken() -> None:
+    try:
+        camera_view.watch(set(), set())
+        assert camera_view.wants("cam_color") == (False, False)
+        assert not camera_view.accept_frame("cam_color")
+        camera_view.watch({"cam_color"}, set())
+        assert camera_view.wants("cam_color") == (True, False)
+        assert camera_view.accept_frame("cam_color")
+        assert not camera_view.accept_frame("cam_color")
+        camera_view.watch(set(), {"cam_color"})
+        assert camera_view.wants("cam_color") == (False, True)
+        assert camera_view.accept_frame("cam_color")
+    finally:
+        camera_view.watch(set(), set())
+
+
+def test_depth_preview_stays_small_and_keeps_invalid_pixels_black() -> None:
+    blank = np.zeros((48, 64), np.uint16)
+    black = camera_view.depth_preview(blank)
+    assert black.shape == (48, 64, 3)
+    assert int(black.sum()) == 0
+
+    wide = np.full((90, 1280), 400, np.uint16)
+    colored = camera_view.depth_preview(wide)
+    assert colored.shape[1] == 480
+    assert colored.shape[0] < 90
+    assert colored.shape[2] == 3
+    assert int(colored.sum()) > 0
+
+
+def test_color_preview_is_rgb_and_not_wider_than_the_widget_budget() -> None:
+    image = np.zeros((20, 30, 3), np.uint8)
+    image[5, 5] = (0, 0, 255)
+    out = camera_view.rgb_preview(image, bgr=True)
+    assert tuple(int(v) for v in out[5, 5]) == (255, 0, 0)
+
+    wide = np.zeros((720, 1280, 3), np.uint8)
+    wide[:, :, 0] = 10
+    fitted = camera_view.rgb_preview(wide, bgr=True)
+    assert fitted.shape == (720, 1280, 3)
+    wrist = np.zeros((480, 848, 3), np.uint8)
+    assert camera_view.rgb_preview(wrist, bgr=False).shape == (480, 848, 3)
+    assert camera_view._COLOR_WIDTH >= 1280
+    assert camera_view._INTERVAL <= 1.0 / 30.0 + 1e-9
+    with pytest.raises(ValueError, match="彩色预览"):
+        camera_view.rgb_preview(np.zeros((8, 8), np.uint8), bgr=True)
+
+
+def test_preview_build_stays_inside_the_frame_interval() -> None:
+    """三路 720p 彩色加三路深度。旧路径光深度就要大约 60ms，100ms 的刷新会被占满。"""
+    color = np.random.randint(0, 255, (720, 1280, 3), np.uint8)
+    depth = np.random.randint(1, 3000, (720, 1280), np.uint16)
+
+    def once() -> None:
+        for _cam in range(3):
+            camera_view.rgb_preview(color, bgr=True)
+            camera_view.depth_preview(depth)
+
+    once()
+    samples = []
+    for _step in range(5):
+        started = time.perf_counter()
+        once()
+        samples.append(time.perf_counter() - started)
+    samples.sort()
+    assert samples[len(samples) // 2] < 0.045
+
+
+def test_frame_label_paints_rgb_without_set_pixmap() -> None:
+    """setPixmap 会把父布局重算一遍。预览框的 pixmap() 必须一直是空的。"""
+    import os
+
+    pytest.importorskip("PySide6")
+    os.environ.setdefault("QT_QPA_PLATFORM", "offscreen")
+    from PySide6.QtWidgets import QApplication
+
+    from arm_application.operator_app import FrameLabel
+
+    _app = QApplication.instance() or QApplication([])
+    label = FrameLabel("没有画面")
+    label.resize(500, 320)
+    label.show()
+    rgb = np.zeros((270, 480, 3), np.uint8)
+    rgb[:, :, 0] = 220
+    blob = rgb.tobytes()
+    label.show_rgb(blob, 480, 270)
+    assert label.pixmap() is None or label.pixmap().isNull()
+    assert not label._scaled.isNull()
+    again = []
+    for _step in range(5):
+        fresh = bytes(blob)
+        started = time.perf_counter()
+        label.show_rgb(fresh, 480, 270)
+        again.append(time.perf_counter() - started)
+    again.sort()
+    assert again[len(again) // 2] < 0.02
+    label.close()
+
+
+def test_preview_interval_is_fast_enough_for_the_main_view() -> None:
+    """主界面要看运动和推理框。退回 0.10s 就是大约 10 帧，这里会红。"""
+    assert camera_view._INTERVAL <= 1.0 / 30.0 + 1e-9
+
+
+def test_detection_box_scales_onto_the_preview_and_empty_clears() -> None:
+    rgb = np.zeros((50, 100, 3), np.uint8)
+    boxed = camera_view.paint_detection(rgb, (0, 0, 50, 25), 100, 50)
+    assert int(rgb.sum()) == 0
+    assert int(boxed[0, 10, 1]) == 220
+    assert int(boxed[40, 90].sum()) == 0
+    same = camera_view.paint_detection(rgb, (), 100, 50)
+    assert int(same.sum()) == 0
+
+    camera_view._DET.clear()
+    try:
+        assert "没有框" in camera_view.inference_status()
+        camera_view._remember_detection("wrist", [0, 0, 10, 10, 0.86, 0, 0, 0, 0])
+        text = camera_view.inference_status()
+        assert "腕部 置信 0.86" in text
+        assert "手外 没有框" in text
+        camera_view._remember_detection("wrist", [])
+        assert "腕部 没有框" in camera_view.inference_status()
+    finally:
+        camera_view._DET.clear()
+        camera_view._CLEAN.clear()
+
+
+def test_detection_is_drawn_on_the_camera_frame() -> None:
+    """框、跟踪点要落在相机预览上，不能只写在画面外面的一行字里。"""
+    camera_view._DET.clear()
+    camera_view._CLEAN.clear()
+    camera_view._PREVIEW.pop("cam_color", None)
+    try:
+        rgb = np.zeros((40, 80, 3), np.uint8)
+        camera_view._remember_clean("cam_color", rgb, "wrist", 160, 80)
+        camera_view._remember_detection(
+            "wrist", [20, 10, 60, 40, 0.91, 1, 40, 24, 0.2]
+        )
+        blob, width, height = camera_view.previews()["cam_color"]
+        frame = np.frombuffer(blob, np.uint8).reshape(height, width, 3)
+        assert int(frame[12, 20, 0]) == 255
+        assert int(frame[12, 20, 1]) == 48
+    finally:
+        camera_view._DET.clear()
+        camera_view._CLEAN.clear()
+        camera_view._PREVIEW.pop("cam_color", None)
+
+
+def test_color_burst_keeps_native_resolution() -> None:
+    """两帧紧挨着来也不能抽掉，预览必须仍是相机原分辨率。"""
+    camera_view._RAW.clear()
+    camera_view._PREVIEW.clear()
+    camera_view._META.clear()
+    camera_view._STAMPS.clear()
+    camera_view._PAINTED.clear()
+    try:
+        camera_view.watch({"cam_color"}, set())
+        image = np.zeros((720, 1280, 3), np.uint8)
+        camera_view.offer_color("cam_color", image, bgr=True, role="wrist")
+        camera_view.offer_color("cam_color", image, bgr=True, role="wrist")
+        camera_view.publish_latest()
+        blob, width, height = camera_view.previews()["cam_color"]
+        assert (height, width) == (720, 1280)
+        assert len(blob) == 720 * 1280 * 3
+        assert camera_view.preview_meta()["cam_color"][:2] == (1280, 720)
+    finally:
+        camera_view.watch(set(), set())
+        camera_view._RAW.clear()
+        camera_view._PREVIEW.clear()
+        camera_view._META.clear()
+        camera_view._STAMPS.clear()
+        camera_view._PAINTED.clear()
+
+
+def test_rate_uses_the_last_second_and_keeps_the_source_size() -> None:
+    camera_view._META.clear()
+    camera_view._STAMPS.clear()
+    try:
+        camera_view._mark_rate("cam_color", 1280, 720)
+        time.sleep(0.05)
+        camera_view._mark_rate("cam_color", 1280, 720)
+        width, height, fps = camera_view.preview_meta()["cam_color"]
+        assert (width, height) == (1280, 720)
+        assert fps > 1.0
+    finally:
+        camera_view._META.clear()
+        camera_view._STAMPS.clear()
+
+
+def test_sim_cameras_are_a_separate_pair_for_the_same_view() -> None:
+    """仿真腕部和手外要能在主界面里看，但不能把真机那两路挤掉。"""
+    site = {
+        "robot": {
+            "robots": [
+                {"sn": "Rizon4-063989", "arm": "left", "camera": {"name": "d405_left"}},
+                {"sn": "sim", "arm": "sim", "camera": {"name": "sim_d405"}},
+            ]
+        },
+        "cameras": {
+            "d405_left": {"name": "d405_left", "model": "d405", "role": "left_wrist"},
+            "sim_d405": {"name": "sim_d405", "model": "d405", "role": "sim_wrist", "arm": "sim"},
+            "d435": {"name": "d435", "model": "d435", "role": "overhead_eye_to_hand"},
+            "sim_d435": {
+                "name": "sim_d435",
+                "model": "d435",
+                "role": "overhead_eye_to_hand",
+                "arm": "sim",
+            },
+        },
+    }
+    real = camera_view.preview_targets(site)
+    sim = camera_view.sim_preview_targets(site)
+    both = camera_view.stream_targets(site)
+    assert [cam["id"] for cam in real] == ["d405_left", "d435"]
+    assert [cam["id"] for cam in sim] == ["sim_d405", "sim_d435"]
+    assert [cam["id"] for cam in both] == ["d405_left", "d435", "sim_d405", "sim_d435"]
+
+
+def test_main_preview_is_left_wrist_and_shared_overhead_only() -> None:
+    """右臂腕部和仿真手外不进主界面。左腕和共用 D435 才进。"""
+    site = {
+        "robot": {
+            "robots": [
+                {
+                    "sn": "Rizon4-063989",
+                    "arm": "left",
+                    "label": "左",
+                    "camera": {"name": "d405_left"},
+                },
+                {
+                    "sn": "Rizon4-063991",
+                    "arm": "right",
+                    "label": "右",
+                    "camera": {"name": "d405_right"},
+                },
+                {"sn": "sim", "arm": "sim", "camera": {"name": "sim_d405"}},
+            ]
+        },
+        "cameras": {
+            "d405_left": {"name": "d405_left", "model": "d405", "role": "left_wrist"},
+            "d405_right": {"name": "d405_right", "model": "d405", "role": "right_wrist"},
+            "sim_d435": {
+                "name": "sim_d435",
+                "model": "d435",
+                "role": "overhead_eye_to_hand",
+                "arm": "sim",
+            },
+            "d435": {
+                "name": "d435",
+                "model": "d435",
+                "role": "overhead_eye_to_hand",
+            },
+        },
+    }
+    cams = camera_view.preview_targets(site)
+    assert [cam["id"] for cam in cams] == ["d405_left", "d435"]
+    assert [cam["role"] for cam in cams] == ["wrist", "overhead"]
+    assert cams[0]["label"] == "左腕 D405"
+    assert cams[1]["label"] == "手外 D435"

src/arm_application/test/test_command_lanes.py

@@ -0,0 +1,103 @@
+"""命令按设备分通道。相机不该等夹爪，软拖拽也不该叠在位置环上。"""
+
+import subprocess
+
+import pytest
+
+from arm_application.operator_console import command_blocked_message, command_lane
+from arm_application import operator_teach as teach
+
+
+def test_camera_does_not_wait_for_the_gripper():
+    assert command_lane("camera_d405") == "camera"
+    assert command_lane("gripper_open") == "grip"
+    assert command_lane("tactile_up") == "tactile"
+    assert command_blocked_message({"grip"}, "camera") == ""
+    assert command_blocked_message({"camera"}, "teach") == ""
+
+
+def test_drag_waits_only_when_this_arm_is_still_moving():
+    assert command_lane("stage_track") == "arm"
+    assert command_lane("handeye_teach") == "teach"
+    assert command_blocked_message({"arm"}, "teach") == "这台臂还在动，软拖拽先不开"
+    assert command_blocked_message({"teach"}, "arm") == "软拖拽占着这台臂，运动先不开"
+    assert command_blocked_message({"teach"}, "teach") == "软拖拽还没返回"
+    assert command_blocked_message(set(), "teach") == ""
+
+
+def test_stop_is_not_queued_behind_another_command():
+    assert command_lane("stop_commands") == "stop"
+    assert command_blocked_message({"arm", "teach", "grip", "camera"}, "stop") == ""
+
+
+def test_stop_kills_only_the_recorded_process_group():
+    from arm_application import operator_console as console
+
+    keep = subprocess.Popen(["sleep", "30"], start_new_session=True)
+    victim = subprocess.Popen(["sleep", "30"], start_new_session=True)
+    console._remember("demo", victim)
+    try:
+        result = console.stop_started()
+        assert result["ok"] is True
+        assert "demo" in result["message"]
+        victim.wait(timeout=2)
+        assert keep.poll() is None
+        assert "demo" not in console._LIVE
+    finally:
+        for proc in (keep, victim):
+            if proc.poll() is None:
+                proc.kill()
+                proc.wait(timeout=2)
+
+
+def test_finger_names_sit_above_the_pictures():
+    import os
+
+    os.environ.setdefault("QT_QPA_PLATFORM", "offscreen")
+    pytest.importorskip("PySide6")
+    from PySide6.QtWidgets import QApplication, QLabel, QPushButton
+
+    from arm_application.operator_app import FrameLabel, StationApp
+
+    app = QApplication.instance() or QApplication([])
+    win = StationApp()
+    try:
+        win.resize(1440, 900)
+        win.show()
+        app.processEvents()
+        names = [
+            label for label in win._pages["main"].findChildren(QLabel)
+            if label.text() in ("左指", "右指") and not isinstance(label, FrameLabel)
+        ]
+        assert sorted(label.text() for label in names) == ["右指", "左指"]
+        pictures = win._pic
+        assert pictures["left_shear"]._badge_title == "切向"
+        assert pictures["right_thermal"]._badge_title == "热像"
+        for label in names:
+            side = "left" if label.text() == "左指" else "right"
+            shot = pictures[f"{side}_shear"]
+            assert shot.geometry().top() > 0
+            assert label.geometry().bottom() <= shot.geometry().top()
+        shot = pictures["left_shear"]
+        shot.resize(180, 100)
+        assert shot._picture_rect().bottom() <= shot.height() - 24
+        assert any(button.text() == "停止命令" for button in win._pages["main"].findChildren(QPushButton))
+    finally:
+        win.close()
+        app.processEvents()
+
+
+def test_drag_refuses_before_opening_another_link_when_the_position_loop_stays(monkeypatch):
+    monkeypatch.setattr(
+        "arm_application.operator_arm_state.stack_running",
+        lambda: True,
+    )
+    monkeypatch.setattr(
+        teach,
+        "_switch_arm_controller",
+        lambda _activate: "没能停下关节轨迹控制器：拒绝",
+    )
+    result = teach.drag_on("Rizon4-063989")
+    assert result["ok"] is False
+    assert "位置环还在推" in result["message"]
+    assert teach._BRIDGE is None

src/arm_application/test/test_dh_pgea_latch.py

@@ -14,6 +14,8 @@ from __future__ import annotations
 from pathlib import Path
 
 from arm_application.dh_pgea import (
+    ctrl_values,
+    open_failure_text,
     REG_POSITION,
     S_GRIP_ARRIVED,
     S_GRIP_CAUGHT,
@@ -94,6 +96,62 @@ def test_force_init_does_not_return_on_the_stale_finished_bit():
     assert wrote["n"] == 1
 
 
+def test_node_owned_port_uses_ctrl_topic_not_a_second_open() -> None:
+    assert ctrl_values("open", force=30, speed=40) == (0.0, 1000.0, 30.0, 40.0)
+    assert ctrl_values("close", force=20, speed=10) == (0.0, 0.0, 20.0, 10.0)
+    assert ctrl_values("pos", "250", force=33, speed=25) == (0.0, 250.0, 33.0, 25.0)
+    assert ctrl_values("init") == (1.0, 0.0, 50.0, 50.0)
+    assert ctrl_values("status") is None
+    try:
+        ctrl_values("pos", "1200")
+    except ValueError as exc:
+        assert "0–1000" in str(exc)
+    else:
+        raise AssertionError("超范围位置应当失败")
+
+
+def test_exclusive_lock_is_not_called_a_power_failure():
+    exc = OSError(11, "Could not exclusively lock port /dev/ttyUSB0: Resource temporarily unavailable")
+    text = open_failure_text("/dev/ttyUSB0", exc)
+    assert "占用" in text
+    assert "24 V" not in text
+
+
+def test_a_real_open_failure_still_names_power_and_wiring():
+    exc = FileNotFoundError(2, "No such file or directory")
+    text = open_failure_text("/dev/ttyUSB9", exc)
+    assert "24 V" in text
+    assert "RS485" in text
+
+
+def test_parallel_cli_stops_before_start_when_the_port_is_missing(tmp_path):
+    """缺串口必须在拉起驱动之前失败，不能去开真口，也不能写回零。"""
+    import os
+    import subprocess
+
+    root = Path(__file__).resolve().parents[3]
+    site = (root / "config" / "site.yaml").read_text(encoding="utf-8")
+    cfg = tmp_path / "site.yaml"
+    cfg.write_text(site.replace("/dev/ttyUSB0", "/dev/ttyUSB-missing-test"), encoding="utf-8")
+    env = os.environ.copy()
+    env["SITE_CONFIG"] = str(cfg)
+    proc = subprocess.run(
+        ["bash", str(root / "scripts" / "run_gripper.sh"), "--arm", "89", "--cli", "probe"],
+        cwd=str(root),
+        env=env,
+        capture_output=True,
+        text=True,
+        timeout=90,
+    )
+    out = (proc.stdout or "") + (proc.stderr or "")
+    assert proc.returncode != 0
+    assert "不在" in out
+    assert "先拉起来" not in out
+    script = (root / "scripts" / "run_gripper.sh").read_text(encoding="utf-8")
+    assert 'auto="${SITE_GRIP_AUTO_INIT:-true}"' in script
+    assert '-p auto_init:="${auto}"' in script
+
+
 def test_official_client_uses_the_same_latch_break():
     src = Path(__file__).resolve().parents[1] / "arm_application" / "dh_official.py"
     text = src.read_text(encoding="utf-8")

src/arm_application/test/test_handeye_preview.py

@@ -0,0 +1,81 @@
+"""手眼页的棋盘预览和采点参数。不打开相机，也不动臂。"""
+
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from arm_application.operator_console import render_action_args
+from arm_application.operator_handeye_board import (
+    annotate_board,
+    coverage_fraction,
+    coverage_words,
+)
+
+
+def test_coverage_words_follow_the_thirty_to_seventy_band():
+    assert "偏小" in coverage_words(0.29)
+    assert coverage_words(0.30) == "大小合适"
+    assert coverage_words(0.70) == "大小合适"
+    assert "偏大" in coverage_words(0.71)
+
+
+def test_coverage_fraction_is_the_corner_box():
+    corners = np.array([[10.0, 10.0], [30.0, 10.0], [30.0, 40.0], [10.0, 40.0]])
+    assert coverage_fraction(corners, 100, 100) == pytest.approx(0.06)
+    assert coverage_fraction(corners, 0, 100) == 0.0
+
+
+def test_blank_frame_is_not_called_a_board():
+    import cv2
+
+    image = np.full((80, 120, 3), 255, np.uint8)
+    ok, buf = cv2.imencode(".jpg", image)
+    assert ok
+    blob, text = annotate_board(buf.tobytes(), 5, 4)
+    assert blob
+    assert "没有这块板" in text
+
+
+def test_drawn_board_is_found_and_keeps_a_jpeg():
+    import cv2
+
+    cols, rows, cell, border = 5, 4, 30, 20
+    width = (cols + 1) * cell + 2 * border
+    height = (rows + 1) * cell + 2 * border
+    image = np.full((height, width), 255, np.uint8)
+    for y in range(rows + 1):
+        for x in range(cols + 1):
+            if (x + y) % 2 == 0:
+                image[
+                    border + y * cell:border + (y + 1) * cell,
+                    border + x * cell:border + (x + 1) * cell,
+                ] = 0
+    color = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
+    ok, buf = cv2.imencode(".jpg", color)
+    assert ok
+    blob, text = annotate_board(buf.tobytes(), cols, rows)
+    assert "棋盘已找到" in text
+    assert cv2.imdecode(np.frombuffer(blob, np.uint8), cv2.IMREAD_COLOR) is not None
+
+
+def test_missing_board_size_does_not_pretend_to_search():
+    blob, text = annotate_board(b"not-a-jpeg", 0, 0)
+    assert blob == b"not-a-jpeg"
+    assert "内角点" in text
+
+
+def test_teach_args_take_the_point_count_and_append():
+    template = ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "{points}"]
+    assert render_action_args(template, "01", "eye_to_hand", {"points": 8}) == [
+        "--arm", "01", "teach", "--mode", "eye_to_hand", "--points", "8",
+    ]
+    appended = render_action_args(template, "01", "eye_in_hand", {"points": 12, "append": True})
+    assert appended[-1] == "--append"
+    with pytest.raises(ValueError, match="4–30"):
+        render_action_args(template, "01", "eye_in_hand", {"points": 2})
+
+
+def test_append_is_refused_on_a_command_that_is_not_teach():
+    with pytest.raises(ValueError, match="示教采点"):
+        render_action_args(["--arm", "{arm}", "calibrate"], "01", "eye_in_hand", {"append": True})

src/arm_application/test/test_listed_arms.py

@@ -0,0 +1,168 @@
+"""换一台 Linux 上的机械臂，界面和命令必须认 site.yaml，不能只认本机 89/91。"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+import pytest
+
+from arm_application.site_io import arm_short_id, listed_arms, load_site_dict, usb_expectations
+
+
+def test_short_id_is_the_last_two_digits():
+    assert arm_short_id("Rizon4-777001") == "01"
+    assert arm_short_id("Rizon4-063989") == "89"
+    assert arm_short_id("sim") == ""
+    assert arm_short_id("") == ""
+
+
+def test_another_station_is_listed_and_this_stations_ids_are_not_invented():
+    site = {
+        "robot": {
+            "robots": [
+                {
+                    "sn": "Rizon4-777001",
+                    "arm": "left",
+                    "label": "甲",
+                    "gripper": {"model": "PGEA-9", "port": "/dev/ttyFAKE0"},
+                },
+                {
+                    "sn": "Rizon4-777002",
+                    "arm": "right",
+                    "label": "乙",
+                    "gripper": {"model": "RGI-9", "port": "/dev/ttyFAKE1"},
+                },
+                {"sn": "sim", "arm": "sim", "gripper": {"model": "RGI-9"}},
+            ]
+        }
+    }
+    rows = listed_arms(site)
+    assert [row["id"] for row in rows] == ["01", "02"]
+    assert rows[0]["rotary"] is False
+    assert rows[0]["side"] == "left"
+    assert rows[1]["rotary"] is True
+    assert "89" not in {row["id"] for row in rows}
+
+
+def test_right_arm_with_a_parallel_gripper_is_not_treated_as_rotary():
+    site = {
+        "robot": {
+            "robots": [
+                {"sn": "Rizon4-555091", "arm": "right", "gripper": {"model": "PGEA-100-20-W"}},
+            ]
+        }
+    }
+    rows = listed_arms(site)
+    assert rows[0]["id"] == "91"
+    assert rows[0]["rotary"] is False
+
+
+def test_duplicate_short_ids_fail_instead_of_dropping_an_arm():
+    site = {
+        "robot": {
+            "robots": [
+                {"sn": "Rizon4-100089", "arm": "left"},
+                {"sn": "Rizon4-200089", "arm": "right"},
+            ]
+        }
+    }
+    with pytest.raises(ValueError, match="89"):
+        listed_arms(site)
+
+
+def test_usb_count_follows_the_camera_list():
+    site = {
+        "cameras": {"d405_only": {"model": "d405"}},
+        "tactile": {"left_video": 0},
+    }
+    expect = usb_expectations(site)
+    assert expect == {"d405": 1, "d435": 0, "orisys": 1}
+
+
+def test_this_station_still_lists_89_and_91():
+    rows = listed_arms(load_site_dict())
+    ids = [row["id"] for row in rows]
+    assert ids == ["89", "91"]
+    by_id = {row["id"]: row for row in rows}
+    assert by_id["89"]["rotary"] is False
+    assert by_id["91"]["rotary"] is True
+
+
+def test_main_page_holds_status_sim_cameras_tactile_and_actions():
+    import os
+
+    os.environ.setdefault("QT_QPA_PLATFORM", "offscreen")
+    pytest.importorskip("PySide6")
+    from PySide6.QtWidgets import QApplication, QPushButton, QScrollArea
+
+    from arm_application.operator_app import StationApp
+
+    app = QApplication.instance() or QApplication([])
+    win = StationApp()
+    try:
+        assert [key for key, _button in win._nav] == ["main", "handeye", "teach"]
+        assert set(win._pages) == {"main", "handeye", "teach"}
+        assert win._stack.currentWidget() is win._pages["main"]
+        assert win.minimumWidth() <= 1600
+        assert win.minimumHeight() <= 900
+        main = win._pages["main"]
+        assert main.findChildren(QScrollArea) == []
+        win.resize(1440, 900)
+        app.processEvents()
+        assert main.minimumSizeHint().height() <= 860
+        texts = [button.text() for button in main.findChildren(QPushButton)]
+        for name in ("跟踪并对孔", "只测旋转装配", "复位到 home", "本臂腕部相机"):
+            assert any(name in text for text in texts)
+        power = [button for button in main.findChildren(QPushButton) if "上电控制栈" in button.text()]
+        assert power and not power[0].icon().isNull()
+        assert win._cam_pic
+        assert all(label.minimumHeight() <= 96 for label in win._cam_pic.values())
+        from arm_application.operator_camera_view import targets
+
+        cams = targets()
+        assert [cam["role"] for cam in cams] == ["wrist", "overhead"]
+        assert "right" not in cams[0]["id"] and not cams[0]["id"].startswith("sim")
+        assert not cams[1]["id"].startswith("sim")
+        assert set(win._cam_pic) == {
+            f"{cam['id']}_{kind}" for cam in cams for kind in ("color", "depth")
+        }
+        assert win._color_box.hasHeightForWidth()
+        assert 240 <= win._color_box.heightForWidth(960) <= 340
+        work = win._color_box.parentWidget().layout()
+        assert work.stretch(work.indexOf(win._color_box)) == 0
+        assert work.stretch(work.indexOf(win._tac_box)) >= 1
+        assert win._tac_box.minimumHeight() >= 160
+        assert win._tac_box.maximumHeight() > 400
+        split = main.layout()
+        assert split.stretch(0) < split.stretch(1)
+        assert set(win._pic) == {"left_shear", "left_thermal", "right_shear", "right_thermal"}
+        assert win._arm_sketch is not None
+        assert win._grip_sketch.parent() is main or main.isAncestorOf(win._grip_sketch)
+        assert 140 <= win._grip_sketch.height() or win._grip_sketch.maximumHeight() <= 180
+        assert win._grip_sketch.maximumHeight() <= 180
+        assert win._pos.value() == 500
+        assert any("按此夹持" in text for text in texts)
+        assert not win._rotary.isVisible()
+        back = [
+            button.text()
+            for button in win._pages["handeye"].findChildren(QPushButton)
+        ]
+        assert any("返回主界面" in text for text in back)
+        assert "眼在手上" in back or any("眼在手上" in text for text in back)
+    finally:
+        win.close()
+        app.processEvents()
+
+
+def test_operator_code_does_not_keep_a_local_serial_table():
+    root = Path(__file__).resolve().parents[1] / "arm_application"
+    for name in (
+        "operator_console.py",
+        "operator_gripper_state.py",
+        "operator_arm_state.py",
+        "operator_teach.py",
+        "operator_app.py",
+    ):
+        text = (root / name).read_text(encoding="utf-8")
+        assert 'endswith("063989")' not in text
+        assert 'not in ("89", "91")' not in text

src/arm_application/test/test_operator_arm_mesh.py

@@ -0,0 +1,270 @@
+"""厂家 Rizon4 网格能按关节角摆起来，而且零位时法兰比底座高。"""
+
+import time
+
+from arm_application.operator_arm_mesh import posed_gl, posed_triangles
+
+
+def test_vendor_mesh_stands_at_zero() -> None:
+    triangles = posed_triangles([0.0] * 7)
+    assert len(triangles) > 1000
+    zs = [p[2] for tri in triangles for p in tri[:3]]
+    assert min(zs) < 0.05
+    assert max(zs) > 0.9
+    assert all(v == v for tri in triangles for p in tri[:3] for v in p)
+
+
+def test_vendor_materials_are_not_flat_gray() -> None:
+    """白壳和灰蓝关节来自两份 Kd。抽稀之后两种都还在，否则界面又会变成一片灰。"""
+    colors = {tuple(round(c, 2) for c in tri[4]) for tri in posed_triangles([0.0] * 7)}
+    assert len(colors) >= 2
+    assert max(max(rgb) for rgb in colors) > 0.9
+    assert min(min(rgb) for rgb in colors) < 0.6
+
+
+def test_joint2_moves_the_shell() -> None:
+    upright = posed_triangles([0.0] * 7)
+    bent = posed_triangles([0.0, 1.2, 0.0, 0.0, 0.0, 0.0, 0.0])
+    z_up = max(p[2] for tri in upright for p in tri[:3])
+    z_bent = max(p[2] for tri in bent for p in tri[:3])
+    assert abs(z_up - z_bent) > 0.05
+
+
+def test_numpy_pose_matches_the_single_placement() -> None:
+    """连杆旋转不是单位阵时，数组乘法和逐点公式必须重合。差一个下标整条臂会散开。"""
+    from arm_application.operator_arm_mesh import _models, _place, _posed_arrays
+    from arm_application.operator_arm_state import link_frames
+
+    joints = [0.2, -0.8, 0.4, 1.1, -0.3, 0.6, 0.2]
+    frames = link_frames(joints)
+    world, _shade, _rgb = _posed_arrays(joints)
+    offset = 0
+    checked = 0
+    for link, parts in _models().items():
+        frame = frames.get(link)
+        if frame is None:
+            continue
+        rot, origin = frame
+        for part in parts:
+            count = part["verts"].shape[0]
+            if count == 0:
+                continue
+            sample = world[offset + count // 2, 1]
+            vertex = tuple(float(v) for v in part["verts"][count // 2, 1])
+            erot = tuple(tuple(float(v) for v in row) for row in part["erot"])
+            epos = tuple(float(v) for v in part["epos"])
+            placed = _place(rot, origin, erot, epos, vertex)
+            assert max(abs(placed[axis] - float(sample[axis])) for axis in range(3)) < 1e-6
+            offset += count
+            checked += 1
+    assert checked >= 8
+    assert offset == world.shape[0]
+
+
+def test_pose_update_stays_under_the_ui_budget() -> None:
+    """界面刷新是 800ms。姿态计算若回到逐顶点的 Python 循环（约 0.3s），这里会红。"""
+    posed_gl([0.0] * 7)
+    samples = []
+    for index in range(5):
+        joints = [0.0, 0.2 * index, 0.0, 0.0, 0.0, 0.0, 0.0]
+        started = time.perf_counter()
+        blob, count, _center, radius = posed_gl(joints)
+        samples.append(time.perf_counter() - started)
+        assert count > 1000
+        assert len(blob) == count * 24
+        assert radius > 0.2
+    samples.sort()
+    assert samples[len(samples) // 2] < 0.08
+
+
+def test_gpu_drag_stays_under_the_frame_budget() -> None:
+    """拖视角只改观察矩阵。以前每帧在 CPU 上重投影要 150ms，这里必须远低于这个数，并且真的画出了外壳。"""
+    import os
+    from pathlib import Path
+
+    import pytest
+
+    pytest.importorskip("PySide6")
+    display = os.environ.get("DISPLAY", "")
+    if os.environ.get("QT_QPA_PLATFORM") == "offscreen" or not display.startswith(":"):
+        pytest.skip("没有可用的显示，OpenGL 控件画不出来")
+    if not Path(f"/tmp/.X11-unix/X{display[1:]}").exists():
+        pytest.skip("显示套接字不在")
+    from PySide6.QtWidgets import QApplication
+
+    from arm_application.operator_app import ArmSketch
+
+    app = QApplication.instance() or QApplication([])
+    view = ArmSketch()
+    view.resize(800, 600)
+    view.show()
+    view.set_pose([0.2, -0.6, 0.3, 1.0, 0.0, 0.4, 0.0], True)
+    frame = view.grabFramebuffer()
+    assert _lit_pixels(frame) > 2000
+    samples = []
+    for _step in range(12):
+        view._yaw += 0.2
+        started = time.perf_counter()
+        frame = view.grabFramebuffer()
+        samples.append(time.perf_counter() - started)
+    view.close()
+    samples.sort()
+    assert _lit_pixels(frame) > 2000
+    assert samples[len(samples) // 2] < 0.02
+
+
+def test_base_and_flange_frames_follow_the_kinematics() -> None:
+    """基座在原点，法兰和正运动学的最后一个点是同一个点，旋转不另写一套。"""
+    from arm_application.operator_arm_state import display_frames, fk_points, link_frames
+
+    joints = [0.2, -0.5, 0.1, 0.8, -0.2, 0.4, 0.1]
+    frames = display_frames(joints)
+    assert [item["name"] for item in frames] == ["基座", "法兰"]
+    assert frames[0]["pos"] == (0.0, 0.0, 0.0)
+    tip = fk_points(joints)[-1]
+    assert max(abs(frames[1]["pos"][axis] - tip[axis]) for axis in range(3)) < 1e-9
+    assert frames[1]["rot"] == link_frames(joints)["link7"][0]
+
+
+def test_zero_pose_flange_stands_above_the_base() -> None:
+    from arm_application.operator_arm_state import display_frames
+
+    frames = display_frames([0.0] * 7)
+    assert frames[1]["pos"][2] > frames[0]["pos"][2] + 0.5
+
+
+def test_surface_height_is_not_a_workpiece_frame() -> None:
+    """上表面只有一个 z。把它画成坐标系会编出原点和朝向。"""
+    from arm_application.operator_arm_state import workpiece_frame
+    from arm_application.site_io import load_site_dict
+
+    assert workpiece_frame({"gauge_assembly": {"workpiece_surface_z_m": 0.022}}) is None
+    heights = []
+    for robot in (load_site_dict().get("robot") or {}).get("robots") or []:
+        gauge = robot.get("gauge_assembly") or {}
+        if not isinstance(gauge, dict):
+            continue
+        if "workpiece_frame" in gauge:
+            raise AssertionError("现场多了工件坐标系，要改成断言法兰切到工件上")
+        height = gauge.get("workpiece_surface_z_m")
+        if isinstance(height, (int, float)) and not isinstance(height, bool) and float(height) != 0.0:
+            heights.append(float(height))
+            assert workpiece_frame(robot) is None
+    assert heights
+
+
+def test_a_real_workpiece_frame_replaces_the_flange() -> None:
+    import pytest
+
+    from arm_application.operator_arm_state import display_frames, workpiece_frame
+
+    robot = {
+        "gauge_assembly": {
+            "workpiece_surface_z_m": 0.022,
+            "workpiece_frame": {"xyz": [0.3, -0.1, 0.022], "rpy": [0.0, 0.0, 1.2]},
+        }
+    }
+    workpiece = workpiece_frame(robot)
+    assert workpiece is not None
+    frames = display_frames([0.2, -0.4, 0.0, 0.6, 0.0, 0.2, 0.0], workpiece)
+    assert [item["name"] for item in frames] == ["基座", "工件"]
+    assert frames[1]["pos"] == (0.3, -0.1, 0.022)
+    flange = display_frames([0.2, -0.4, 0.0, 0.6, 0.0, 0.2, 0.0])[1]["pos"]
+    assert frames[1]["pos"] != flange
+    with pytest.raises(ValueError, match="工件坐标系"):
+        workpiece_frame({"gauge_assembly": {"workpiece_frame": {"xyz": [0.0, 0.0, 0.02]}}})
+
+
+def test_ground_grid_lies_on_the_floor() -> None:
+    """RViz 的地面格子改画在仿真里。线必须在 z=0，间距写坏时要报错。"""
+    import pytest
+
+    from arm_application.operator_arm_mesh import ground_grid
+
+    lines = ground_grid()
+    assert len(lines) == 18
+    assert all(start[2] == 0.0 and end[2] == 0.0 for start, end in lines)
+    assert ((-1.0, 0.0, 0.0), (1.0, 0.0, 0.0)) in lines
+    assert ((0.0, -1.0, 0.0), (0.0, 1.0, 0.0)) in lines
+    with pytest.raises(ValueError, match="地平面格子"):
+        ground_grid(step=0)
+
+
+def test_launch_defaults_do_not_open_rviz() -> None:
+    """入口脚本和 MoveIt 启动默认不再拉起 RViz。改回 true 这里会红。"""
+    from pathlib import Path
+
+    root = Path(__file__).resolve().parents[3]
+    assert "use_rviz:=false" in (root / "scripts/run_real.sh").read_text(encoding="utf-8")
+    assert "use_rviz:=false" in (root / "scripts/run_sim.sh").read_text(encoding="utf-8")
+    move_group = (root / "src/arm_moveit_config/launch/move_group.launch.py").read_text(encoding="utf-8")
+    assert "return [move_group, rviz]" not in move_group
+    assert "default_value='false'" in move_group
+
+
+def test_screen_projection_matches_the_shader() -> None:
+    """坐标系和网格用同一套投影。+Z 在画面上方，基座落在地平面上。改回 ``-p.z`` 会红。"""
+    from arm_application.operator_app import _MESH_VERT
+    from arm_application.operator_arm_mesh import axis_tip, screen_point
+
+    assert "p.z * u_sy" in _MESH_VERT
+    assert "-p.z * u_sy" not in _MESH_VERT
+    origin_x, origin_y = screen_point((0.0, 0.0, 0.0), 0.0, 0.0, (0.0, 0.0, 0.0), 1.0, 800, 600)
+    _zx, z_y = screen_point((0.0, 0.0, 0.2), 0.0, 0.0, (0.0, 0.0, 0.0), 1.0, 800, 600)
+    x_x, _xy = screen_point((0.2, 0.0, 0.0), 0.0, 0.0, (0.0, 0.0, 0.0), 1.0, 800, 600)
+    assert abs(origin_x - 400) < 1.0
+    assert abs(origin_y - 300) < 1.0
+    assert z_y < origin_y - 5.0
+    assert x_x > origin_x + 5.0
+    ident = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))
+    assert axis_tip((0.0, 0.0, 0.0), ident, 2, 0.15) == (0.0, 0.0, 0.15)
+
+
+def test_double_click_resets_the_view_and_leaves_the_joints() -> None:
+    """双击只把观察角放回初始值。实时关节继续画，不能被改成零位。"""
+    import os
+
+    import pytest
+
+    pytest.importorskip("PySide6")
+    os.environ.setdefault("QT_QPA_PLATFORM", "offscreen")
+    from PySide6.QtCore import QEvent, QPointF, Qt
+    from PySide6.QtGui import QMouseEvent
+    from PySide6.QtWidgets import QApplication
+
+    from arm_application.operator_app import _VIEW_PITCH, _VIEW_YAW, ArmSketch
+
+    _app = QApplication.instance() or QApplication([])
+    view = ArmSketch()
+    bent = [0.4, -0.8, 0.2, 1.0, 0.1, 0.3, -0.2]
+    view.set_pose(bent, True)
+    assert view._frames[1]["name"] == "法兰"
+    view._yaw = 1.7
+    view._pitch = 0.4
+    event = QMouseEvent(
+        QEvent.Type.MouseButtonDblClick,
+        QPointF(30, 30),
+        Qt.MouseButton.LeftButton,
+        Qt.MouseButton.LeftButton,
+        Qt.KeyboardModifier.NoModifier,
+    )
+    view.mouseDoubleClickEvent(event)
+    assert view._yaw == _VIEW_YAW
+    assert view._pitch == _VIEW_PITCH
+    assert view._pose_key[0] == tuple(bent)
+    assert "复位" in view._caption()
+    view.mouseDoubleClickEvent(event)
+    assert view._yaw == _VIEW_YAW
+    assert view._pitch == _VIEW_PITCH
+    assert view._pose_key[0][1] == -0.8
+    view.close()
+
+
+def _lit_pixels(image) -> int:
+    count = 0
+    for y in range(0, image.height(), 3):
+        for x in range(0, image.width(), 3):
+            color = image.pixelColor(x, y)
+            if abs(color.red() - 12) + abs(color.green() - 14) + abs(color.blue() - 18) > 24:
+                count += 1
+    return count

src/arm_application/test/test_operator_arm_state.py

@@ -0,0 +1,218 @@
+"""机械臂状态页：使能结论和零位外形不连真机也能红。"""
+
+from arm_application.operator_arm_state import (
+    REMOTE_PROMPT,
+    _parse_joint_echo,
+    _probe_cmd,
+    control_holds_rdk,
+    fk_points,
+    motion_blocked,
+    present,
+    release_for_control,
+    resume_after_control,
+    retain_pose,
+)
+
+
+def test_manual_mode_asks_for_remote_and_is_not_enabled() -> None:
+    row = present(
+        "89",
+        stack_up=False,
+        bridge_up=False,
+        probe={
+            "connected": True,
+            "operational": False,
+            "op_status": "OperationalStatus.IN_MANUAL_MODE",
+            "mode": "Mode.IDLE",
+            "level": "fail",
+            "summary": "示教器在 Manual",
+            "fault": False,
+            "estop_released": True,
+        },
+        joints=None,
+    )
+    assert row["enabled"] is False
+    assert row["prompt_hot"] is True
+    assert "Remote" in row["prompt"]
+    assert row["prompt"] == REMOTE_PROMPT
+    assert row["pose_live"] is False
+
+
+def test_operational_is_enabled_but_remote_line_stays() -> None:
+    row = present(
+        "89",
+        stack_up=False,
+        bridge_up=False,
+        probe={
+            "connected": True,
+            "operational": True,
+            "op_status": "OperationalStatus.READY",
+            "mode": "Mode.IDLE",
+            "level": "ok",
+            "summary": "已使能",
+            "fault": False,
+            "estop_released": True,
+            "q": [0.0] * 7,
+        },
+        joints=None,
+    )
+    assert row["enabled"] is True
+    assert row["prompt_hot"] is False
+    assert "Remote" in row["prompt"]
+    assert row["pose_live"] is True
+    assert len(row["points"]) == 9
+
+
+def test_stack_without_joints_tells_you_to_enable() -> None:
+    row = present("91", stack_up=True, bridge_up=False, probe=None, joints=None)
+    assert row["enabled"] is False
+    assert row["prompt_hot"] is True
+    assert "operational" in row["summary"]
+
+
+def test_zero_pose_stands_up() -> None:
+    points = fk_points([0.0] * 7)
+    assert len(points) == 9
+    assert points[-1][2] > 0.8
+    assert all(len(p) == 3 and all(v == v for v in p) for p in points)
+
+
+def test_failed_poll_keeps_joints_briefly_then_drops_them() -> None:
+    previous = present(
+        "89",
+        stack_up=False,
+        bridge_up=False,
+        probe={"connected": True, "operational": True, "q": [0.2] * 7, "level": "ok", "summary": "已使能"},
+        joints=None,
+    )
+    missed = present("89", stack_up=False, bridge_up=False, probe=None, joints=None, error="读 RDK 超时")
+    held = retain_pose(missed, previous, now=previous["stamp"] + 1.0)
+    assert held["q"] == [0.2] * 7
+    assert held["pose_live"] is True
+    assert "沿用上次读数" in held["summary"]
+    dropped = retain_pose(missed, previous, now=previous["stamp"] + 30.0)
+    assert dropped["q"] is None
+    assert dropped["pose_live"] is False
+
+
+def test_control_hold_keeps_status_probe_off_until_released() -> None:
+    assert control_holds_rdk() is False
+    release_for_control()
+    release_for_control()
+    assert control_holds_rdk() is True
+    resume_after_control()
+    assert control_holds_rdk() is True
+    resume_after_control()
+    assert control_holds_rdk() is False
+
+
+def test_arm_motion_says_why_when_the_stack_is_down() -> None:
+    text = motion_blocked(
+        "reset_home",
+        stack_up=False,
+        op_status="OperationalStatus.IN_MANUAL_MODE",
+    )
+    assert "Manual" in text
+    assert "上电控制栈" in text
+    assert motion_blocked("reset_home", stack_up=True, op_status="IN_MANUAL_MODE") == ""
+    assert motion_blocked("gripper_open", stack_up=False, op_status="IN_MANUAL_MODE") == ""
+    screw = motion_blocked("stage_screw", stack_up=False, op_status="IN_MANUAL_MODE")
+    assert "上电控制栈" in screw
+    assert motion_blocked("stage_screw", stack_up=True, op_status="IN_MANUAL_MODE") == ""
+
+
+def test_probe_command_passes_pc_ip_not_only_iface_name() -> None:
+    cmd = _probe_cmd("/tmp/python", "/tmp/rdk_status.py", "Rizon4-063989", "eth0", "192.168.2.10")
+    assert "--iface" in cmd and "eth0" in cmd
+    assert cmd[cmd.index("--pc-ip") + 1] == "192.168.2.10"
+
+
+def test_joint_echo_picks_named_joints() -> None:
+    text = """
+header:
+  stamp: 0
+name:
+- joint1
+- joint2
+- finger
+- joint3
+- joint4
+- joint5
+- joint6
+- joint7
+position:
+- 0.1
+- 0.2
+- 9.9
+- 0.3
+- 0.4
+- 0.5
+- 0.6
+- 0.7
+---
+"""
+    q = _parse_joint_echo(text)
+    assert q == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
+
+
+def test_screw_stage_uses_site_pitch_and_refuses_zero(tmp_path) -> None:
+    """旋转装配的深度和螺距只从 site 读。写成 0 时脚本必须失败，不能当成搜孔轻压混过去。"""
+    import os
+    import subprocess
+
+    from arm_application.operator_console import catalog
+    from arm_application.site_io import ros2_root
+
+    action = next(item for item in catalog()["actions"] if item["id"] == "stage_screw")
+    assert action["moves"] is True
+    assert action["args"][-1] == "--screw"
+    root = ros2_root()
+    site = (root / "config/site.yaml").read_text(encoding="utf-8")
+    assert "screw_stage_depth_mm: 6.0" in site
+    assert "screw_stage_pitch_mm: 1.0" in site
+    broken = tmp_path / "site.yaml"
+    broken.write_text(site.replace("screw_stage_pitch_mm: 1.0", "screw_stage_pitch_mm: 0.0"), encoding="utf-8")
+    env = os.environ.copy()
+    env["SITE_CONFIG"] = str(broken)
+    proc = subprocess.run(
+        ["bash", str(root / "scripts/run_stage3_search.sh"), "--arm", "89", "--screw"],
+        cwd=root,
+        env=env,
+        capture_output=True,
+        text=True,
+        timeout=90,
+        check=False,
+    )
+    assert proc.returncode != 0
+    assert "旋不进去" in proc.stderr
+
+
+def test_search_stage_is_refused() -> None:
+    """搜孔轻压不能从界面动作、单独脚本或整体流程里再打开。"""
+    import subprocess
+
+    from arm_application.operator_console import catalog
+    from arm_application.site_io import ros2_root
+
+    assert "stage_search" not in {item["id"] for item in catalog()["actions"]}
+    root = ros2_root()
+    script = subprocess.run(
+        ["bash", str(root / "scripts/run_stage3_search.sh"), "--arm", "89"],
+        cwd=root,
+        capture_output=True,
+        text=True,
+        timeout=90,
+        check=False,
+    )
+    assert script.returncode != 0
+    assert "搜孔轻压已取消" in script.stderr
+    overall = subprocess.run(
+        ["bash", str(root / "scripts/run_all.sh"), "--arm", "89", "--depth", "6"],
+        cwd=root,
+        capture_output=True,
+        text=True,
+        timeout=30,
+        check=False,
+    )
+    assert overall.returncode != 0
+    assert "搜孔轻压已取消" in overall.stderr

src/arm_application/test/test_operator_gripper_mesh.py

@@ -0,0 +1,61 @@
+"""平行爪外壳按 PGEA-100-26-W-F 尺寸图，开口仍按现场 20 mm，不拿图上的 26 mm。"""
+
+from arm_application.operator_gripper_mesh import PGEA_SIZE, RGI_SIZE, triangles
+
+
+def _extent(tris):
+    pts = [p for tri in tris for p in tri[:3]]
+    return tuple(max(p[i] for p in pts) - min(p[i] for p in pts) for i in range(3))
+
+
+def _inner_gap(tris, front: float) -> float:
+    ys = [p[1] for tri in tris for p in tri[:3] if p[0] > front]
+    return min(y for y in ys if y >= 0.0) - max(y for y in ys if y <= 0.0)
+
+
+def test_pgea_envelope_is_the_braked_catalog_body() -> None:
+    tris, note = triangles(False, 0, 0, 20)
+    ext = _extent(tris)
+    assert abs(ext[0] - PGEA_SIZE[0]) < 1e-9
+    assert abs(ext[1] - PGEA_SIZE[1]) < 1e-9
+    assert abs(ext[2] - PGEA_SIZE[2]) < 1e-9
+    assert "138" in note and "20" in note
+
+
+def test_pgea_full_open_uses_site_stroke_not_26mm() -> None:
+    shut, _ = triangles(False, 0, None, 20)
+    opened, _ = triangles(False, 1000, None, 20)
+    assert abs(_inner_gap(opened, 0.12) - _inner_gap(shut, 0.12) - 0.020) < 1e-9
+
+
+def test_parts_stay_distinguishable_on_a_dark_view() -> None:
+    """壳体、抱闸、手指、导轨必须是对比色。黑手指在深色底上会消失。"""
+    from arm_application.operator_gripper_mesh import _BODY, _BRAKE, _FINGER, _RAIL
+
+    colors = (_BODY, _BRAKE, _FINGER, _RAIL)
+    for index, left in enumerate(colors):
+        for right in colors[index + 1 :]:
+            assert max(abs(left[channel] - right[channel]) for channel in range(3)) >= 0.35
+    assert min(_BODY) > 0.75
+    assert _FINGER[0] > 0.85 and _FINGER[0] > _FINGER[2] + 0.4
+    assert _BRAKE[2] > 0.75 and _BRAKE[2] > _BRAKE[0] + 0.4
+    tris, _note = triangles(False, 500, None, 20)
+    used = {tri[4] for tri in tris}
+    assert {_BODY, _BRAKE, _FINGER, _RAIL} <= used
+    shades = {tri[3] for tri in tris}
+    assert max(shades) - min(shades) > 0.2
+
+
+def test_rgi_envelope_and_spin() -> None:
+    shut, note = triangles(True, 0, 0, 30)
+    ext = _extent(shut)
+    assert abs(ext[0] - RGI_SIZE[0]) < 1e-9
+    assert abs(ext[1] - RGI_SIZE[1]) < 1e-9
+    assert abs(ext[2] - RGI_SIZE[2]) < 1e-9
+    assert "158" in note
+    spun, _ = triangles(True, 0, 90, 30)
+
+    def front_z(tris):
+        return max(abs(p[2]) for tri in tris for p in tri[:3] if p[0] > 0.13)
+
+    assert abs(front_z(shut) - front_z(spun)) > 0.005

src/arm_application/test/test_operator_gripper_state.py

@@ -0,0 +1,128 @@
+"""夹爪状态页：开口和转角不连串口也能红。"""
+
+from arm_application.operator_gripper_state import (
+    _parse_states_echo,
+    _parse_status_echo,
+    pause_serial,
+    present,
+    resume_serial,
+    retain_reading,
+    serial_paused,
+)
+
+
+def test_closed_jaw_is_zero_opening() -> None:
+    row = present(
+        "89",
+        rotary=False,
+        stroke_mm=20.0,
+        reading={"connected": True, "permille": 0, "grip_state": 1, "initialized": True},
+    )
+    assert row["connected"] is True
+    assert row["opening_mm"] == 0.0
+    assert row["pose_live"] is True
+    assert row["grip_text"] == "已到位"
+
+
+def test_open_jaw_uses_stroke() -> None:
+    row = present(
+        "89",
+        rotary=False,
+        stroke_mm=20.0,
+        reading={"connected": True, "permille": 1000, "grip_state": 1, "force": 20},
+    )
+    assert row["opening_mm"] == 20.0
+    assert row["force"] == 20.0
+
+
+def test_rotary_keeps_angle() -> None:
+    row = present(
+        "91",
+        rotary=True,
+        stroke_mm=30.0,
+        reading={
+            "connected": True,
+            "permille": 500,
+            "rotation_deg": 90,
+            "rotation_state": 1,
+            "grip_state": 2,
+        },
+    )
+    assert row["opening_mm"] == 15.0
+    assert row["rotation_deg"] == 90.0
+    assert row["rotation_text"] == "已到位"
+    assert row["gripped"] is True
+
+
+def test_missing_read_is_not_a_live_pose() -> None:
+    row = present("89", rotary=False, stroke_mm=20.0, reading=None, error="串口没有应答")
+    assert row["connected"] is False
+    assert row["pose_live"] is False
+    assert row["opening_mm"] is None
+    assert "应答" in row["summary"]
+
+
+def test_missed_echo_keeps_opening_briefly_then_drops_it() -> None:
+    previous = present(
+        "89",
+        rotary=False,
+        stroke_mm=20.0,
+        reading={"connected": True, "permille": 939, "grip_state": 1, "initialized": True},
+    )
+    missed = present("89", rotary=False, stroke_mm=20.0, reading=None, error="话题没有数据")
+    held = retain_reading(missed, previous, now=previous["stamp"] + 1.0)
+    assert held["connected"] is True
+    assert held["permille"] == 939
+    assert held["pose_live"] is True
+    assert "沿用上次读数" in held["summary"]
+    dropped = retain_reading(missed, previous, now=previous["stamp"] + 30.0)
+    assert dropped["connected"] is False
+    assert dropped["permille"] is None
+
+
+def test_gripper_failure_message_keeps_the_script_reason() -> None:
+    from arm_application.operator_console import _grip_failure_message
+
+    detail = "准备\nERROR: /dev/ttyUSB0 不在。USB–RS485 没插上，夹爪驱动起不来。\n"
+    assert _grip_failure_message(1, detail).startswith("ERROR:")
+    assert "不在" in _grip_failure_message(1, detail)
+    assert _grip_failure_message(1, "没有原因") == "夹爪退出码 1"
+
+
+def test_serial_pause_blocks_a_second_open() -> None:
+    assert serial_paused() is False
+    pause_serial()
+    assert serial_paused() is True
+    resume_serial()
+    assert serial_paused() is False
+
+
+def test_parallel_topic_array() -> None:
+    text = "data:\n- 1.0\n- 2.0\n- 250.0\n- 250.0\n- 40.0\n---\n"
+    parsed = _parse_states_echo(text)
+    assert parsed is not None
+    assert parsed["permille"] == 250.0
+    assert parsed["gripped"] is True
+    assert parsed["initialized"] is True
+
+
+def test_rotary_topic_yaml() -> None:
+    text = """
+connected: true
+initialized: true
+is_moving: false
+is_gripped: false
+finger_position_per_mille: 800.0
+rotation_deg: -15.5
+gripper_state: 1
+rotation_state: 1
+force: 33.0
+velocity: 25.0
+error_msg: ''
+---
+"""
+    parsed = _parse_status_echo(text)
+    assert parsed is not None
+    assert parsed["permille"] == 800.0
+    assert parsed["rotation_deg"] == -15.5
+    assert parsed["speed"] == 25.0

src/arm_application/test/test_real_preflight.py

@@ -402,6 +402,27 @@ def test_cameras_skips_sim_arm(tmp_path: Path) -> None:
 # ---------------------------------------------------------------------------
 
 
+def test_joint_angles_survive_status_packing() -> None:
+    """关节角曾经让 classify_state(**state) 抛 TypeError，JSON 出不来，界面不更新。"""
+    import rdk_status
+    from rdk_helpers import rdk_whitelist_lists
+
+    payload = rdk_status.pack_status({
+        "connected": True,
+        "operational": True,
+        "op_status": "READY",
+        "mode": "IDLE",
+        "fault": False,
+        "estop_released": True,
+        "error": "",
+        "q": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7],
+    })
+    assert payload["q"] == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
+    assert payload["level"] == rdk_status.LEVEL_OK
+    # 内核把 USB 网卡从 eth0 改名后，白名单必须仍是本机机器人网段地址。
+    assert rdk_whitelist_lists("eth0", "192.168.2.10")[0] == ["192.168.2.10"]
+
+
 def test_network_action_appears_once_in_the_manual_list() -> None:
     """check_network 与 rdk_status 给的"去查网线"必须是**同一串字**。
 

src/arm_application/test/test_site_arm_sections.py

@@ -15,6 +15,7 @@ site.yaml 有两层：顶层是站点配置，``robots[]`` 里每台臂可以覆
 
 from __future__ import annotations
 
+import os
 import sys
 from pathlib import Path
 from typing import Iterable, List, Mapping
@@ -94,3 +95,55 @@ def test_sim_robot_carries_the_truth_backend_override(arm_section_keys):
     assert sim, "site.yaml 里没有 sim 机器人 —— 仿真流程会挑不到配置"
     src = (sim[0].get("perception") or {}).get("source")
     assert src == "truth", f"sim 的 perception.source 应为 truth，现在是 {src!r}"
+
+
+def _flatten_for(sn: str) -> dict:
+    import export_site_env  # type: ignore
+
+    previous = os.environ.get("ROBOT_SN")
+    os.environ["ROBOT_SN"] = sn
+    try:
+        return export_site_env.flatten(export_site_env.load_site())
+    finally:
+        if previous is None:
+            os.environ.pop("ROBOT_SN", None)
+        else:
+            os.environ["ROBOT_SN"] = previous
+
+
+def test_live_left_gripper_does_not_auto_home(arm_section_keys):
+    """左臂现场 auto_init 是 false。拉起驱动若把它弄成 true，会写 0xA5 回零。"""
+    flat = _flatten_for("89")
+    assert flat["SITE_GRIP_AUTO_INIT"] == "false"
+    assert flat["SITE_GRIP_PORT"] == "/dev/ttyUSB0"
+
+
+def test_auto_init_override_is_not_ignored(arm_section_keys, tmp_path):
+    """把现场改成 true 必须真的变成 true。否则上面那条是 exporter 写死的假通过。"""
+    import export_site_env  # type: ignore
+
+    doc = yaml.safe_load((ROOT / "config" / "site.yaml").read_text(encoding="utf-8"))
+    changed = False
+    for robot in robot_entries(doc):
+        if str(robot.get("sn") or "").endswith("063989"):
+            robot["gripper"]["auto_init"] = True
+            changed = True
+    assert changed
+    cfg = tmp_path / "site.yaml"
+    cfg.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
+    previous_cfg = os.environ.get("SITE_CONFIG")
+    previous_sn = os.environ.get("ROBOT_SN")
+    os.environ["SITE_CONFIG"] = str(cfg)
+    os.environ["ROBOT_SN"] = "89"
+    try:
+        flat = export_site_env.flatten(export_site_env.load_site())
+    finally:
+        if previous_cfg is None:
+            os.environ.pop("SITE_CONFIG", None)
+        else:
+            os.environ["SITE_CONFIG"] = previous_cfg
+        if previous_sn is None:
+            os.environ.pop("ROBOT_SN", None)
+        else:
+            os.environ["ROBOT_SN"] = previous_sn
+    assert flat["SITE_GRIP_AUTO_INIT"] == "true"

src/arm_bringup/config/bringup.yaml

@@ -4,7 +4,8 @@ rizon_type: Rizon4
 # Flexiv Studio 夹爪（GN01）—— 保持 false。DH PGEA 使用 run_gripper.sh。
 load_gripper: false
 use_fake_hardware: false
-start_rviz: true
+# 模型在工位程序的仿真里看，启动栈不再弹出 RViz。
+start_rviz: false
 
 # Flexiv 官方关节命名：Rizon4-063989_joint1 ... joint7
 joint_prefix: Rizon4-063989_

src/arm_bringup/launch/real.launch.py

@@ -74,6 +74,10 @@ def generate_launch_description():
         DeclareLaunchArgument('use_fake_hardware', default_value='false'),
         # 仅适用于 Flexiv-GN01。DH PGEA 用: bash ~/ros2/scripts/run_gripper.sh
         DeclareLaunchArgument('load_gripper', default_value='false'),
-        DeclareLaunchArgument('start_rviz', default_value='true'),
+        DeclareLaunchArgument(
+            'start_rviz',
+            default_value='false',
+            description='机械臂模型在工位程序里看，默认不再弹出 RViz',
+        ),
         OpaqueFunction(function=_launch_setup),
     ])

src/arm_bringup/launch/sim.launch.py

@@ -35,7 +35,11 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument(
+            'use_rviz',
+            default_value='false',
+            description='机械臂模型在工位程序里看，默认不再弹出 RViz',
+        ),
         DeclareLaunchArgument('use_moveit', default_value='false'),
         # 仿真**故意**不用真机的 SN 前缀：两边同时跑时，同名关节会让 TF 与
         # 控制器互相打架（症状是 RViz 里模型乱跳）。见 devices/sim/gazebo/README.md。

src/arm_bringup/launch/stack.launch.py

@@ -201,7 +201,11 @@ def generate_launch_description():
             default_value='mock',
             description='mock | moveit | real | isaac',
         ),
-        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument(
+            'use_rviz',
+            default_value='false',
+            description='机械臂模型在工位程序里看，默认不再弹出 RViz',
+        ),
         DeclareLaunchArgument('robot_sn', default_value=SITE_SN),
         DeclareLaunchArgument('load_gripper', default_value='false'),
         OpaqueFunction(function=_setup),

src/arm_control/launch/mock_control.launch.py

@@ -47,7 +47,11 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument(
+            'use_rviz',
+            default_value='false',
+            description='机械臂模型在工位程序里看，默认不再弹出 RViz',
+        ),
         DeclareLaunchArgument(
             'robot_sn',
             default_value=SITE_ROBOT_SN,

src/arm_description/launch/display.launch.py

@@ -34,7 +34,11 @@ def generate_launch_description():
 
     return LaunchDescription([
         DeclareLaunchArgument('use_gui', default_value='true'),
-        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument(
+            'use_rviz',
+            default_value='false',
+            description='机械臂模型在工位程序里看，默认不再弹出 RViz',
+        ),
         # 默认取当前臂（scripts/env.sh 已导出）；不再写死某个 SN。
         DeclareLaunchArgument(
             'robot_sn',

src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py

@@ -70,13 +70,22 @@ def default_rdk_python() -> str:
 
 
 def default_bridge_script() -> Path:
-    """桥脚本路径。可用 ``SITE_ROS2_ROOT`` / ``ROS2_WS`` 指向别的工作区。"""
+    """桥脚本路径。可用 ``SITE_ROS2_ROOT`` / ``ROS2_WS`` 指向别的工作区。
+
+    仓库不在 ``~/ros2`` 时，从本文件往上找，避免换目录之后去连一个空路径。
+    """
     root = (
         os.environ.get("SITE_ROS2_ROOT", "").strip()
         or os.environ.get("ROS2_WS", "").strip()
     )
-    base = Path(root).expanduser() if root else Path.home() / "ros2"
+    if root:
-    return base / BRIDGE_SCRIPT_REL
+        return Path(root).expanduser() / BRIDGE_SCRIPT_REL
+    here = Path(__file__).resolve()
+    for parent in here.parents:
+        candidate = parent / BRIDGE_SCRIPT_REL
+        if candidate.is_file():
+            return candidate
+    return Path.home() / "ros2" / BRIDGE_SCRIPT_REL
 
 
 class RdkClient:

src/arm_moveit_config/launch/move_group.launch.py

@@ -69,12 +69,21 @@ def _launch_setup(context, *args, **kwargs):
         ],
     )
 
-    return [move_group, rviz]
+    # 模型在工位程序里看。只有显式 start_rviz:=true 才再开 RViz。
+    nodes = [move_group]
+    if LaunchConfiguration('start_rviz').perform(context).strip().lower() in ('1', 'true', 'yes'):
+        nodes.append(rviz)
+    return nodes
 
 
 def generate_launch_description():
     return LaunchDescription([
         DeclareLaunchArgument('use_fake_hardware', default_value='true'),
+        DeclareLaunchArgument(
+            'start_rviz',
+            default_value='false',
+            description='机械臂模型在工位程序里看，默认不再弹出 RViz',
+        ),
         # 从环境取当前臂 SN，避免靠 xacro 里的写死默认值"蒙对"。
         DeclareLaunchArgument(
             'robot_sn',