orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
36a59c81bf107e4c2a8cc329483524fa59dc3005
ros_contorl/docs/GAUGE_ASSEMBLY.md
T
arm_dev bda2fa7129 通止规装配：配置段进 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

224 lines
14 KiB
Markdown
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
# 通止规装配（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)：新手从这读起
Reference in New Issue View Git Blame Copy Permalink