# 入孔检测

只做视觉定位（像素孔心），**不包含**装配插入运动 / 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_*`）。

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

一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入**
（拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。

```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_all.sh --arm 89 --depth 6 --pitch 1.0
```

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

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

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

1. `[pipeline] === stage1_track ===` → 等跟踪锁定
2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入）

急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看
`/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 孔心 →
深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动**规尖**到目标孔正上方 →
复拍验证（≤`align_max_iters` 次，收敛判据 `align_tol_m`）→ 锁定保持。

> **对的是"规尖"，不是"法兰轴线"**（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             # 只验证精对准、不轻降
```

悬停锁高接口：`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） |

## 三阶段柔顺搜孔（stage3，Orisys 触觉 + 力闭环螺旋）

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

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；二阶段已完成（臂保持不动）
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  # 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 读数是形变任意单位且
夹持基线会漂移，节点每次启动自动测基线后派生全部阈值。要调灵敏度就改
`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 触觉断流。