# 入孔检测

只做视觉定位（像素孔心），**不包含**装配插入运动 / HMI。跟踪仍用 `run_visual_servo_rt.sh`。

日常改 **`config/site.yaml` → `hole_detect:`**（Hough、居中门控、间距都会覆盖包内 yaml）。  
`src/arm_application/config/hole_detect.yaml` 只是缺省值。

源码：

| 文件 | 作用 |
|------|------|
| `src/arm_application/arm_application/hole_detector.py` | 统一入口：`yolo` / `hough` / `two_stage` |
| `src/arm_application/arm_application/hole_detect_hough.py` | YOLO 工件框 + HoughCircles 左右孔 |
| `src/arm_application/arm_application/hole_detect_node.py` | 订当前臂 D405 彩图 |
| `src/arm_application/config/hole_detect.yaml` | `two_stage` 参数 |
| `src/arm_application/config/hole_insert.yaml` | `yolo`/`hough` 参数 |
| `models/hole_insert/` | 孔模型权重（二进制不进 git） |

## 跑

先开该臂相机，再开检测：

```bash
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
```

91 把 `--arm` 换成 `91`（话题 `/d405_right/...`）。

`site.yaml`：

```yaml
hole_detect:
  backend: "two_stage"   # two_stage | yolo | hough
  weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
  workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"  # 空 = paths.yolo_weights
  conf: 0.25
  hough_param2: 26
  hole_distance_min: 150
```

- `two_stage`：用工件 YOLO 找 bbox，再在 ROI 里 Hough 找左右螺纹孔（原 `camera_test/hole_detect.py`）。
- `yolo`：模型框直接当孔（原 `hole_insert` HoleDetector）。
- `hough`：整幅图圆检测，不用权重。

权重放到 `~/ros2/models/hole_insert/`（当前 `detect_best_1.onnx`，二进制不进 git，布局见 `models/README.md`）。`two_stage` 的工件模型默认就是跟踪用的 `paths.yolo_weights`（`hole_detect.workpiece_weights` 可覆盖）。

## ROS

| 话题 | 类型 |
|------|------|
| `/hole_detect/result` | `arm_msgs/msg/HoleDetections` |
| `/hole_detect/holes` | `std_msgs/Float32MultiArray` `[ok, n, u,v,r,conf, …]` |
| `/hole_detect/overlay` | `sensor_msgs/Image` 画框画孔 |

改 `hole_detect.yaml` 的 Hough / 居中门控后重启 `run_hole_detect.sh`。`two_stage` 会做时序稳帧（`stabilize_*`）。

## 三阶段串联（推荐日常入口）

一条命令跑完：**视觉跟踪锁定 → 拍照对孔**。搜孔轻压已取消，这条命令不再下压，也不能用参数重新打开。旋入是后面的单独命令。

```bash
# 终端 1 —— 控制栈（一直开着）
bash ~/ros2/scripts/run_real.sh --arm 89

# 终端 2 —— 相机预览（一直开着，看跟踪画面）
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview

# 终端 3 —— 跟踪 + 对孔（到这里停，不搜孔）
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
```

`run_pipeline.sh` 是 `run_all.sh` 的别名。

**跑之前确认**：夹爪夹着通止规；工件在相机大概能看见的位置（一阶段会自己找）。
左指触觉如果掉线会自动降级成单指。

盯这几行就知道跑到哪了：

1. `[pipeline] === stage1_track ===` → 等跟踪锁定
2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
对孔结束后臂停在当前高度。搜孔轻压不再接着跑。

急停：终端 3 Ctrl-C。失败时臂保持当前位姿，看
`/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。

高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、
`robots[89].servo.hole_hover_z_m`（拍照悬停高）。

分阶段单独跑仍用 `run_visual_servo_rt.sh`、`run_stage2_locate.sh`、
`run_stage3_search.sh`，下面两节。

## 二阶段一次性拍照定位（stage2，与一阶段跟踪分开跑）

先悬停垂直朝下 → 拍一张照 → `detect_best_1.onnx` 框工件 → Hough 解 k1/k2 孔心 →
像素射线交到 `workpiece_surface_z_m`（不读深度图）+ 内参 + 手眼换算基座坐标 →
移动**规尖**到目标孔正上方。`hole_visual_only` 时粗居中框心也走同一条平面交点，
不用深度（30° 相机下深度框心会让 `K1.x−规尖.x` 随站位翻号）。

本分支 `hole_keypoint_once: true`：悬停拍一次，把框心到 k1 的半孔距（约 30mm）
一次走完（不再用多走多拍的 12mm 封顶）。

现行实验 `hole_once_lock_xy_descend: true`：走完后不补拍，一条进程内厂家
MoveL 收到 `final_gap_mm`（规尖离面，毫米；`--final-gap` 覆盖，默认 2）。
`once_ff_look_per_m: 0` 时 ΔXY=0、只降 Z（笛卡尔锁 XY）。k>0 才
`ΔXY = -look × k × 剩余下降`。法兰 Z = 工具长 + 工件面 + 间隙，不许直接写法兰高度。MoveL 目标是**当前厂家 `tcp_pose` 加
相对位移**，不是 ROS 规尖绝对坐标。
2026-10-14：k=0.018 在框心改平面交点后，D435+D405 与单 D405 下臂都往 +X 过补
约 1.3mm、插不进——悬停 X 已对上，常数前馈是多余的。锁 XY 折线失败是关节 hop
漂 XY，不是笛卡尔 MoveL 锁 XY。对照 `37b85aa`：观察高约 15mm 再交孔斜线补 XY。
前馈超过 8mm 或工作盒会截断则不动。没有新鲜 `tcp_pose` 则不下发。

轻降力接触门控是 `final_gap_mm + 4mm`（贴面 2mm 时 = 6mm）。看
`/tmp/stage2_result.json` 的 `snaps[]`。`main` 保存点是多走多拍闭环。

> **对的是"规尖"，不是"法兰轴线"**（2026-10-08 定稿）。通止规夹在爪里，相对法兰轴线
> 可以整体偏出若干毫米；旧写法把**法兰**对到孔上，规尖就偏着压在孔沿上，轻降一压
> 腕力蹦到 63N。现在每一帧同时解「规尖基座 XY」和「孔基座 XY」——两者走同一条手眼
> 链，链路误差与装夹变动一起抵消。规尖偏移是**刚体量**，量准一份（`tip_calib_n` 拍
> 一致）就冻结。
> 真机验证（arm 89，独立重测那一帧）：规尖到 k1 **0.34mm**、到 k2 61.15mm、
> 反投影工具长 222mm（与 site 的 223mm 一致），偏移冻结值 (2.6, −1.0)mm。
> 相关开关在 `site.yaml → hole_detect.stage2:`（`align_tip_vision` / `tip_*`）。
> 量不到规尖偏移时二阶段**判失败**（exit 8），不会拿"法兰对上了"冒充成功。

```bash
# 前置：T1 run_real.sh --arm 89；T3 run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89                # 完整流程（自动拉起触觉服务，仅左臂）
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only   # 只垂直+悬停锁定，用来调高度
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2      # 对准 k2（默认 k1）
bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高，不动 yaml
bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent             # 只验证精对准、不轻降
bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8            # 锁 XY，规尖停在离面 8mm
```

悬停锁高接口：`site.yaml → robots[89].servo.hole_hover_z_m`（91 在同名段）。
工件权重：`site.yaml → hole_detect.workpiece_weights`（默认 `models/hole_insert/detect_best_1.onnx`）。
其余二阶段参数：`site.yaml → hole_detect.stage2:`（对准容差、迭代次数、垂直判据等）。

| 话题 | 类型 |
|------|------|
| `/stage2/state` | `std_msgs/String` 阶段状态机 |
| `/stage2/result` | `std_msgs/String` JSON：k1/k2 基座坐标、残差、悬停高 |
| `/stage2/overlay` | `sensor_msgs/Image` 标注图（同时落盘 /tmp/stage2_overlay.png） |

## 旋转装配（原三阶段节点；搜孔轻压入口已取消）

不带 `--screw` 时 `run_stage3_search.sh` 直接退出，不会触面，也不会螺旋搜孔。
下面是这个节点在旋入时仍会走的力控步骤，不是一条还能单独打开的搜孔命令。

前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程：

1. **verify** — 垂直度核验；Orisys 双指触觉话题判活（connected=true 才算数），
   并自动测 ~1.2s **静止基线 μ0/σ0**（日志打印「触觉基线/派生阈值」两行）
2. **approach** — 1.5mm 小步下降，`fnormal > μ0 + max(4σ0, touch_delta)` 判定触面，记录 `z_contact`
3. **seat** — Z 力闭环建立轻触（fn ≈ 触面阈值 + `press_delta`）
4. **spiral** — 阿基米德螺旋搜孔：XY 沿螺旋走（螺距 `spiral_pitch_mm`，保证轨迹重叠
   覆盖），Z 由力闭环维持轻触；`fnormal` 相对接触均值跌落 `drop_ratio` 持续
   `drop_frames` 帧 → **尖端入孔**。找不到则每次扩大 `spiral_expand_step_mm` 再搜
5. **center** — 入孔点十字微扫（±`center_scan_mm`），剪切力最小点 = 孔心
6. **insert** — 力限下压 `insert_depth_mm`（超限回退微抖重试 `insert_retry` 次）；
   `screw_pitch_mm ≠ 0` 时按螺距同步绕世界 Z 旋入（**螺纹通止规**：>0 右旋 / <0 左旋；
   光面规保持 0）。`insert_depth_mm: 0` = 只搜孔定心不插入
7. **hold** — 锁定关节，发布结果

```bash
# 前置：T1 run_real.sh --arm 89；对孔已完成（臂保持不动）
# 不带 --screw 会直接退出，不会搜孔。
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
```

**力阈值是「相对静止均值的增量」，不用标定牛顿**：Orisys 读数是形变任意单位且
夹持基线会漂移，节点每次启动自动测基线后派生全部阈值。要调灵敏度就改
`site.yaml → hole_detect.stage3:` 的 `touch_delta / press_delta / safe_press_delta /
shear_delta`（日志里能看到实际派生值）。

安全（任一触发立即抬 5mm 中止）：Orisys `fnormal > μ0 + safe_press_delta`；腕部 F/T
`|F| > ft_safe_n`（订 `/<sn>/flexiv_robot_states`，订不到只警告）；触觉断流 >
`tactile_timeout_s`。切向力超 `μ0shear + shear_delta` 自动抬 0.3mm 减拖。

| 话题 | 类型 |
|------|------|
| `/stage3/state` | `std_msgs/String` 阶段状态机 |
| `/stage3/result` | `std_msgs/String` JSON：孔心基座坐标、触面 Z、插入深度、峰值力 |

结果同时落盘 `/tmp/stage3_result.json`。退出码：0 成功；3 未触面；4 搜孔失败；
5 插入受阻；6 安全中止；7 触觉断流。