ros_contorl/docs/HOLE_DETECT.md

入孔检测

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

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 只起一次。

# 终端 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 孔心 → 像素射线交到 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），不会拿"法兰对上了"冒充成功。

# 前置：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）

三阶段柔顺搜孔（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 — 锁定关节，发布结果

# 前置：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 触觉断流。