应用层修复:S00 自检假阴性、S07 触觉假断流、S07 下探静默卡死

三处都是"看着像硬件问题、其实是判据问题"的假警报:

1. S00 自检假阴性(gauge_boot.py)
   探测等待时用 time.sleep 不 spin → 订阅回调根本没机会执行,
   于是**健康的硬件也会被判"没有数据"**。改成等待期间持续 spin。

2. S07 触觉假断流(stage3_search_node.py)
   判据原本是 |fnormal| > 1.0,但力本来就可能是零或很小(真机轻触、
   仿真刚接触)→ 明明在发数据却判"触觉断流"。
   改成"数据够新(TACTILE_MAX_AGE_S 内)+ connected" —— 判活看的是
   **有没有在发**,不是**数值大不大**。

3. S07 下探静默卡死(gauge_fsm.py / gauge_assembly_node.py)
   _goto 的到位容差 ≥ 步长时,每一步都被判成"已在容差内",臂一步都不走,
   而外层只看到"没进度"→ 死循环到超时,日志里看不出原因。
   新增 descend_tol_m / descend_iter_budget / descend_progress:
   容差从步长推导(不该手工调大),并显式判"连续 N 步零位移"。
   容差 ≥ 步长的反例已进 site 回归测试,防止再犯。

4. site 分段化(export_site_env.py):89/91 两臂的 servo/handeye/点位
   各一段,避免"改 91 影响 89"。

另:run_unit_tests.sh 作为统一测试入口(932→933 项),
    devices/sim/gazebo/README.md 同步。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-09-24 14:43:19 +08:00
parent b29339e522
commit 36a59c81bf
21 changed files with 1573 additions and 99 deletions
+59
View File
@@ -246,6 +246,7 @@ UserCommands 系统)会让**所有**模型都 spawn 不进去(见 [坑 2](#
| 相机的视场/坐标系/话题 | `config/cameras.yaml`,**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对,不一致告警 |
| 相机图幅(影响像素尺度) | `config/cameras.yaml` 的 `width`/`height` **且** SDF 的 `image_width/height` | ✔ 启动时自动比对(这是最容易漏的一处,见 4.3) |
| 触觉标度/轴向/基线 | `config/tactile.yaml` | ✔ 适配器每次启动都读 |
| 感知来源(一阶段目标点 + 二阶段孔位) | `site.yaml` → `perception.source` | ✔ 仿真那台写 `truth`:一阶段由 `sim_truth_perception` 按场景位姿算目标点,二阶段由 `arm_sim_gazebo.truth_holes` 按场景几何投影孔位(见 §5 坑 13) |
| 装配的悬高/速度 | **复用真机那套**(`servo.*` / `hole_detect.stage2.*`) | ✔ 有意共用 |
| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关,改 `worlds/assembly.world` 的 `<gravity>` |
| 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。改要动三处:`urdf/arm.sim.urdf.xacro` 的 `arm_prefix`、`config/controllers.yaml` 的 `sim_joint1..7`、本项 |
@@ -471,6 +472,64 @@ mimic 只能是 revolute",挂在 `unused_finger_width_link` 上(那个 link
适配器的做法:反馈取"与主关节直接相连、|multiplier| 最大的那个从动关节"
(GN01 是 `left_outer_knuckle_joint`),再按耦合关系反推开口。
### 坑 13:二阶段的孔,在仿真里"每拍都丢"
现象(`log_sim/gauge_flow.log`):
```
一阶段已锁定 — 停跟踪、钉住当前关节,直接进二阶段
[pipeline] === S05_snap ===
精拍 5帧中值图 L=(265.2,320.4) R=(529.7,288.8)
WARN 两孔基座间距 107.9mm 不像 k1/k2(期望 32–85mm),丢弃本拍
...(同样一行重复 8 次)
ERROR S05 精拍定位失败 code=4
```
一阶段(S04)完全正常:真值目标锁定、跟踪到位、悬停到 0.632m。所以看起来像
"孔检测的门槛配错了"。真因有两段,**第二段才是关键**:
1. 工件检测模型在合成图上出不了框(实测最高分 0.017,见 `models/` 的说明)→
`hole_detect_hough` 拿不到 ROI,于是按调用方的允许退到**整幅画面**当 ROI。
这一步本身是设计好的退路(`detect_holes(allow_full_frame_roi=True)`),
真机上"权重不匹配但孔看得见"时就靠它。
2. 整幅画面里还站着**夹爪和通止规**(它们就在工件上方、比工件更近)。
HoughCircles 在它们身上找到一对"看起来对称的圆",间距 108mm —— 真值
58mm。于是每一拍都被间距门槛丢掉,丢满 8 拍超时。
**为什么不能靠"把门槛放宽"解决**:107.9mm 离 58mm 不算远,放宽一点就会让这对
**假孔**通过,然后机械臂去对准一个错的点 —— 症状推迟到 S07 才爆(拧不进 /
掉件重放),比现在难查得多。宁可现在"每拍都丢"。
修法:仿真里不要用检测器,用**真值孔位** —— 与一阶段的 `sim_truth_perception`
同一套思路(都是"场景里本来就知道,何必去推"):
```
孔世界点 = scene.yaml 的物体位姿 + 模型 <visual name="*_ring"> 的局部位置
孔像素 = K · T_cam_world · 孔世界点 (T 由 TF 查,K 来自 camera_info)
```
* 实现在 `arm_sim_gazebo/truth_holes.py`:**纯几何**,没有 ROS 依赖,
所以在没有 Gazebo 的机器上也能测(`test_truth_holes.py`,13 条)。
* 由 `stage2_locate_node` 在 `perception.source == truth` 时接管
`_detect_holes_img` / `_best_bbox`(`gauge_assembly_node` 继承这两个方法,
所以 S05 的整条链一起生效)。
* 真机(`source: yolo`)那条路**一行都没改**。
* 为什么用 `perception.source` 而不是新加一个"仿真孔位"开关:这个问题本来就是
"谁产生视觉测量",一阶段的目标点与二阶段的孔位只是它的两个消费者 ——
拆成两个开关,迟早出现"一阶段走真值、二阶段走模型"的半仿状态。
顺带被这条坑钉住的两个行为:
* 孔在**相机背后**时真值路径返回"不可用"而不是硬给一个点(给伺服幻影目标
会让机械臂朝错误方向走,看起来像"标定坏了");
* 模型里嵌件亮环不是**正好 2 个**时直接报"有几个环",不猜。
验收:
```
python3 -m pytest src/arm_sim_gazebo/test/test_truth_holes.py -q
```
---
## 6. 现状与进度