贴面后不再抬回悬高；MoveL 切回关节后不再把整段位移当前进速度。

搜孔开局会抬到悬高再落回 hole_hover_z_m，螺纹还没咬合臂就离开孔口。没有接触传感器，到位只认 final_gap 算出的法兰高度，贴面后停在这个 Z。09:25 厂家 MoveL 切回 NRT_JOINT_POSITION 的第一拍把整段关节差算成 1.686 rad/s，前瞻再推 0.07 rad，光心从 5 mm 冲到 276 mm 后急停。切回后钉住结束瞬间的关节角并清零速度，JTC 还指着出发前的关节时不放行。 Co-authored-by: Cursor <cursoragent@cursor.com>
2026-10-19 11:30:49 +08:00
parent 8c4dccccb1
commit 7af9e3d4b6
103 changed files with 5594 additions and 2498 deletions
@@ -10,8 +10,8 @@
 # 用法：复制本文件为 config/site.yaml，然后把所有 <占位符> 换成现场实际值。
 #       现场值包括：机械臂 SN / IP、相机序列号、夹爪串口、传感器 USB 路径。
 #
-# 说明：仓库里的文件用相对仓库根的路径（例如 models/yolo/...）。
+# 说明：site.yaml 里的路径出现 /home/USER 是因为生成时匿名化了用户名；
-#       不要改回某个用户的绝对家目录，换台电脑就会找不到文件。
+#       部署时请改成实际用户的 home 目录。
 # =============================================================================
 # =============================================================================
@@ -61,17 +61,15 @@ robot:
        #     （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
        # 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
        auto_init: false
-      named_poses_file: "src/arm_control/config/named_poses_89.yaml"
+      named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml"
      paths:
-        handeye_file: "handeye/results/89_eye_in_hand.yaml"
+        handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
-        handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
+        handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
-        handeye_workdir: "handeye/tmp/89"
+        handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
      handeye:
        # Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
        # 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
        # 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
        gripper_tool: "tool10120938"
-        gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
+        gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
        gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
      # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
      # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
@@ -179,9 +177,8 @@ robot:
        #   落在本文件下方注释记录的合理区间 0.204~0.233 内。
        #   复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX
        #         python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
-        # 2026-10-12：下臂安全距离改按 Studio 工具 tool10120938 的 TCP +Z。
+        # 下臂安全距离按 tool10120938 的 TCP +Z，不再用旧 tool1 的 0.223。
-        # 上面 0.223 是旧 tool1（夹爪 TCP 0.147 + 规露出 0.076），这把工具不再用。
+        tool_tip_below_flange_m: 0.226587
        tool_tip_below_flange_m: 0.2245
    # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
    # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
    #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
@@ -211,7 +208,7 @@ robot:
        # 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
        # T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
        #（"手眼标定结果缺失"），S04 跟踪起不来。
-        handeye_file: "handeye/results/sim_eye_in_hand.yaml"
+        handeye_file: "/home/USER/ros2/handeye/results/sim_eye_in_hand.yaml"
      # 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
      #
      # 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
@@ -305,11 +302,11 @@ robot:
        port: "/dev/ttyUSB1"
        stroke_mm: 30.0
        auto_init: false
-      named_poses_file: "src/arm_control/config/named_poses_91.yaml"
+      named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_91.yaml"
      paths:
-        handeye_file: "handeye/results/91_eye_in_hand.yaml"
+        handeye_file: "/home/USER/ros2/handeye/results/91_eye_in_hand.yaml"
-        handeye_waypoints: "handeye/poses/91_eye_in_hand.yaml"
+        handeye_waypoints: "/home/USER/ros2/handeye/poses/91_eye_in_hand.yaml"
-        handeye_workdir: "handeye/tmp/91"
+        handeye_workdir: "/home/USER/ros2/handeye/tmp/91"
      # >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
      # 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
      #   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
@@ -330,7 +327,7 @@ robot:
 paths:
  # >>> YOLO 权重（标准放法，不必改代码）<<<
-  #   1) 把 .onnx / .pt 放到 models/yolo/
+  #   1) 把 .onnx / .pt 放到 /home/USER/ros2/models/yolo/
  #   2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
  #   3) backend=tensorrt 时必须先导出引擎（只需做一次）：
  #        bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
@@ -344,18 +341,18 @@ paths:
  #    ultralytics 解不了 pose 的 TensorRT engine，运行时走
  #    arm_application/pose_detector.py 的自解码（见该模块头）。
  #    仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
-  yolo_weights: "models/yolo/gongjian0929.onnx"
+  yolo_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
  # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
  # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
  # 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
  # pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
  # 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
-  hole_weights: "models/yolo/gongjian0929.onnx"
+  hole_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
  # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
-  handeye_file: "handeye/results/89_eye_in_hand.yaml"
+  handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
-  handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
+  handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
-  handeye_calibrate: "handeye/calibrate.yaml"
+  handeye_calibrate: "/home/USER/ros2/handeye/calibrate.yaml"
-  handeye_workdir: "handeye/tmp/89"
+  handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
  # 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
  fastdds_profile: ""
@@ -595,9 +592,8 @@ servo:
  lock_hold_sec: 0.7        # 旧死区驻留（混合模式不再用它进二阶段）
  # 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
  lock_still_sec: 3.0
-  # 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
+  # 厂家 MoveL 结束后，法兰连续这么久没再动，才打开 D405 框心。
-  # 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
+  d435_handback_sec: 2.0
  d435_handback_sec: 0.5
  # 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
  wrist_align_sec: 0.5
  lock_still_move_m: 0.015   # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
@@ -679,10 +675,10 @@ gripper:
 hole_detect:
  backend: "two_stage"   # two_stage | yolo | hough
  # backend=yolo 时用。
-  weights: "models/yolo/gongjian0929.onnx"
+  weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
  # 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
  # 所以和上面的跟踪权重是同一份。
-  workpiece_weights: "models/yolo/gongjian0929.onnx"
+  workpiece_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
  # pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
  # 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
  # 亮环 + Hough 那套老路径。
@@ -690,7 +686,7 @@ hole_detect:
  keypoint_min_conf: 0.30
  keypoint_min_dist_px: 15.0
  keypoint_hole_radius_px: 12.0   # 关键点没有半径；反投影时按这个半径取孔周深度环
-  config: "src/arm_application/config/hole_detect.yaml"
+  config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml"
  conf: 0.25
  iou: 0.7
  imgsz: 640
@@ -768,26 +764,22 @@ hole_detect:
    # bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
    # **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
    # 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
-    coarse_max_offset_m: 0.12   # 目标离夹爪轴线超过此值就不像粗居中量 [m]
+    coarse_max_offset_m: 0.08   # 目标离夹爪轴线超过此值就不像粗居中量 [m]；0.12 放行了 2026-10-16 的 110mm 回撤
    coarse_border_px: 2         # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
    # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
    align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
    move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
    # 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。
    hole_visual_only: true
-    # 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完
+    # 本分支实验：true=悬停拍一次并走完到孔；main 保存点应保持 false。
    # （40mm 上限）。main 保存点是 false（多走多拍）。
    hole_keypoint_once: true
-    # true=悬停走完后一条厂家 MoveL 贴面（不补拍）。对照 37b85aa 改回 false。
+    # true=悬停走完后一条厂家 MoveL 贴面。对照观察高补交时改 false。
    hole_once_lock_xy_descend: true
-    # 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
+    # 0=笛卡尔锁 XY 只降 Z。k>0 才沿 -look 加前馈。
    # 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
    once_ff_look_per_m: 0.0
-    final_gap_mm: 2.0        # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY
+    final_gap_mm: 2.0        # 对准后贴面目标：规尖离面 [mm]。CLI --final-gap 覆盖。
                             # 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
    observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                             # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
-                             # 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
+                             # 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。
    final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
    # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
    #   深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -800,7 +792,7 @@ hole_detect:
    #     ⇒ 读数往**小**错一次就永远到不了目标。
    #   同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
    #   和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
-    # 现行几何：悬停 0.320 − 工具长 0.2245 − 工件面 0.022 = 73.5mm 间隙 ⇒ 降 71.5mm 到 2mm。
+    # 现行几何：悬停 0.320 − 工具长 0.223 − 工件面 0.022 = 75mm 间隙 ⇒ 降 73mm 到 2mm。
    #   三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
    #   + 任一步 _goto 走不到位就停。
    #   缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
@@ -812,9 +804,11 @@ hole_detect:
    max_depth_m: 0.60
    linger_sec: 8.0          # 完成后继续发布结果/叠图的时长
-  # ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
+  # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
-  # 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。
+  # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
-  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89                # 搜孔+落座
  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0      # 只搜孔定心不插入
  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0  # 螺纹规旋入
  stage3:
    # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
    #    节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
@@ -841,12 +835,9 @@ hole_detect:
    drop_frames: 2           # 跌落需持续的帧数（防毛刺）
    center_scan: false       # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
    center_scan_mm: 0.3
-    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入。旋转装配不读这项
+    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
    insert_retry: 3
-    screw_pitch_mm: 0.0      # 搜孔轻压不旋。旋转装配不读这项
+    screw_pitch_mm: 0.0      # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
    # 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。
    screw_stage_depth_mm: 6.0
    screw_stage_pitch_mm: 1.0
    search_speed: 0.03       # 搜孔笛卡尔速度 [m/s]
    settle_sec: 0.25         # 每个微步后的静止时间
    tactile_timeout_s: 3.0   # 触觉话题断流多久中止
@@ -1102,7 +1093,7 @@ tactile:
  # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
  model: "dd03"
  # 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
-  config_file: "devices/sensors/orisys/models/dd03/config.json"
+  config_file: "/home/USER/ros2/devices/sensors/orisys/models/dd03/config.json"
  # 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
  # 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
  left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:X.Y.Z:1.0-video-index0"
@@ -1129,8 +1120,7 @@ go_home:
 network:
  # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
-  #   MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enxXXXXXXXXXXXX 之间变，
+  #   MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enxXXXXXXXXXXXX）。
  #   NetworkManager 按 MAC 绑定，不按名字。
  #   本机 <PC_IP>  ↔  <左臂SN>(左89)=.101  /  <右臂SN>(右91)=.100
  #   NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
  # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
@@ -23,7 +23,7 @@ robot:
      # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
      # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
      tactile: true
-      # 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
+      # 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。
      tool_name: "tool10120938"
      camera:
        name: d405_left          # 话题 /d405_left/... 与 91 分开
@@ -45,17 +45,17 @@ robot:
        #     （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
        # 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
        auto_init: false
-      named_poses_file: "src/arm_control/config/named_poses_89.yaml"
+      named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_89.yaml"
      paths:
-        handeye_file: "handeye/results/89_eye_in_hand.yaml"
+        handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
-        handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
+        handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
-        handeye_workdir: "handeye/tmp/89"
+        handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
      handeye:
        # Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
        # 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
        # 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
        gripper_tool: "tool10120938"
-        gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
+        gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
        gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
      # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
      # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
@@ -163,9 +163,11 @@ robot:
        #   落在本文件下方注释记录的合理区间 0.204~0.233 内。
        #   复核：python3 scripts/sense_tool_length.py --sn Rizon4-063989
        #         python3 scripts/lib/real_geometry_check.py --sn Rizon4-063989
-        # 2026-10-12：下臂安全距离改按 Studio 工具 tool10120938 的 TCP +Z。
+        # 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location
-        # 上面 0.223 是旧 tool1（夹爪 TCP 0.147 + 规露出 0.076），这把工具不再用。
+        #   [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0]
-        tool_tip_below_flange_m: 0.2245
+        #   示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。
        # 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。
        tool_tip_below_flange_m: 0.226587
    # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
    # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
    #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
@@ -195,7 +197,7 @@ robot:
        # 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
        # T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
        #（"手眼标定结果缺失"），S04 跟踪起不来。
-        handeye_file: "handeye/results/sim_eye_in_hand.yaml"
+        handeye_file: "/home/cc/ros2/handeye/results/sim_eye_in_hand.yaml"
      # 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
      #
      # 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
@@ -289,11 +291,11 @@ robot:
        port: "/dev/ttyUSB1"
        stroke_mm: 30.0
        auto_init: false
-      named_poses_file: "src/arm_control/config/named_poses_91.yaml"
+      named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_91.yaml"
      paths:
-        handeye_file: "handeye/results/91_eye_in_hand.yaml"
+        handeye_file: "/home/cc/ros2/handeye/results/91_eye_in_hand.yaml"
-        handeye_waypoints: "handeye/poses/91_eye_in_hand.yaml"
+        handeye_waypoints: "/home/cc/ros2/handeye/poses/91_eye_in_hand.yaml"
-        handeye_workdir: "handeye/tmp/91"
+        handeye_workdir: "/home/cc/ros2/handeye/tmp/91"
      # >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
      # 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
      #   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
@@ -314,7 +316,7 @@ robot:
 paths:
  # >>> YOLO 权重（标准放法，不必改代码）<<<
-  #   1) 把 .onnx / .pt 放到 models/yolo/
+  #   1) 把 .onnx / .pt 放到 /home/cc/ros2/models/yolo/
  #   2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
  #   3) backend=tensorrt 时必须先导出引擎（只需做一次）：
  #        bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
@@ -328,18 +330,18 @@ paths:
  #    ultralytics 解不了 pose 的 TensorRT engine，运行时走
  #    arm_application/pose_detector.py 的自解码（见该模块头）。
  #    仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
-  yolo_weights: "models/yolo/gongjian0929.onnx"
+  yolo_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
  # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
  # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
  # 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
  # pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
  # 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
-  hole_weights: "models/yolo/gongjian0929.onnx"
+  hole_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
  # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
-  handeye_file: "handeye/results/89_eye_in_hand.yaml"
+  handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
-  handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
+  handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
-  handeye_calibrate: "handeye/calibrate.yaml"
+  handeye_calibrate: "/home/cc/ros2/handeye/calibrate.yaml"
-  handeye_workdir: "handeye/tmp/89"
+  handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
  # 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
  fastdds_profile: ""
@@ -579,9 +581,8 @@ servo:
  lock_hold_sec: 0.7        # 旧死区驻留（混合模式不再用它进二阶段）
  # 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
  lock_still_sec: 3.0
-  # 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
+  # 厂家 MoveL 结束后，法兰连续这么久没再动，才打开 D405 框心。
-  # 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
+  d435_handback_sec: 2.0
  d435_handback_sec: 0.5
  # 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
  wrist_align_sec: 0.5
  lock_still_move_m: 0.015   # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
@@ -663,10 +664,10 @@ gripper:
 hole_detect:
  backend: "two_stage"   # two_stage | yolo | hough
  # backend=yolo 时用。
-  weights: "models/yolo/gongjian0929.onnx"
+  weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
  # 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
  # 所以和上面的跟踪权重是同一份。
-  workpiece_weights: "models/yolo/gongjian0929.onnx"
+  workpiece_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
  # pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
  # 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
  # 亮环 + Hough 那套老路径。
@@ -674,7 +675,7 @@ hole_detect:
  keypoint_min_conf: 0.30
  keypoint_min_dist_px: 15.0
  keypoint_hole_radius_px: 12.0   # 关键点没有半径；反投影时按这个半径取孔周深度环
-  config: "src/arm_application/config/hole_detect.yaml"
+  config: "/home/cc/ros2/src/arm_application/config/hole_detect.yaml"
  conf: 0.25
  iou: 0.7
  imgsz: 640
@@ -752,7 +753,7 @@ hole_detect:
    # bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
    # **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
    # 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
-    coarse_max_offset_m: 0.12   # 目标离夹爪轴线超过此值就不像粗居中量 [m]
+    coarse_max_offset_m: 0.08   # 目标离夹爪轴线超过此值就不像粗居中量 [m]；0.12 放行了 2026-10-16 的 110mm 回撤
    coarse_border_px: 2         # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
    # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
    align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
@@ -771,7 +772,7 @@ hole_detect:
                             # 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
    observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                             # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
-                             # 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
+                             # 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
    final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
    # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
    #   深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -784,7 +785,7 @@ hole_detect:
    #     ⇒ 读数往**小**错一次就永远到不了目标。
    #   同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
    #   和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
-    # 现行几何：悬停 0.320 − 工具长 0.2245 − 工件面 0.022 = 73.5mm 间隙 ⇒ 降 71.5mm 到 2mm。
+    # 现行几何：悬停 0.320 − 工具长 0.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。
    #   三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
    #   + 任一步 _goto 走不到位就停。
    #   缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
@@ -796,9 +797,11 @@ hole_detect:
    max_depth_m: 0.60
    linger_sec: 8.0          # 完成后继续发布结果/叠图的时长
-  # ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
+  # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
-  # 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。
+  # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
-  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89                # 搜孔+落座
  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0      # 只搜孔定心不插入
  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0  # 螺纹规旋入
  stage3:
    # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
    #    节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
@@ -825,12 +828,9 @@ hole_detect:
    drop_frames: 2           # 跌落需持续的帧数（防毛刺）
    center_scan: false       # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
    center_scan_mm: 0.3
-    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入。旋转装配不读这项
+    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
    insert_retry: 3
-    screw_pitch_mm: 0.0      # 搜孔轻压不旋。旋转装配不读这项
+    screw_pitch_mm: 0.0      # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
    # 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。
    screw_stage_depth_mm: 6.0
    screw_stage_pitch_mm: 1.0
    search_speed: 0.03       # 搜孔笛卡尔速度 [m/s]
    settle_sec: 0.25         # 每个微步后的静止时间
    tactile_timeout_s: 3.0   # 触觉话题断流多久中止
@@ -1086,7 +1086,7 @@ tactile:
  # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
  model: "dd03"
  # 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
-  config_file: "devices/sensors/orisys/models/dd03/config.json"
+  config_file: "/home/cc/ros2/devices/sensors/orisys/models/dd03/config.json"
  # 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
  # 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
  left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0"
@@ -1113,8 +1113,7 @@ go_home:
 network:
  # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
-  #   MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enx6c1ff7d06f17 之间变，
+  #   MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enx6c1ff7d06f17）。
  #   NetworkManager 按 MAC 绑定，不按名字。
  #   本机 192.168.2.10  ↔  063989(左89)=.101  /  063991(右91)=.100
  #   NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
  # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
@@ -34,7 +34,7 @@
 ### 1.3 一次完整任务分几步
-代码里跟踪和对孔串成一条命令。搜孔轻压已取消，`run_all.sh` 不会再进入那一步，也不能用参数打开。旋入是单独的 `run_stage3_search.sh --screw`。
+代码里用 **stage1 / stage2 / stage3** 三段串起来，一条命令跑完：
 ```
 run_all.sh
@@ -46,13 +46,18 @@ run_all.sh
  │                  YOLO 找工件外框 → Hough 找左右两个孔心 →
  │                  深度 + 内参 + 手眼 → 换算出孔的基座坐标 →
  │                  移动夹爪轴线到目标孔正上方 → 复拍验证 → 锁定
  │
  └─ stage3_search   柔顺搜孔：力闭环下的螺旋搜索，直到规的尖端"掉进"孔里，
                     再十字微扫定心，最后在力限内下压（可选按螺距旋入）
 ```
-跟踪和对孔在同一个进程里交接。盯日志：
+三段之间**在同一个进程里交接**，所以相机、触觉、手眼 TF 只初始化一次。
 盯日志就能知道跑到哪了 —— 三行关键输出：
 ```
 [pipeline] === stage1_track ===      → 在等跟踪锁定
 [pipeline] === stage2_locate ===     → 在拍照对孔
 [pipeline] === stage3_search ===     → 触面 → ★ 入孔 → 轻压到位
 ```
 日常入口（三个终端，**必须同一个 `--arm`**）：
@@ -60,7 +65,7 @@ run_all.sh
 ```bash
 bash ~/ros2/scripts/run_real.sh --arm 89                    # T1 控制栈，一直开着
 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview   # T2 相机预览，一直开着
-bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 跟踪 + 对孔（不含搜孔、不含旋入）
+bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（默认不拧入）
 bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
 ```
@@ -68,15 +73,15 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
 `D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
 的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
 这一段运动是进程内厂家 `MoveL`（不杀 `run_real.sh`）：当前厂家 `tcp_pose` 加
-ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后法兰 XY 连续停稳
+ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这一点变到基座系后，只下发厂家 `MoveL`。赶路期间 D405 推理开着，
-`servo.d435_handback_sec`（0.5 s）才交回 D405。赶路期间不下发 D405（两套控制互抢会晃）。
+控制点截断。法兰连续停稳 `servo.d435_handback_sec`（2 s）后才打开 D405，
-停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
+追工件框的矩形中心。框心对准并停稳后才精拍 k1/k2、下臂。
 启动高度用 `--init-z`（法兰 Z，米），对孔悬停用 `--hover-z`。两个都是法兰高度。
 下臂贴面用 `--final-gap`（规尖离面，毫米；site 默认 `final_gap_mm` 2）。笛卡尔锁 XY
 时只改 Z：法兰 = 工具长 + 工件面 + 间隙。不能和 `--no-descent` 一起写。
 会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
 不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。
-框心对准连续 `servo.wrist_align_sec`（0.5 s）→ `/visual_servo/locked`。
+框心对准连续 `servo.wrist_align_sec`（0.5 s）才 `/visual_servo/locked`，然后才精拍下臂。
 本分支（`exp/hole-keypoint-once`）二阶段是**单次精拍**：悬停拍一次 k1/k2，
 把框心到目标孔一次走完（步长上限 40mm）。现行实验 `hole_once_lock_xy_descend: true`：
 走完后不补拍，一条厂家 MoveL 收到贴面。`once_ff_look_per_m: 0` 时 MoveL 只降 Z
@@ -84,7 +89,8 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后
 不是 ROS 绝对坐标。保存点 `37b85aa` 是观察高 15mm 再斜线补 XY。看 `/tmp/stage2_result.json`。
 `main` 保存点仍是多走多拍。
 孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
-`tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
+`tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
 贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器，到位只认 `final_gap_mm`，不另设一个 Z。
 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
 ```bash
@@ -107,8 +113,8 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
 | 能力 | 状态 | 怎么自己确认 |
 |------|------|--------------|
-| 跟踪 → 对孔 | ✅ 真机跑通过。搜孔轻压已取消，这条命令不再下压搜孔 | `run_all.sh --arm 89` |
+| 三段串联（跟踪 → 对孔 → 搜孔轻压） | ✅ 真机跑通 | `run_all.sh --arm 89` |
-| 按螺距旋入（螺纹规） | ✅ 单独阶段。不带 `--screw` 的旧搜孔命令会直接拒绝 | `run_stage3_search.sh --arm 89 --screw` |
+| 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
 | Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
 | Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
 | D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
@@ -505,7 +511,7 @@ bash scripts/run_unit_tests.sh -k gauge              # 只跑名字带 gauge 的
 | **89 / 91** | 左臂 / 右臂的现场编号，**各有独立的手眼、相机、夹爪、锁高，禁止混用** |
 | **法兰** | 机械臂末端安装面；手眼 `T_4x4` 仍是相机→法兰。左臂 89 现行按 Studio `tool10120938` 的 TCP 求解，再折回法兰 |
 | **eye-in-hand / eye-to-hand** | 相机装在手上 / 装在手上之外（D405 在手上，D435 在外部） |
-| **stage1/2** | 跟踪 / 对孔，见 1.3。搜孔轻压已取消 |
+| **stage1/2/3** | 跟踪 / 对孔 / 搜孔，见 1.3 |
 | **S03 / S04 / S07 / S08** | 整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，**没有一一对应**，别硬套 |
 | **基线** | 触觉静止时的读数均值。力的语义是"相对基线的增量"，不是绝对牛顿值 |
 | **R1.1 / M1 …** | [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 里的规则编号，文档和提交信息里直接引用 |
@@ -57,7 +57,8 @@ hole_detect:
 ## 三阶段串联（推荐日常入口）
-一条命令跑完：**视觉跟踪锁定 → 拍照对孔**。搜孔轻压已取消，这条命令不再下压，也不能用参数重新打开。旋入是后面的单独命令。
+一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入**
 （拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。
 ```bash
 # 终端 1 —— 控制栈（一直开着）
@@ -66,12 +67,12 @@ bash ~/ros2/scripts/run_real.sh --arm 89
 # 终端 2 —— 相机预览（一直开着，看跟踪画面）
 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
-# 终端 3 —— 跟踪 + 对孔（到这里停，不搜孔）
+# 终端 3 —— 串联（搜到孔后轻压保持，不拧）
-bash ~/ros2/scripts/run_all.sh --arm 89                  # 跟踪 + 对孔
+bash ~/ros2/scripts/run_all.sh --arm 89                  # 跟踪 + 对孔 + 搜孔轻压
 bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2        # 换 k2
 bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1    # 臂已在工件上方，跳过跟踪
-# 旋入不写在这条命令里：
+# 若这一次就要拧入（一般留给后续阶段）：
-bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
+bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
 ```
 `run_pipeline.sh` 是 `run_all.sh` 的别名。
@@ -84,9 +85,9 @@ bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
 1. `[pipeline] === stage1_track ===` → 等跟踪锁定
 2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
 3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
-对孔结束后臂停在当前高度。搜孔轻压不再接着跑。
+4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入）
-急停：终端 3 Ctrl-C。失败时臂保持当前位姿，看
+急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看
 `/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。
 高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、
@@ -148,10 +149,7 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8            # 锁 XY，规
 | `/stage2/result` | `std_msgs/String` JSON：k1/k2 基座坐标、残差、悬停高 |
 | `/stage2/overlay` | `sensor_msgs/Image` 标注图（同时落盘 /tmp/stage2_overlay.png） |
-## 旋转装配（原三阶段节点；搜孔轻压入口已取消）
+## 三阶段柔顺搜孔（stage3，Orisys 触觉 + 力闭环螺旋）
 不带 `--screw` 时 `run_stage3_search.sh` 直接退出，不会触面，也不会螺旋搜孔。
 下面是这个节点在旋入时仍会走的力控步骤，不是一条还能单独打开的搜孔命令。
 前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程：
@@ -169,9 +167,12 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8            # 锁 XY，规
 7. **hold** — 锁定关节，发布结果
 ```bash
-# 前置：T1 run_real.sh --arm 89；对孔已完成（臂保持不动）
+# 前置：T1 run_real.sh --arm 89；二阶段已完成（臂保持不动）
-# 不带 --screw 会直接退出，不会搜孔。
+bash ~/ros2/scripts/run_stage3_search.sh --arm 89                 # 搜孔+落座
-bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
+bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0      # 只搜孔定心，不插入
 bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0  # M6 螺纹规旋入
 bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5      # 临时改搜索半径
 bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap       # 搜前用 D405 复拍对准一次
 ```
 **力阈值是「相对静止均值的增量」，不用标定牛顿**：Orisys 读数是形变任意单位且
@@ -8,49 +8,59 @@
 /**:
  ros__parameters:
-    num_poses: 21
+    num_poses: 26
-    # 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
+    # 2026-10-16 重新示教 26 点（旧 21 点在 poses/backup/）；A5 11×8，tool10120938
    waypoint_positions: [
      # pose 0
-      -0.928421, -1.237477, 1.528823, 1.526334, -1.230146, 0.098562, 0.538531,
+      -0.211261, -0.571428, 0.247301, 1.552793, -0.119748, 0.614676, -0.023410,
      # pose 1
-      -0.970203, -1.315554, 1.553230, 1.669383, -1.407869, 0.125907, 0.578173,
+      -0.169531, -0.647538, 0.404777, 1.568624, -0.268686, 0.700171, 0.395947,
      # pose 2
-      -0.967819, -1.525572, 1.706057, 1.357062, -1.211051, 0.023232, 0.193541,
+      -0.759204, -0.846289, 0.735872, 1.387307, -0.539553, 0.576374, -0.060241,
      # pose 3
-      -1.065106, -1.624218, 1.793644, 1.279402, -1.120929, 0.019151, 0.139658,
+      -0.925104, -1.394748, 1.532423, 0.826872, -0.686369, -0.074315, -0.145523,
      # pose 4
-      -0.386279, -1.551838, 1.563578, 1.552444, -1.775337, -0.341974, 0.957445,
+      -1.004515, -1.491910, 1.596406, 1.245659, -0.864236, 0.212990, 0.460589,
      # pose 5
-      -0.227779, -1.219414, 1.026882, 0.828801, -0.916809, -0.166985, 0.034154,
+      -0.373052, -1.441561, 1.624783, 0.325602, -1.361228, -0.404116, 0.511430,
      # pose 6
-      -0.324968, -1.273082, 1.092344, 0.723132, -0.922095, -0.206413, 0.152082,
+      -0.281212, -1.452318, 1.627866, 0.517637, -1.412135, -0.420614, 0.217023,
      # pose 7
-      -0.421966, -1.309101, 1.143065, 0.606456, -0.906673, -0.219282, 0.237953,
+      -0.184868, -1.473244, 1.670229, 0.827772, -1.522330, -0.529014, -0.212158,
      # pose 8
-      -0.851598, -1.260600, 1.302895, 1.127033, -0.969809, 0.032856, 0.642673,
+      -0.246577, -1.532828, 1.679171, 1.204498, -1.697638, -0.607779, -0.517785,
      # pose 9
-      -0.951418, -0.550124, 0.848598, 2.139787, -1.025778, 1.184418, 0.598394,
+      -0.463435, -1.549157, 1.650964, 1.948746, -1.841124, -0.638016, -0.842437,
      # pose 10
-      -0.834498, -0.635767, 0.970038, 2.058410, -1.035138, 1.032040, 0.738619,
+      -1.573539, -0.359801, 1.261534, 2.423352, -0.405671, 1.620875, -0.045014,
      # pose 11
-      -0.519528, -0.883321, 1.135035, 1.830274, -1.100901, 0.234065, -0.069068,
+      -1.549088, -0.710364, 1.347227, 2.111776, -0.606133, 1.116345, -0.567788,
      # pose 12
-      -0.347358, -1.081426, 1.192233, 1.514557, -1.244594, -0.116668, -0.564362,
+      -1.512218, -0.983202, 1.445454, 1.808168, -0.602205, 0.699420, -0.803771,
      # pose 13
-      -0.364832, -1.102689, 1.224844, 1.520320, -1.265760, -0.136855, -0.582459,
+      -1.023248, -1.342205, 1.465909, 1.092911, -0.843418, 0.099672, 1.287399,
      # pose 14
-      -1.130796, -1.316186, 1.711089, 1.821409, -1.278003, 0.010885, -1.337210,
+      -0.870257, -1.473283, 1.657987, 0.831271, -1.034030, -0.144608, 1.263203,
      # pose 15
-      -0.732222, -1.376363, 1.812339, 1.293854, -1.410826, -0.226771, -0.217230,
+      -0.384368, -1.417984, 1.403442, 2.075614, -1.952106, -0.394139, 1.022592,
      # pose 16
-      -0.885017, -1.493416, 1.894070, 1.379021, -1.520015, -0.257115, 1.025331,
+      -0.320712, -1.513967, 1.508547, 1.652983, -1.854378, -0.409816, 1.024834,
      # pose 17
-      -0.911769, -1.532695, 1.944123, 1.427787, -1.560513, -0.267425, 1.004398,
+      -0.046066, -1.630857, 1.639221, 1.105078, -1.971509, -0.601654, 1.076452,
      # pose 18
-      -1.436271, -1.768595, 1.559397, 2.244717, -2.057112, 0.341785, 0.749883,
+      -0.183628, -1.588663, 1.989621, 0.918793, -1.873979, -0.629910, 0.092012,
      # pose 19
-      -1.511828, -1.759337, 1.620075, 2.131574, -1.946595, 0.328227, 0.584657,
+      -1.300709, -1.159500, 1.611790, 1.761984, -0.963027, 0.391619, -0.468980,
      # pose 20
-      -1.180482, -1.643241, 1.865851, 1.597699, -1.464982, 0.097605, -0.217780,
+      -0.750548, -1.221231, 1.566283, 1.128755, -1.163176, -0.102841, 0.958420,
      # pose 21
      -0.874185, -1.297230, 1.655940, 1.303348, -1.334815, -0.047425, 2.047690,
      # pose 22
      -0.671314, -1.510973, 1.813908, 1.235398, -1.778996, -0.462958, 2.899902,
      # pose 23
      -0.710795, -1.522124, 1.653705, 1.316372, -1.574330, 0.086268, 1.453877,
      # pose 24
      -0.533642, -1.545775, 1.837151, 1.467081, -1.463448, -0.161183, 0.387563,
      # pose 25
      -0.850451, -1.523689, 1.982673, 1.082314, -1.771622, -0.354609, 0.583659,
    ]
@@ -7,48 +7,38 @@
 /**:
  ros__parameters:
-    num_poses: 21
+    num_poses: 16
    waypoint_positions: [
      # pose 0
-      -0.798667, -0.611079, 0.054912, 2.246988, 0.049199, 1.389219, 0.196135,
+      -0.850450, -1.523690, 1.982676, 1.082314, -1.771616, -0.354603, 0.583665,
      # pose 1
-      -0.787304, -0.531383, 0.240108, 2.183440, 0.367026, 1.370311, 0.472489,
+      -2.098137, -0.606148, 1.213612, 2.290328, -0.795715, 0.618633, 0.213293,
      # pose 2
-      -1.615896, -0.687070, 1.187600, 2.243693, -0.162789, 1.650045, 1.026121,
+      -2.115870, -0.711154, 1.204674, 2.128285, -0.937034, 0.514766, -0.356714,
      # pose 3
-      -1.787431, -0.668004, 1.408195, 2.025431, -0.064718, 0.565415, 1.324635,
+      -2.017820, -0.798105, 1.516945, 2.002096, -0.392371, -0.035888, 0.657789,
      # pose 4
-      -2.024343, -0.863793, 1.349640, 2.359469, -0.720629, 0.084467, 0.821412,
+      -2.021874, -0.805058, 1.533201, 2.001407, -0.399671, -0.045924, 0.653138,
      # pose 5
-      -1.884100, -1.117584, 1.183093, 2.127937, -1.258013, 0.151934, 0.985555,
+      -2.135665, -1.020864, 1.346306, 2.334297, -1.268378, 0.406336, 0.897832,
      # pose 6
-      -1.866465, -1.194137, 1.143263, 1.872811, -1.320028, 0.280719, 0.777663,
+      -1.793944, -1.143422, 1.244557, 1.942177, -1.315611, 0.473616, 1.619695,
      # pose 7
-      -1.729402, -1.139685, 1.127574, 1.913544, -1.329630, -0.194162, 1.383834,
+      -2.075950, -1.215149, 1.322761, 2.142359, -1.372597, 0.398777, 1.008202,
      # pose 8
-      -1.611937, -1.158158, 1.064049, 1.918313, -1.480676, -0.392994, 1.833894,
+      -2.153500, -1.215892, 1.345656, 2.306931, -1.412319, 0.415145, 0.997235,
      # pose 9
-      -2.252978, -1.387014, 1.686186, 2.101298, -1.333349, -0.111994, 0.036817,
+      -2.127777, -1.256654, 1.370562, 2.009440, -1.369584, 0.219094, -0.132912,
      # pose 10
-      -2.179722, -1.297659, 1.497171, 2.167675, -1.272158, 0.100548, 0.673603,
+      -2.138686, -1.270867, 1.591854, 2.150744, -0.898729, 0.079835, 0.840791,
      # pose 11
-      -1.921897, -1.543024, 1.000863, 1.897273, -1.455972, 1.437388, -0.140968,
+      -2.063605, -1.300562, 1.582835, 2.080463, -0.904827, 0.243915, 1.110180,
      # pose 12
-      -1.884059, -1.063347, 1.439248, 2.253200, -1.164963, -0.423306, 0.907197,
+      -1.867772, -1.316320, 1.376713, 1.888199, -1.274029, 0.284609, 1.405712,
      # pose 13
-      -1.987022, -1.235459, 1.354108, 2.370862, -1.354044, 0.065378, 0.923079,
+      -1.829092, -1.355347, 1.315742, 1.808403, -1.413826, 0.338349, 1.459711,
      # pose 14
-      -1.770869, -1.333861, 1.241996, 1.800723, -1.395819, -0.255590, 1.389660,
+      -2.192461, -1.374082, 1.330037, 2.154137, -1.528020, 0.279283, 0.657702,
      # pose 15
-      -1.761759, -1.369860, 1.213137, 1.791639, -1.467956, -0.170537, 1.473557,
+      -2.137444, -1.468886, 1.525289, 2.348419, -1.025385, 1.011855, 1.148855,
      # pose 16
      -1.504294, -1.346227, 1.206550, 1.660186, -1.320782, -0.393676, 1.657215,
      # pose 17
      -1.846546, -1.476930, 0.946255, 1.910615, -2.435429, 1.535168, 2.151544,
      # pose 18
      -1.700067, -1.452760, 0.909035, 1.758222, -2.103930, 1.445249, 2.174743,
      # pose 19
      -2.173713, -0.983647, 1.399849, 2.384939, -1.379509, -0.216471, 0.253561,
      # pose 20
      -2.160187, -0.967904, 1.376288, 2.451713, -1.393524, -0.212278, 0.473606,
    ]
@@ -1,22 +1,22 @@
 # 手眼标定结果
 # mode: eye_to_hand
 # camera_source: d435
-# views: 24
+# views: 15
-# utc: 2026-10-12T05:20:30.137123+00:00
+# utc: 2026-10-16T03:02:49.214344+00:00
 # board: 11x8 square_m=0.02
 # gripper_tool: tool10120938
 # T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）
 # 旧结果备份在 results/backup/
 gripper_tool: tool10120938
 T_flange_to_tool:
-  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [1.00000000, 0.00000000, 0.00000000, 0.00075900]
-  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 1.00000000, 0.00000000, -0.00008800]
-  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22627400]
  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
 # T_cam_to_base (eye_to_hand)
-translation_m: [0.37094529, -0.52805928, 0.73745585]
+translation_m: [0.38078822, -0.52208142, 0.70359964]
 T_4x4:
-  - [-0.00990083, 0.99909706, -0.04131632, 0.37094529]
+  - [-0.03086969, 0.99833255, -0.04877684, 0.38078822]
-  - [0.99991380, 0.01024832, 0.00820703, -0.52805928]
+  - [0.99951823, 0.03067553, -0.00472440, -0.52208142]
-  - [0.00862304, -0.04123150, -0.99911241, 0.73745585]
+  - [-0.00322027, -0.04889918, -0.99879853, 0.70359964]
  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
@@ -1,28 +1,27 @@
 # 手眼标定结果
 # mode: eye_in_hand
 # camera_source: d405_left
-# views: 21
+# views: 25
-# utc: 2026-10-12T03:29:51.243487+00:00
+# utc: 2026-10-16T02:29:07.572249+00:00
 # board: 11x8 square_m=0.015
 # gripper_tool: tool10120938
 # T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）
 # T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/
 # 2026-10-12 第一次写出的 T_4x4 乘序反了（tz≈-296 mm），已按同一份 T_cam_to_tool 重折。
 gripper_tool: tool10120938
 T_flange_to_tool:
-  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [1.00000000, 0.00000000, 0.00000000, 0.00075900]
-  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 1.00000000, 0.00000000, -0.00008800]
-  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22627400]
  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
 T_cam_to_tool:
-  - [-0.03435184, -0.86951953, 0.49270249, -0.05961995]
+  - [0.01117708, -0.86715374, 0.49791511, -0.06192365]
-  - [0.99932767, -0.03620511, 0.00577974, -0.01523625]
+  - [0.99992647, 0.01203574, -0.00148504, -0.00762539]
-  - [0.01281276, 0.49256977, 0.87017863, -0.09761517]
+  - [-0.00470501, 0.49789509, 0.86722450, -0.09779711]
  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
 # T_cam_to_flange (eye_in_hand，由工具 TCP 折算)
-translation_m: [-0.06011995, -0.00853625, 0.12688483]
+translation_m: [-0.06116465, -0.00771339, 0.12847689]
 T_4x4:
-  - [-0.03435184, -0.86951953, 0.49270249, -0.06011995]
+  - [0.01117708, -0.86715374, 0.49791511, -0.06116465]
-  - [0.99932767, -0.03620511, 0.00577974, -0.00853625]
+  - [0.99992647, 0.01203574, -0.00148504, -0.00771339]
-  - [0.01281276, 0.49256977, 0.87017863, 0.12688483]
+  - [-0.00470501, 0.49789509, 0.86722450, 0.12847689]
  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','assembly_pipeline'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','assembly_pipeline'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'assembly_pipeline')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'assembly_pipeline')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','demo_named_poses'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','demo_named_poses'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'demo_named_poses')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'demo_named_poses')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dh_pgea'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dh_pgea'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dh_pgea')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dh_pgea')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dh_pgea_cli'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dh_pgea_cli'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dh_pgea_cli')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dh_pgea_cli')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dual_gripper'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dual_gripper'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dual_gripper')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dual_gripper')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dual_gripper_cli'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dual_gripper_cli'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dual_gripper_cli')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dual_gripper_cli')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','gauge_assembly'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','gauge_assembly'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'gauge_assembly')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'gauge_assembly')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','goto_ee_plane'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','goto_ee_plane'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'goto_ee_plane')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'goto_ee_plane')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_calibrate'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_calibrate'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_calibrate')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_calibrate')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_teach'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_teach'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_teach')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_teach')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_waypoints'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_waypoints'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_waypoints')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_waypoints')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','hole_detect'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','hole_detect'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'hole_detect')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'hole_detect')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','move_to_pose_client'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','move_to_pose_client'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'move_to_pose_client')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'move_to_pose_client')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','perception_node'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','perception_node'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'perception_node')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'perception_node')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','realsense_color_pub'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','realsense_color_pub'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'realsense_color_pub')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'realsense_color_pub')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','realsense_view'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','realsense_view'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'realsense_view')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'realsense_view')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','run_joint_trajectory'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','run_joint_trajectory'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'run_joint_trajectory')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'run_joint_trajectory')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','servo_node'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','servo_node'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'servo_node')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'servo_node')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','stage2_locate'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','stage2_locate'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'stage2_locate')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'stage2_locate')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','stage3_search'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','stage3_search'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'stage3_search')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'stage3_search')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','tactile'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','tactile'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'tactile')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'tactile')())
@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','tactile_cli'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','tactile_cli'
 import re
 import sys
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 try:
    from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 if __name__ == '__main__':
    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'tactile_cli')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'tactile_cli')())
@@ -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(
@@ -137,9 +137,7 @@ class HandeyeCalibrate(TolerantNode):
        self.declare_parameter('pin_hold_sec', 0.45)        # 每拍多长
        self.declare_parameter('skip_unstable_poses', True)
-        from arm_application.site_io import ros2_root
+        self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
        self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
        self.declare_parameter('result_file', '')
        self.declare_parameter('cleanup_images', True)
        # 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
@@ -108,12 +108,10 @@ class RdkBridge:
        self.robot_sn = robot_sn
        self._last_q: Optional[List[float]] = None
        self._io = threading.Lock()
-        from arm_application.site_io import ros2_root
+        py = os.environ.get('RDK_PYTHON', '/home/cc/rdk_py310/bin/python')
-
+        bridge = Path.home() / 'ros2' / 'scripts' / 'rdk_drag_bridge.py'
-        py = os.environ.get('RDK_PYTHON', '').strip() or str(
+        if not bridge.is_file():
-            Path.home() / 'rdk_py310' / 'bin' / 'python'
+            bridge = Path('/home/cc/ros2/scripts/rdk_drag_bridge.py')
        )
        bridge = ros2_root() / 'scripts' / 'rdk_drag_bridge.py'
        if not bridge.is_file():
            raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}')
@@ -222,13 +220,6 @@ class RdkBridge:
                return self._last_q
        return self._last_q
    def move_joints(self, joints: List[float], speed: float = 0.15) -> dict:
        """按关节阻抗把臂缓慢送到已记录的 7 个关节角。speed 单位 rad/s。"""
        return self._ask(
            {'cmd': 'move_joints', 'q': [float(v) for v in joints[:7]], 'speed': float(speed)},
            timeout=180.0,
        )
    def close(self) -> None:
        try:
            self._ask({'cmd': 'quit'}, timeout=3.0)
@@ -248,9 +239,7 @@ class RdkBridge:
 class HandeyeTeach(TolerantNode):
    def __init__(self) -> None:
        super().__init__('handeye_teach')
-        from arm_application.site_io import ros2_root
+        self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
        self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
        self.declare_parameter('width', 1280)
        self.declare_parameter('height', 720)
        self.declare_parameter('fps', 15)
@@ -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])
@@ -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
@@ -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(
                    Image,
                    f"/{name}/aligned_depth_to_color/image_raw",
                    self._depth(name),
                    qos_profile_sensor_data,
                )
-        def _color(self, name: str):
+        def sync_depth(self) -> None:
            """深度没打开就不订。订上之后每一帧都会整幅拷进进程，彩色帧率会被拖下去。"""
            with _LOCK:
                want = {
                    cam["id"]
                    for cam in cams
                    if f"{cam['id']}_depth" in _PREVIEW_WANT
                }
            for cam in cams:
                name = cam["id"]
                if name in want and name not in self._depth_subs:
                    self._depth_subs[name] = self.create_subscription(
                        Image,
                        f"/{name}/aligned_depth_to_color/image_raw",
                        self._depth(name),
                        qos_profile_sensor_data,
                    )
                elif name not in want and name in self._depth_subs:
                    self.destroy_subscription(self._depth_subs.pop(name))
        def _det(self, role: str):
            def _cb(msg: Float32MultiArray) -> None:
                _remember_detection(role, [float(v) for v in msg.data])
            return _cb
        def _color(self, name: str, role: str):
            key = f"{name}_color"
            def _cb(msg: Image) -> None:
-                arr = _color_array(msg)
+                # 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。
-                if arr is None:
+                try:
-                    return
+                    view = _color_view(msg)
-                with lock:
+                    if view is None:
-                    latest[f"{name}_color"] = arr
+                        return
                    offer_color(key, view, bgr=(msg.encoding == "bgr8"), role=role)
                except Exception as exc:  # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
                    with _LOCK:
                        _STATE["error"] = str(exc)
            return _cb
        def _depth(self, name: str):
            key = f"{name}_depth"
            def _cb(msg: Image) -> None:
-                arr = _depth_array(msg)
+                try:
-                if arr is None:
+                    view = _depth_view(msg)
-                    return
+                    if view is None:
-                with lock:
+                        return
-                    latest[f"{name}_depth"] = arr
+                    offer_depth(key, view)
                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()
-    if thread is not None:
+    for thread in (_THREAD, _PAINT_THREAD):
-        thread.join(timeout=1.5)
+        if thread is not None:
            thread.join(timeout=1.5)
@@ -12,6 +12,7 @@ from __future__ import annotations
 import json
 import os
 import re
 import signal
 import subprocess
 import threading
 import time
@@ -23,7 +24,7 @@ from urllib.parse import urlparse
 import yaml
 from arm_application import operator_tactile_view as tactile_view
-from arm_application.site_io import eye_to_hand_result_name, overhead_camera
+from arm_application.site_io import eye_to_hand_result_name, listed_arms, overhead_camera, usb_expectations
 HOST = "127.0.0.1"
 PORT = 8766
@@ -36,11 +37,60 @@ else:
    ROOT = Path.home() / "ros2"
 LOG_DIR = Path("/tmp/orisys_console")
 # 动臂和软拖拽都要这台臂，不能一边下发位置一边再开拖拽。
 # 相机、夹爪、触觉不碰这条，也不该排在它后面等。
 _ROBOT_LANES = frozenset({"arm", "teach"})
 _ARM_ACTIONS = frozenset({
    "pipeline", "stage_track", "stage_locate", "stage_screw",
    "reset_home", "gauge_run", "real_up", "handeye_calibrate",
 })
 _LANE_WAIT = {
    "grip": "夹爪上一条还没到位",
    "camera": "相机命令还没返回",
    "tactile": "触觉命令还没返回",
    "arm": "这台臂上一条运动还没结束",
    "teach": "软拖拽还没返回",
 }
 def command_lane(action: str) -> str:
    """每类命令一条通道。同一条还没返回才拒绝，不让相机去等夹爪。"""
    if action == "stop_commands":
        return "stop"
    if action in ("camera_d405", "camera_d435"):
        return "camera"
    if action.startswith("tactile"):
        return "tactile"
    if action.startswith("gripper"):
        return "grip"
    if action == "handeye_teach":
        return "teach"
    if action in _ARM_ACTIONS:
        return "arm"
    return action
 def command_blocked_message(running: set[str], lane: str) -> str:
    """这条通道，或同一台臂上的另一条运动，还没结束时返回要显示的句子。"""
    if lane == "stop":
        return ""
    if lane in running:
        return _LANE_WAIT.get(lane, "这一条命令还没返回")
    if lane in _ROBOT_LANES:
        other = next((name for name in _ROBOT_LANES if name != lane and name in running), "")
        if other == "arm":
            return "这台臂还在动，软拖拽先不开"
        if other == "teach":
            return "软拖拽占着这台臂，运动先不开"
    return ""
 ASSETS = Path(__file__).resolve().parent
 _LOCK = threading.Lock()
 _TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
 _HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
 _PROCS: dict[str, int] = {}
 _LIVE: dict[str, subprocess.Popen] = {}
 _PROC_LOCK = threading.Lock()
 def _push_force(side: str, sample: dict) -> None:
@@ -76,29 +126,26 @@ def _site() -> dict:
 def _arms() -> list[dict]:
-    robots = (_site().get("robot") or {}).get("robots") or []
+    """真机臂来自 site.yaml。没有清单就空着，不补本机的 89/91。"""
-    out = []
+    return [
-    for robot in robots:
+        {
-        if not isinstance(robot, dict):
+            "id": row["id"],
-            continue
+            "label": row["label"],
-        sn = str(robot.get("sn") or "")
+            "sn": row["sn"],
-        arm_no = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
+            "tactile": row["tactile"],
-        if not arm_no:
+            "camera": row["camera"],
-            continue
+            "gripper": row["gripper"],
-        out.append({
+            "rotary": row["rotary"],
-            "id": arm_no,
+        }
-            "label": str(robot.get("label") or f"臂{arm_no}"),
+        for row in listed_arms(_site())
            "sn": sn,
            "tactile": bool(robot.get("tactile")),
            "camera": str((robot.get("camera") or {}).get("name") or ""),
            "gripper": str((robot.get("gripper") or {}).get("model") or ""),
        })
    return out or [
        {"id": "89", "label": "左臂89", "sn": "", "tactile": True, "camera": "d405_left", "gripper": ""},
        {"id": "91", "label": "右臂91", "sn": "", "tactile": False, "camera": "d405_right", "gripper": ""},
    ]
 def _default_arm() -> str:
    rows = _arms()
    return str(rows[0]["id"]) if rows else ""
 def _usb() -> list[dict]:
    found = {"d405": [], "d435": [], "orisys": []}
    want = {
@@ -119,10 +166,11 @@ def _usb() -> list[dict]:
            continue
        speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?"
        found[key].append({"port": dev.name, "speed": speed})
    expect = usb_expectations(_site())
    return [
-        {"id": "d405", "label": "腕部 D405", "expect": 2, "items": found["d405"]},
+        {"id": "d405", "label": "腕部 D405", "expect": expect["d405"], "items": found["d405"]},
-        {"id": "d435", "label": "手外 D435", "expect": 1, "items": found["d435"]},
+        {"id": "d435", "label": "手外 D435", "expect": expect["d435"], "items": found["d435"]},
-        {"id": "orisys", "label": "视触觉", "expect": 2, "items": found["orisys"]},
+        {"id": "orisys", "label": "视触觉", "expect": expect["orisys"], "items": found["orisys"]},
    ]
@@ -214,13 +262,16 @@ def _poll_tactile() -> None:
 def _robot(arm: str) -> dict:
    from arm_application.site_io import arm_short_id
    robots = (_site().get("robot") or {}).get("robots") or []
    for robot in robots:
        if not isinstance(robot, dict):
            continue
        sn = str(robot.get("sn") or "")
-        short = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
+        if sn.lower() == "sim":
-        if short == arm:
+            continue
        if arm_short_id(sn) == arm:
            return robot
    return {}
@@ -305,12 +356,30 @@ def _handeye(arm: str) -> dict:
        or shared.get("handeye_waypoints_eye_to_hand")
        or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml")
    ))
    wrist = _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {})
    wrist_name = ""
    if isinstance(robot.get("camera"), dict):
        wrist_name = str(robot["camera"].get("name") or "").strip().strip("/")
    wrist["preview_camera"] = wrist_name
    wrist["preview_label"] = "腕部相机"
    outside = _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {})
    outside["preview_camera"] = cam_name.strip().strip("/")
    outside["preview_label"] = "手外相机"
    return {
-        "eye_in_hand": _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {}),
+        "eye_in_hand": wrist,
-        "eye_to_hand": _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {}),
+        "eye_to_hand": outside,
    }
 def _by_arm(reader) -> dict:
    return {row["id"]: reader(row["id"]) for row in _arms()}
 def _gripper_live() -> dict:
    from arm_application.operator_gripper_state import snapshot as grip_snapshot
    return _by_arm(grip_snapshot)
 def status() -> dict:
    with _LOCK:
        tactile = {
@@ -322,20 +391,23 @@ def status() -> dict:
            "left": list(_HIST["left"]),
            "right": list(_HIST["right"]),
        }
    from arm_application.operator_arm_state import snapshot as arm_snapshot
    return {
        "arms": _arms(),
        "arm_live": _by_arm(arm_snapshot),
        "gripper_live": _gripper_live(),
        "usb": _usb(),
        "services": _services(),
        "grippers": _grippers(),
        "tactile": tactile,
        "history": history,
        "view": tactile_view.state(),
-        "handeye": {"89": _handeye("89"), "91": _handeye("91")},
+        "handeye": _by_arm(_handeye),
        "now": time.time(),
    }
-# 会动真机的动作必须 confirm。argv 里的 {arm} 只接受 89 或 91。
+# 会动真机的动作必须 confirm。臂号必须是 site.yaml 里某台真机的末两位。
 _ACTIONS = {
    "real_up": {
        "title": "上电控制栈",
@@ -377,21 +449,21 @@ _ACTIONS = {
        "script": "run_gripper.sh",
        "args": ["--arm", "{arm}", "--cli", "probe"],
        "moves": False,
-        "blurb": "读夹爪是否在线，不动手指。",
+        "blurb": "读夹爪是否在线，不动手指。驱动没在跑时先拉起，不回零。",
    },
    "gripper_open": {
        "title": "夹爪张开",
        "script": "run_gripper.sh",
        "args": ["--arm", "{arm}", "--cli", "open"],
        "moves": True,
-        "blurb": "张开当前臂夹爪。",
+        "blurb": "张开当前臂夹爪。驱动没在跑时先拉起，不回零。",
    },
    "gripper_close": {
        "title": "夹爪闭合",
        "script": "run_gripper.sh",
        "args": ["--arm", "{arm}", "--cli", "close"],
        "moves": True,
-        "blurb": "闭合当前臂夹爪。",
+        "blurb": "闭合当前臂夹爪。驱动没在跑时先拉起，不回零。",
    },
    "gripper_rotate": {
        "title": "右爪转 90°",
@@ -401,11 +473,11 @@ _ACTIONS = {
        "blurb": "右臂旋转爪转到 90°。左臂平行爪没有这一轴。",
    },
    "pipeline": {
-        "title": "整体三段",
+        "title": "跟踪并对孔",
        "script": "run_all.sh",
        "args": ["--arm", "{arm}"],
        "moves": True,
-        "blurb": "跟踪、对孔、搜孔轻压一次跑完。会动臂。",
+        "blurb": "跟踪和对孔一次跑完。搜孔轻压已取消，这一步不含旋入。会动臂。",
    },
    "stage_track": {
        "title": "只测跟踪",
@@ -421,12 +493,12 @@ _ACTIONS = {
        "moves": True,
        "blurb": "只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。",
    },
-    "stage_search": {
+    "stage_screw": {
-        "title": "只测搜孔",
+        "title": "只测旋转装配",
        "script": "run_stage3_search.sh",
-        "args": ["--arm", "{arm}"],
+        "args": ["--arm", "{arm}", "--screw"],
        "moves": True,
-        "blurb": "只跑三阶段：触面、搜孔、轻压。会动臂。臂应已停在孔上方。",
+        "blurb": "按现场配置的拧入深度和螺距把螺纹规旋进去。会动臂。臂应已在孔口。",
    },
    "reset_home": {
        "title": "复位到 home",
@@ -462,9 +534,9 @@ _ACTIONS = {
    "handeye_teach": {
        "title": "示教采点",
        "script": "run_handeye.sh",
-        "args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "12"],
+        "args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "{points}"],
        "moves": True,
-        "blurb": "软拖拽示教，目标 12 个姿态。要先停控制栈，会动臂。",
+        "blurb": "软拖拽示教。点数和是否接着采由手眼页上的控件决定。要先停控制栈，会动臂。",
    },
    "handeye_merge": {
        "title": "合并示教点",
@@ -507,6 +579,63 @@ def catalog() -> dict:
    }
 def _remember(action: str, proc: subprocess.Popen) -> None:
    with _PROC_LOCK:
        _LIVE[action] = proc
        _PROCS[action] = proc.pid
 def _forget(action: str) -> None:
    with _PROC_LOCK:
        _LIVE.pop(action, None)
 def _pid_alive(pid: int) -> bool:
    if pid <= 0:
        return False
    try:
        os.kill(pid, 0)
    except OSError:
        return False
    return True
 def stop_started() -> dict:
    """停掉本界面拉起的命令。只向登记过的进程组发信号，不扫全机。"""
    with _PROC_LOCK:
        items = list(_LIVE.items())
    if not items:
        return {"ok": True, "message": "没有正在跑的命令"}
    names: list[str] = []
    for action, proc in items:
        pid = int(getattr(proc, "pid", 0) or 0)
        if not _pid_alive(pid):
            _forget(action)
            continue
        try:
            os.killpg(os.getpgid(pid), signal.SIGTERM)
        except ProcessLookupError:
            _forget(action)
            continue
        except OSError as exc:
            return {"ok": False, "message": f"停不了 {action}：{exc}"}
        names.append(action)
    time.sleep(0.3)
    for action, proc in items:
        pid = int(getattr(proc, "pid", 0) or 0)
        if not _pid_alive(pid):
            _forget(action)
            continue
        try:
            os.killpg(os.getpgid(pid), signal.SIGKILL)
        except OSError:
            pass
        _forget(action)
    if not names:
        return {"ok": True, "message": "没有正在跑的命令"}
    return {"ok": True, "message": "已停止：" + "、".join(names)}
 def _stop_tactile() -> str:
    subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False)
    subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False)
@@ -514,7 +643,39 @@ def _stop_tactile() -> str:
    return "已发送停止视触觉节点"
-def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand") -> dict:
+def render_action_args(
    template: list[str], arm: str, mode: str, extras: dict | None = None,
 ) -> list[str]:
    """把动作模板里的臂号、模式、点数换成这次界面上的值。"""
    extra = extras or {}
    needs_points = any("{points}" in str(token) for token in template)
    points_text = ""
    if needs_points:
        try:
            points = int(extra.get("points", 12))
        except (TypeError, ValueError):
            raise ValueError("目标点数不是整数") from None
        if points < 4 or points > 30:
            raise ValueError("目标点数只接受 4–30")
        points_text = str(points)
    repl = {"{arm}": arm, "{mode}": mode, "{points}": points_text}
    out: list[str] = []
    for token in template:
        text = str(token)
        for key, value in repl.items():
            text = text.replace(key, value)
        out.append(text)
    if extra.get("append"):
        if "teach" not in out:
            raise ValueError("只有示教采点能接着采")
        out.append("--append")
    return out
 def run_action(
    action: str, arm: str, confirm: bool, mode: str = "eye_in_hand",
    extras: dict | None = None,
 ) -> dict:
    if action == "tactile_down":
        tactile_view.stop()
        return {"ok": True, "message": _stop_tactile()}
@@ -528,8 +689,8 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
    spec = _ACTIONS.get(action)
    if spec is None:
        return {"ok": False, "message": "没有这个动作"}
-    if arm not in ("89", "91"):
+    if arm not in {row["id"] for row in _arms()}:
-        return {"ok": False, "message": "臂号只接受 89 或 91"}
+        return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
    if mode not in ("eye_in_hand", "eye_to_hand"):
        return {"ok": False, "message": "手眼模式只接受 eye_in_hand 或 eye_to_hand"}
    if spec["moves"] and not confirm:
@@ -537,40 +698,86 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
    script = ROOT / "scripts" / spec["script"]
    if not script.is_file():
        return {"ok": False, "message": f"找不到 {script}"}
-    args = [a.replace("{arm}", arm).replace("{mode}", mode) for a in spec["args"]]
+    from arm_application.operator_arm_state import (
        hold_until_idle,
        motion_blocked,
        release_for_control,
        resume_after_control,
        snapshot as arm_snapshot,
        stack_running,
    )
    blocked = motion_blocked(
        action,
        stack_up=stack_running(),
        op_status=str((arm_snapshot(arm) or {}).get("op_status") or ""),
    )
    if blocked:
        return {"ok": False, "message": blocked}
    uses_rdk = action in {
        "real_up", "handeye_teach", "pipeline", "stage_track", "stage_locate",
        "stage_screw", "reset_home", "gauge_run", "handeye_calibrate",
    }
    if uses_rdk:
        release_for_control()
    from arm_application.operator_gripper_state import hold_serial
    grip_port = action in {
        "gripper_probe", "gripper_open", "gripper_close", "gripper_rotate", "gripper_up",
    }
    try:
        args = render_action_args(list(spec["args"]), arm, mode, extras)
    except ValueError as exc:
        return {"ok": False, "message": str(exc)}
    LOG_DIR.mkdir(parents=True, exist_ok=True)
    log_path = LOG_DIR / f"{action}_{arm}.log"
    shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
-    if spec.get("sync"):
+    try:
-        try:
+        if spec.get("sync"):
-            proc = subprocess.run(
+            proc = subprocess.Popen(
                ["bash", str(script), *args],
                cwd=str(ROOT),
-                capture_output=True,
+                stdout=subprocess.PIPE,
                stderr=subprocess.STDOUT,
                text=True,
-                timeout=float(spec.get("timeout") or 40),
+                start_new_session=True,
            )
-        except subprocess.TimeoutExpired:
+            _remember(action, proc)
-            return {"ok": False, "message": "命令超时还没返回", "detail": shown}
+            try:
-        detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+                detail, _ = proc.communicate(timeout=float(spec.get("timeout") or 40))
-        log_path.write_text(detail)
+            except subprocess.TimeoutExpired:
-        ok = proc.returncode == 0
+                try:
-        return {
+                    os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
-            "ok": ok,
+                except OSError:
-            "message": shown if ok else f"退出码 {proc.returncode}",
+                    pass
-            "detail": detail[-4000:],
+                return {"ok": False, "message": "命令超时还没返回", "detail": shown}
-            "log": str(log_path),
+            finally:
-        }
+                _forget(action)
-    log_f = log_path.open("ab")
+            detail = (detail or "").strip()
-    proc = subprocess.Popen(
+            log_path.write_text(detail)
-        ["bash", str(script), *args],
+            ok = proc.returncode == 0
-        cwd=str(ROOT),
+            return {
-        stdout=log_f,
+                "ok": ok,
-        stderr=subprocess.STDOUT,
+                "message": shown if ok else f"退出码 {proc.returncode}",
-        start_new_session=True,
+                "detail": detail[-4000:],
-    )
+                "log": str(log_path),
-    _PROCS[action] = proc.pid
+            }
-    return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
+        log_f = log_path.open("ab")
        proc = subprocess.Popen(
            ["bash", str(script), *args],
            cwd=str(ROOT),
            stdout=log_f,
            stderr=subprocess.STDOUT,
            start_new_session=True,
        )
        _remember(action, proc)
        if grip_port:
            hold_serial(proc.pid)
        if uses_rdk:
            hold_until_idle(proc.pid, until_stack=(action == "real_up"))
            uses_rdk = False
        return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
    finally:
        if uses_rdk:
            resume_after_control()
 def _bounded(name: str, raw, low: float, high: float) -> float:
@@ -585,12 +792,12 @@ def _bounded(name: str, raw, low: float, high: float) -> float:
 def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
    """按界面上的位置、力度、速度下发夹爪。右臂另有角度、圈数。"""
-    if arm not in ("89", "91"):
+    if arm not in {row["id"] for row in _arms()}:
-        return {"ok": False, "message": "臂号只接受 89 或 91"}
+        return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
    if kind not in ("pos", "rotate", "rotate_by", "revolutions"):
        return {"ok": False, "message": "没有这种夹爪调节"}
-    if kind != "pos" and arm != "91":
+    if kind != "pos" and not any(row["id"] == arm and row.get("rotary") for row in _arms()):
-        return {"ok": False, "message": "左臂平行爪没有旋转轴"}
+        return {"ok": False, "message": "这台夹爪没有旋转轴"}
    try:
        if kind == "pos":
            pos = int(round(_bounded("位置", numbers.get("pos"), 0, 1000)))
@@ -615,7 +822,17 @@ def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
                summary = f"转 {turns:.3f} 圈  力矩 {force}%  速度 {speed}%"
    except ValueError as exc:
        return {"ok": False, "message": str(exc)}
    from arm_application.operator_gripper_state import pause_serial, resume_serial
    script = ROOT / "scripts" / "run_gripper.sh"
    pause_serial()
    try:
        return _run_gripper_argv(script, arm, force, speed, cli, summary)
    finally:
        resume_serial()
 def _run_gripper_argv(script, arm: str, force: int, speed: int, cli: list[str], summary: str) -> dict:
    argv = [
        "bash", str(script),
        "--arm", arm,
@@ -624,19 +841,40 @@ def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
        "--cli", *cli,
    ]
    shown = " ".join(argv)
    proc = subprocess.Popen(
        argv, cwd=str(ROOT), stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
        text=True, start_new_session=True,
    )
    _remember(f"gripper_{arm}", proc)
    try:
-        proc = subprocess.run(argv, cwd=str(ROOT), capture_output=True, text=True, timeout=90)
+        try:
-    except subprocess.TimeoutExpired:
+            detail, _ = proc.communicate(timeout=90)
-        return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
+        except subprocess.TimeoutExpired:
-    detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+            try:
                os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
            except OSError:
                pass
            return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
    finally:
        _forget(f"gripper_{arm}")
    detail = (detail or "").strip()
    ok = proc.returncode == 0
    return {
        "ok": ok,
-        "message": summary if ok else f"夹爪退出码 {proc.returncode}",
+        "message": summary if ok else _grip_failure_message(proc.returncode, detail),
        "detail": (summary + "\n" + shown + "\n" + detail)[-4000:],
    }
 def _grip_failure_message(code: int, detail: str) -> str:
    """退出码旁边带上脚本打出来的原因，别只剩一个数字。"""
    for line in reversed(detail.splitlines()):
        text = line.strip()
        if text.startswith("ERROR:") or text.startswith("无法"):
            return text
    return f"夹爪退出码 {code}"
 class Handler(BaseHTTPRequestHandler):
    def log_message(self, fmt: str, *args) -> None:
        return
@@ -700,12 +938,12 @@ class Handler(BaseHTTPRequestHandler):
            return
        if path == "/api/tactile/view":
            action = "tactile_view_on" if body.get("on") else "tactile_view_off"
-            result = run_action(action, "89", False)
+            result = run_action(action, _default_arm(), False)
            self._json(200 if result.get("ok") else 400, result)
            return
        result = run_action(
            str(body.get("action") or ""),
-            str(body.get("arm") or "89"),
+            str(body.get("arm") or _default_arm()),
            bool(body.get("confirm")),
            str(body.get("mode") or "eye_in_hand"),
        )
@@ -715,6 +953,10 @@ class Handler(BaseHTTPRequestHandler):
 def main() -> None:
    import atexit
    atexit.register(tactile_view.stop)
    from arm_application.operator_arm_state import start as start_arm_state
    from arm_application.operator_gripper_state import start as start_gripper_state
    start_arm_state()
    start_gripper_state()
    threading.Thread(target=_poll_tactile, daemon=True).start()
    server = ThreadingHTTPServer((HOST, PORT), Handler)
    print(f"Orisys 工位界面  http://{HOST}:{PORT}", flush=True)
@@ -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
@@ -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))
@@ -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}
@@ -8,6 +8,8 @@
 from __future__ import annotations
 import re
 import subprocess
 import threading
 from datetime import datetime, timezone
 from pathlib import Path
@@ -17,13 +19,17 @@ import yaml
 _LOCK = threading.Lock()
 _BRIDGE = None
 _SN = ""
 _YIELDED = False
 _ARM_CONTROLLER = "rizon_arm_controller"
 def _file(arm: str) -> Path:
    from arm_application.operator_console import ROOT
    from arm_application.site_io import listed_arms, load_site_dict
-    if arm not in ("89", "91"):
+    known = {row["id"] for row in listed_arms(load_site_dict())}
-        raise ValueError("臂号只接受 89 或 91")
+    if arm not in known:
        raise ValueError(f"臂号 {arm} 不在 site.yaml 里")
    return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml"
@@ -36,9 +42,14 @@ def _bridge_for(sn: str):
        return _BRIDGE
    close()
    from arm_application.handeye_teach import RdkBridge
    from arm_application.operator_arm_state import release_for_control, resume_after_control
-    _BRIDGE = RdkBridge(sn)
+    release_for_control()
-    _SN = sn
+    try:
        _BRIDGE = RdkBridge(sn)
        _SN = sn
    finally:
        resume_after_control()
    return _BRIDGE
@@ -55,19 +66,95 @@ def close() -> None:
        pass
 def _switch_arm_controller(activate: bool) -> str:
    """停或恢复关节轨迹控制器。硬件还在，只是不再下发关节位置。
    控制栈在 ``NRT_JOINT_POSITION`` 里持续 ``SendJointPosition``，
    模式一旦被改走，写循环两秒内又切回去。软拖拽要低阻抗，必须先停这条控制器。
    """
    if activate:
        payload = (
            "{activate_controllers: ['%s'], deactivate_controllers: [], "
            "strictness: 2, activate_asap: true, timeout: {sec: 3, nanosec: 0}}"
            % _ARM_CONTROLLER
        )
    else:
        payload = (
            "{activate_controllers: [], deactivate_controllers: ['%s'], "
            "strictness: 1, activate_asap: true, timeout: {sec: 3, nanosec: 0}}"
            % _ARM_CONTROLLER
        )
    try:
        proc = subprocess.run(
            [
                "ros2", "service", "call",
                "/controller_manager/switch_controller",
                "controller_manager_msgs/srv/SwitchController",
                payload,
            ],
            capture_output=True,
            text=True,
            timeout=15,
        )
    except FileNotFoundError:
        return "找不到 ros2，停不了关节轨迹控制器"
    except subprocess.TimeoutExpired:
        return "关节轨迹控制器没回来" if activate else "停关节轨迹控制器超时"
    text = ((proc.stdout or "") + (proc.stderr or "")).strip()
    if proc.returncode == 0 and re.search(r"ok\s*[:=]\s*true", text, re.IGNORECASE):
        return ""
    tail = text.splitlines()[-1] if text else f"退出码 {proc.returncode}"
    verb = "恢复" if activate else "停下"
    return f"没能{verb}关节轨迹控制器：{tail}"
 def _yield_position_stream() -> str:
    """控制栈没起来就什么都不做。起来了就先把位置控制器让开。"""
    global _YIELDED
    from arm_application.operator_arm_state import stack_running
    if not stack_running():
        return ""
    err = _switch_arm_controller(False)
    if err:
        return err + "。位置环还在推，软拖拽切不过去。"
    _YIELDED = True
    return ""
 def _restore_position_stream() -> str:
    global _YIELDED
    if not _YIELDED:
        return ""
    err = _switch_arm_controller(True)
    _YIELDED = False
    return err
 def drag_on(sn: str) -> dict:
    with _LOCK:
        try:
            yielded = _yield_position_stream()
            if yielded:
                return {"ok": False, "message": yielded}
            bridge = _bridge_for(sn)
            result = bridge.drag_on(mode="soft")
        except Exception as exc:
-            return {"ok": False, "message": str(exc)}
+            restore = _restore_position_stream()
-    if not result.get("ok"):
+            message = str(exc)
-        return {"ok": False, "message": str(result.get("error") or result)}
+            if restore:
-    return {
+                message = f"{message}；{restore}"
-        "ok": True,
+            return {"ok": False, "message": message}
-        "message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。",
+        if not result.get("ok"):
-    }
+            restore = _restore_position_stream()
            message = str(result.get("error") or result)
            if restore:
                message = f"{message}；{restore}"
            return {"ok": False, "message": message}
        note = "软拖拽已开启。轻推机械臂，再记点。"
        if _YIELDED:
            note += "关节轨迹控制器已停下，关掉拖拽后会回来。"
        return {"ok": True, "message": note}
 def drag_off(sn: str) -> dict:
@@ -76,9 +163,16 @@ def drag_off(sn: str) -> dict:
            bridge = _bridge_for(sn)
            result = bridge.drag_off()
        except Exception as exc:
-            return {"ok": False, "message": str(exc)}
+            restore = _restore_position_stream()
            message = str(exc)
            if restore:
                message = f"{message}；{restore}"
            return {"ok": False, "message": message}
    if not result.get("ok"):
        return {"ok": False, "message": str(result.get("error") or result)}
    restore = _restore_position_stream()
    if restore:
        return {"ok": False, "message": f"软拖拽已关闭，但{restore}"}
    return {"ok": True, "message": "软拖拽已关闭，臂保持当前位置。"}
@@ -45,6 +45,11 @@ from arm_application.path_profile import (  # 伺服和测试从本模块再导
 # 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
 D435_MIN_PROGRESS_FRAC = 0.40
 # SwitchMode 返回不等于已经进 primitive。连试仍失败必须让一阶段立刻中止，
 # 不许空等 150s 装成"还在粗定位"（2026-10-16 17:27）。
 D435_MOVEL_RETRY = 3
 D435_MOVEL_RETRY_S = 0.3
 D435_MOVEL_GAVE_UP = "厂家 MoveL 连试 3 次没走成"
 def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
@@ -154,16 +159,27 @@ def d405_commands_arm(
 ) -> bool:
    """D405 这一帧的点能不能下发给左臂。
-    推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
+    推理从启动就开着。没有高置信度目标、或 D435 厂家 MoveL 还没结束、
-    停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
+    法兰还没连续停稳时，这帧**不下发**（输出截断，不许拿去控臂）。
-    也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
+    厂家移动结束并且法兰停稳（``settled``，时长是 ``d435_handback_sec``）
-    ``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
+    之后才放行，伺服用它追工件框的矩形中心。框心对准停稳后才进精拍下臂。
    ``move_only``：只测 D435 位移，D405 点一律不下发。
    """
    if move_only:
        return False
-    if not using_d435:
+    if using_d435:
-        return True
+        return bool(arrived) and bool(settled)
-    return bool(arrived) and bool(settled)
+    return True
 def retract_goal_z(z_now: float, z_target: float) -> float:
    """收臂目标 Z：已经高于目标就钉在当前高度，不许为了追 5 mm 往下补。
    2026-10-16：贴面 MoveL 之后 JTC 还握着悬停关节，收臂 hop1 计划 +5 mm
    实际抬了 18–25 mm；hop2 把「抬过头」当成误差 ``Δz=−13 mm`` 往回压，
    看起来就是进孔再弹出。抬过了 = 成功。
    """
    return max(float(z_now), float(z_target))
 def oneshot_needs_verticalize(
@@ -105,7 +105,7 @@ class PerceptionNode(TolerantNode):
        self.declare_parameter("image_topic", "")
        self.declare_parameter("camera_info_topic", "")
        self.declare_parameter("depth_topic", "/d405/aligned_depth_to_color/image_raw")
-        self.declare_parameter("weights", "models/yolo/gongjian0929.onnx")
+        self.declare_parameter("weights", "/home/cc/ros2/models/yolo/gongjian0929.onnx")
        self.declare_parameter("conf", 0.10)
        self.declare_parameter("iou", 0.45)
        self.declare_parameter("imgsz", 640)
@@ -336,9 +336,7 @@ class PerceptionNode(TolerantNode):
            r_pos=float(self.get_parameter("kf_r_pos").value),
        )
-        from arm_application.site_io import resolve_repo_path
+        weights_raw = str(self.get_parameter("weights").value)
        weights_raw = resolve_repo_path(str(self.get_parameter("weights").value))
        weights = str(resolve_weights(weights_raw, self.backend_name))
        yolo_cfg = {
            "weights": weights,
@@ -1141,8 +1139,8 @@ class PerceptionNode(TolerantNode):
            filt[2] = z_use
            self._last_filt = filt.copy()
-        if handoff:
+        if handoff or self._d435_commanding:
-            # 画面里没有 conf≥门槛的目标：绿框可以留，三维点不许发 —— 伺服会拿去追。
+            # 没有高置信度，或 D435 厂家移动还没交接：推理留着，控制点不许发。
            return
        self._publish_point(filt, stamp)
        self.get_logger().info(
@@ -1427,15 +1425,11 @@ class PerceptionNode(TolerantNode):
        self._z_locked = bool(msg.data)
    def _on_d435_commanding(self, msg: Bool) -> None:
-        """伺服还在执行 D435 那一个点时，不再提交下一张。
+        """伺服还在执行 D435 那一个点时，不再提交下一张，D405 控制点也不发。
-        这一段结束（到位并停稳）后清掉「已拍过」，漏检重新计 0.5 s。
+        这一轮已经拍过并走完。停稳后打开 D405 框心，不因交接瞬间漏检再 MoveL。
        """
-        commanding = bool(msg.data)
+        self._d435_commanding = bool(msg.data)
        if self._d435_commanding and not commanding:
            self._oneshot_shot = False
            self._low_since = None
        self._d435_commanding = commanding
    def _publish_overhead_k0(self, p_base: np.ndarray, is_k0: bool) -> None:
        pt = PointStamped()
@@ -15,6 +15,7 @@ import os
 import signal
 import subprocess
 import time
 from pathlib import Path
 from typing import Optional
 import numpy as np
@@ -23,6 +24,7 @@ from geometry_msgs.msg import PointStamped
 from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 from std_msgs.msg import Bool, String
 from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP
 from arm_application.stage3_search_node import Stage3Search
@@ -70,6 +72,19 @@ def init_z_refusal(
    return ""
 def hold_at_insert_z(final, *, skip3: bool) -> bool:
    """贴面已经按 ``final_gap_mm`` 停下时，不再进三阶段。
    三阶段开局会抬到悬高上方测触觉，再落回 ``hole_hover_z_m``。
    没有接触传感器。到位高度就是这次的贴面间隙，不另设一个 Z。
    """
    if skip3:
        return True
    if not isinstance(final, dict):
        return False
    return bool(final.get("insert_held"))
 def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
    """一阶段锁高。
@@ -85,6 +100,20 @@ def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float
    return hover + TRACK_CLEARANCE_M
 def stage1_log_fatal(log_path: str) -> str:
    """一阶段日志里已经写明失败时，不许再空等锁定。空串 = 还没到这一步。"""
    try:
        text = Path(log_path).read_text(encoding="utf-8", errors="replace")
    except OSError:
        return ""
    if D435_MOVEL_GAVE_UP in text:
        return (
            f"D435 {D435_MOVEL_GAVE_UP}（RDK 控制模式没切到 primitive），"
            f"一阶段中止。不是视野/标定。看 {log_path}"
        )
    return ""
 def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
    """一阶段超时了，到底是"感知没给点"还是"给了点但没收敛"。
@@ -99,6 +128,9 @@ def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) ->
    以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
    因为缺依赖早退了"被读成"算法不收敛"，白查半天。
    """
    fatal = stage1_log_fatal(log_path)
    if fatal:
        return fatal
    if points_seen <= 0:
        return (
            f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
@@ -308,6 +340,11 @@ class AssemblyPipeline(Stage3Search):
            self._spin(0.1)
            if self._locked:
                break
            fatal = stage1_log_fatal(log_path)
            if fatal:
                self._stop_stage1()
                self.get_logger().error(fatal)
                return 1
            if self._s1.poll() is not None:
                self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
                return 1
@@ -363,24 +400,42 @@ class AssemblyPipeline(Stage3Search):
                if code != 0:
                    self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
                    return code
-                self.get_logger().info(
+                stay = hold_at_insert_z(final, skip3=skip3)
-                    f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
+                if stay and final.get("insert_held"):
-                    + (
+                    self.get_logger().info(
-                        " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
+                        f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm — "
-                        if skip3
+                        f"插孔停在法兰 Z={final.get('flange_z_m')}m，"
-                        else " — 立刻下压搜孔并轻压（不拧入）"
+                        f"规尖离面 {final.get('tip_gap_mm')}mm"
                        f"（目标 {final.get('commanded_gap_mm')}mm，来自 final_gap_mm）。"
                        "没有接触传感器，不另设 Z，不抬回悬高。"
                    )
                else:
                    self.get_logger().info(
                        f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
                        + (
                            " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
                            if skip3
                            else " — 立刻下压搜孔并轻压（不拧入）"
                        )
                    )
                )
            else:
                self._state("skip_stage2")
                stay = hold_at_insert_z(final, skip3=skip3)
-            if skip3:
+            if stay:
                self._state("skip_stage3")
                if self.d435_move_only:
                    self.get_logger().info(
                        "D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
                        "15s 后本进程退出，臂保持不动"
                    )
                elif isinstance(final, dict) and final.get("insert_held"):
                    if q is not None:
                        self._pin(q)
                    self.get_logger().info(
                        "插孔已停在贴面高度，不进搜孔、不抬回悬高。"
                        "15s 后本进程退出，臂保持这个 Z。"
                    )
                else:
                    self.get_logger().info(
                        "跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
@@ -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,23 +313,26 @@ class RealsenseColorPub(TolerantNode):
        self._info_pub.publish(info)
        if depth_u16 is not None:
-            dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
+            self._publish_depth(depth_u16, stamp)
-            dmsg = Image()
+
-            dmsg.header.stamp = stamp
+    def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None:
-            dmsg.header.frame_id = self._frame_id
+        dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
-            dmsg.height = dh
+        dmsg = Image()
-            dmsg.width = dw
+        dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
-            dmsg.encoding = '16UC1'
+        dmsg.header.frame_id = self._frame_id
-            dmsg.is_bigendian = 0
+        dmsg.height = dh
-            dmsg.step = dw * 2
+        dmsg.width = dw
-            dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
+        dmsg.encoding = '16UC1'
-            self._depth_pub.publish(dmsg)
+        dmsg.is_bigendian = 0
-            if not self._logged_depth:
+        dmsg.step = dw * 2
-                self._logged_depth = True
+        dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
-                self.get_logger().info(
+        self._depth_pub.publish(dmsg)
-                    f'首个对齐深度 {dw}x{dh} → '
+        if not self._logged_depth:
-                    f'/{self._name}/aligned_depth_to_color/image_raw'
+            self._logged_depth = True
-                )
+            self.get_logger().info(
                f'首个对齐深度 {dw}x{dh} → '
                f'/{self._name}/aligned_depth_to_color/image_raw'
            )
    def _start_pipeline(self, reconnect: bool = False) -> None:
        try:
@@ -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()
-                    continue
+                else:
-                if misses == 1:
+                    self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
                    self.get_logger().warn(f'wait_for_frames：{exc}')
                continue
            if not color:
                misses += 1
                continue
            misses = 0
            bgr = np.asanyarray(color.get_data())
            depth = frames.get_depth_frame()
            depth_u16 = np.asanyarray(depth.get_data()) if depth else None
            if depth_u16 is not None and self._depth_to_mm != 1.0:
                # 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
                depth_u16 = np.clip(
                    depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
                ).astype(np.uint16)
            if depth_u16 is not None:
                try:
                    self._publish_depth(depth_u16)
                except Exception as exc:  # noqa: BLE001
                    if not self._running:
                        break
                    self.get_logger().warn(f'发布深度失败：{exc}', throttle_duration_sec=2.0)
            self._frame_i += 1
            self._fps_meter(color, depth, depth_u16)
            # 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
@@ -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:
@@ -50,6 +50,9 @@ from arm_application.geom_ik import (
 )
 from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
 from arm_application.overhead_oneshot import (
    D435_MOVEL_GAVE_UP,
    D435_MOVEL_RETRY,
    D435_MOVEL_RETRY_S,
    D435_NEAR_CRUISE_MPS,
    D435_SETTLE_S,
    D435_VENDOR_MOVEL_VEL_MPS,
@@ -126,7 +129,7 @@ class ServoNode(TolerantNode):
        self.declare_parameter("point_topic", "visual_servo/filtered_point")
        # 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。
        # 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。
-        self.declare_parameter("handeye_file", "handeye/results/89_eye_in_hand.yaml")
+        self.declare_parameter("handeye_file", "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml")
        # eye_in_hand = cam→法兰；eye_to_hand = cam→基座。写错会把固定相机当腕部用。
        self.declare_parameter("handeye_mode", "eye_in_hand")
        self.declare_parameter("use_static_handeye_tf", False)
@@ -224,8 +227,7 @@ class ServoNode(TolerantNode):
        self.declare_parameter("lock_hold_sec", 2.0)
        # D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
        # lock_still_sec 仍须从 site 接上（缺了就报错）。
-        # 交回 D405：进程内 MoveL 到位后，再叠 d435_handback_sec（法兰 XY 停稳）。
+        # 厂家 MoveL 结束后，法兰连续停稳 d435_handback_sec 才打开 D405 框心。
        # 旧杀栈冷启动曾把这 3s 垫在控制器重启后面；不杀栈后只留机械停稳。
        self.declare_parameter("lock_still_sec", -1.0)
        self.declare_parameter("d435_handback_sec", -1.0)
        self.declare_parameter("wrist_align_sec", -1.0)
@@ -329,7 +331,7 @@ class ServoNode(TolerantNode):
            and self.wrist_handoff_conf >= 0.0
            and self.overhead_hold_s >= 0.0
        )
-        # 框心对准进对孔用 wrist_align_sec。D435 交回只用 d435_handback_sec，两段不要合成一个数。
+        # 框心对准进精拍用 wrist_align_sec。D435 交接只用 d435_handback_sec，两段不要合成一个数。
        self._still = StillLock(
            still_sec=max(0.0, self.wrist_align_sec),
            still_move_m=max(0.0, self.lock_still_move_m),
@@ -342,6 +344,8 @@ class ServoNode(TolerantNode):
        self._d435_vendor_done = False
        self._d435_vendor_busy = False
        self._d435_vendor_failed = False
        self._d435_vendor_tries = 0
        self._d435_retry_after = 0.0
        self._d435_settled = False
        self._d435_exec_until = 0.0
        self._d435_pinned = False
@@ -374,10 +378,9 @@ class ServoNode(TolerantNode):
        self.ws_min = np.array([float(v) for v in ws_min[:3]], dtype=float)
        self.ws_max = np.array([float(v) for v in ws_max[:3]], dtype=float)
-        from arm_application.site_io import resolve_repo_path
+        self.T_cam_to_gripper = load_handeye_T(
-
+            str(self.get_parameter("handeye_file").value)
-        handeye_path = resolve_repo_path(str(self.get_parameter("handeye_file").value))
+        )
        self.T_cam_to_gripper = load_handeye_T(handeye_path)
        self.handeye_mode = str(
            self.get_parameter("handeye_mode").value or "eye_in_hand"
        ).strip().lower()
@@ -400,7 +403,7 @@ class ServoNode(TolerantNode):
        t_he = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float)
        axis_z = float(np.clip(R_he[2, 2], -1.0, 1.0))
        self._cam_tilt_deg = float(np.degrees(np.arccos(axis_z)))
-        he_path = handeye_path
+        he_path = str(self.get_parameter("handeye_file").value)
        if self.handeye_mode == "eye_to_hand":
            self.get_logger().warn(
                f"HANDEYE yaml={he_path} mode=eye_to_hand "
@@ -709,6 +712,8 @@ class ServoNode(TolerantNode):
            self._d435_vendor_done = False
            self._d435_vendor_busy = False
            self._d435_vendor_failed = False
            self._d435_vendor_tries = 0
            self._d435_retry_after = 0.0
            self._overhead_t = now_m
            self._last_seen_time = now_m
            self._stamp_sec = now_sec
@@ -732,6 +737,8 @@ class ServoNode(TolerantNode):
        self._d435_vendor_done = False
        self._d435_vendor_busy = False
        self._d435_vendor_failed = False
        self._d435_vendor_tries = 0
        self._d435_retry_after = 0.0
        self._publish_d435_commanding(True)
        self.get_logger().info(
            f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
@@ -759,9 +766,9 @@ class ServoNode(TolerantNode):
            settled=settled,
            move_only=self.d435_move_only,
        ):
-            # 推理服务不关。这一帧的点不下发给臂，目标仍是 D435 那一个点。
+            # 推理不关。没到「法兰停稳」之前，这帧不能进运动控制。
            self.get_logger().info(
-                "D435 位置还没到并且停稳，D405 这帧不下发",
+                "D405 输出已截断：D435 厂家移动还没结束或法兰还没停稳",
                throttle_duration_sec=1.0,
            )
            return
@@ -1407,9 +1414,12 @@ class ServoNode(TolerantNode):
        return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
    def _d435_oneshot_holds_the_arm(self) -> bool:
-        """D435 到点平移：action 短跳，等结果再重测。D405 主跟踪仍走原来的伺服。"""
+        """D435 粗定位只走厂家 MoveL。停稳后先钉住，等 D405 框心接上再放开。
-        if self.d435_move_only and self._hybrid and self._track_source == "overhead" and self._d435_settled:
+
-            # 位移测完了：守着，不许掉进 D405 的 _execute_twist。hold 只在到位那一拍发一次。
+        这段不许再用流式伺服去追 D435 那个点。D405 点被 ``_on_point`` 接上后
        跟踪源变成腕部，这里返回 False，框心才开始带臂。
        """
        if self._hybrid and self._track_source == "overhead" and self._d435_settled:
            return True
        if not (self._hybrid and self._track_source == "overhead" and not self._d435_settled):
            return False
@@ -1519,7 +1529,7 @@ class ServoNode(TolerantNode):
        """停或重新打开关节轨迹控制器。关节状态广播留着，硬件接口不下线。
        用节点里的客户端，不走 ``ros2 service call``：那条命令自己就要一两秒，
-        交回 D405 的预算会被它吃掉。
+        停稳锁定的预算会被它吃掉。
        """
        from controller_manager_msgs.srv import SwitchController
@@ -1659,18 +1669,32 @@ class ServoNode(TolerantNode):
        if self._d435_vendor_failed or self._d435_vendor_busy:
            return True
        if not self._d435_vendor_done:
-            # busy 先于调用置位：另一条控制回调不能再开一次 MoveL，也不能提前交回 D405。
+            if time.monotonic() < float(self._d435_retry_after):
                return True
            if int(self._d435_vendor_tries) >= int(D435_MOVEL_RETRY):
                self._d435_vendor_failed = True
                self.get_logger().error(
                    f"{D435_MOVEL_GAVE_UP} —— 一阶段中止，D405 也不带臂"
                )
                return True
            # busy 先于调用置位：另一条控制回调不能再开一次 MoveL，也不能提前锁定。
            self._d435_vendor_busy = True
            ok = self._d435_vendor_movel()
            self._d435_vendor_busy = False
            if not ok:
-                self._d435_vendor_failed = True
+                self._d435_vendor_tries = int(self._d435_vendor_tries) + 1
                self._d435_retry_after = time.monotonic() + float(D435_MOVEL_RETRY_S)
                self.get_logger().error(
-                    "厂家 MoveL 没走成 —— 停在原地，D405 的点继续不下发"
+                    f"厂家 MoveL 第{self._d435_vendor_tries}次没走成，"
                    f"{D435_MOVEL_RETRY_S:.1f}s 后再试"
                )
            else:
                self._d435_vendor_done = True
                self._arrive.reset()
                # MoveL 切回关节后 JTC 还握着出发前的悬停。不钉住的话
                # 硬件 3 s 窗口一过，臂会按悬停关节走回去（比交回 D405 的 10 cm 更远）。
                self._send_hold()
                self._d435_pinned = True
            return True
        return True
@@ -1729,7 +1753,7 @@ class ServoNode(TolerantNode):
        ):
            self._d435_pin_and_stop_hops(
                f"D435 上一跳几乎没走近，剩差还是 {dist*1000:.0f}mm "
-                f"—— 钉住不再补刀，等停稳后交回 D405"
+                f"—— 钉住不再补刀，等停稳后锁定进二阶段"
            )
            return True
        if not at:
@@ -1836,7 +1860,7 @@ class ServoNode(TolerantNode):
        return True
    def _tick_d435_arrive(self) -> None:
-        """走到 D435 那个点，并且法兰按一阶段的静止判据停稳，才允许下发 D405。"""
+        """厂家 MoveL 已结束：看法兰还在不在动。连续停稳后才打开 D405 框心。"""
        if not self._hybrid:
            return
        if (
@@ -1858,11 +1882,15 @@ class ServoNode(TolerantNode):
            return
        xy = np.asarray(T[:2, 3], dtype=float)
        dist = float(np.hypot(float(xy[0]) - goal[0], float(xy[1]) - goal[1]))
-        arrived = dist <= float(self.lock_still_move_m) or self._d435_give_up
+        # 到位看法兰是否还在动，不看离视觉点还差几毫米。
-        settled = self._arrive.update(
+        settled = self._arrive.update(xy, time.monotonic(), tracking_ok=True)
            xy, time.monotonic(), tracking_ok=arrived
        )
        if not settled:
            self.get_logger().info(
                f"D435 厂家 MoveL 已结束，法兰还在动或未满 "
                f"{self.d435_handback_sec:.1f}s（离点 {dist*1000:.0f}mm），"
                "D405 输出仍截断",
                throttle_duration_sec=1.0,
            )
            return
        with self._lock:
            self._d435_settled = True
@@ -1885,10 +1913,12 @@ class ServoNode(TolerantNode):
                self._lock_pub_t = now_m
                self._lock_pub.publish(Bool(data=True))
            return
        # 法兰已停稳：打开 D405。不在这里锁定；框心对准停稳后才精拍下臂。
        self._publish_d435_commanding(False)
        self.get_logger().info(
-            f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
+            f"法兰已连续停稳 {self.d435_handback_sec:.1f}s"
-            f"{self.d435_handback_sec:.1f}s —— 改回下发 D405 框心"
+            f"（离 D435 点 {dist*1000:.0f}mm）"
            " —— 打开 D405 输出，追工件框矩形中心；对准后才精拍下臂"
        )
    def _tick_still_lock(self) -> None:
@@ -10,83 +10,13 @@ import yaml
 def ros2_root() -> Path:
    """仓库根目录。先看环境变量，再从本文件向上找 ``config/site.yaml``。
    不能默认 ``~/ros2``：别人把仓库克隆到别的目录时，写死的家目录路径是空的，
    启动会报文件不存在。
    """
    for key in ("SITE_ROS2_ROOT", "ROS2_WS"):
        v = os.environ.get(key, "").strip()
        if v:
            return Path(v).expanduser()
    here = Path(__file__).resolve()
    for parent in here.parents:
        if (parent / "config" / "site.yaml").is_file() and (parent / "scripts" / "env.sh").is_file():
            return parent
    return Path.home() / "ros2"
 _PATH_KEYS = frozenset({
    "named_poses_file",
    "handeye_file",
    "handeye_waypoints",
    "handeye_workdir",
    "handeye_calibrate",
    "yolo_weights",
    "hole_weights",
    "weights",
    "workpiece_weights",
    "config_file",
    "append_poses_file",
 })
 def resolve_repo_path(value: str, root: Path | None = None) -> str:
    """仓库内的相对路径展开成绝对路径。已经是绝对路径的原样保留。"""
    text = str(value or "").strip()
    if not text:
        return ""
    path = Path(text).expanduser()
    if path.is_absolute():
        return str(path)
    base = root or ros2_root()
    return str((base / path).resolve())
 def _is_path_key(key: str) -> bool:
    return (
        key in _PATH_KEYS
        or key.startswith("handeye_file")
        or key.startswith("handeye_waypoints")
        or key.endswith("_weights")
        or key.endswith("_file")
        or key.endswith("_workdir")
    )
 def absolutize_site_paths(data: Any, root: Path | None = None) -> Any:
    """把 site 里的仓库相对路径展开。只动路径字段，不动别的字符串。"""
    base = root or ros2_root()
    if isinstance(data, dict):
        out: dict[Any, Any] = {}
        for key, val in data.items():
            name = str(key)
            if isinstance(val, str) and _is_path_key(name):
                out[key] = resolve_repo_path(val, base)
            elif (
                name == "config"
                and isinstance(val, str)
                and (val.endswith((".yaml", ".yml", ".json")) or "/" in val)
            ):
                out[key] = resolve_repo_path(val, base)
            else:
                out[key] = absolutize_site_paths(val, base)
        return out
    if isinstance(data, list):
        return [absolutize_site_paths(item, base) for item in data]
    return data
 def site_path() -> Path:
    env = os.environ.get("SITE_CONFIG", "").strip()
    if env:
@@ -99,11 +29,7 @@ def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
    if not p.is_file():
        return {}
    data = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
-    if not isinstance(data, dict):
+    return data if isinstance(data, dict) else {}
        return {}
    resolved = p.resolve()
    root = resolved.parent.parent if resolved.parent.name == "config" else ros2_root()
    return absolutize_site_paths(data, root)
 def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:
@@ -69,6 +69,7 @@ from arm_application.overhead_oneshot import (
    once_ff_tcp_xyz,
    oneshot_align_step_m,
    pixel_on_base_plane,
    retract_goal_z,
 )
 from arm_application.handeye_realsense import resolve_realsense_topics
 from arm_application.gauge_boot import perception_source_of
@@ -249,7 +250,7 @@ class Stage2Locate(TolerantNode):
        # _coarse_target_reject / _bbox_on_border 的注释）：粗居中的目的是把臂
        # 挪到工件正上方，**挪错比不挪危险得多** —— 一挪错精定位就再也看不到
        # k1/k2，而流程会把它当"孔检测失败"，现场根本想不到是粗居中把臂带走的。
-        self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.12)
+        self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.08)
        self.coarse_border_px = int(stage2.get("coarse_border_px") or 2)
        # 最后轻降到孔面上方 final_gap_mm（不接触）。启动参数 ≥0 覆盖 site。
        self._final_gap_refused = ""
@@ -901,10 +902,13 @@ class Stage2Locate(TolerantNode):
        min_dur: float = 1.6,
        hold_xy: bool = False,
        strict_xy: bool = False,
        z_up_only: bool = False,
    ) -> Tuple[bool, np.ndarray, np.ndarray]:
        """闭环点到点：规划短 hop → 执行 → 重新看 TF → 重复，直到容差内。
        hold_xy=True：只收 Z/姿态（悬停降锁高），不把下降中漂出的 XY 再拽回去。
        z_up_only=True：当前已经高于目标 Z 就停，不许为了追 5 mm 往下补
        （贴面 MoveL 后收臂 hop1 抬过头、hop2 回压进孔，2026-10-16）。
        """
        target = self._clamp_ws(target)
        last_dist: Optional[float] = None
@@ -912,7 +916,14 @@ class Stage2Locate(TolerantNode):
        stall = 0
        for hop in range(max_hops):
            p = Tee[:3, 3]
-            goal = np.array([p[0], p[1], target[2]]) if hold_xy else target
+            z_tgt = float(target[2])
            if z_up_only:
                if float(p[2]) >= z_tgt - float(tol_m):
                    return True, q, Tee
                z_tgt = retract_goal_z(float(p[2]), z_tgt)
            goal = np.array([p[0], p[1], z_tgt]) if hold_xy else np.array(
                [target[0], target[1], z_tgt], dtype=float
            )
            if hold_xy:
                goal = self._clamp_ws(goal)
            dp = goal - p
@@ -1491,14 +1502,15 @@ class Stage2Locate(TolerantNode):
        1. 目标点落在 ``workspace_xyz_*`` 的 XY 之外（含 5mm 余量）—— 一阶段刚锁
           定时臂就在工件上方，工件**不该**在盒外；
-        2. 目标点离当前夹爪轴线超过 ``coarse_max_offset_m``（缺省 0.12m）—— 粗居中
+        2. 目标点离当前夹爪轴线超过 ``coarse_max_offset_m``（缺省 0.08m）—— 粗居中
-           是"把臂挪到工件正上方"，合理位移是厘米级；十几厘米只可能是坏读数。
+           是"把臂挪到工件正上方"，合理位移是厘米级；十几厘米只可能是坏读数
           （2026-10-16 三次 110mm 回撤，0.12 放行了）。
        注意：判误检后**原地不动**，由精定位（直接量 k1/k2）继续。不动的代价只是
        "少挪一次"，而挪错的代价是精定位彻底看不到孔 —— 所以这里必须选不动。
        """
        if max_offset_m is None:
-            max_offset_m = float(getattr(self, "coarse_max_offset_m", 0.12))
+            max_offset_m = float(getattr(self, "coarse_max_offset_m", 0.08))
        p = np.asarray(p_base, dtype=float).reshape(3)
        if not np.all(np.isfinite(p[:2])):
            return "目标点非有限值（深度/三角化失败）"
@@ -2241,7 +2253,9 @@ class Stage2Locate(TolerantNode):
        )
        fl0 = (float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]))
        ok = self._vendor_movel_tcp(movel_xyz, ONCE_FF_MOVEL_VEL_MPS)
-        q2, Tee2 = self._wait_state(2.0)
+        # MoveL 刚切回关节模式：JTC 还握着贴面前的悬停关节。等 2 s 再钉
        # 等于让 vmax=0.05 往悬停留 0.1 rad（2026-10-16 收臂实测多抬 13–20 mm）。
        q2, Tee2 = self._wait_state(0.3)
        if q2 is not None and Tee2 is not None:
            q, Tee = q2, Tee2
        self._pin(q)
@@ -3392,14 +3406,36 @@ class Stage2Locate(TolerantNode):
        )
        # ---- 5) 锁定保持 ----
        # 贴面之后就停在这个法兰 Z。没有接触传感器，到位只认 final_gap_mm
        # （规尖离面）。三阶段开局会抬回悬高，这里标明已经插到位，流水线不再上抬。
        if (
            self.descent_final_gap_m is not None
            and self.final_descent_enable
            and not self.hover_only
        ):
            final = dict(final)
            final["insert_held"] = True
            final["flange_z_m"] = round(float(Tee[2, 3]), 4)
            final["tip_gap_mm"] = round(float(self.descent_final_gap_m) * 1000.0, 2)
            final["commanded_gap_mm"] = round(float(self.final_gap_m) * 1000.0, 2)
            final["insert_z_source"] = "final_gap_mm"
        self._state("hold")
        q_fin = self._q()
        self._pin(q_fin if q_fin is not None else q)
-        self.get_logger().info(
+        if final.get("insert_held"):
-            f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
+            self.get_logger().info(
-            f"规尖悬在孔面上方 ≈{gap_txt}mm（不接触）。"
+                f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
-            f"结果: /stage2/result, /tmp/stage2_result.json ==="
+                f"法兰 Z={final.get('flange_z_m')}m，规尖离面 {gap_txt}mm"
-        )
+                f"（final_gap {self.final_gap_m * 1000:.1f}mm，无接触传感器）。"
                "停在这一高度，不抬回悬高。"
                f"结果: /stage2/result, /tmp/stage2_result.json ==="
            )
        else:
            self.get_logger().info(
                f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
                f"规尖悬在孔面上方 ≈{gap_txt}mm（不接触）。"
                f"结果: /stage2/result, /tmp/stage2_result.json ==="
            )
        return 0, q_fin if q_fin is not None else q, Tee, final
    def _linger_hold(self, final: Dict[str, Any], seconds: Optional[float] = None) -> None:
@@ -526,7 +526,8 @@ class Stage3Search(Stage2Locate):
            tgt[2] += dz_m
            self._goto(q, Tee, tgt, verticalize=True, tol_m=0.001,
                       tol_tilt=self.vert_tol_rad * 3.0, tag="retract",
-                       v=0.03, max_hops=3, settle_s=0.2, min_dur=0.6)
+                       v=0.03, max_hops=2, settle_s=0.2, min_dur=0.6,
                       hold_xy=True, z_up_only=True)
        except Exception:
            pass
@@ -12,7 +12,7 @@ perception_node:
    image_topic: ""
    camera_info_topic: ""
    depth_topic: "/d405/aligned_depth_to_color/image_raw"
-    weights: "models/yolo/gongjian0929.onnx"
+    weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
    conf: 0.10
    iou: 0.45
    imgsz: 640
@@ -112,6 +112,5 @@ $env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.Ful
 colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_application/hook/pythonpath.ps1"
 colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_application/hook/ament_prefix_path.ps1"
 colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/../../build/arm_application/share/arm_application/hook/pythonpath_develop.ps1"
 Remove-Item Env:\COLCON_CURRENT_PREFIX
@@ -1 +1 @@
-arm_kinematics:arm_msgs:geometry_msgs:python3-serial:python3-yaml:rclpy:sensor_msgs:std_msgs:std_srvs:tf2_ros:trajectory_msgs
+arm_kinematics:arm_msgs:control_msgs:geometry_msgs:python3-serial:python3-yaml:rclpy:sensor_msgs:std_msgs:std_srvs:tf2_ros:trajectory_msgs
@@ -1,139 +0,0 @@
 /**
 * @file flexiv_dual_hardware_interface.hpp
 * @brief Hardware interface to a pair of Flexiv robots for ROS 2 control.
 * @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved.
 * @author Flexiv
 */
 #ifndef FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_
 #define FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_
 #include <memory>
 #include <string>
 #include <vector>
 #include <map>
 // ROS
 #include <rclcpp/clock.hpp>
 #include <rclcpp/duration.hpp>
 #include <rclcpp/macros.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/time.hpp>
 #include <rclcpp_lifecycle/state.hpp>
 // ros2_control hardware_interface
 #include <hardware_interface/hardware_info.hpp>
 #include <hardware_interface/system_interface.hpp>
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
 #include "flexiv_hardware/visibility_control.h"
 // Flexiv
 #include "flexiv/drdk/robot_pair.hpp"
 namespace flexiv_hardware {
 enum StoppingInterface
 {
    NONE,
    STOP_POSITION,
    STOP_VELOCITY,
    STOP_EFFORT
 };
 class FlexivDualHardwareInterface : public hardware_interface::SystemInterface
 {
 public:
    RCLCPP_SHARED_PTR_DEFINITIONS(FlexivDualHardwareInterface)
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::CallbackReturn on_init(
        const hardware_interface::HardwareInfo& info) override;
    FLEXIV_HARDWARE_PUBLIC
    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
    FLEXIV_HARDWARE_PUBLIC
    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::return_type prepare_command_mode_switch(
        const std::vector<std::string>& start_interfaces,
        const std::vector<std::string>& stop_interfaces) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::return_type perform_command_mode_switch(
        const std::vector<std::string>& start_interfaces,
        const std::vector<std::string>& stop_interfaces) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::CallbackReturn on_activate(
        const rclcpp_lifecycle::State& previous_state) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::CallbackReturn on_deactivate(
        const rclcpp_lifecycle::State& previous_state) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::return_type read(
        const rclcpp::Time& time, const rclcpp::Duration& period) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::return_type write(
        const rclcpp::Time& time, const rclcpp::Duration& period) override;
 private:
    // Flexiv DRDK
    std::unique_ptr<flexiv::drdk::RobotPair> robot_pair_;
    // RDK control mode for joint position and velocity interfaces
    flexiv::rdk::Mode rdk_control_mode_;
    // External axis type
    std::string external_axis_type_ = "";
    // Joint commands
    std::vector<double> hw_commands_joint_positions_;
    std::vector<double> hw_commands_joint_velocities_;
    std::vector<double> hw_commands_joint_efforts_;
    // Joint states
    std::vector<double> hw_states_joint_positions_;
    std::vector<double> hw_states_joint_velocities_;
    std::vector<double> hw_states_joint_efforts_;
    // Robot States
    flexiv::rdk::RobotStates hw_flexiv_robot_states_left_;
    flexiv::rdk::RobotStates hw_flexiv_robot_states_right_;
    flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_left_ = &hw_flexiv_robot_states_left_;
    flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_right_ = &hw_flexiv_robot_states_right_;
    // GPIO commands and states
    std::vector<double> hw_commands_gpio_out_;
    std::vector<double> hw_states_gpio_in_;
    // Current digital output map
    std::map<unsigned int, bool> current_digital_outputs_left_;
    std::map<unsigned int, bool> current_digital_outputs_right_;
    // Joint mapping
    struct JointMap
    {
        int robot_index; // 0: Left, 1: Right
        int dof_index;   // Index in the robot's q vector
    };
    std::vector<JointMap> joint_map_;
    static rclcpp::Logger getLogger();
    // Control modes
    bool controllers_initialized_;
    std::vector<uint> stop_modes_;
    std::vector<std::string> start_modes_;
    bool position_controller_running_;
    bool velocity_controller_running_;
    bool torque_controller_running_;
 };
 } /* namespace flexiv_hardware */
 #endif /* FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_ */
@@ -0,0 +1 @@
 /home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/flexiv_dual_hardware_interface.hpp
@@ -1,156 +0,0 @@
 /**
 * @file flexiv_hardware_interface.hpp
 * @brief Hardware interface to Flexiv robots for ROS 2 control. Adapted from
 * ros2_control_demos/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp
 * @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved.
 * @author Flexiv
 */
 #ifndef FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
 #define FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
 #include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
 // ROS
 #include <rclcpp/clock.hpp>
 #include <rclcpp/duration.hpp>
 #include <rclcpp/macros.hpp>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/time.hpp>
 #include <rclcpp_lifecycle/state.hpp>
 // ros2_control hardware_interface
 #include <hardware_interface/hardware_info.hpp>
 #include <hardware_interface/system_interface.hpp>
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
 #include "flexiv_hardware/visibility_control.h"
 // Flexiv
 #include "flexiv/rdk/robot.hpp"
 namespace flexiv_hardware {
 enum StoppingInterface
 {
    NONE,
    STOP_POSITION,
    STOP_VELOCITY,
    STOP_EFFORT
 };
 class FlexivHardwareInterface : public hardware_interface::SystemInterface
 {
 public:
    RCLCPP_SHARED_PTR_DEFINITIONS(FlexivHardwareInterface)
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::CallbackReturn on_init(
        const hardware_interface::HardwareInfo& info) override;
    FLEXIV_HARDWARE_PUBLIC
    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
    FLEXIV_HARDWARE_PUBLIC
    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::return_type prepare_command_mode_switch(
        const std::vector<std::string>& start_interfaces,
        const std::vector<std::string>& stop_interfaces) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::return_type perform_command_mode_switch(
        const std::vector<std::string>& start_interfaces,
        const std::vector<std::string>& stop_interfaces) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::CallbackReturn on_activate(
        const rclcpp_lifecycle::State& previous_state) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::CallbackReturn on_deactivate(
        const rclcpp_lifecycle::State& previous_state) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::return_type read(
        const rclcpp::Time& time, const rclcpp::Duration& period) override;
    FLEXIV_HARDWARE_PUBLIC
    hardware_interface::return_type write(
        const rclcpp::Time& time, const rclcpp::Duration& period) override;
 private:
    // Flexiv RDK
    std::unique_ptr<flexiv::rdk::Robot> robot_;
    // RDK control mode for joint position and velocity interfaces
    flexiv::rdk::Mode rdk_control_mode_;
    // Joint commands
    std::vector<double> hw_commands_joint_positions_;
    std::vector<double> hw_commands_joint_velocities_;
    std::vector<double> hw_commands_joint_efforts_;
    // Joint states
    std::vector<double> hw_states_joint_positions_;
    std::vector<double> hw_states_joint_velocities_;
    std::vector<double> hw_states_joint_efforts_;
    // Robot States
    flexiv::rdk::RobotStates hw_flexiv_robot_states_;
    flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_ = &hw_flexiv_robot_states_;
    // GPIO commands and states
    std::vector<double> hw_commands_gpio_out_;
    std::vector<double> hw_states_gpio_in_;
    // Map from RDK joint index to ROS joint index
    // RDK expects: [ext_axis_1, ..., ext_axis_N, arm_joint_1, ..., arm_joint_7]
    std::vector<size_t> rdk_to_ros_map_;
    // Current digital output map
    std::map<unsigned int, bool> current_digital_outputs_;
    static rclcpp::Logger getLogger();
    // Control modes
    bool controllers_initialized_;
    std::vector<uint> stop_modes_;
    std::vector<std::string> start_modes_;
    bool position_controller_running_;
    bool velocity_controller_running_;
    bool torque_controller_running_;
    // ===== 连接存活看门狗（2026-09-28 真机踩到）=====
    //
    // 现象：RDK 断连后 `operational()` 会**一直保持 true**，于是 read() 照跑却只
    // 返回最后一帧缓存（关节状态永久冻结），write() 把命令发进虚空也不抛异常，
    // 而 rizon_arm_controller 是本地插值、不校验反馈 —— 最终表现为
    // **「控制器报 Goal reached/success、关节状态纹丝不动、臂一步没动」**。
    //
    // 实测：14:53:44 RDK 打印 "Disconnected from the robot, waiting for reconnection"，
    // 此后 50 分钟里 /joint_states 的 robot_timestamp 推进量恒为 0，而 `op=1` 没变过。
    // 根因是两台臂出厂 MAC 相同（88:88:88:88:87:88）在同一 L2 段抢转发，链路丢包 40~53%。
    //
    // 这里用 connected() + states().timestamp 双重判活：任一失效就返回 ERROR，
    // 让 ros2_control 把栈停下来报错，而不是静默地"成功"。
    int64_t last_state_stamp_ns_ = 0;   // 上一帧的机器人时间戳
    int64_t last_state_change_ns_ = 0;  // 上一帧时间戳发生变化的本地时刻
    // D435 粗定位的 MoveL 走已经连上的这份 RDK。write() 里一步一步推进，
    // 不另开线程，也不在这段里往关节接口灌点。请求文件是 /tmp/flexiv_movel_req。
    enum class VendorMovelPhase { kIdle, kRunning };
    VendorMovelPhase vendor_movel_phase_ = VendorMovelPhase::kIdle;
    long vendor_movel_seq_ = -1;
    long vendor_movel_done_seq_ = -1;
    std::chrono::steady_clock::time_point vendor_movel_deadline_ {};
    bool poll_vendor_movel();
    void finish_vendor_movel(long seq, bool ok, const std::string& detail);
 };
 } /* namespace flexiv_hardware */
 #endif /* FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_ */
@@ -0,0 +1 @@
 /home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp
@@ -1,35 +0,0 @@
 #ifndef FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
 #define FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
 // This logic was borrowed (then namespaced) from the examples on the gcc wiki:
 //     https://gcc.gnu.org/wiki/Visibility
 #if defined _WIN32 || defined __CYGWIN__
 #ifdef __GNUC__
 #define FLEXIV_HARDWARE_EXPORT __attribute__((dllexport))
 #define FLEXIV_HARDWARE_IMPORT __attribute__((dllimport))
 #else
 #define FLEXIV_HARDWARE_EXPORT __declspec(dllexport)
 #define FLEXIV_HARDWARE_IMPORT __declspec(dllimport)
 #endif
 #ifdef FLEXIV_HARDWARE_BUILDING_DLL
 #define FLEXIV_HARDWARE_PUBLIC FLEXIV_HARDWARE_EXPORT
 #else
 #define FLEXIV_HARDWARE_PUBLIC FLEXIV_HARDWARE_IMPORT
 #endif
 #define FLEXIV_HARDWARE_PUBLIC_TYPE FLEXIV_HARDWARE_PUBLIC
 #define FLEXIV_HARDWARE_LOCAL
 #else
 #define FLEXIV_HARDWARE_EXPORT __attribute__((visibility("default")))
 #define FLEXIV_HARDWARE_IMPORT
 #if __GNUC__ >= 4
 #define FLEXIV_HARDWARE_PUBLIC __attribute__((visibility("default")))
 #define FLEXIV_HARDWARE_LOCAL __attribute__((visibility("hidden")))
 #else
 #define FLEXIV_HARDWARE_PUBLIC
 #define FLEXIV_HARDWARE_LOCAL
 #endif
 #define FLEXIV_HARDWARE_PUBLIC_TYPE
 #endif
 #endif // FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
@@ -0,0 +1 @@
 /home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/visibility_control.h
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/libflexiv_hardware.so
@@ -1 +0,0 @@
 share/flexiv_hardware/flexiv_hardware_interface_plugin.xml
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/hardware_interface__pluginlib__plugin/flexiv_hardware
@@ -1 +0,0 @@
 rclcpp;rclcpp_lifecycle;hardware_interface;pluginlib;ament_lint_auto;ament_lint_common;ament_cmake_gtest
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/package_run_dependencies/flexiv_hardware
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/packages/flexiv_hardware
@@ -1 +0,0 @@
 /home/cc/ros2/install/gpio_controller:/home/cc/ros2/install/flexiv_bringup:/home/cc/ros2/install/flexiv_test_nodes:/home/cc/ros2/install/flexiv_robot_states_broadcaster:/home/cc/ros2/install/flexiv_gripper:/home/cc/ros2/install/flexiv_msgs:/home/cc/ros2/install/flexiv_moveit_config:/home/cc/ros2/install/flexiv_hardware:/home/cc/ros2/install/arm_sim_gazebo:/home/cc/ros2/install/arm_bringup:/home/cc/ros2/install/arm_moveit_config:/home/cc/ros2/install/arm_description:/home/cc/ros2/install/flexiv_description:/home/cc/ros2/install/arm_driver_flexiv:/home/cc/ros2/install/arm_control:/home/cc/ros2/install/arm_application:/home/cc/ros2/install/arm_msgs:/home/cc/ros2/install/arm_kinematics:/home/cc/ros2/install/arm_interfaces:/home/cc/anaconda3/envs/ros_flexiv
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/parent_prefix_path/flexiv_hardware
@@ -1,94 +0,0 @@
 # generated from ament_cmake_export_dependencies/cmake/ament_cmake_export_dependencies-extras.cmake.in
 set(_exported_dependencies "hardware_interface;pluginlib;rclcpp;rclcpp_lifecycle")
 find_package(ament_cmake_libraries QUIET REQUIRED)
 # find_package() all dependencies
 # and append their DEFINITIONS INCLUDE_DIRS, LIBRARIES, and LINK_FLAGS
 # variables to flexiv_hardware_DEFINITIONS, flexiv_hardware_INCLUDE_DIRS,
 # flexiv_hardware_LIBRARIES, and flexiv_hardware_LINK_FLAGS.
 # Additionally collect the direct dependency names in
 # flexiv_hardware_DEPENDENCIES as well as the recursive dependency names
 # in flexiv_hardware_RECURSIVE_DEPENDENCIES.
 if(NOT _exported_dependencies STREQUAL "")
  find_package(ament_cmake_core QUIET REQUIRED)
  set(flexiv_hardware_DEPENDENCIES ${_exported_dependencies})
  set(flexiv_hardware_RECURSIVE_DEPENDENCIES ${_exported_dependencies})
  set(_libraries)
  foreach(_dep ${_exported_dependencies})
    if(NOT ${_dep}_FOUND)
      find_package("${_dep}" QUIET REQUIRED)
    endif()
    # if a package provides modern CMake interface targets use them
    # exclusively assuming the classic CMake variables only exist for
    # backward compatibility
    set(use_modern_cmake FALSE)
    if(NOT "${${_dep}_TARGETS}" STREQUAL "")
      foreach(_target ${${_dep}_TARGETS})
        # only use actual targets
        # in case a package uses this variable for other content
        if(TARGET "${_target}")
          get_target_property(_include_dirs ${_target} INTERFACE_INCLUDE_DIRECTORIES)
          if(_include_dirs)
            list_append_unique(flexiv_hardware_INCLUDE_DIRS "${_include_dirs}")
          endif()
          get_target_property(_imported_configurations ${_target} IMPORTED_CONFIGURATIONS)
          if(_imported_configurations)
            string(TOUPPER "${_imported_configurations}" _imported_configurations)
            if(DEBUG_CONFIGURATIONS)
              string(TOUPPER "${DEBUG_CONFIGURATIONS}" _debug_configurations_uppercase)
            else()
              set(_debug_configurations_uppercase "DEBUG")
            endif()
            foreach(_imported_config ${_imported_configurations})
              get_target_property(_imported_implib ${_target} IMPORTED_IMPLIB_${_imported_config})
              if(_imported_implib)
                set(_imported_implib_config "optimized")
                if(${_imported_config} IN_LIST _debug_configurations_uppercase)
                  set(_imported_implib_config "debug")
                endif()
                list(APPEND _libraries ${_imported_implib_config} ${_imported_implib})
              else()
                get_target_property(_imported_location ${_target} IMPORTED_LOCATION_${_imported_config})
                if(_imported_location)
                  list(APPEND _libraries "${_imported_location}")
                endif()
              endif()
            endforeach()
          endif()
          get_target_property(_link_libraries ${_target} INTERFACE_LINK_LIBRARIES)
          if(_link_libraries)
            list(APPEND _libraries "${_link_libraries}")
          endif()
          set(use_modern_cmake TRUE)
        endif()
      endforeach()
    endif()
    if(NOT use_modern_cmake)
      if(${_dep}_DEFINITIONS)
        list_append_unique(flexiv_hardware_DEFINITIONS "${${_dep}_DEFINITIONS}")
      endif()
      if(${_dep}_INCLUDE_DIRS)
        list_append_unique(flexiv_hardware_INCLUDE_DIRS "${${_dep}_INCLUDE_DIRS}")
      endif()
      if(${_dep}_LIBRARIES)
        list(APPEND _libraries "${${_dep}_LIBRARIES}")
      endif()
      if(${_dep}_LINK_FLAGS)
        list_append_unique(flexiv_hardware_LINK_FLAGS "${${_dep}_LINK_FLAGS}")
      endif()
      if(${_dep}_RECURSIVE_DEPENDENCIES)
        list_append_unique(flexiv_hardware_RECURSIVE_DEPENDENCIES "${${_dep}_RECURSIVE_DEPENDENCIES}")
      endif()
    endif()
    if(_libraries)
      ament_libraries_deduplicate(_libraries "${_libraries}")
      list(APPEND _libraries ${flexiv_hardware_LIBRARIES})
      list(REMOVE_DUPLICATES _libraries)
      set(flexiv_hardware_LIBRARIES ${_libraries})
    endif()
  endforeach()
 endif()
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_export_dependencies/ament_cmake_export_dependencies-extras.cmake
@@ -1,16 +0,0 @@
 # generated from ament_cmake_export_include_directories/cmake/ament_cmake_export_include_directories-extras.cmake.in
 set(_exported_include_dirs "${flexiv_hardware_DIR}/../../../include")
 # append include directories to flexiv_hardware_INCLUDE_DIRS
 # warn about not existing paths
 if(NOT _exported_include_dirs STREQUAL "")
  find_package(ament_cmake_core QUIET REQUIRED)
  foreach(_exported_include_dir ${_exported_include_dirs})
    if(NOT IS_DIRECTORY "${_exported_include_dir}")
      message(WARNING "Package 'flexiv_hardware' exports the include directory '${_exported_include_dir}' which doesn't exist")
    endif()
    normalize_path(_exported_include_dir "${_exported_include_dir}")
    list(APPEND flexiv_hardware_INCLUDE_DIRS "${_exported_include_dir}")
  endforeach()
 endif()
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_export_include_directories/ament_cmake_export_include_directories-extras.cmake
@@ -1,141 +0,0 @@
 # generated from ament_cmake_export_libraries/cmake/template/ament_cmake_export_libraries.cmake.in
 set(_exported_libraries "flexiv_hardware")
 set(_exported_library_names "")
 # populate flexiv_hardware_LIBRARIES
 if(NOT _exported_libraries STREQUAL "")
  # loop over libraries, either target names or absolute paths
  list(LENGTH _exported_libraries _length)
  set(_i 0)
  while(_i LESS _length)
    list(GET _exported_libraries ${_i} _arg)
    # pass linker flags along
    if("${_arg}" MATCHES "^-" AND NOT "${_arg}" MATCHES "^-[l|framework]")
      list(APPEND flexiv_hardware_LIBRARIES "${_arg}")
      math(EXPR _i "${_i} + 1")
      continue()
    endif()
    if("${_arg}" MATCHES "^(debug|optimized|general)$")
      # remember build configuration keyword
      # and get following library
      set(_cfg "${_arg}")
      math(EXPR _i "${_i} + 1")
      if(_i EQUAL _length)
        message(FATAL_ERROR "Package 'flexiv_hardware' passes the build configuration keyword '${_cfg}' as the last exported library")
      endif()
      list(GET _exported_libraries ${_i} _library)
    else()
      # the value is a library without a build configuration keyword
      set(_cfg "")
      set(_library "${_arg}")
    endif()
    math(EXPR _i "${_i} + 1")
    if(NOT IS_ABSOLUTE "${_library}")
      # search for library target relative to this CMake file
      set(_lib "NOTFOUND")
      find_library(
        _lib NAMES "${_library}"
        PATHS "${flexiv_hardware_DIR}/../../../lib"
        NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
      )
      if(NOT _lib)
        # warn about not existing library and ignore it
        message(FATAL_ERROR "Package 'flexiv_hardware' exports the library '${_library}' which couldn't be found")
      elseif(NOT IS_ABSOLUTE "${_lib}")
        # the found library must be an absolute path
        message(FATAL_ERROR "Package 'flexiv_hardware' found the library '${_library}' at '${_lib}' which is not an absolute path")
      elseif(NOT EXISTS "${_lib}")
        # the found library must exist
        message(FATAL_ERROR "Package 'flexiv_hardware' found the library '${_lib}' which doesn't exist")
      else()
        list(APPEND flexiv_hardware_LIBRARIES ${_cfg} "${_lib}")
      endif()
    else()
      if(NOT EXISTS "${_library}")
        # the found library must exist
        message(WARNING "Package 'flexiv_hardware' exports the library '${_library}' which doesn't exist")
      else()
        list(APPEND flexiv_hardware_LIBRARIES ${_cfg} "${_library}")
      endif()
    endif()
  endwhile()
 endif()
 # find_library() library names with optional LIBRARY_DIRS
 # and add the libraries to flexiv_hardware_LIBRARIES
 if(NOT _exported_library_names STREQUAL "")
  # loop over library names
  # but remember related build configuration keyword if available
  list(LENGTH _exported_library_names _length)
  set(_i 0)
  while(_i LESS _length)
    list(GET _exported_library_names ${_i} _arg)
    # pass linker flags along
    if("${_arg}" MATCHES "^-" AND NOT "${_arg}" MATCHES "^-[l|framework]")
      list(APPEND flexiv_hardware_LIBRARIES "${_arg}")
      math(EXPR _i "${_i} + 1")
      continue()
    endif()
    if("${_arg}" MATCHES "^(debug|optimized|general)$")
      # remember build configuration keyword
      # and get following library name
      set(_cfg "${_arg}")
      math(EXPR _i "${_i} + 1")
      if(_i EQUAL _length)
        message(FATAL_ERROR "Package 'flexiv_hardware' passes the build configuration keyword '${_cfg}' as the last exported target")
      endif()
      list(GET _exported_library_names ${_i} _library)
    else()
      # the value is a library target without a build configuration keyword
      set(_cfg "")
      set(_library "${_arg}")
    endif()
    math(EXPR _i "${_i} + 1")
    # extract optional LIBRARY_DIRS from library name
    string(REPLACE ":" ";" _library_dirs "${_library}")
    list(GET _library_dirs 0 _library_name)
    list(REMOVE_AT _library_dirs 0)
    set(_lib "NOTFOUND")
    if(NOT _library_dirs)
      # search for library in the common locations
      find_library(
        _lib
        NAMES "${_library_name}"
      )
      if(NOT _lib)
        # warn about not existing library and later ignore it
        message(WARNING "Package 'flexiv_hardware' exports library '${_library_name}' which couldn't be found")
      endif()
    else()
      # search for library in the specified directories
      find_library(
        _lib
        NAMES "${_library_name}"
        PATHS ${_library_dirs}
        NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
      )
      if(NOT _lib)
        # warn about not existing library and later ignore it
        message(WARNING
          "Package 'flexiv_hardware' exports library '${_library_name}' with LIBRARY_DIRS '${_library_dirs}' which couldn't be found")
      endif()
    endif()
    if(_lib)
      list(APPEND flexiv_hardware_LIBRARIES ${_cfg} "${_lib}")
    endif()
  endwhile()
 endif()
 # TODO(dirk-thomas) deduplicate flexiv_hardware_LIBRARIES
 # while maintaining library order
 # as well as build configuration keywords
 # as well as linker flags
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_export_libraries/ament_cmake_export_libraries-extras.cmake
@@ -1,14 +0,0 @@
 # generated from ament/cmake/core/templates/nameConfig-version.cmake.in
 set(PACKAGE_VERSION "1.9.1")
 set(PACKAGE_VERSION_EXACT False)
 set(PACKAGE_VERSION_COMPATIBLE False)
 if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}")
  set(PACKAGE_VERSION_EXACT True)
  set(PACKAGE_VERSION_COMPATIBLE True)
 endif()
 if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}")
  set(PACKAGE_VERSION_COMPATIBLE True)
 endif()
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_core/flexiv_hardwareConfig-version.cmake
@@ -1,42 +0,0 @@
 # generated from ament/cmake/core/templates/nameConfig.cmake.in
 # prevent multiple inclusion
 if(_flexiv_hardware_CONFIG_INCLUDED)
  # ensure to keep the found flag the same
  if(NOT DEFINED flexiv_hardware_FOUND)
    # explicitly set it to FALSE, otherwise CMake will set it to TRUE
    set(flexiv_hardware_FOUND FALSE)
  elseif(NOT flexiv_hardware_FOUND)
    # use separate condition to avoid uninitialized variable warning
    set(flexiv_hardware_FOUND FALSE)
  endif()
  return()
 endif()
 set(_flexiv_hardware_CONFIG_INCLUDED TRUE)
 # output package information
 if(NOT flexiv_hardware_FIND_QUIETLY)
  message(STATUS "Found flexiv_hardware: 1.9.1 (${flexiv_hardware_DIR})")
 endif()
 # warn when using a deprecated package
 if(NOT "" STREQUAL "")
  set(_msg "Package 'flexiv_hardware' is deprecated")
  # append custom deprecation text if available
  if(NOT "" STREQUAL "TRUE")
    set(_msg "${_msg} ()")
  endif()
  # optionally quiet the deprecation message
  if(NOT ${flexiv_hardware_DEPRECATED_QUIET})
    message(DEPRECATION "${_msg}")
  endif()
 endif()
 # flag package as ament-based to distinguish it after being find_package()-ed
 set(flexiv_hardware_FOUND_AMENT_PACKAGE TRUE)
 # include all config extra files
 set(_extras "ament_cmake_export_include_directories-extras.cmake;ament_cmake_export_libraries-extras.cmake;ament_cmake_export_dependencies-extras.cmake")
 foreach(_extra ${_extras})
  include("${flexiv_hardware_DIR}/${_extra}")
 endforeach()
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_core/flexiv_hardwareConfig.cmake
@@ -1 +0,0 @@
 prepend-non-duplicate;AMENT_PREFIX_PATH;
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/ament_prefix_path.dsv
@@ -1,4 +0,0 @@
 # copied from
 # ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh
 ament_prepend_unique_value AMENT_PREFIX_PATH "$AMENT_CURRENT_PREFIX"
@@ -0,0 +1 @@
 /home/cc/anaconda3/envs/ros_flexiv/share/ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh
@@ -1 +0,0 @@
 prepend-non-duplicate-if-exists;PATH;bin
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/path.dsv
@@ -1,5 +0,0 @@
 # copied from ament_cmake_core/cmake/environment_hooks/environment/path.sh
 if [ -d "$AMENT_CURRENT_PREFIX/bin" ]; then
  ament_prepend_unique_value PATH "$AMENT_CURRENT_PREFIX/bin"
 fi
@@ -0,0 +1 @@
 /home/cc/anaconda3/envs/ros_flexiv/share/ament_cmake_core/cmake/environment_hooks/environment/path.sh
@@ -1,20 +0,0 @@
 <library path="flexiv_hardware">
    <class name="flexiv_hardware/FlexivHardwareInterface"
           type="flexiv_hardware::FlexivHardwareInterface"
           base_class_type="hardware_interface::SystemInterface">
        <description>
            ROS 2 Control Hardware interface for the Flexiv robots. This driver
            uses Flexiv RDK. Position, Velocity and Effort interfaces are supported.
        </description>
    </class>
    <class name="flexiv_hardware/FlexivDualHardwareInterface"
           type="flexiv_hardware::FlexivDualHardwareInterface"
           base_class_type="hardware_interface::SystemInterface">
        <description>
            ROS 2 Control Hardware interface for a pair of Flexiv robots. This driver
            uses Flexiv DRDK. Position, Velocity and Effort interfaces are supported.
        </description>
    </class>
 </library>
@@ -0,0 +1 @@
 /home/cc/ros2/src/flexiv_hardware/flexiv_hardware_interface_plugin.xml
@@ -1,46 +0,0 @@
 # generated from ament_package/template/package_level/local_setup.bash.in
 # source local_setup.sh from same directory as this file
 _this_path=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" && pwd)
 # provide AMENT_CURRENT_PREFIX to shell script
 AMENT_CURRENT_PREFIX=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." && pwd)
 # store AMENT_CURRENT_PREFIX to restore it before each environment hook
 _package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
 # trace output
 if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
  echo "# . \"$_this_path/local_setup.sh\""
 fi
 . "$_this_path/local_setup.sh"
 unset _this_path
 # unset AMENT_ENVIRONMENT_HOOKS
 # if not appending to them for return
 if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
  unset AMENT_ENVIRONMENT_HOOKS
 fi
 # restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
 AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
 # list all environment hooks of this package
 # source all shell-specific environment hooks of this package
 # if not returning them
 if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
  _package_local_setup_IFS=$IFS
  IFS=":"
  for _hook in $AMENT_ENVIRONMENT_HOOKS; do
    # restore AMENT_CURRENT_PREFIX for each environment hook
    AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
    # restore IFS before sourcing other files
    IFS=$_package_local_setup_IFS
    . "$_hook"
  done
  unset _hook
  IFS=$_package_local_setup_IFS
  unset _package_local_setup_IFS
  unset AMENT_ENVIRONMENT_HOOKS
 fi
 unset _package_local_setup_AMENT_CURRENT_PREFIX
 unset AMENT_CURRENT_PREFIX
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/local_setup.bash
@@ -1,2 +0,0 @@
 source;share/flexiv_hardware/environment/ament_prefix_path.sh
 source;share/flexiv_hardware/environment/path.sh
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/local_setup.dsv
@@ -1,184 +0,0 @@
 # generated from ament_package/template/package_level/local_setup.sh.in
 # since this file is sourced use either the provided AMENT_CURRENT_PREFIX
 # or fall back to the destination set at configure time
 : ${AMENT_CURRENT_PREFIX:="/home/cc/ros2/install/flexiv_hardware"}
 if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then
  if [ -z "$COLCON_CURRENT_PREFIX" ]; then
    echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \
      "exist. Consider sourcing a different extension than '.sh'." 1>&2
  else
    AMENT_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
  fi
 fi
 # function to append values to environment variables
 # using colons as separators and avoiding leading separators
 ament_append_value() {
  # arguments
  _listname="$1"
  _value="$2"
  #echo "listname $_listname"
  #eval echo "list value \$$_listname"
  #echo "value $_value"
  # avoid leading separator
  eval _values=\"\$$_listname\"
  if [ -z "$_values" ]; then
    eval export $_listname=\"$_value\"
    #eval echo "set list \$$_listname"
  else
    # field separator must not be a colon
    _ament_append_value_IFS=$IFS
    unset IFS
    eval export $_listname=\"\$$_listname:$_value\"
    #eval echo "append list \$$_listname"
    IFS=$_ament_append_value_IFS
    unset _ament_append_value_IFS
  fi
  unset _values
  unset _value
  unset _listname
 }
 # function to append non-duplicate values to environment variables
 # using colons as separators and avoiding leading separators
 ament_append_unique_value() {
  # arguments
  _listname=$1
  _value=$2
  #echo "listname $_listname"
  #eval echo "list value \$$_listname"
  #echo "value $_value"
  # check if the list contains the value
  eval _values=\$$_listname
  _duplicate=
  _ament_append_unique_value_IFS=$IFS
  IFS=":"
  if [ "$AMENT_SHELL" = "zsh" ]; then
    ament_zsh_to_array _values
  fi
  for _item in $_values; do
    # ignore empty strings
    if [ -z "$_item" ]; then
      continue
    fi
    if [ $_item = $_value ]; then
      _duplicate=1
    fi
  done
  unset _item
  # append only non-duplicates
  if [ -z "$_duplicate" ]; then
    # avoid leading separator
    if [ -z "$_values" ]; then
      eval $_listname=\"$_value\"
      #eval echo "set list \$$_listname"
    else
      # field separator must not be a colon
      unset IFS
      eval $_listname=\"\$$_listname:$_value\"
      #eval echo "append list \$$_listname"
    fi
  fi
  IFS=$_ament_append_unique_value_IFS
  unset _ament_append_unique_value_IFS
  unset _duplicate
  unset _values
  unset _value
  unset _listname
 }
 # function to prepend non-duplicate values to environment variables
 # using colons as separators and avoiding trailing separators
 ament_prepend_unique_value() {
  # arguments
  _listname="$1"
  _value="$2"
  #echo "listname $_listname"
  #eval echo "list value \$$_listname"
  #echo "value $_value"
  # check if the list contains the value
  eval _values=\"\$$_listname\"
  _duplicate=
  _ament_prepend_unique_value_IFS=$IFS
  IFS=":"
  if [ "$AMENT_SHELL" = "zsh" ]; then
    ament_zsh_to_array _values
  fi
  for _item in $_values; do
    # ignore empty strings
    if [ -z "$_item" ]; then
      continue
    fi
    if [ "$_item" = "$_value" ]; then
      _duplicate=1
    fi
  done
  unset _item
  # prepend only non-duplicates
  if [ -z "$_duplicate" ]; then
    # avoid trailing separator
    if [ -z "$_values" ]; then
      eval export $_listname=\"$_value\"
      #eval echo "set list \$$_listname"
    else
      # field separator must not be a colon
      unset IFS
      eval export $_listname=\"$_value:\$$_listname\"
      #eval echo "prepend list \$$_listname"
    fi
  fi
  IFS=$_ament_prepend_unique_value_IFS
  unset _ament_prepend_unique_value_IFS
  unset _duplicate
  unset _values
  unset _value
  unset _listname
 }
 # unset AMENT_ENVIRONMENT_HOOKS
 # if not appending to them for return
 if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
  unset AMENT_ENVIRONMENT_HOOKS
 fi
 # list all environment hooks of this package
 ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/flexiv_hardware/environment/ament_prefix_path.sh"
 ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/flexiv_hardware/environment/path.sh"
 # source all shell-specific environment hooks of this package
 # if not returning them
 if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
  _package_local_setup_IFS=$IFS
  IFS=":"
  if [ "$AMENT_SHELL" = "zsh" ]; then
    ament_zsh_to_array AMENT_ENVIRONMENT_HOOKS
  fi
  for _hook in $AMENT_ENVIRONMENT_HOOKS; do
    if [ -f "$_hook" ]; then
      # restore IFS before sourcing other files
      IFS=$_package_local_setup_IFS
      # trace output
      if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
        echo "# . \"$_hook\""
      fi
      . "$_hook"
    fi
  done
  unset _hook
  IFS=$_package_local_setup_IFS
  unset _package_local_setup_IFS
  unset AMENT_ENVIRONMENT_HOOKS
 fi
 # reset AMENT_CURRENT_PREFIX after each package
 # allowing to source multiple package-level setup files
 unset AMENT_CURRENT_PREFIX
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/local_setup.sh
@@ -1,59 +0,0 @@
 # generated from ament_package/template/package_level/local_setup.zsh.in
 AMENT_SHELL=zsh
 # source local_setup.sh from same directory as this file
 _this_path=$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)
 # provide AMENT_CURRENT_PREFIX to shell script
 AMENT_CURRENT_PREFIX=$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)
 # store AMENT_CURRENT_PREFIX to restore it before each environment hook
 _package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
 # function to convert array-like strings into arrays
 # to wordaround SH_WORD_SPLIT not being set
 ament_zsh_to_array() {
  local _listname=$1
  local _dollar="$"
  local _split="{="
  local _to_array="(\"$_dollar$_split$_listname}\")"
  eval $_listname=$_to_array
 }
 # trace output
 if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
  echo "# . \"$_this_path/local_setup.sh\""
 fi
 # the package-level local_setup file unsets AMENT_CURRENT_PREFIX
 . "$_this_path/local_setup.sh"
 unset _this_path
 # unset AMENT_ENVIRONMENT_HOOKS
 # if not appending to them for return
 if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
  unset AMENT_ENVIRONMENT_HOOKS
 fi
 # restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
 AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
 # list all environment hooks of this package
 # source all shell-specific environment hooks of this package
 # if not returning them
 if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
  _package_local_setup_IFS=$IFS
  IFS=":"
  for _hook in $AMENT_ENVIRONMENT_HOOKS; do
    # restore AMENT_CURRENT_PREFIX for each environment hook
    AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
    # restore IFS before sourcing other files
    IFS=$_package_local_setup_IFS
    . "$_hook"
  done
  unset _hook
  IFS=$_package_local_setup_IFS
  unset _package_local_setup_IFS
  unset AMENT_ENVIRONMENT_HOOKS
 fi
 unset _package_local_setup_AMENT_CURRENT_PREFIX
 unset AMENT_CURRENT_PREFIX
@@ -0,0 +1 @@
 /home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/local_setup.zsh
@@ -1,25 +0,0 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
  <name>flexiv_hardware</name>
  <version>1.9.1</version>
  <description>Hardware interfaces to Flexiv robots for ROS 2 control</description>
  <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
  <license>Apache 2.0</license>
  <author email="munseng.phoon@flexiv.com">Mun Seng Phoon</author>
  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>rclcpp_lifecycle</depend>
  <depend>hardware_interface</depend>
  <depend>pluginlib</depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
  <test_depend>ament_cmake_gtest</test_depend>
  <export>
    <build_type>ament_cmake</build_type>
  </export>
 </package>
@@ -0,0 +1 @@
 /home/cc/ros2/src/flexiv_hardware/package.xml
@@ -19,19 +19,8 @@ _colcon_prefix_chain_bash_source_script() {
 # source chained prefixes
 # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-# 不写死某一台机器的 conda。已激活的环境优先，否则找当前用户的 ros_flexiv。
+COLCON_CURRENT_PREFIX="/home/cc/anaconda3/envs/ros_flexiv"
-if [ -n "${CONDA_PREFIX:-}" ] && [ -f "${CONDA_PREFIX}/local_setup.bash" ]; then
+_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
  COLCON_CURRENT_PREFIX="${CONDA_PREFIX}"
 elif [ -f "${HOME}/anaconda3/envs/ros_flexiv/local_setup.bash" ]; then
  COLCON_CURRENT_PREFIX="${HOME}/anaconda3/envs/ros_flexiv"
 elif [ -f "${HOME}/miniconda3/envs/ros_flexiv/local_setup.bash" ]; then
  COLCON_CURRENT_PREFIX="${HOME}/miniconda3/envs/ros_flexiv"
 else
  COLCON_CURRENT_PREFIX=""
 fi
 if [ -n "${COLCON_CURRENT_PREFIX}" ]; then
  _colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
 fi
 # source this prefix
 # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
--- a/Show More
+++ b/Show More
		`@@ -1 +1 @@`
			`arm_kinematics:arm_msgs:geometry_msgs:python3-serial:python3-yaml:rclpy:sensor_msgs:std_msgs:std_srvs:tf2_ros:trajectory_msgs`				`arm_kinematics:arm_msgs:control_msgs:geometry_msgs:python3-serial:python3-yaml:rclpy:sensor_msgs:std_msgs:std_srvs:tf2_ros:trajectory_msgs`
		`@@ -0,0 +1 @@`
							`/home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/flexiv_dual_hardware_interface.hpp`
		`@@ -0,0 +1 @@`
							`/home/cc/ros2/build/flexiv_hardware/libflexiv_hardware.so`
		`@@ -1 +0,0 @@`
			`share/flexiv_hardware/flexiv_hardware_interface_plugin.xml`
		`@@ -0,0 +1 @@`
							`/home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/hardware_interface__pluginlib__plugin/flexiv_hardware`
		`@@ -1 +0,0 @@`
			`rclcpp;rclcpp_lifecycle;hardware_interface;pluginlib;ament_lint_auto;ament_lint_common;ament_cmake_gtest`
		`@@ -1 +0,0 @@`
			/home/cc/ros2/install/gpio_controller:/home/cc/ros2/install/flexiv_bringup:/home/cc/ros2/install/flexiv_test_nodes:/home/cc/ros2/install/flexiv_robot_states_broadcaster:/home/cc/ros2/install/flexiv_gripper:/home/cc/ros2/install/flexiv_msgs:/home/cc/ros2/install/flexiv_moveit_config:/home/cc/ros2/install/flexiv_hardware:/home/cc/ros2/install/arm_sim_gazebo:/home/cc/ros2/install/arm_bringup:/home/cc/ros2/install/arm_moveit_config:/home/cc/ros2/install/arm_description:/home/cc/ros2/install/flexiv_description:/home/cc/ros2/install/arm_driver_flexiv:/home/cc/ros2/install/arm_control:/home/cc/ros2/install/arm_application:/home/cc/ros2/install/arm_msgs:/home/cc/ros2/install/arm_kinematics:/home/cc/ros2/install/arm_interfaces:/home/cc/anaconda3/envs/ros_flexiv
		`@@ -0,0 +1 @@`
							`/home/cc/anaconda3/envs/ros_flexiv/share/ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh`
		`@@ -0,0 +1 @@`
							`/home/cc/ros2/src/flexiv_hardware/flexiv_hardware_interface_plugin.xml`
		`@@ -1,2 +0,0 @@`
			`source;share/flexiv_hardware/environment/ament_prefix_path.sh`
			`source;share/flexiv_hardware/environment/path.sh`