通止规装配：配置段进 site.yaml + 中文手册 + 成败判定「不看 $?」

配置（site.yaml → gauge_assembly:，38 键全部带中文注释） ------------------------------------------------------ - 键名/单位/类型/交叉矛盾由 gauge_config.py 校验：拼错键名报错并给最相近的候选，单位后缀（_deg/_mm/_m/_s/_pct/_delta）逐条对表，`contact_safe_delta ≤ contact_tangential_delta` 这类矛盾启动就拦（R1.2：没接线的键不许静默存在）。 - 借机清掉 4 个**没接线**的键（require_preview / track_*_s / hover_m：前两个是 stage1 那层的、后两个是 servo 那层的，放在这里等于第二份真相），并把 contact_speed_m_s / coarse_timeout_s / 夹爪力速真的接上。 - site.example.yaml 由 site.yaml 生成（gen_site_example.py --check 通过）。成败判定：`ros2 launch` 的退出码**恒为 0** ------------------------------------------ 实测（Humble）：子进程 exit 3，`ros2 launch` 仍返回 0。用 `$?` 判成败的脚本会把 "装配失败"记成"成功"——没人会去查一个说成功的脚本。改成： - 节点把结论**落盘**（--result-file，默认 /tmp/gauge_assembly_result.json，原子写、先落盘再发话题），任何"没跑到收尾"的退出也会补一份 ok=false； - 脚本读结论文件判成败：0=成功、1=失败、**文件不存在也算失败**并说清； - 新增 test_gauge_result_gate.py：把脚本里那段判成败的 Python 抽出来，对成功/失败/干跑/缺文件/坏 JSON 五种输入断言退出码 —— 判据本身也有测试。顺带修掉两个"看着像玄学"的现象 ------------------------------ - **流程打完了终端不回提示符**：launch 里的静态 TF 发布者是常驻的，主节点退出带不走它 —— 挂 OnProcessExit → Shutdown。 - **"已装 1/2 孔"**：结果里 hole_index 是 0 基当前孔号，不是"装完几个"，新增 holes_done 给人和脚本看。文档（全部中文，含"怎么调参/还没做"） ------------------------------------ - 新增 docs/GAUGE_ASSEMBLY.md：状态机全图、每步判据表、E1 掉件处理（**按实现**：掉件后该孔作废、换下一个孔；最后一个孔掉件则序号被夹住=重试本孔）、话题契约、调参顺序、验收命令、踩过的坑。 - FRAMEWORK_RULES 新增 R5.8（同一份数学只许一份 + 守卫测试）、R5.9（自检的期望类型与调用处同源）、R5.10（缺依赖 skip，别留常年红）、R6.9（硬退出前打 traceback）、R6.10（ros2 launch 退出码恒 0）＋ R5.7 补真机/仿真"生效方式不同" （真机软链即改即生效，仿真拷贝必须重编）＋ 5 条症状反例、.cursor/rules 同步。 - WORKSPACE / INTERFACES / SITE_CONFIG / HANDBOOK / 两份 README 索引与决策表补齐。验证（本机真跑） ---------------- pytest（仿真环境） 436 passed, 14 skipped（缺 onnx 跳过）干跑 --sim --dry-run → 脚本退出码 0，结论 ok=true 已装 2/2 孔不干跑 --sim → 脚本退出码 1，S00 拦住，未发任何运动指令 Co-authored-by: Cursor <cursoragent@cursor.com>
2026-09-24 10:30:11 +08:00
parent dfc945b7e0
commit bda2fa7129
16 changed files with 741 additions and 37 deletions
@@ -0,0 +1,223 @@
+# 通止规装配（P6 看板流程）
+
+> 一句话：**把通止规（螺纹规）从"拿着"做到"拧进 k1、拧出、拧进 k2、拧出、回 home"**，
+> 全过程由一台状态机盯着，任何一步不满足判据就停在那里、说清原因，而不是"接着往下走"。
+>
+> 入口：`bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89`
+> 配置：`config/site.yaml` → `gauge_assembly:` 段（**每个键都有中文注释，先读那一段**）
+> 代码：`src/arm_application/arm_application/gauge_*.py`（纯逻辑）+ `gauge_assembly_node.py`（ROS 壳）
+
+---
+
+## 1. 先跑干跑（不发任何运动指令）
+
+```bash
+source ~/ros2/scripts/env.sh
+bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --dry-run
+```
+
+干跑会：S00 自检全跑一遍 → S01…S11 每步只打印"我打算做什么" → 最后打印
+`[gauge] /gauge/result`。**干跑不会给臂/夹爪发任何指令**，可以随时按，
+用来确认"编排对不对、配置读到没有、缺哪些服务"。
+
+真跑（会动臂）：
+
+```bash
+bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89
+```
+
+前置状态（脚本不会替你开）：
+
+| 要什么 | 命令 |
+|--------|------|
+| 臂的伺服 + ros2_control | `bash ~/ros2/scripts/run_real.sh --arm 89` |
+| 腕部 D405（S05 要拍照） | `bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview` |
+| 夹爪 | `bash ~/ros2/scripts/run_gripper.sh --arm 89` |
+| 视触觉（S06 悬停时测基线） | `bash ~/ros2/scripts/run_tactile.sh` |
+| 手外 D435（只有 S03 用，可选） | `bash ~/ros2/scripts/run_camera_d435.sh --preview` |
+
+缺了会怎样：**S00 自检会逐项报出来**，缺"必检项"就直接停（exit 1，不发指令）。
+S00 的清单与"缺了会怎样"在 `gauge_boot.py` 里是全表声明的，不在脚本里拼。
+
+---
+
+## 2. 状态机（`gauge_fsm.py` 是唯一权威）
+
+```
+S00 ─boot_ok─▶ S01 ─pose_ok─▶ S02 ─grip_ok─▶ S04 ─track_locked─▶ S05 ─snap_ok─▶ S06
+              (joint_limit 也进 S02)         │                                (hover_k)
+                                             │ track_lost        align_out │  │ align_ok
+                                             ▼                             │  ▼
+                                            S03 ─coarse_ok─▶ S05 ◀────────┘ S07 ─contact_ok─▶ S08
+                                             │                          (snap)     ▲            │
+                                        coarse_fail ─▶ S04                            │      screw_done
+                                                                                       │            ▼
+   E1 掉件：S08 ─gauge_dropped─▶ S01 ─RELOADED─▶ S10 ─pose_ok─▶ S06              S09 ─unscrew_done─┐
+                                                  ▲                                （最后一孔？）    │
+                                                  └── 还有孔：S10 = 回该孔记录位姿 │              │
+                                                                                   ▼              ▼
+                                                                                 S10 ───────▶ S11 ─pose_ok─▶ DONE
+```
+
+注：S05 `snap_unstable` 是**自环**（继续等稳定），S08 掉件那一路是 E1；
+S09 不做掉件判据（规正在往外退，"掉了"和"拧出来了"分不出来）。
+
+**E1 掉件后"哪个孔"**（这条是看板定的，容易误会，单独说）：
+
+- 掉件后孔序号**前进一格** → 该孔**作废**，直接去做下一个孔（掉件多半意味着螺纹/夹持已经
+  不对劲，原地重来风险更大）；结果里会留一条 note，请人去检查夹持与螺纹。
+- 在**最后一个孔**掉件时序号被夹住不再前进 → 表现为"重试本孔"（否则就没孔可做了）。
+- 想让"掉件后重试本孔"，改 `gauge_fsm.py` 里 `(SCREW_IN, GAUGE_DROPPED)` 的落点
+  与 `handle()` 里推进序号那一段，同时改 `test_gauge_fsm.py` 的两条掉件测试。
+
+| 状态 | 干什么 | 过了判据 | 不过 |
+|------|--------|----------|------|
+| S00 | 自检：臂/相机/夹爪/触觉/算法都在且**有真数据** | `boot_ok` | `boot_fail` → 中止，不打任何指令 |
+| S01 | 回 `home` → 切工具系 → **逆时针预绕到极限角**（为棘轮留回转行程） | `pose_ok` | `pose_fail` → 中止；撞关节限位**不算失败**（`joint_limit` → 继续） |
+| S02 | 夹持确认（看夹爪状态字，**当前默认跳过**） | `grip_pass` / `grip_skipped` | `grip_fail` → 中止 |
+| S04 | 起视觉跟踪（一阶段伺服） | `track_locked` | `track_lost` → **回 S03 粗定位兜底** |
+| S03 | 手外相机粗定位 → 悬停到工件上方 `coarse_hover_m` | `coarse_ok` → 回 S05 | `coarse_fail` → 回 S04 继续跟踪 |
+| S05 | 等画面**稳定**再精拍定位（判据：到均值最远距离 ≤ `snap_tol_px`） | `snap_ok` | 抖动 → `snap_unstable` → 留在 S05 重来 |
+| S06 | 正悬停在孔上方 `servo.hole_hover_z_m`；偏差 > `align_tol_mm` 回 S05 | `align_ok` | 超差 → `align_out` → 回 S05 重拍 |
+| S07 | 测力基线 → 步进下压（每步 `contact_step_mm`）→ 切向突变即停 | `contact_ok` | 法向力 > `contact_safe_delta` → **抬起 + 中止(E2)**；下探超 `contact_max_depth_mm` → 中止 |
+| S08 | 棘轮：慢转 `screw_step_deg` → 松爪 → 快回转 → 夹紧，最多 `screw_max_cycles` 轮 | `screw_done` | 掉件 → `gauge_dropped` → **E1**；力矩到位 → `screw_stop` → S09 |
+| S09 | 反向拧出（比拧入多 `unscrew_extra_cycles` 轮） | `unscrew_done` → S11（最后一孔）或 S10 | 失败 → 中止（此阶段不判掉件） |
+| S10 | 回该孔记录位姿，重复 S06–S09（下一孔） | `pose_ok` → S06 | 超时 → 中止 |
+| S11 | 回 home + 开爪，收尾 | `pose_ok` → **DONE** | 超时 → 中止 |
+| **E1** | 掉件：**立即停转**（不补转），→ S01 回 home + 开爪，等人工把规塞回爪里，发 `/gauge/reload_ok` 后 → S10「换下一个孔」 | `reloaded` | 超时 `reload_timeout_s` → 中止；累计超过重放上限 → 中止并留言 |
+| **E2** | 任何中止：先退到安全高度（`_safe_retract`），再报错退出 | — | — |
+
+**状态机是"数据"不是"代码"**：`TRANSITIONS` 是一张 `(状态, 事件) → 下一状态` 的表，
+节点里每个 `_step_*` 只允许返回表里登记的合法事件。
+`test_gauge_step_events.py` 会**读节点源码的 AST**，把每个 `_step_*` 可能返回的事件
+逐个问状态机"合法吗" —— 节点和转移表是两份数据，漏登记要在 pytest 里炸，
+而不是在真机上跑到那一步才炸。
+
+---
+
+## 3. 代码分工（改哪一块，找哪个文件）
+
+| 文件 | 管什么 | 有测试吗 |
+|------|--------|----------|
+| `gauge_fsm.py` | 状态/事件/转移表/上下文（**不改 ROS**，纯数据） | `test_gauge_fsm.py` |
+| `gauge_kin.py` | URDF 关节限位解析、预绕角计算、**实测绕工具 Z 转了多少度** | `test_gauge_kin.py` |
+| `gauge_config.py` | `site.yaml → gauge_assembly` 段的解析与校验（键名/单位/类型/交叉矛盾） | `test_gauge_config.py` |
+| `gauge_gripper.py` | 夹爪后端选择（契约 vs 真机 Trigger）+ 状态字→"夹住没/掉了没" | `test_gauge_gripper.py` |
+| `gauge_boot.py` | S00 自检清单与报告（必检/选检、缺了会怎样） | `test_gauge_boot.py` |
+| `gauge_assembly_node.py` | ROS 壳：发指令、读反馈、把事件交给状态机 | 间接（见上表） |
+| `launch/gauge_assembly.launch.py` | 只做一件事：发布手眼静态 TF（S05 的前提） | — |
+| `scripts/run_gauge_assembly.sh` | 环境 + 参数 + 前置说明 | — |
+
+设计原则：**能离线测的逻辑一律不放 ROS 里**。所以状态机、限位、配置、夹爪判据
+都可以在没有机器人、没有 Gazebo 的机器上跑（`pytest src/arm_application/test -q`）。
+
+---
+
+## 4. 话题 / 服务契约
+
+| 名字 | 类型 | 方向 | 说明 |
+|------|------|------|------|
+| `/gauge/state` | `std_msgs/String` | 出 | 每次转移都发一条：`#序号 状态 --事件--> 状态` |
+| `/gauge/progress` | `std_msgs/String` | 出 | 当前进度（第几孔 / 第几轮棘轮），给人看的 |
+| `/gauge/result` | `std_msgs/String` | 出 | 结束时的结论（JSON：`ok`/`state`/`holes_done`/`failures`/`fsm` 轨迹…） |
+| `--result-file`（默认 `/tmp/gauge_assembly_Rizon4-<SN>.json`） | 文件 | 出 | **同一份结论落盘**。`ros2 launch` 的退出码恒为 0（R6.9），所以脚本/CI 靠这个文件判成败 |
+| `/gauge/reload_ok` | `std_msgs/Bool` | 入 | **E1 人工确认**：掉件后把规塞回爪里，发一条 `true` |
+| `/gripper/open` `/gripper/close` | `arm_msgs/srv/GripperExecute` 或 `std_srvs/srv/Trigger` | 出 | 两种后端自动识别，见 `docs/GRIPPER.md` |
+| `…/joint_states`、`…/tactile/*`、`…/color/image_raw` | 标准 | 入 | 全部来自既有的相机/触觉/驱动节点，本流程**不自己开设备** |
+
+人工处理 E1（终端 2）：
+
+```bash
+ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}"
+```
+
+---
+
+## 5. 怎么调参（顺序很重要）
+
+1. **先标定手眼**（否则 S05 拍出来的坐标是错的，后面全白做）：
+   见 `docs/HANDEYE.md`。腕部相机要有 eye-in-hand 结果文件，S03 才用得上 eye-to-hand。
+2. **再看触觉量级**：`bash ~/ros2/scripts/run_tactile.sh --cli`，手压一下看读数变化。
+   所有力判据（`contact_*_delta`、`screw_*_delta`）都是"相对基线的增量"，单位是任意单位。
+3. **再确认 S01 能转**：启动日志里会打印"预绕 150°（方向=逆时针，限位余量 3°）"，
+   以及**实测绕工具 Z 转了多少度**。实测与目标差 >5° 会中止 —— 因为后面所有
+   "转 XX 度"都建立在这条假设上，假设不成立时继续跑只会拧坏东西。
+4. **最后调力阈值**：太灵敏（刚靠近就停）→ 调大 `contact_tangential_delta`；
+   不灵敏（压很深才停）→ 调小。
+
+配置读错会怎样：拼错键名 → **直接报错并给最相近的候选名**；单位/类型不对 →
+报"哪一行、期望什么、现在什么"；交叉矛盾（如 `contact_safe_delta ≤ contact_tangential_delta`）
+→ 启动就报。**不会静默退回默认值。**
+
+---
+
+## 6. 还没做 / 未验证（照实说）
+
+| 项 | 状态 |
+|----|------|
+| S02 夹持确认 | 预留（`grip_check: false`）。现在没有"规确实在爪里"的**可信**判据，靠触觉猜会误判 |
+| S03 手外粗定位 | 需要 eye-to-hand 标定文件；没有就明确报缺哪个文件、怎么生成，并回 S04 |
+| S07 接触判据 | 阈值**未在真机上标定**（仿真里做过通路验证） |
+| S08/S09 力矩到位判据 | 同上，属现场标定项 |
+| 真机全流程 | **未跑过**（本仓库当前无真机）。干跑 / 仿真 / 单测已过，见 §7 |
+
+---
+
+## 7. 验收命令（都是真跑过的）
+
+```bash
+# 1) 纯逻辑层（不需要机器人）
+PYTHONPATH=src/arm_application python3 -m pytest src/arm_application/test -q
+#    仿真环境：349 passed, 14 skipped（缺 onnx 的用例跳过，不伪装成失败）
+
+# 2) 干跑全流程：S00→S01→…→S11→DONE，脚本退出码 0，结论 ok=true
+bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim --dry-run
+
+# 3) 仿真里不干跑（缺 /home / 相机 / 触觉 / 夹爪状态）：S00 拦住，
+#    脚本退出码 1，结论 ok=false、failures 逐条列出"缺了会怎样"，不发任何运动指令
+bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim
+```
+
+> 退出码是**脚本**给的：脚本读节点落盘的结论文件（`ros2 launch` 自己的退出码
+> 恒为 0，见 R6.9）。
+
+---
+
+## 8. 踩过的坑（都是真发生过的，别再犯）
+
+1. **四元数换算被抄成了四份，而且已经互相抄歪了。**
+   `pipeline/stage2/stage3/visual_servo` 四个 launch 各有一份 `_quat_from_R`，
+   退化分支（旋转 180°/90°）的公式各不相同 —— 意味着不同 launch 发布的同一个
+   手眼 TF 可能是**不同**的。现在全部收口到 `handeye_lib.read_transform_yaml`，
+   并加了回环测试 + "launch 里不许再自己实现四元数"的守卫测试。
+2. **两个环境的"生效方式"不一样，"改了没生效"是幽灵问题。**
+   真机 `build.sh` 用 `--symlink-install`（软链，改源码立即生效）；
+   仿真 `build_sim.sh` **有意不用**它（仿真 conda 的 setuptools 旧，不认 `--editable`），
+   所以 `install_sim/` 里是**拷贝** —— **改完源码必须重编，否则跑的还是旧代码**，
+   而且不会有任何提示。判据一句话：
+
+   ```bash
+   diff src/arm_application/arm_application/<改过的文件> \
+        install_sim/arm_application/lib/python3*/site-packages/arm_application/<同名文件>
+   ```
+
+   不一致就 `bash scripts/build_sim.sh`（只重编 `arm_application` 更快：
+   `bash scripts/build_sim.sh` 之后看日志里 `Finished <<< arm_application`）。
+3. **`os._exit()` 会把异常吞掉。** 节点入口一开始用 `os._exit` 退出，导致
+   "进程退出码 1、日志里什么都没有"。现在 `main()` 里 `traceback.print_exc()` 先打印。
+4. **action 的"期望类型"必须写流程真正会调的类型。**
+   S00 曾把 `arm_jtc` 的期望类型写成 `arm_msgs/FollowJointTrajectory`，
+   而本流程直接走 `ros2_control` 的 JTC（`control_msgs/FollowJointTrajectory`），
+   于是自检永远说"类型不对"。自检的期望类型和代码里 `ActionClient` 用的是同一个常量。
+5. **测试红一片时，人就不看测试了。** 仿真环境没装 `onnx`（只有真机 conda 有），
+   14 个用例常年报红。现在缺依赖的用例 `skip`（`importorskip`），
+   让"红"只表示真出问题。
+
+---
+
+## 9. 相关文档
+
+- `config/site.yaml` → `gauge_assembly:` 段：**每个参数的注释与取值建议**
+- [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md)：改代码前先读（本文件第 8 节已进反例表）
+- [`GRIPPER.md`](GRIPPER.md)、[`HANDEYE.md`](HANDEYE.md)、[`TACTILE.md`](TACTILE.md)、[`HOLE_DETECT.md`](HOLE_DETECT.md)
+- [`HANDBOOK.md`](HANDBOOK.md)：新手从这读起