ros_contorl/docs/HANDBOOK.md

# 新手手册 —— 从「这是干什么的」到「我能自己改」

> **这份文档记录什么事实**
> 它不是 API 参考，也不是环境安装说明书。它回答三个问题：
> 1. 这台机器到底在干什么活（业务），代码里哪一段对应哪一步；
> 2. 我要改一个东西时，**改哪个文件、用哪条命令验证**；
> 3. 出问题时，**先看哪份文档**（不重复写，只给索引）。
>
> **怎么读**：第 1 节读完你才知道后面在讲什么；第 3 节的决策表是日常最高频的一页；
> 第 5 节是排障入口。细节一律外链到权威文档，避免两份说法不一致（见
> [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 的 M2）。
>
> **真值来源**：本文所有结论都能用命令复现；凡本文说得含糊的，以
> `config/site.yaml`、各 `devices/*/manifest.yaml` 和源码为准（R6.1）。

---

## 1. 这台机器在干什么

### 1.1 一句话

把一支**螺纹通止规**自动送进工件上的螺纹孔，旋入到指定深度，用"能不能旋进去、
旋入过程中的力怎么变"来判断这个孔的螺纹加工是否合格。

"通止规"是机械行业检验螺纹的标准量具：**通端**应当能顺利旋到底，
**止端**应该旋不进去（或只能进很少几圈）。人来测就是拿规往里旋、靠手感判断；
这个项目要做的是**让机械臂拿着规自己旋，并把判断依据（力）记下来**。

### 1.2 为什么要自动化

人手旋规的判断依赖老师傅手感，无法留证据、无法 24 小时连续测。
换成机械臂 + 视触觉传感器之后，每一次旋入都能留下力曲线，
"合格 / 不合格"就有了可追溯的数据依据。

### 1.3 一次完整任务分几步

代码里用 **stage1 / stage2 / stage3** 三段串起来，一条命令跑完：

```
run_all.sh
  │
  ├─ stage1_track    视觉跟踪：D405 看住工件，法兰维持悬停高度跟着它走，
  │                  直到"锁定"（连续 N 帧稳定）→ 停跟踪、钉住当前关节
  │
  ├─ stage2_locate   拍照对孔：垂直朝下、降到拍摄高度、拍一张照，
  │                  YOLO 找工件外框 → Hough 找左右两个孔心 →
  │                  深度 + 内参 + 手眼 → 换算出孔的基座坐标 →
  │                  移动夹爪轴线到目标孔正上方 → 复拍验证 → 锁定
  │
  └─ stage3_search   柔顺搜孔：力闭环下的螺旋搜索，直到规的尖端"掉进"孔里，
                     再十字微扫定心，最后在力限内下压（可选按螺距旋入）
```

三段之间**在同一个进程里交接**，所以相机、触觉、手眼 TF 只初始化一次。
盯日志就能知道跑到哪了 —— 三行关键输出：

```
[pipeline] === stage1_track ===      → 在等跟踪锁定
[pipeline] === stage2_locate ===     → 在拍照对孔
[pipeline] === stage3_search ===     → 触面 → ★ 入孔 → 轻压到位
```

日常入口（三个终端，**必须同一个 `--arm`**）：

```bash
bash ~/ros2/scripts/run_real.sh --arm 89                    # T1 控制栈，一直开着
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview   # T2 相机预览，一直开着
bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（默认不拧入）
bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
```

一阶段默认 **D405 实时推理**，D435 待命。YOLO-pose 这一路不关。连续
`D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
这一段运动是厂家 `MoveL`：脚本先停真机控制栈（感知不停），XY 对准该点，
工具尖高度保持当前，再冷启动 `run_real.sh --no-stop`。冷启动回来之后才开始计
`servo.d435_handback_sec`（3 s）。赶路和杀栈期间不下发 D405。
停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
启动高度用 `--init-z`（法兰 Z，米），对孔悬停用 `--hover-z`。两个都是法兰高度。
会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。
框心对准连续 `servo.wrist_align_sec`（0.5 s）→ `/visual_servo/locked`。
二阶段粗居中工件框，再多拍 k1/k2 闭环对准，轻降后在观测高度补一次 XY。
孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
`tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。

```bash
bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera --d435-only
```

```bash
bash ~/ros2/scripts/run_real.sh --arm 89
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_camera_d435.sh --preview            # 混合跟踪必须两台都开
bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89         # 先只看框
bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
```

`track_camera: overhead` 仍是整段只用 D435 的实验开关。非法值启动即报错。

细节、参数、退出码见 [`HOLE_DETECT.md`](HOLE_DETECT.md)。

### 1.4 现在到哪一步了（如实标注，别信印象）

| 能力 | 状态 | 怎么自己确认 |
|------|------|--------------|
| 三段串联（跟踪 → 对孔 → 搜孔轻压） | ✅ 真机跑通 | `run_all.sh --arm 89` |
| 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
| Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
| Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
| D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
| 自动启动全部服务（一条命令拉起所有进程） | ❌ 还没做，目前靠三个终端手工起 | 见第 6 节待办 |
| 「规有没有被夹住」自检 | ❌ 还没做 | 见第 6 节待办 |
| 装配合格判定（通/止） | ⚠️ **通止规装配流程**能按力矩判据拧到底/拧出来，但"通/止合格"结论还没做 | `run_gauge_assembly.sh --arm 89 --dry-run` |
| 通止规装配流程（S00–S11 状态机） | ⚠️ **干跑与单测通过，真机未跑**（无真机）。详见 [`GAUGE_ASSEMBLY.md`](GAUGE_ASSEMBLY.md) | 同上（干跑不动臂） |
| Gazebo 仿真 | ✅ 场景、相机、触觉、夹爪可跑 | [`devices/sim/gazebo/README.md`](../devices/sim/gazebo/README.md) |

> 这张表是**承诺书**：❌ 的别当成已具备。文档说"已就绪"而实际没有，
> 是本仓库最忌讳的那类问题（R6.1）。

---

## 2. 系统长什么样

### 2.1 三层，各有硬边界

```
┌─ 现场数值层 ─────────────────────────────────────────────┐
│  config/site.yaml          唯一权威：SN / IP / 串口 / 阈值 / 锁高 │
│  devices/registry.yaml     只用哪个品牌的驱动（不含任何数值）      │
└──────────────────────────────────────────────────────────┘
                          ↓ 启动时注入
┌─ 设备模块层 ─────────────────────────────────────────────┐
│  devices/<类别>/<品牌>/     manifest.yaml + driver/ + 厂商物料   │
│  换品牌 = 加一个目录 + 改 registry 一行，**不改应用代码**         │
└──────────────────────────────────────────────────────────┘
                          ↓ 框架接口（src/arm_interfaces 定义的契约）
┌─ 应用层 ─────────────────────────────────────────────────┐
│  src/arm_application/    感知 / 伺服 / 手眼 / 夹爪 / 入孔 / 触觉  │
│  src/arm_control/        命名位姿、状态、轨迹适配                 │
│  src/arm_bringup/        launch 总装（real | mock | sim）        │
└──────────────────────────────────────────────────────────┘
```

**唯一允许的配置入口是 `config/site.yaml`。** 包内 `config/*.yaml` 只是缺省值，
改了会被 site 覆盖 —— 这是新手最常踩的一个坑（见 3.2）。

两块硬资源是**独占**的，同时开会互相咬：

| 资源 | 谁占着 | 冲突表现 |
|------|--------|----------|
| RDK（机器人控制接口） | `run_real.sh`、`run_handeye.sh teach` | 后启动的连不上 / 报错 |
| D405 USB | 相机节点、示教、标定 | 后启动的拿不到图 |

所以 `run_handeye.sh` 会**先停掉旧相机**；`go_home.sh` 会**先杀掉 perception/servo**
再回零，否则两条轨迹抢同一个臂。

### 2.2 从像素到关节：一条链路看懂全栈

```
D405        YOLO              手眼 T              伺服             ros2_control
出图   →   找工件框   →   像素+深度 →  相机系 3D →  法兰系  →   基座系  →  关节角
                                                              ↓
                                              /visual_servo/filtered_point
                                                        100 Hz
```

链路里每一步都有一个"口径"，写错任何一处都不会报错，只会让点**偏掉**：

| 口径 | 写错的表现 | 规矩 |
|------|-----------|------|
| 深度是米还是毫米 | 偏 1000 倍 | R2.3 |
| `frame_id` 是光学系还是相机本体系 | 点转到别处，但 `tf2` 查得到 | R2.2 |
| `rpy` 复合顺序 | 相机背对目标，只看得见地面 | R2.4 |
| 内参从哪来 | 偏半米量级，但单目预览完全正常 | R2.5 |
| 手眼 `T_4x4` 是哪台臂的 | 始终差几厘米，很难怀疑到标定值上 | 第 1 节末尾 |

这些坑**全部**在 [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 第 8 节有症状表。

### 2.3 三条设计取舍（理解了才不会误改）

1. **仿真与真机共用同一套参数。** 所以你在仿真里调的悬停高、阈值，搬到真机直接生效；
   代价是仿真里不能有"仿真专用参数"，只有物理引擎自己的参数（重力、步长）放 `sim.*`。
2. **设备模块里不许出现现场数值。** 数值只有 `site.yaml` 一个来源。
   这样换一台臂就是改 `site.yaml`，而不是满仓库找硬编码。
3. **校验必须能真的失败。** 故意弄坏它、检查会红，才算校验；
   不然就是"把没检查伪装成检查通过"（M1）。

---

## 3. 我要改东西，改哪里

### 3.1 ★ 决策表（先查这张，再动手）

| 我想改… | 改这里 | 验证命令 | 细节 |
|---------|--------|----------|------|
| 换机械臂 / 换臂号 | `site.yaml` → `robot.sn`，或所有终端加同一 `--arm` | `scripts/show_site_config.sh` | [`SITE_CONFIG.md`](SITE_CONFIG.md) |
| 悬停高 / 锁高 / 伺服参数 | `site.yaml` → `robots[].servo.*` | 重启第三终端即可 | [`TRACKING.md`](TRACKING.md) |
| 夹爪速度 / 力 / 行程 | `site.yaml` → `gripper:` | `run_gripper.sh --arm 89 --cli open` | [`GRIPPER.md`](GRIPPER.md) |
| 入孔检测算法参数 | `site.yaml` → `hole_detect:` | `run_hole_detect.sh --arm 89 --preview` | [`HOLE_DETECT.md`](HOLE_DETECT.md) |
| 触觉灵敏度 | `site.yaml` → `hole_detect.stage3:` 的 `*_delta` | 看启动日志打印的派生阈值 | [`TACTILE.md`](TACTILE.md) |
| 相机曝光 / 图幅 / 视场 | `site.yaml` → `cameras:`（真机） | `list_realsense.sh` | [`SITE_CONFIG.md`](SITE_CONFIG.md) |
| 仿真场景里放什么、放哪 | `devices/sim/gazebo/config/scene.yaml` | `python3 scripts/check_sim_scene.py` | [仿真手册 §4.1](../devices/sim/gazebo/README.md) |
| 仿真里开合夹爪（开口 mm） | 服务 `/gripper/{open,close,move}`，与真机同一套 | `python3 scripts/check_sim_gripper.py` | [仿真手册 §4.4](../devices/sim/gazebo/README.md) |
| 仿真相机视场/图幅 | 仿真手册 §4.3，**三处要同时改** | `python3 scripts/check_sim_cameras.py` | 同上 §4.3 |
| 算法权重（换模型） | `models/<分组>/` + `site.yaml` 的路径 | `scripts/weights.sh list` | [`models/README.md`](../models/README.md) |
| 接一个新品牌设备 | `devices/<类别>/<品牌>/` + `registry.yaml` 一行 | `ros2 run arm_interfaces device_check` | [`devices/README.md`](../devices/README.md) |
| 命名位姿（回零点等） | `src/arm_control/config/named_poses_89.yaml` / `_91.yaml` | `go_home.sh --arm 89` | [`INTERFACES.md`](INTERFACES.md) |
| 通止规装配的参数（预绕角/力阈值/孔数） | `site.yaml` → `gauge_assembly:`（每个键都有注释） | `run_gauge_assembly.sh --arm 89 --dry-run` | [`GAUGE_ASSEMBLY.md`](GAUGE_ASSEMBLY.md) |
| 框架规矩本身 | `docs/FRAMEWORK_RULES.md`，**同时**改 `.cursor/rules/` | `python3 scripts/check_text_only.py` | R6.1 |

### 3.2 改一个现场数值（最高频）

**唯一入口是 `config/site.yaml`。** 不要改 `src/arm_application/config/*.yaml` ——
那只是缺省值，会被 site 覆盖，改了不生效**而且不报错**（典型静默失效）。

```bash
cp config/site.example.yaml config/site.yaml   # 只做一次（site.yaml 不进 git）
vim config/site.yaml
bash scripts/show_site_config.sh               # 看解析结果对不对
# 重启对应终端
```

生效范围不一样，别白改：

| 改了什么 | 要重启什么 |
|----------|-----------|
| 锁高 / YOLO / 伺服参数 | 跑伺服的那一个终端 |
| 相机分辨率 / 曝光 | 相机那个终端 |
| 夹爪参数 | 夹爪进程 |
| 设备选型（`registry.yaml`） | 对应的启动脚本 |

### 3.3 加一个场景物体（仿真）

三步，**只改一处配置文件**：

1. 在 `devices/sim/gazebo/models/<物体名>/` 下建 `model.sdf` + `model.config`
   （建模要求见[仿真手册 §2](../devices/sim/gazebo/README.md)：原点必须落在**底面中心**，
   碰撞体用简单几何，质量与惯性别省）。
2. 在 `devices/sim/gazebo/config/scene.yaml` 的 `objects:` 里加一条（位姿、是否 static）。
3. 校验：

```bash
source scripts/sim_env.sh
python3 scripts/check_sim_scene.py     # 悬空 / 穿模 / 出界 / 互相嵌入 / 像素几何
```

### 3.4 在仿真里开合夹爪

服务名与真机一致，所以脚本两边通用：

```bash
source scripts/sim_env.sh
ros2 service call /gripper/move arm_msgs/srv/GripperExecute \
    "{command: {command: move, position: 20.0}}"   # 开口 20 mm（真的 20 mm）
ros2 topic echo /gripper/status --once             # 开口 / 是否夹住 / 是否在动
python3 scripts/check_sim_gripper.py               # ★ 验收：拿两指尖的实际间距说话
```

`position` 是**开口毫米**，仿真里由 URDF 正运动学反解保证是真的毫米
（上游的线性 mimic 近似会让"开 20 mm"变成 21.75 mm，抓不住工件 ——
见[仿真手册 §4.4](../devices/sim/gazebo/README.md)）。

如果启动日志说行程是 **99.93 mm**：那是对的，是实测值，不是名义的 100.0。

### 3.5 加一个算法权重

放进去 → 打标签 → 改路径 → 体检。TensorRT 转换也在这条链上：

```bash
bash scripts/weights.sh convert <文件> --name <模型名>   # PT→ONNX→TensorRT，自动改名归位
bash scripts/weights.sh labels <模型名> --set "工件,通止规"
bash scripts/weights.sh doctor --check-engines          # 查版本不匹配这类静默失效
```

布局是 `models/<分组>/<模型名>.{pt,onnx,engine}` + 旁挂 `<模型名>.labels.yaml`。
**二进制不进 git**，标签文件进。规则见 [`models/README.md`](../models/README.md)。

### 3.6 接一个新品牌设备

四步（完整说明在 [`devices/README.md`](../devices/README.md)，这里只给顺序）：

1. 把厂商物料丢进 `devices/<类别>/<品牌>/`（格式与命名按该类别 `README.md`，**必须照做**）。
2. 复制同类别的 `manifest.yaml`，改 `id` / `label` / `driver.package` / `config_schema`。
3. 在 `driver/` 下实现框架接口契约（见 `src/arm_interfaces/`）。
   **这一步躲不掉** —— 厂商给的是 C++/Python 库而不是 ROS 2 驱动时，
   外面永远需要一层薄适配器；框架能做的是把它写到最薄（只翻译 API，不含业务逻辑）。
4. 改 `devices/registry.yaml` 一行。

验：

```bash
ros2 run arm_interfaces device_check                    # 契约校验
ros2 run arm_interfaces device_check --list-brands      # 看每类现在有哪些可选
```

> 校验会读 `registry.yaml` → 各 `manifest.yaml` → `site.yaml`，检查三件事：
> 选中的品牌目录在不在、`assets` 里声明为必需的物料文件在不在、
> `required: true` 的配置键在 `site.yaml` 里有没有值。
> 任何一项不满足**立即报错并指出该去哪个文件补什么**，不静默降级。

### 3.7 改相机分辨率（最容易漏改）

真机改 `site.yaml` 就行。**仿真要改三处**，因为像素门槛是绑在图幅上的 ——
改完不算清楚，孔检测在仿真里会"根本不可能通过"，而看起来像算法不行。
详见[仿真手册 §4.3](../devices/sim/gazebo/README.md)。

### 3.7.1 换一台机械臂（最容易漏改，而且很难查）

`site.yaml` 里写的是**哪台臂**，但有一批一级配置文件里写死了**臂的关节名**
（Flexiv 官方把 SN 编进关节名：`Rizon4-063989_joint1`）。这些文件必须跟着改：

```bash
python3 scripts/check_arm_prefixes.py          # 逐条列出哪个文件哪一行对不上，并给修法
python3 scripts/check_arm_prefixes.py --list   # 只看当前臂的应然前缀
```

漏改的症状有两种，**都不像"配置错了"**：

- 控制器报 `Joint 'Rizon4-063989_joint1' not found`（这条还算好查）；
- URDF 换了名字、`srdf/arm.srdf` 没换 → MoveIt **规划成功但机械臂不动**。

换臂四步（少一步就出上面那种症状）：

```bash
vim config/site.yaml                # 1. robot.sn + robot.robots[]
source scripts/env.sh --arm 新臂    # 2. 选定当前臂
python3 scripts/check_arm_prefixes.py   # 3. 必须输出 OK
bash scripts/build.sh               # 4. 让 install/ 同步
```

哪些文件该跟着改、哪些是"按臂各留一份"的现场资产，
见 [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 的 R1.1.1 表。

> 排查提示：`ROBOT_SN` / `ROBOT_ARM` 环境变量会**覆盖** `site.yaml` 的 `robot.sn`。
> "明明改了配置却没生效"时先 `echo $ROBOT_SN`，或看 `--list` 打印的"来源"。

### 3.8 改完必须做的一件事

**问自己：我故意弄坏它，哪个检查会红？**

如果答不上来，说明这个改动没有任何东西在守着 —— 出问题只会以"结果是错的"形式出现。
这条写在 [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 第 7 节的改动前检查清单里，
逐条过一遍只要两分钟。

另外：**只改注释/文档**时，跑 `python3 scripts/check_text_only.py` 自证没碰逻辑（R6.5）。

---

## 4. 怎么验

### 4.1 三层，只有后两层算验收

| 层 | 判据 | 够不够 |
|----|------|--------|
| 1 | 不报错、有话题、TF 查得到 | ❌ **不算验收**（R4.9） |
| 2 | 物理量落在已知真值上（深度反投影命中已知平面、内参与公式吻合） | ✅ 模块级 |
| 3 | 任务真的成功（跑出结果、力曲线符合预期） | ✅ 端到端 |

第 1 层是陷阱：仿真里相机背对目标**照样出图**、`create` 打印成功但模型**没进 Gazebo**、
夹爪控制器 `active` 但手指**不动**。这些全都不报错。

### 4.2 命令清单

```bash
# —— 环境 ——
source scripts/env.sh                                # 真机环境
source scripts/sim_env.sh                            # 仿真环境（独立 conda）

# —— 设备与配置 ——
bash scripts/run_real_preflight.sh --arm 89          # 真机开工体检（只读，含 RDK/相机/USB/几何）
ros2 run arm_interfaces device_check                 # 设备契约 + site.yaml 必填项
bash scripts/show_site_config.sh                     # site.yaml 解析结果
python3 scripts/check_arm_prefixes.py                # 含臂名的配置是否跟着当前臂（换臂必跑）

# —— 仿真验收（★ 必做，别靠肉眼）——
python3 scripts/check_sim_cameras.py                 # 深度反投影命中已知平面才算过
python3 scripts/check_sim_scene.py                   # 摆放 / 像素门槛漂移 / 成像几何 / 可见性
python3 scripts/check_sim_gripper.py                 # 夹爪：命令开口 mm == 两指实际张开 mm

# —— 只改文字时自证 ——
python3 scripts/check_text_only.py

# —— 权重体检 ——
bash scripts/weights.sh doctor --check-engines
```

### 4.3 单元测试怎么跑

**每个功能包都有自己的测试**，改完对应包就跑一下。当前 pytest 收集 **1301 项**
（另有 1 项按环境 skip）。

```bash
source scripts/env.sh                                # 或 sim_env.sh

bash scripts/run_unit_tests.sh                       # 全部
bash scripts/run_unit_tests.sh src/arm_kinematics/test          # 只跑一个包
bash scripts/run_unit_tests.sh src/arm_application/test/test_real_preflight.py
bash scripts/run_unit_tests.sh -k gauge              # 只跑名字带 gauge 的
```

> 用这个脚本，别直接 `python3 -m pytest src/xxx/test`。脚本会把 `src/*` 全部
> 挂到 `PYTHONPATH` 最前面，两个原因：
>
> 1. 测试按**包名**导入，少挂一个包会在收集阶段 `ModuleNotFoundError` ——
>    现象是"0 个测试 / 1 个 error"，很容易被读成测试环境坏了。
> 2. 更隐蔽：`install/` 里那份**旧的**同名模块会先被找到（它在 `PYTHONPATH` 上）。
>    实测 `src/arm_sim_gazebo/test` 直接跑会报
>    `cannot import name 'RESET_GIVE_UP'` —— 导的是 `install/` 里的旧副本，
>    源码里明明有这个符号。挂上 `src/` 之后才测的是你改的那份。

按包统计（改动前后对比数量是最省事的回归判据）：

| 包 | 测试数 | 覆盖什么 |
|----|--------|----------|
| `arm_application` | 547 | 纯逻辑：滤波器、解析器、几何、参数合并、装配状态机 |
| `arm_sim_gazebo` | 400 | 场景校验、URDF 归一化、相机内参、触觉适配 |
| `arm_interfaces` | 78 | 路径解析、清单校验、配置守卫 |
| `arm_driver_flexiv` | 69 | 驱动适配、下压控制、超时 |
| `arm_kinematics` | 24 | 正逆运动学 |

> `src/flexiv_rdk/test` 需要厂商的 `flexivrdk` Python 包，仿真环境里没有，
> 跑不过属正常，与我们的改动无关。

### 4.4 验收工具自己也会失效

这是本仓库最贵的教训，单独写一节：

- 校验代码可能**永远通过** —— 因为 `sdformat` 会用 `__default__` /
  `__default_topic__` 这种**假值**把缺失字段填上，"检查是否为空"的代码就查不出来（M1）。
- 解析外部输出可能**格式对不上**，然后代码"优雅降级成跳过检查" ——
  于是"没检查"被伪装成"通过"（R5.3、R5.4）。
- `bash -n` 说脚本语法 OK，脚本却**跑不起来** —— CRLF 行尾会漏报，
  只有真跑 `--help` 才暴露（R6.6）。
- 验收脚本自己**算错了单位** —— `check_sim_gripper.py` 把 TF 的"米"当成"毫米"
  去比阈值，于是 127 mm 被显示成 0.13 mm，**恰好看起来像夹爪没张开**，
  让人跑去查夹爪代码（R4.12）。
- 上一轮的**残留进程**伪造出"代码写错了"的现场 —— 两个同名适配器抢话题/服务，
  残留的 `static_transform_publisher` 把旧外参按在新仿真上，读数全是乱的（R4.11）。
- **同一份数学写了四份**（四元数换算），退化角度的公式各不相同 ——
  没有哪一份"看起来坏"，只是不同 launch 发出来的手眼 TF 不一样（R5.8）。
- 测试长期十几条红，于是**没人再看测试结果** —— 真出问题时也看不出来（R5.10）。

**所以**：

1. 写完校验，先故意弄坏被校验的对象，确认检查真的会红（负向测试和正向测试
   一样重要，R5.2）；
2. 判据要挑**与被测实现无关的物理量**，单位只在一处换算；
3. 结论不对时，先怀疑**判据和环境**，再怀疑被测对象 —— 上面后两条都是
   "先怀疑了代码，结果是自己错"（R4.12）。

---

## 5. 出问题按现象查

### 5.1 三个权威处，按"你在问什么"分工

| 你在问 | 去哪 | 为什么 |
|--------|------|--------|
| 「这个现象可能是什么原因」 | [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) **第 8 节** | 按**症状**排序的反例表，一条一个真实原因 |
| 「这是 Gazebo 的问题」 | [仿真手册 §5](../devices/sim/gazebo/README.md) | 11 条仿真专属坑，每条写了"怎么发现" |
| 「这个脚本/话题/参数是干嘛的」 | [`WORKSPACE.md`](WORKSPACE.md) / [`INTERFACES.md`](INTERFACES.md) | 目录、脚本、话题、参数契约 |
| 「手眼/跟踪/夹爪/入孔/触觉怎么用」 | 各自那份 `docs/*.md` | 见下节对应表 |

**先查症状表，再动手改代码。** 这个仓库的坑绝大多数"看着像新问题，其实是老问题复发"。

### 5.2 常见现象 → 先想到什么

| 现象 | 先怀疑 | 详细 |
|------|--------|------|
| 跟踪一直差几厘米 | 用了**别的臂**的手眼文件，或没重新标定 | [`HANDEYE.md`](HANDEYE.md) |
| 图像有、但点算错 | 深度单位 / `frame_id` / 内参来源 | 第 2.2 节、R2.2–R2.5 |
| 模型"根本没进 Gazebo"，但 `create` 打印成功 | world 里声明了插件，挤掉了默认的 UserCommands | 仿真手册坑 2 |
| 物体一 spawn 就消失 | 上游 URDF 缺 `<inertial>` / 有零质量 link | 仿真手册坑 1 |
| 夹爪控制器 active 但手指不动 | `<mimic>` 被 `sdformat` 丢了 | 仿真手册坑 10 |
| 触觉"离开"之后接触标志永远是 `True` | contact 是**事件式**消息，离开时不发 | 仿真手册坑 6 |
| 消息定义里有 `wrenches` 字段，运行时却永远是空的 | Fortress 的 contact-system 根本不填力 | 仿真手册坑 7 |
| 触觉"有力、会响应、松开归零"但语义全错 | 轴向映射是猜的，没实测 | R3.3 |
| 明明夹住了，`fnormal` 恒为 0 | 基线把真实接触吃掉了 | R3.1 |
| 校验代码永远通过 | `sdformat` 用 `__default__` / `__default_topic__` 假值填了缺失字段 | 仿真手册坑 9 |
| 脚本报 `$'\r': 未找到命令` | 脚本是 CRLF 行尾 | R6.6 |
| 改了参数不生效 | 改的是包内 yaml，被 `site.yaml` 覆盖了 | 3.2 |
| MoveIt 说规划成功、机械臂不动 | SRDF / controllers 里还是**旧臂**的关节名 | 3.7.1、R1.1.1 |
| 控制器报 `Joint 'Rizon4-xxxxxx_joint1' not found` | 换了臂但一级配置没跟着改（或改了两处漏一处） | 3.7.1 |
| 仿真镜头位置不对、只看得见地面 | 相机 `rpy` 复合顺序 | 仿真手册坑 5 |
| 脚本说"已停止"，进程还在 | 进程模式表漏项 | R4.10 |
| 孔检测在仿真里怎么都不过 | 图幅与真机不同，像素门槛失效 | 仿真手册 §4.3 |
| `site.yaml` 改了参数，行为一点没变 | 那个键**没接线**（配置表里有、代码里从没读） | R1.2 |
| 流程退出码 1，日志里一个字都没有 | 入口 `os._exit()` 把异常吞了 | R6.9 |
| 同一个手眼 TF，换个脚本跑出来不一样 | 四元数换算被抄了多份 | R5.8 |
| 通止规流程 S00 就停住、说不缺的东西缺了 | 自检的"期望类型"与流程实际调用的类型不一致 | R5.9 |

---

## 6. 术语表与待办

### 6.1 术语

| 词 | 意思 |
|----|------|
| **通止规** | 检验螺纹的量具。通端应能旋到底，止端应旋不进（或只进几圈） |
| **89 / 91** | 左臂 / 右臂的现场编号，**各有独立的手眼、相机、夹爪、锁高，禁止混用** |
| **法兰** | 机械臂末端安装面；手眼 `T_4x4` 仍是相机→法兰。左臂 89 现行按 Studio `tool10120938` 的 TCP 求解，再折回法兰 |
| **eye-in-hand / eye-to-hand** | 相机装在手上 / 装在手上之外（D405 在手上，D435 在外部） |
| **stage1/2/3** | 跟踪 / 对孔 / 搜孔，见 1.3 |
| **S03 / S04 / S07 / S08** | 整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，**没有一一对应**，别硬套 |
| **基线** | 触觉静止时的读数均值。力的语义是"相对基线的增量"，不是绝对牛顿值 |
| **R1.1 / M1 …** | [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 里的规则编号，文档和提交信息里直接引用 |
| **静默失效** | 不报错但结果是错的。本仓库的头号敌人，见 M1 |

### 6.2 还没做的事（别按已具备来用）

按优先级：

1. **自动启动全部服务** —— 目前靠三个终端手工起，容易漏、
   且顺序错了会出"控制器没起来但脚本继续跑"这类问题。
2. **夹爪持规自检** —— 启动后先用触觉判断"规真的被夹着"，没有就报错停机
   （现在没有这一步，规掉了会一路跑到撞工件才发现）。
3. **D435 手眼标定（91）** —— 89 按 `tool10120938` 重标（旧 10-09 / 18 视角在 `handeye/results/backup/`）。91 还没标。S03 粗定位要用这份 TF 在真机上对一次工件才算链路验过。
4. **装配合格判定** —— 把力曲线转成"通/止"结论。当前只输出原始几何与力，
   判定逻辑（含螺纹旋入圈数）还没写。

### 6.3 怎么往下读

```
你刚来        → 第 1、2 节，然后照着 1.3 的三个终端跑一次
要动手改      → 第 3 节决策表
要交付        → 第 4 节验收 + FRAMEWORK_RULES 第 7 节检查清单
被 bug 咬了   → 第 5 节
要给 AI 下指令 → FRAMEWORK_RULES 第 9 节
```