通止规装配:配置段进 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>
This commit is contained in:
arm_dev
2026-09-24 10:30:11 +08:00
parent dfc945b7e0
commit bda2fa7129
16 changed files with 741 additions and 37 deletions
+12
View File
@@ -67,6 +67,18 @@ python3 scripts/check_text_only.py
它比对语法树(删 docstring + 抹平字符串常量 + f-string 只留插值), 它比对语法树(删 docstring + 抹平字符串常量 + f-string 只留插值),
不一致就报出来。基准是 `git HEAD`,所以要在提交前跑。 不一致就报出来。基准是 `git HEAD`,所以要在提交前跑。
## 三个"看着不像坑"的坑(R5.8–R5.10、R6.9)
- **同一份数学/换算只许一份**,并写一条**守卫测试**扫描调用方禁止私有实现(四元数
曾被抄成四份且退化角公式各不相同)。测试要覆盖**退化分支**(180°/270°),不只测一般值。
- **自检里的"期望类型/话题名"必须与调用处同源**,不许两处各写一遍字面量 ——
否则自检会永远报"类型不对",而人只会去怀疑服务本身。
- **硬退出前先 `traceback.print_exc()`**(`os._exit` 会把"退出码 1"变成"日志空白");
**环境缺依赖用 `importorskip`**,不许留下常年红 —— 长期红等于没人看测试结果。
- **`ros2 launch` 的退出码恒为 0**(子进程 exit 3,它照样返回 0)——判成败必须靠节点
**落盘**的结论文件,不许看 `$?`;"没有结论文件"要当失败。常驻节点(静态 TF 发布者)
还不会随主节点退出,launch 要显式挂 `OnProcessExit → Shutdown`。
## 出问题先查症状表 ## 出问题先查症状表
`docs/FRAMEWORK_RULES.md` 第 8 节按**症状**列出全部历史反例 `docs/FRAMEWORK_RULES.md` 第 8 节按**症状**列出全部历史反例
+1
View File
@@ -54,6 +54,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
| 入孔检测 | `scripts/run_hole_detect.sh --arm … --preview` | | 入孔检测 | `scripts/run_hole_detect.sh --arm … --preview` |
| Orisys 触觉 | `scripts/run_tactile.sh [--cli]` | | Orisys 触觉 | `scripts/run_tactile.sh [--cli]` |
| Gazebo 仿真 | `scripts/run_sim_gz.sh [--gui]`(停:`scripts/stop_sim_gz.sh`) | | Gazebo 仿真 | `scripts/run_sim_gz.sh [--gui]`(停:`scripts/stop_sim_gz.sh`) |
| 通止规装配 | `scripts/run_gauge_assembly.sh --arm … [--dry-run]` |
| 看配置 | `scripts/show_site_config.sh` | | 看配置 | `scripts/show_site_config.sh` |
89 与 91 **各有** 手眼、相机序列号、锁高、named poses,禁止混用。 89 与 91 **各有** 手眼、相机序列号、锁高、named poses,禁止混用。
+87
View File
@@ -450,6 +450,93 @@ hole_detect:
# Orisys 视触觉(夹爪左右指摄像机,不是腕部 D405)。SDK 单独 pip 安装。 # Orisys 视触觉(夹爪左右指摄像机,不是腕部 D405)。SDK 单独 pip 安装。
# bash ~/ros2/scripts/run_tactile.sh --cli # bash ~/ros2/scripts/run_tactile.sh --cli
# bash ~/ros2/scripts/run_tactile.sh # bash ~/ros2/scripts/run_tactile.sh
# =============================================================================
# 通止规装配(看板 S00–S11):bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89
#
# 这一段全是**会动的量**,也是现场最常改的地方。改完不必改代码:
# 解析在 arm_application/gauge_config.py(键名、单位、类型都在一张表里校验),
# 拼错键名会直接报错并给出最相近的候选名,不会静默退回默认值。
#
# 单位体现在键名后缀上(_deg/_mm/_m/_s/_pct/_delta),三张表逐条校验;
# 带 _delta 的是**力增量**(Orisys 读数是形变任意单位且基线会漂移,
# 绝对值没有意义),必须现场标定:
# bash ~/ros2/scripts/run_tactile.sh --cli # 看读数量级
# 启动时节点会打印本段**实际生效**的值(含派生结论),先核对再上电。
# =============================================================================
gauge_assembly:
# ---- 总开关 ----
holes_total: 2 # 一次任务装几个孔(k1/k2);--holes N 可临时覆盖
dry_run: false # true = 只验编排与自检,不发任何运动指令
# E1(掉件)重放时等人工确认 /gauge/reload_ok 的最长时间 [s]。超时=中止。
# 不是"再等等":臂停在 home,让人来处理比继续跑安全。
reload_timeout_s: 180.0
# S00 自检每项(相机/触觉/夹爪状态)等"第一帧真数据"的最长秒数。
# 只看话题名会假阳性(发布者不在也算在),所以必须等真数据。
boot_timeout_s: 30.0
# ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
# 预绕是为了给 S08 的棘轮留出回转行程:不预绕,拧两圈就贴关节限位。
extreme_angle_deg: 150.0 # 想转多少度(按工具系逆时针)
extreme_speed_deg_s: 20.0 # 关节角速度 [deg/s],慢一点防冲击
extreme_direction: 1 # +1=逆时针 / -1=顺时针(改符号,别去改代码)
extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg];撞限位不算失败(按 JOINT_LIMIT 继续)
# 但余量太小会让规划器频繁拒收,表现为"偶尔转不到位"
# ---- S02 夹持确认(预留,默认跳过)----
# 现在的触觉判据与 D435 手眼都还没就绪,先留接口:开成 true 时,
# 夹爪状态字必须明确报"夹住了",否则 S02 判失败并中止。
grip_check: false
grip_hold_s: 0.5 # 合爪后等多久再读状态 [s]
# ---- S03 手外粗定位(S04 跟踪的兜底:跟踪丢了才来)----
# 前提是手外相机做过 eye-to-hand 标定:handeye/results/<相机>_eye_to_hand.yaml。
# 没有那个文件时 S03 会明确报"缺哪个文件、怎么生成"并回 S04,不做假装成功的反投影。
coarse_enabled: true
coarse_camera: "d435" # 手外相机命名空间(话题 /d435/...)
coarse_hover_m: 0.25 # 粗定位后悬停在工件上方多高 [m](可调)
coarse_conf: 0.35 # 工件检测置信度阈值(0~1)
coarse_timeout_s: 20.0 # 等手外相机出图的最长秒数(相机刚起来要一两秒)
# ---- S05 精拍定位:等画面稳了再拍 ----
# 判据是"所有采样点到均值的距离 ≤ 容差",不是"相邻两帧差 ≤ 容差":
# 跟踪抖动是零均值的,相邻帧之差会在画面明明在晃时判成稳定,拍出糊图。
snap_stable_s: 1.0 # 连续观察多久 [s]
snap_tol_px: 2.0 # 允许的抖动半径 [px](越小越严,越难满)
snap_min_frames: 6 # 至少采到多少帧才敢说"稳定"
# ---- S06 正悬停(悬停高度不在这里:见 servo.hole_hover_z_m)----
align_tol_mm: 1.0 # 对位残差容差 [mm];超了回 S05 重拍(不是硬往下走)
# 孔口径 mm 级时别放大这个值:放大=直接蹭孔壁
# ---- S07 柔顺接触:步进下压,切向力突变即停 ----
contact_speed_m_s: 0.02 # 每步下压速度 [m/s];这一步必须慢,快了就是一次冲击
contact_step_mm: 0.5 # 每步下压量 [mm](越大越快,越小越不会过冲)
contact_max_depth_mm: 25.0 # 最多下探多少 [mm];到了还没接触判失败(不会一直压)
contact_tangential_delta: 300.0 # 接触判据:两路切向任一相对基线增量超过它 [任意单位]
contact_safe_delta: 2000.0 # 法向安全上限 [任意单位]:超了立刻抬起并中止(E2)
# 必须 > contact_tangential_delta,否则自检就报矛盾
# ---- S08 棘轮拧入(慢转 → 松爪 → 快回转 → 夹紧,循环)----
# 停止判据:切向力 > screw_shear_delta **且** 法向力突增 > screw_normal_delta。
screw_step_deg: 180.0 # 每次慢转多少度;要 ≤ 预绕角(否则第一轮就贴限位)
screw_speed_deg_s: 30.0 # 慢转速度 [deg/s]
screw_back_speed_deg_s: 120.0 # 松爪后的空转回转速度 [deg/s](爪里没东西,可以快)
screw_max_cycles: 12 # 最多几轮(12×180°=2160°,够 M6 螺纹规走完)
screw_normal_delta: 400.0 # 拧到位时的法向力突增 [任意单位](螺纹到底/顶住)
screw_shear_delta: 600.0 # 拧到位的切向力增量 [任意单位]
screw_grip_force_pct: 50.0 # 夹紧力百分比 [%](**只有契约后端生效**:仿真/新驱动)
screw_grip_speed_pct: 50.0 # 夹紧速度百分比 [%](同上;真机 dh_pgea 驱动不支持这两个参数)
screw_release_wait_s: 0.3 # 松爪后等多久再回转 [s](等爪真的张开,否则会带着规磨)
# ---- S09 反向拧出 ----
unscrew_extra_cycles: 1 # 比拧入多转几轮,确保螺纹完全脱开
# ---- 力判据怎么调(现场最常问)----
# 1) 先看量级:bash ~/ros2/scripts/run_tactile.sh --cli,手压一下看读数变化;
# 2) S07 的接触判据是"相对基线的增量",基线在 S06 悬停时(还没碰工件)测;
# 3) 太灵敏(刚靠近就停)→ 调大 contact_tangential_delta;
# 不灵敏(压很深才停)→ 调小;同时看启动日志里的基线/实测值再定。
tactile: tactile:
backend: "orisys" # orisys | mock backend: "orisys" # orisys | mock
# 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。 # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
+88
View File
@@ -405,6 +405,53 @@ ROS 构建对非 ASCII 路径会出各种问题。工作目录保持 ASCII(推
(`scripts/build_sim.sh`)。两套并存是刻意的:同一份源码,两个环境的依赖路径不同, (`scripts/build_sim.sh`)。两套并存是刻意的:同一份源码,两个环境的依赖路径不同,
共用 `install/` 会互相覆盖。 共用 `install/` 会互相覆盖。
**但两者的"生效方式"不一样,这一点比隔离本身更容易坑人**:
| 环境 | 构建 | 产物形态 | 改了源码之后 |
|------|------|----------|--------------|
| 真机 | `build.sh`(`--symlink-install`) | **软链** | 立即生效 |
| 仿真 | `build_sim.sh`(**有意不加** `--symlink-install`:仿真 conda 的 setuptools 旧,不认 `--editable`) | **拷贝** | **必须重新构建**,否则跑的是旧拷贝 |
所以"在仿真里验证一个改动"之前,先确认 `install_sim/` 里的那份和源码一致:
```bash
diff src/arm_application/arm_application/<文件> \
install_sim/arm_application/lib/python3*/site-packages/arm_application/<同名>
```
不一致时的现象是"我明明改了,行为却和没改一样",而且**没有任何提示** ——
最容易被误判成"我的改动没效果"。
### R5.8 同一份数学/解析实现只许有一份,并加**守卫测试**
四元数换算曾经在 `pipeline` / `stage2` / `stage3` / `visual_servo` 四个 launch 里
各写一份,退化分支(180°/90°)的公式**各不相同** —— 也就是"同一个手眼 TF,
不同 launch 发出来不是同一个",而四种都能"看起来正常"。
规矩:
- 换算/解析只留一处(本例:`handeye_lib.read_transform_yaml`);
- 加一条**守护测试**扫描调用方,禁止它们再出现私有实现:
见 `test_handeye_lib_roundtrip.py::test_launch_files_do_not_reimplement_the_quaternion_math`;
- 测试要覆盖**分支点**(本例用 180°/270° 这些会让公式退化的角度),而不是只测一般值。
### R5.9 自检/探头里的"期望类型"必须与调用处同源
S00 自检曾把 `arm_jtc` 的期望类型写成 `arm_msgs/action/FollowJointTrajectory`,
而流程真正调用的是 `ros2_control` 的 `control_msgs/action/FollowJointTrajectory` ——
于是自检**永远**说"类型不对",而人只会去怀疑服务本身。
规矩:期望类型/话题名/服务名不要在两处各写一遍字面量,取同一个来源
(常量或构造函数);自检报"类型不对"时要说清**期望什么、实际什么**。
### R5.10 环境缺依赖就 `skip`,不要让"红"变成常态
仿真环境没装 `onnx`(只有真机 conda 装了),14 个用例长期报红。
长期红的结果是**没有人再看测试结果** —— 真出问题时也看不出来("狼来了")。
规矩:缺依赖用 `pytest.importorskip(...)` 并写清原因;只有"本环境本该能过"的
用例才允许失败。
--- ---
## 6. 文档与提交 ## 6. 文档与提交
@@ -511,6 +558,39 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0
**预防**:构建脚本开头应当**断言**当前 conda 环境与目标一致(待补)。 **预防**:构建脚本开头应当**断言**当前 conda 环境与目标一致(待补)。
### R6.9 硬退出(`os._exit` / `sys.exit`)前必须先把 traceback 打出来
节点入口一度直接 `os._exit(1)`,现场现象是"**退出码 1,日志里什么都没有**",
只能靠猜。规矩:
```python
try:
...
except Exception:
traceback.print_exc() # 先落日志
os._exit(1) # 再硬退
```
同理,`main()` 里凡是"exit 非 0 却没有任何输出"的路径,都算缺陷。
### R6.10 `ros2 launch` 的退出码**恒为 0**,判成败必须另有依据
实测(Humble):子进程 `exit 3`,`ros2 launch` 仍返回 **0**。所以任何
"`ros2 launch …` 之后看 `$?`"的脚本都会把**失败记成成功**,在自动化里极其危险。
规矩:
- 节点把结论**落盘**(`gauge_assembly` 写 `--result-file`,默认 `/tmp/gauge_assembly_result.json`),
脚本/CI 读文件判成败,不读 `$?`;
- "没有结论文件"要**当成失败**并说明(节点在写结论前就退出了),不能当成"没跑";
- 落盘要先于发话题(进程一退,话题里的最后一条就没了)。
另一个同类现象:**流程节点退出不会带走常驻节点**(如 `static_transform_publisher`),
launch 会一直挂着、终端不回提示符。要显式挂 `OnProcessExit → Shutdown`。
--- ---
## 7. 改动前的检查清单 ## 7. 改动前的检查清单
@@ -577,6 +657,14 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0
| 服务/话题/TF 全在,但读数全是乱的 | 上一轮的 `sim_gripper_adapter` / `static_transform_publisher` 还活着 | R4.11 | | 服务/话题/TF 全在,但读数全是乱的 | 上一轮的 `sim_gripper_adapter` / `static_transform_publisher` 还活着 | R4.11 |
| 命令"开 20 mm",两指实际 21.75 mm,夹不住工件 | 上游 `<mimic>` 是线性近似,开口必须走正运动学反解 | R4.4、坑 12 | | 命令"开 20 mm",两指实际 21.75 mm,夹不住工件 | 上游 `<mimic>` 是线性近似,开口必须走正运动学反解 | R4.4、坑 12 |
| 残留进程把旧相机外参按在新仿真上 | `static_transform_publisher` 发的是 latched `/tf_static` | R4.11 | | 残留进程把旧相机外参按在新仿真上 | `static_transform_publisher` 发的是 latched `/tf_static` | R4.11 |
| 同一个手眼 TF,不同 launch 发出来不一样 | 四元数换算被抄了四份,退化分支公式各不相同 | R5.8 |
| 自检永远说"服务类型不对",而服务明明是好的 | 期望类型写成了适配器的类型,流程实际调的是 `control_msgs` 那份 | R5.9 |
| 节点退出码 1,日志里一个字都没有 | 入口 `os._exit()` 把异常吞了 | R6.9 |
| 每次 pytest 十几条红,没人看测试结果了 | 环境缺 `onnx`,用例直接失败而不是 `skip` | R5.10 |
| `site.yaml` 里改了参数,行为一点没变 | 那个键**没接线**(配置表里有、代码里从没读过) | R1.2 |
| 改了源码,仿真里行为一点没变,也没有任何提示 | 仿真 `install_sim/` 是**拷贝**(不能 `--symlink-install`),没重编就跑 | R5.7 |
| 装配失败了,脚本/CI 却报成功 | `ros2 launch` 的退出码恒为 0,脚本看了 `$?` | R6.9 |
| 流程明明打完了,终端一直不回到提示符 | launch 里的静态 TF 发布者是常驻的,主节点退出不会带走它 | R6.9 |
--- ---
+223
View File
@@ -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):新手从这读起
+10 -1
View File
@@ -81,7 +81,8 @@ bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联(
| D435 手外相机(S03 粗定位兜底) | ❌ **还没做手眼标定**,链路不可用 | 见 `devices/cameras/README.md` | | D435 手外相机(S03 粗定位兜底) | ❌ **还没做手眼标定**,链路不可用 | 见 `devices/cameras/README.md` |
| 自动启动全部服务(一条命令拉起所有进程) | ❌ 还没做,目前靠三个终端手工起 | 见第 6 节待办 | | 自动启动全部服务(一条命令拉起所有进程) | ❌ 还没做,目前靠三个终端手工起 | 见第 6 节待办 |
| 「规有没有被夹住」自检 | ❌ 还没做 | 见第 6 节待办 | | 「规有没有被夹住」自检 | ❌ 还没做 | 见第 6 节待办 |
| 装配合格判定(通/止) | ❌ 还没做。当前只输出几何与力的原始结果 | `/tmp/stage3_result.json` | | 装配合格判定(通/止) | ⚠️ **通止规装配流程**能按力矩判据拧到底/拧出来,但"通/止合格"结论还没做 | `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) | | Gazebo 仿真 | ✅ 场景、相机、触觉、夹爪可跑 | [`devices/sim/gazebo/README.md`](../devices/sim/gazebo/README.md) |
> 这张表是**承诺书**:❌ 的别当成已具备。文档说"已就绪"而实际没有, > 这张表是**承诺书**:❌ 的别当成已具备。文档说"已就绪"而实际没有,
@@ -175,6 +176,7 @@ D405 YOLO 手眼 T 伺服 ros2_cont
| 算法权重(换模型) | `models/<分组>/` + `site.yaml` 的路径 | `scripts/weights.sh list` | [`models/README.md`](../models/README.md) | | 算法权重(换模型) | `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) | | 接一个新品牌设备 | `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) | | 命名位姿(回零点等) | `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 | | 框架规矩本身 | `docs/FRAMEWORK_RULES.md`,**同时**改 `.cursor/rules/` | `python3 scripts/check_text_only.py` | R6.1 |
### 3.2 改一个现场数值(最高频) ### 3.2 改一个现场数值(最高频)
@@ -397,6 +399,9 @@ python3 -m pytest src/arm_sim_gazebo/test -q
让人跑去查夹爪代码(R4.12)。 让人跑去查夹爪代码(R4.12)。
- 上一轮的**残留进程**伪造出"代码写错了"的现场 —— 两个同名适配器抢话题/服务, - 上一轮的**残留进程**伪造出"代码写错了"的现场 —— 两个同名适配器抢话题/服务,
残留的 `static_transform_publisher` 把旧外参按在新仿真上,读数全是乱的(R4.11)。 残留的 `static_transform_publisher` 把旧外参按在新仿真上,读数全是乱的(R4.11)。
- **同一份数学写了四份**(四元数换算),退化角度的公式各不相同 ——
没有哪一份"看起来坏",只是不同 launch 发出来的手眼 TF 不一样(R5.8)。
- 测试长期十几条红,于是**没人再看测试结果** —— 真出问题时也看不出来(R5.10)。
**所以**: **所以**:
@@ -442,6 +447,10 @@ python3 -m pytest src/arm_sim_gazebo/test -q
| 仿真镜头位置不对、只看得见地面 | 相机 `rpy` 复合顺序 | 仿真手册坑 5 | | 仿真镜头位置不对、只看得见地面 | 相机 `rpy` 复合顺序 | 仿真手册坑 5 |
| 脚本说"已停止",进程还在 | 进程模式表漏项 | R4.10 | | 脚本说"已停止",进程还在 | 进程模式表漏项 | R4.10 |
| 孔检测在仿真里怎么都不过 | 图幅与真机不同,像素门槛失效 | 仿真手册 §4.3 | | 孔检测在仿真里怎么都不过 | 图幅与真机不同,像素门槛失效 | 仿真手册 §4.3 |
| `site.yaml` 改了参数,行为一点没变 | 那个键**没接线**(配置表里有、代码里从没读) | R1.2 |
| 流程退出码 1,日志里一个字都没有 | 入口 `os._exit()` 把异常吞了 | R6.9 |
| 同一个手眼 TF,换个脚本跑出来不一样 | 四元数换算被抄了多份 | R5.8 |
| 通止规流程 S00 就停住、说不缺的东西缺了 | 自检的"期望类型"与流程实际调用的类型不一致 | R5.9 |
--- ---
+15
View File
@@ -21,6 +21,7 @@
| `run_dual_gripper.sh` | 两侧 `robots[].gripper` | 双爪 ROS 节点 | | `run_dual_gripper.sh` | 两侧 `robots[].gripper` | 双爪 ROS 节点 |
| `run_hole_detect.sh --arm …` | `hole_detect.*` `camera` | 入孔像素 | | `run_hole_detect.sh --arm …` | `hole_detect.*` `camera` | 入孔像素 |
| `run_tactile.sh [--cli]` | `tactile.*` | Orisys 左右指 | | `run_tactile.sh [--cli]` | `tactile.*` | Orisys 左右指 |
| `run_gauge_assembly.sh --arm 89\|91 [--dry-run] [--holes N] [--backend auto\|contract\|legacy]` | `gauge_assembly.*` | **通止规装配主流程**(S00–S11);`--dry-run` 只验编排不动臂。流程见 [GAUGE_ASSEMBLY.md](GAUGE_ASSEMBLY.md) |
| `weights.sh list\|labels\|convert\|doctor\|show` | 权重模块 | 管理权重与标签,见 `models/README.md` | | `weights.sh list\|labels\|convert\|doctor\|show` | 权重模块 | 管理权重与标签,见 `models/README.md` |
| `export_yolo_trt.sh [onnx] [imgsz]` | 权重 | **旧命令**,转发壳 → `weights.sh convert --to engine` | | `export_yolo_trt.sh [onnx] [imgsz]` | 权重 | **旧命令**,转发壳 → `weights.sh convert --to engine` |
| `show_site_config.sh` | `site.yaml` | 打印解析结果 | | `show_site_config.sh` | `site.yaml` | 打印解析结果 |
@@ -107,6 +108,20 @@ PGEA 单爪节点(`--arm 89` 无 `--cli`):
| `/tactile/left/status` | `arm_msgs/msg/TactileStatus` | | `/tactile/left/status` | `arm_msgs/msg/TactileStatus` |
| `/tactile/right/status` | `arm_msgs/msg/TactileStatus` | | `/tactile/right/status` | `arm_msgs/msg/TactileStatus` |
### 通止规装配(`gauge_assembly` 节点)
| 话题 | 类型 | 方向 | 说明 |
|------|------|------|------|
| `/gauge/state` | `std_msgs/msg/String` | 出 | 每次状态转移一条:`#序号 状态 --事件--> 状态` |
| `/gauge/progress` | `std_msgs/msg/String` | 出 | 进度(第几孔 / 第几轮棘轮) |
| `/gauge/result` | `std_msgs/msg/String` | 出 | 结束结论(成功 / 失败原因) |
| `/gauge/reload_ok` | `std_msgs/msg/Bool` | 入 | **E1 掉件**:把规塞回爪里后发 `true`,流程继续(超时 = 中止) |
E1 人工确认:`ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}"`
夹爪走 `/gripper/open` `/gripper/close`,类型按后端自动识别(`arm_msgs/srv/GripperExecute`
或真机驱动的 `std_srvs/srv/Trigger`),见 [GRIPPER.md](GRIPPER.md)。
--- ---
## 3. `site.yaml` 契约 ## 3. `site.yaml` 契约
+8
View File
@@ -50,6 +50,7 @@ bash ~/ros2/scripts/run_handeye.sh --arm 91 teach # 91 必须单独标定
| `hole_detect` | `run_hole_detect.sh`(算法参数也在这一段) | | `hole_detect` | `run_hole_detect.sh`(算法参数也在这一段) |
| `tactile` | `run_tactile.sh` | | `tactile` | `run_tactile.sh` |
| `go_home` | `go_home.sh` | | `go_home` | `go_home.sh` |
| `gauge_assembly` | `run_gauge_assembly.sh`(通止规装配 S00–S11;`gauge_config.py` 校验键名/单位) |
## 常改项 ## 常改项
@@ -70,5 +71,12 @@ bash ~/ros2/scripts/run_handeye.sh --arm 91 teach # 91 必须单独标定
| `robots[].gripper.port` `model` | 该臂串口与型号(91 为 RGI) | | `robots[].gripper.port` `model` | 该臂串口与型号(91 为 RGI) |
| `hole_detect.backend` `hough_*` `conf` | 入孔检测 | | `hole_detect.backend` `hough_*` `conf` | 入孔检测 |
| `tactile.left_video` `right_video` `config` | Orisys 左右指设备与 dd03 | | `tactile.left_video` `right_video` `config` | Orisys 左右指设备与 dd03 |
| `gauge_assembly.extreme_angle_deg` `_direction` | S01 预绕多少度、往哪边转(逆时针=+1) |
| `gauge_assembly.contact_*_delta` `screw_*_delta` | 通止规装配的**力判据**(任意单位增量),必须现场标定,见 [TACTILE.md](TACTILE.md) |
| `gauge_assembly.holes_total` `reload_timeout_s` | 这次任务的孔数、掉件后等人工确认的时限 |
路径写成**绝对路径**。换机器改 `USER` 和 SN,不要改 Python。 路径写成**绝对路径**。换机器改 `USER` 和 SN,不要改 Python。
`gauge_assembly:` 段**每个键都有中文注释与取值建议**,直接读
`config/site.yaml` 那一段即可;流程与判据见 [GAUGE_ASSEMBLY.md](GAUGE_ASSEMBLY.md)。
键名拼错会**直接报错并给出最相近的候选名**,不会静默退回默认值(R1.2)。
+2
View File
@@ -106,6 +106,7 @@ devices/<kind>/<brand>/
| `run_dual_gripper.sh` | 双爪同一 ROS 节点(`arm_msgs`) | | `run_dual_gripper.sh` | 双爪同一 ROS 节点(`arm_msgs`) |
| `run_hole_detect.sh --arm … [--preview]` | 入孔检测(只出像素孔心,不插入) | | `run_hole_detect.sh --arm … [--preview]` | 入孔检测(只出像素孔心,不插入) |
| `run_tactile.sh [--cli]` | Orisys 左右指视触觉 | | `run_tactile.sh [--cli]` | Orisys 左右指视触觉 |
| `run_gauge_assembly.sh --arm …` | **通止规装配**(S00–S11)。`--dry-run` 只验编排不动臂;前置服务要自己先起(见 [GAUGE_ASSEMBLY.md](GAUGE_ASSEMBLY.md)) |
| `env.sh` | 加载 conda + ROS + `install/setup.bash` + `site.yaml` | | `env.sh` | 加载 conda + ROS + `install/setup.bash` + `site.yaml` |
| `build_all.sh` | `build.sh`(arm 包)+ `build_real.sh`(Flexiv 硬件包) | | `build_all.sh` | `build.sh`(arm 包)+ `build_real.sh`(Flexiv 硬件包) |
| `build.sh` | colcon:`arm_*` + `flexiv_description` | | `build.sh` | colcon:`arm_*` + `flexiv_description` |
@@ -191,6 +192,7 @@ devices/<kind>/<brand>/
| `TACTILE.md` | Orisys 视触觉 | | `TACTILE.md` | Orisys 视触觉 |
| `INTERFACES.md` | 脚本 / ROS topic / 参数契约 | | `INTERFACES.md` | 脚本 / ROS topic / 参数契约 |
| `TRAJECTORIES.md` | 关节轨迹回放 | | `TRAJECTORIES.md` | 关节轨迹回放 |
| `GAUGE_ASSEMBLY.md` | **通止规装配流程**(状态机、判据、E1 掉件处理、怎么调参、验收命令) |
--- ---
+9
View File
@@ -8,6 +8,15 @@
**夹爪:** `run_gripper.sh --arm 89|91` · `run_dual_gripper.sh` **夹爪:** `run_gripper.sh --arm 89|91` · `run_dual_gripper.sh`
**入孔:** `run_hole_detect.sh --arm … [--preview]` **入孔:** `run_hole_detect.sh --arm … [--preview]`
**触觉:** `run_tactile.sh [--cli]` **触觉:** `run_tactile.sh [--cli]`
**通止规装配:** `run_gauge_assembly.sh --arm 89 [--dry-run] [--holes N] [--backend …]`
- 一台状态机走完 S00–S11(自检 → 回 home/预绕 → 粗定位 → 跟踪 → 稳定拍照 →
悬停 → 柔顺接触 → 棘轮拧入 → 拧出 → 换孔 → 复位)。
- `--dry-run` **不发任何运动指令**,只验编排与自检 —— 改完配置先这么跑。
- 前置服务(臂 / 腕部相机 / 夹爪 / 触觉)要自己先起;缺哪项由 **S00 自检逐项报出**
并停住(不代你启动,也不"带病往下跑")。
- 掉件(E1):流程停在 home 等人,把规塞回爪里后发一次
`ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}"`。
- 判据、调参顺序、还没做的部分:[docs/GAUGE_ASSEMBLY.md](../docs/GAUGE_ASSEMBLY.md)
**权重:** `weights.sh`(`list` / `labels` / `convert` / `doctor` / `show`) **权重:** `weights.sh`(`list` / `labels` / `convert` / `doctor` / `show`)
- 转换:`weights.sh convert <文件> --name <模型名>` —— PT→ONNX→TensorRT,自动改名归位 + 固化标签 - 转换:`weights.sh convert <文件> --name <模型名>` —— PT→ONNX→TensorRT,自动改名归位 + 固化标签
- 标签:`weights.sh labels <模型名>` 看,`--set "工件,通止规"` 改 - 标签:`weights.sh labels <模型名>` 看,`--set "工件,通止规"` 改
+44 -1
View File
@@ -24,6 +24,10 @@
# 力阈值标定:先 bash ~/ros2/scripts/run_tactile.sh --cli 看读数量级 # 力阈值标定:先 bash ~/ros2/scripts/run_tactile.sh --cli 看读数量级
# 掉件重放:bash ~/ros2/scripts/run_gauge_assembly.sh 之外另开一个终端 # 掉件重放:bash ~/ros2/scripts/run_gauge_assembly.sh 之外另开一个终端
# ros2 topic pub -1 /gauge/reload_ok std_msgs/Bool '{data: true}' # ros2 topic pub -1 /gauge/reload_ok std_msgs/Bool '{data: true}'
#
# 退出码:0=成功(结论文件 ok=true),1=失败(读的是结论文件,不是 ros2 launch 的
# `$?` —— 后者恒为 0,见 docs/FRAMEWORK_RULES.md R6.9)。
# 结论文件:/tmp/gauge_assembly_<SN>.json(节点写,本脚本读)
set -eo pipefail set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
@@ -80,7 +84,11 @@ echo " 孔数=${HOLES:-site} 夹爪后端=${BACKEND} 干跑=${DRY_RUN}"
pkill -f 'lib/arm_application/gauge_assembly' 2>/dev/null || true pkill -f 'lib/arm_application/gauge_assembly' 2>/dev/null || true
sleep 0.5 sleep 0.5
ARGS=(gripper_backend:="${BACKEND}") # 结论落盘路径:**由本脚本决定**(节点只负责写到这里),脚本末尾再读回来判成败。
RESULT_FILE="/tmp/gauge_assembly_${SITE_ROBOT_SN}.json"
rm -f "${RESULT_FILE}"
ARGS=(gripper_backend:="${BACKEND}" result_file:="${RESULT_FILE}")
if [[ -n "${HOLES}" ]]; then ARGS+=(holes_total:="${HOLES}"); fi if [[ -n "${HOLES}" ]]; then ARGS+=(holes_total:="${HOLES}"); fi
if [[ "${DRY_RUN}" == true ]]; then ARGS+=(dry_run:=true); fi if [[ "${DRY_RUN}" == true ]]; then ARGS+=(dry_run:=true); fi
@@ -88,3 +96,38 @@ echo "=== launch gauge_assembly(自检不过会自动报缺什么) ==="
# 不用 exec:exec 之后 Ctrl-C 直接打给 ros2 launch,这里的清理就没了; # 不用 exec:exec 之后 Ctrl-C 直接打给 ros2 launch,这里的清理就没了;
# gauge_assembly 自己也会在收到 KeyboardInterrupt 时退到安全高度。 # gauge_assembly 自己也会在收到 KeyboardInterrupt 时退到安全高度。
ros2 launch arm_application gauge_assembly.launch.py "${ARGS[@]}" ros2 launch arm_application gauge_assembly.launch.py "${ARGS[@]}"
# ---- 判成败:**不能靠 `$?`** ----
# 实测(Humble):`ros2 launch` 的子进程 exit 3 时,ros2 launch 仍返回 0。
# 所以这里读节点落盘的结论文件 —— 否则自动化会把"装配失败"记成"成功"。
python3 - "${RESULT_FILE}" <<'PY'
import json
import sys
path = sys.argv[1]
try:
with open(path, encoding="utf-8") as fh:
res = json.load(fh)
except FileNotFoundError:
sys.exit(f"失败:没有结论文件 {path}\n"
" 节点在写出结论前就退出了(构造失败 / 崩溃 / 被 Ctrl-C)—— 看上面的日志。")
except (OSError, json.JSONDecodeError) as exc:
sys.exit(f"失败:结论文件读不了({path}):{exc}")
ok = bool(res.get("ok"))
print(f"结论:state={res.get('state')} ok={ok} "
f"已装 {res.get('holes_done')}/{res.get('holes_total')} 孔"
f"{'(干跑)' if res.get('dry_run') else ''} 掉件重放 {res.get('relocations')} 次")
if res.get("failures"):
print(" 未就绪(必检):")
for item in res["failures"]:
print(f" - {item}")
if res.get("notes"):
print(" 备注:")
for item in res["notes"]:
print(f" · {item}")
if res.get("reason"):
print(f" 原因:{res['reason']}")
if not ok:
sys.exit(f"失败:state={res.get('state')}(exit_code={res.get('exit_code', 1)})")
PY
+15
View File
@@ -19,9 +19,23 @@
| `goto_ee_plane` | `goto_ee_plane.py` | 可选:抬高再回 home(默认启动不再调用) | | `goto_ee_plane` | `goto_ee_plane.py` | 可选:抬高再回 home(默认启动不再调用) |
| `demo_named_poses` | `demo_named_poses.py` | mock/实机点位示例 | | `demo_named_poses` | `demo_named_poses.py` | mock/实机点位示例 |
| `run_joint_trajectory` | `run_joint_trajectory.py` | 轨迹回放 | | `run_joint_trajectory` | `run_joint_trajectory.py` | 轨迹回放 |
| `gauge_assembly` | `gauge_assembly_node.py` | **通止规装配主流程**(S00–S11 状态机,见 [docs/GAUGE_ASSEMBLY.md](../../docs/GAUGE_ASSEMBLY.md)) |
库模块见包内 `__init__.py`。接口总表:[docs/INTERFACES.md](../../docs/INTERFACES.md)。 库模块见包内 `__init__.py`。接口总表:[docs/INTERFACES.md](../../docs/INTERFACES.md)。
## 通止规装配的模块分工(`gauge_*`)
主流程拆成"纯逻辑 + 一层 ROS 壳",纯逻辑都能离线单测:
| 文件 | 管什么 |
|------|--------|
| `gauge_fsm.py` | 状态/事件/转移表(**唯一权威**,转移表是数据不是 if/else) |
| `gauge_kin.py` | URDF 限位、预绕角、实测"绕工具 Z 转了多少度" |
| `gauge_config.py` | `site.yaml → gauge_assembly` 段解析与校验(键名/单位/类型/交叉矛盾) |
| `gauge_gripper.py` | 夹爪后端选择(契约 / 真机 Trigger)+ 状态字解读 |
| `gauge_boot.py` | S00 自检清单(必检/选检,缺了会怎样) |
| `gauge_assembly_node.py` | ROS 壳:发指令、读反馈、把事件交给状态机 |
## 配置 ## 配置
| 文件 | 角色 | | 文件 | 角色 |
@@ -37,3 +51,4 @@
| `launch/dual_gripper.launch.py` | 双爪 | | `launch/dual_gripper.launch.py` | 双爪 |
| `launch/hole_detect.launch.py` | 入孔检测 | | `launch/hole_detect.launch.py` | 入孔检测 |
| `launch/tactile.launch.py` | 视触觉 | | `launch/tactile.launch.py` | 视触觉 |
| `launch/gauge_assembly.launch.py` | 通止规装配(只发布手眼静态 TF;前置服务自己先起) |
@@ -55,7 +55,7 @@ import math
import os import os
import time import time
from pathlib import Path from pathlib import Path
from typing import Any, Dict, List, Optional, Sequence, Tuple from typing import Any, Dict, List, Mapping, Optional, Sequence, Tuple
import numpy as np import numpy as np
import rclpy import rclpy
@@ -198,13 +198,24 @@ class GripperClient:
return self._ever_gripped and self._last_gripped is False return self._ever_gripped and self._last_gripped is False
# -- 动作 ------------------------------------------------------------- # -- 动作 -------------------------------------------------------------
def _call(self, client: Any, tag: str, timeout_s: float) -> bool: def _call(
self, client: Any, tag: str, timeout_s: float,
force_pct: Optional[float] = None, speed_pct: Optional[float] = None,
) -> bool:
if client is None or not client.wait_for_service(timeout_sec=timeout_s): if client is None or not client.wait_for_service(timeout_sec=timeout_s):
self._node.get_logger().error(f"夹爪服务 {tag} 不在(后端 {self.backend})") self._node.get_logger().error(f"夹爪服务 {tag} 不在(后端 {self.backend})")
return False return False
if self.backend == "contract": if self.backend == "contract":
req: Any = GripperExecute.Request() req: Any = GripperExecute.Request()
req.command.command = tag req.command.command = tag
# 力/速度只对**契约后端**有效:真机今天的 dh_pgea 驱动是
# std_srvs/Trigger(只有开/合两个动作,力与速度在驱动侧配),
# 所以那两个键在真机上不起作用 —— 这是保留下来的规格差异,
# 文档里已写明,不要以为"填了没效果"是 bug。
if force_pct is not None:
req.command.force = float(force_pct)
if speed_pct is not None:
req.command.speed = float(speed_pct)
else: else:
req = Trigger.Request() req = Trigger.Request()
fut = client.call_async(req) fut = client.call_async(req)
@@ -236,7 +247,11 @@ class GripperClient:
if self._node.cfg.dry_run: if self._node.cfg.dry_run:
self._node.get_logger().info("[干跑] 夹爪夹紧") self._node.get_logger().info("[干跑] 夹爪夹紧")
return True return True
return self._call(self._close_cli, "close", timeout_s) return self._call(
self._close_cli, "close", timeout_s,
force_pct=self._node.cfg.screw_grip_force_pct,
speed_pct=self._node.cfg.screw_grip_speed_pct,
)
class GaugeAssembly(AssemblyPipeline): class GaugeAssembly(AssemblyPipeline):
@@ -270,6 +285,11 @@ class GaugeAssembly(AssemblyPipeline):
self.declare_parameter("dry_run", False) self.declare_parameter("dry_run", False)
self.declare_parameter("holes_total", -1) # >0 覆盖 site self.declare_parameter("holes_total", -1) # >0 覆盖 site
self.declare_parameter("gripper_backend", "auto") # auto|contract|legacy self.declare_parameter("gripper_backend", "auto") # auto|contract|legacy
# 结论落盘路径。**为什么要落盘**:`ros2 launch` 在 Humble 下**恒定退出码 0**
# (实测:子进程 exit 3,ros2 launch 仍返回 0),所以包一层脚本时
# `$?` 判不出成败 —— 自动化里会把"流程失败"记成"成功"。结论文件是唯一
# 能被脚本/CI 读到的东西,由 run_gauge_assembly.sh 传入并读回。
self.declare_parameter("result_file", "/tmp/gauge_assembly_result.json")
site = load_site_dict() site = load_site_dict()
overrides: Dict[str, Any] = {} overrides: Dict[str, Any] = {}
@@ -340,6 +360,8 @@ class GaugeAssembly(AssemblyPipeline):
self.create_subscription(Bool, "/gauge/reload_ok", self._on_reload_ok, 1) self.create_subscription(Bool, "/gauge/reload_ok", self._on_reload_ok, 1)
# ---- 发布 ---- # ---- 发布 ----
self._result_file = str(self.get_parameter("result_file").value or "")
self._result_written = False
self._gauge_state_pub = self.create_publisher(String, "/gauge/state", 10) self._gauge_state_pub = self.create_publisher(String, "/gauge/state", 10)
self._gauge_result_pub = self.create_publisher(String, "/gauge/result", 10) self._gauge_result_pub = self.create_publisher(String, "/gauge/result", 10)
self._gauge_progress_pub = self.create_publisher(String, "/gauge/progress", 10) self._gauge_progress_pub = self.create_publisher(String, "/gauge/progress", 10)
@@ -433,10 +455,29 @@ class GaugeAssembly(AssemblyPipeline):
payload = dict(payload) payload = dict(payload)
payload["fsm"] = [str(step) for step in self.fsm.history] payload["fsm"] = [str(step) for step in self.fsm.history]
payload["hole_names"] = [f"k{i + 1}" for i in range(self.cfg.holes_total)] payload["hole_names"] = [f"k{i + 1}" for i in range(self.cfg.holes_total)]
# 先落盘再发话题:落盘是**给脚本/CI 读**的(话题在进程退出后就没了),
# 顺序反过来的话,"话题发出去了、文件还没写完就被杀掉"会让脚本读不到结论。
self._write_result_file(payload)
msg = String() msg = String()
msg.data = json.dumps(payload, ensure_ascii=False) msg.data = json.dumps(payload, ensure_ascii=False)
self._gauge_result_pub.publish(msg) self._gauge_result_pub.publish(msg)
def _write_result_file(self, payload: Mapping[str, Any]) -> None:
"""把结论写到 ``result_file``(原子写:临时文件 + rename)。"""
if not self._result_file:
return
path = Path(self._result_file)
try:
path.parent.mkdir(parents=True, exist_ok=True)
tmp = path.with_suffix(path.suffix + ".tmp")
tmp.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
tmp.replace(path)
except OSError as exc:
# 写不了不要拖垮流程,但**必须**说出来(否则脚本只会看到"文件不存在")
self.get_logger().error(f"结论文件写不了({path}):{exc}")
return
self._result_written = True
def _wait_joint_limits(self, timeout_s: float = 3.0) -> Optional[Tuple[float, float]]: def _wait_joint_limits(self, timeout_s: float = 3.0) -> Optional[Tuple[float, float]]:
"""拿关节7的限位(从 ``/robot_description``)。拿不到返回 None。""" """拿关节7的限位(从 ``/robot_description``)。拿不到返回 None。"""
t0 = time.monotonic() t0 = time.monotonic()
@@ -898,8 +939,11 @@ class GaugeAssembly(AssemblyPipeline):
f" 做法:bash scripts/run_handeye.sh --mode eye_to_hand(标完再跑本流程)" f" 做法:bash scripts/run_handeye.sh --mode eye_to_hand(标完再跑本流程)"
) )
return Event.COARSE_FAIL return Event.COARSE_FAIL
if not self._camera_alive(ns): if not self._wait_coarse_frame(ns):
self.get_logger().warn(f"S03 相机 {ns} 没有图像,回 S04 跟踪") self.get_logger().warn(
f"S03 相机 {ns} 等了 {self.cfg.coarse_timeout_s:.0f}s 没有新鲜图像"
"(相机没开?),回 S04 跟踪"
)
return Event.COARSE_FAIL return Event.COARSE_FAIL
xy = self._coarse_workpiece_xy(ns, he) xy = self._coarse_workpiece_xy(ns, he)
@@ -938,6 +982,19 @@ class GaugeAssembly(AssemblyPipeline):
def _camera_alive(self, ns: str) -> bool: def _camera_alive(self, ns: str) -> bool:
return self._coarse_color is not None and self._img_age(self._coarse_color) < 3.0 return self._coarse_color is not None and self._img_age(self._coarse_color) < 3.0
def _wait_coarse_frame(self, ns: str) -> bool:
"""等手外相机的**新鲜**图像,最多 ``coarse_timeout_s`` 秒。
为什么要等而不是"看一眼没有就放弃":相机节点刚起来时发现+第一帧
可能要一两秒,直接判失败会让 S03 在相机其实是好的情况下退回 S04。
"""
t0 = time.monotonic()
while time.monotonic() - t0 < self.cfg.coarse_timeout_s:
if self._camera_alive(ns):
return True
self._spin(0.1)
return False
def _coarse_workpiece_xy(self, ns: str, he: Path) -> Optional[Tuple[float, float]]: def _coarse_workpiece_xy(self, ns: str, he: Path) -> Optional[Tuple[float, float]]:
"""手外相机拍一张 → 推理工件框中心 → 反投影到基座 XY。 """手外相机拍一张 → 推理工件框中心 → 反投影到基座 XY。
@@ -1164,7 +1221,15 @@ class GaugeAssembly(AssemblyPipeline):
max_depth_m = self.cfg.contact_max_depth_mm / 1000.0 max_depth_m = self.cfg.contact_max_depth_mm / 1000.0
while float(Tee[2, 3]) - z0 > -max_depth_m: while float(Tee[2, 3]) - z0 > -max_depth_m:
target = np.array([float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]) - step_m]) target = np.array([float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]) - step_m])
ok, q, Tee = self._step_to(q, Tee, target, tag="S07_step") # 不用 _step_to:它把速度写死成 search_speed,而这**一步**必须慢
# (接触判据要在"碰到"的瞬间停住,快了就是一个冲击)。速度取
# gauge_assembly.contact_speed_m_s,其余参数与 _step_to 对齐。
ok, q, Tee = self._goto(
q, Tee, target, verticalize=True, tol_m=0.0005,
tol_tilt=self.vert_tol_rad * 2.5, tag="S07_step",
v=self.cfg.contact_speed_m_s, max_hops=2,
settle_s=max(0.06, self.settle3 * 0.3), min_dur=0.16,
)
if not ok: if not ok:
self.get_logger().warn("S07 单步未完全到位,继续按实测高度判断") self.get_logger().warn("S07 单步未完全到位,继续按实测高度判断")
try: try:
@@ -1468,10 +1533,19 @@ class GaugeAssembly(AssemblyPipeline):
self._safe_retract() self._safe_retract()
ok = self.fsm.ok ok = self.fsm.ok
# 两个孔数要分清:`hole_index` 是**0 基的当前孔号**(调试用),
# `holes_done` 才是"装完了几个"(给人/脚本看)。混用会把"两个孔都装完了"
# 显示成"已装 1/2",让人以为少装了一个。
holes_done = (
self.cfg.holes_total
if self.fsm.state is State.DONE
else max(0, self.fsm.ctx.hole_index)
)
payload = { payload = {
"ok": ok, "ok": ok,
"state": str(self.fsm.state), "state": str(self.fsm.state),
"holes_total": self.cfg.holes_total, "holes_total": self.cfg.holes_total,
"holes_done": holes_done,
"hole_index": self.fsm.ctx.hole_index, "hole_index": self.fsm.ctx.hole_index,
"relocations": self.fsm.ctx.relocations, "relocations": self.fsm.ctx.relocations,
"notes": self.fsm.ctx.notes, "notes": self.fsm.ctx.notes,
@@ -1524,6 +1598,16 @@ def main(args: Optional[Sequence[str]] = None) -> None:
node.get_logger().warn("用户中断 —— 按安全高度退出") node.get_logger().warn("用户中断 —— 按安全高度退出")
code = 130 code = 130
finally: finally:
if node is not None:
if not node._result_written:
# 走到这里说明**结论还没发布**(构造失败 / JTC 不在 / 抛异常 / 用户中断)。
# 补一份,内容是"没跑完",这样脚本不会把"没结论"误当成"没跑"。
node._write_result_file({
"ok": False,
"state": str(getattr(node.fsm, "state", "?")),
"exit_code": code,
"reason": "节点在发布结论前就退出了(原因见日志)",
})
if node is not None: if node is not None:
try: try:
node._stop_stage1() node._stop_stage1()
@@ -37,7 +37,6 @@ _UNIT_SUFFIX: Dict[str, str] = {
#: 没有单位又不登记的键,会让"单位自检"变成一句空话(规则 M1)。 #: 没有单位又不登记的键,会让"单位自检"变成一句空话(规则 M1)。
_UNITLESS = frozenset( _UNITLESS = frozenset(
{ {
"require_preview",
"extreme_direction", "extreme_direction",
"grip_check", "grip_check",
"coarse_enabled", "coarse_enabled",
@@ -54,7 +53,6 @@ _UNITLESS = frozenset(
#: 每个键的**类型**(用于类型校验,不做隐式转换:``"20"`` 不当 20 用)。 #: 每个键的**类型**(用于类型校验,不做隐式转换:``"20"`` 不当 20 用)。
_TYPES: Dict[str, type] = { _TYPES: Dict[str, type] = {
"boot_timeout_s": float, "boot_timeout_s": float,
"require_preview": bool,
"extreme_angle_deg": float, "extreme_angle_deg": float,
"extreme_speed_deg_s": float, "extreme_speed_deg_s": float,
"extreme_direction": int, "extreme_direction": int,
@@ -66,12 +64,9 @@ _TYPES: Dict[str, type] = {
"coarse_hover_m": float, "coarse_hover_m": float,
"coarse_conf": float, "coarse_conf": float,
"coarse_timeout_s": float, "coarse_timeout_s": float,
"track_lost_s": float,
"track_prewarm_s": float,
"snap_stable_s": float, "snap_stable_s": float,
"snap_tol_px": float, "snap_tol_px": float,
"snap_min_frames": int, "snap_min_frames": int,
"hover_m": float,
"align_tol_mm": float, "align_tol_mm": float,
"contact_speed_m_s": float, "contact_speed_m_s": float,
"contact_step_mm": float, "contact_step_mm": float,
@@ -96,7 +91,6 @@ _TYPES: Dict[str, type] = {
#: 每个键的单位。``""`` 表示无量纲(须同时登记在 :data:`_UNITLESS`)。 #: 每个键的单位。``""`` 表示无量纲(须同时登记在 :data:`_UNITLESS`)。
_UNITS: Dict[str, str] = { _UNITS: Dict[str, str] = {
"boot_timeout_s": "s", "boot_timeout_s": "s",
"require_preview": "",
"extreme_angle_deg": "deg", "extreme_angle_deg": "deg",
"extreme_speed_deg_s": "deg/s", "extreme_speed_deg_s": "deg/s",
"extreme_direction": "", "extreme_direction": "",
@@ -108,12 +102,9 @@ _UNITS: Dict[str, str] = {
"coarse_hover_m": "m", "coarse_hover_m": "m",
"coarse_conf": "", "coarse_conf": "",
"coarse_timeout_s": "s", "coarse_timeout_s": "s",
"track_lost_s": "s",
"track_prewarm_s": "s",
"snap_stable_s": "s", "snap_stable_s": "s",
"snap_tol_px": "px", "snap_tol_px": "px",
"snap_min_frames": "", "snap_min_frames": "",
"hover_m": "m",
"align_tol_mm": "mm", "align_tol_mm": "mm",
"contact_speed_m_s": "m/s", "contact_speed_m_s": "m/s",
"contact_step_mm": "mm", "contact_step_mm": "mm",
@@ -157,7 +148,6 @@ class GaugeConfigError(ValueError):
#: 默认值只是"能跑起来的起点",必须在 ``describe()`` 里标出来让现场知道要改。 #: 默认值只是"能跑起来的起点",必须在 ``describe()`` 里标出来让现场知道要改。
DEFAULTS: Dict[str, Any] = { DEFAULTS: Dict[str, Any] = {
"boot_timeout_s": 30.0, "boot_timeout_s": 30.0,
"require_preview": True,
# S01:逆时针预绕。符号靠 extreme_direction 定,不靠改代码里的负号。 # S01:逆时针预绕。符号靠 extreme_direction 定,不靠改代码里的负号。
"extreme_angle_deg": 150.0, "extreme_angle_deg": 150.0,
"extreme_speed_deg_s": 20.0, "extreme_speed_deg_s": 20.0,
@@ -172,15 +162,11 @@ DEFAULTS: Dict[str, Any] = {
"coarse_hover_m": 0.25, "coarse_hover_m": 0.25,
"coarse_conf": 0.35, "coarse_conf": 0.35,
"coarse_timeout_s": 20.0, "coarse_timeout_s": 20.0,
# S04
"track_lost_s": 3.0,
"track_prewarm_s": 2.0,
# S05 # S05
"snap_stable_s": 1.0, "snap_stable_s": 1.0,
"snap_tol_px": 2.0, "snap_tol_px": 2.0,
"snap_min_frames": 6, "snap_min_frames": 6,
# S06 # S06
"hover_m": 0.05,
"align_tol_mm": 1.0, "align_tol_mm": 1.0,
# S07 # S07
"contact_speed_m_s": 0.02, "contact_speed_m_s": 0.02,
@@ -214,7 +200,6 @@ class GaugeConfig:
"""通止规装配的全部可调量(解析并校验之后)。""" """通止规装配的全部可调量(解析并校验之后)。"""
boot_timeout_s: float boot_timeout_s: float
require_preview: bool
extreme_angle_deg: float extreme_angle_deg: float
extreme_speed_deg_s: float extreme_speed_deg_s: float
extreme_direction: int extreme_direction: int
@@ -226,12 +211,9 @@ class GaugeConfig:
coarse_hover_m: float coarse_hover_m: float
coarse_conf: float coarse_conf: float
coarse_timeout_s: float coarse_timeout_s: float
track_lost_s: float
track_prewarm_s: float
snap_stable_s: float snap_stable_s: float
snap_tol_px: float snap_tol_px: float
snap_min_frames: int snap_min_frames: int
hover_m: float
align_tol_mm: float align_tol_mm: float
contact_speed_m_s: float contact_speed_m_s: float
contact_step_mm: float contact_step_mm: float
@@ -283,7 +265,9 @@ class GaugeConfig:
f"悬高 {self.coarse_hover_m * 1000:.0f}mm 置信度≥{self.coarse_conf:.2f}", f"悬高 {self.coarse_hover_m * 1000:.0f}mm 置信度≥{self.coarse_conf:.2f}",
f" S05 稳定 {self.snap_stable_s:.1f}s / 抖动≤{self.snap_tol_px:.1f}px / 至少 {self.snap_min_frames} 帧" f" S05 稳定 {self.snap_stable_s:.1f}s / 抖动≤{self.snap_tol_px:.1f}px / 至少 {self.snap_min_frames} 帧"
"(稳定判据用「到均值的最远距离」,不是相邻帧之差)", "(稳定判据用「到均值的最远距离」,不是相邻帧之差)",
f" S06 悬停 {self.hover_m * 1000:.0f}mm,对位容差 {self.align_tol_mm:.1f}mm", # 悬停高度**不在这里**:它取自 servo.hole_hover_z_m(stage2 那一层),
# 这里只印本段的容差 —— 一段配置里出现"另一个段的键"会让人改错地方。
f" S06 对位容差 {self.align_tol_mm:.1f}mm(悬停高取自 servo.hole_hover_z_m)",
f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}(两通道任一)," f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}(两通道任一),"
f"安全上限 +{self.contact_safe_delta:.0f},最多下探 {self.contact_max_depth_mm:.0f}mm" f"安全上限 +{self.contact_safe_delta:.0f},最多下探 {self.contact_max_depth_mm:.0f}mm"
"(触觉读数是任意单位增量,必须现场标定)", "(触觉读数是任意单位增量,必须现场标定)",
@@ -23,7 +23,14 @@ import sys
from pathlib import Path from pathlib import Path
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction from launch.actions import (
DeclareLaunchArgument,
LogInfo,
OpaqueFunction,
RegisterEventHandler,
Shutdown,
)
from launch.event_handlers import OnProcessExit
from launch.substitutions import LaunchConfiguration from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node from launch_ros.actions import Node
@@ -86,6 +93,7 @@ def _launch_setup(context, *args, **kwargs):
dry_run = _flag("dry_run") dry_run = _flag("dry_run")
holes = LaunchConfiguration("holes_total").perform(context).strip() holes = LaunchConfiguration("holes_total").perform(context).strip()
backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto" backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto"
result_file = LaunchConfiguration("result_file").perform(context).strip()
nodes = [] nodes = []
frames = [] frames = []
@@ -116,11 +124,10 @@ def _launch_setup(context, *args, **kwargs):
"—— 不发布静态 TF,S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh" "—— 不发布静态 TF,S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh"
) )
nodes.append( gauge_node = Node(
Node( package="arm_application",
package="arm_application", executable="gauge_assembly",
executable="gauge_assembly", name="gauge_assembly",
name="gauge_assembly",
output="screen", output="screen",
parameters=[ parameters=[
{ {
@@ -129,11 +136,12 @@ def _launch_setup(context, *args, **kwargs):
), ),
"dry_run": dry_run, "dry_run": dry_run,
"holes_total": int(holes) if holes else -1, "holes_total": int(holes) if holes else -1,
"gripper_backend": backend, "gripper_backend": backend,
} "result_file": result_file,
], }
) ],
) )
nodes.append(gauge_node)
return [ return [
LogInfo(msg=f"[gauge] site={path}"), LogInfo(msg=f"[gauge] site={path}"),
@@ -141,8 +149,16 @@ def _launch_setup(context, *args, **kwargs):
LogInfo( LogInfo(
msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} backend={backend}" msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} backend={backend}"
), ),
LogInfo(msg=f"[gauge] 结论文件 {result_file}"),
*[LogInfo(msg=m) for m in frames], *[LogInfo(msg=m) for m in frames],
*nodes, *nodes,
# 流程节点退出 → 整个 launch 退出。
# 为什么必须显式写:静态 TF 发布者是**常驻**的(它不发完就退,
# 要一直挂着供 tf2 查询),所以主流程跑完 DONE 之后 launch 不会自己结束 ——
# 表现为"流程明明打完了,终端一直不回到提示符"(实测会一直挂到 timeout 杀掉)。
RegisterEventHandler(
OnProcessExit(target_action=gauge_node, on_exit=Shutdown())
),
] ]
@@ -152,6 +168,8 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("dry_run", default_value="false"), DeclareLaunchArgument("dry_run", default_value="false"),
DeclareLaunchArgument("holes_total", default_value=""), DeclareLaunchArgument("holes_total", default_value=""),
DeclareLaunchArgument("gripper_backend", default_value="auto"), DeclareLaunchArgument("gripper_backend", default_value="auto"),
# 结论落盘路径(脚本读它判成败:ros2 launch 的退出码恒为 0)
DeclareLaunchArgument("result_file", default_value="/tmp/gauge_assembly_result.json"),
OpaqueFunction(function=_launch_setup), OpaqueFunction(function=_launch_setup),
] ]
) )
@@ -0,0 +1,106 @@
"""`run_gauge_assembly.sh` 的**成败判定**本身的测试(不需要 ROS、不需要机器人)。
为什么单独测这一段
------------------
包一层脚本时最容易犯的错是"用 `$?` 判成败",而 `ros2 launch` 在 Humble 下
**恒返回 0**(实测:子进程 exit 3,它照样返回 0)。于是"装配失败"会被记成
"脚本成功" —— 这是最坏的一类静默失效:**没人会去查一个说成功的脚本**。
所以判成败的那段 Python 被抽成脚本里的 heredoc,这里直接把它取出来跑:
* `ok: true` → 退出码 0;
* `ok: false` → 退出码非 0,并且把失败原因打出来;
* 文件不存在 → 退出码非 0(**不能**当成"没跑"糊过去)。
这样"判据本身坏了"会在 pytest 里就暴露,而不是在下一次装配时。
"""
from __future__ import annotations
import json
import pathlib
import re
import subprocess
import sys
import pytest
SCRIPT = pathlib.Path(__file__).resolve().parents[3] / "scripts" / "run_gauge_assembly.sh"
def _gate_code() -> str:
"""把脚本里判成败的那段 Python(heredoc)取出来。"""
text = SCRIPT.read_text(encoding="utf-8")
m = re.search(r"python3 - \"\$\{RESULT_FILE\}\" <<'PY'\n(.*?)\nPY\n", text, re.S)
assert m, "脚本里找不到判成败的 heredoc(改了写法就要同步改本测试)"
return m.group(1)
def _run_gate(tmp_path: pathlib.Path, payload: object = None, *, missing: bool = False):
path = tmp_path / "result.json"
if not missing:
path.write_text(json.dumps(payload, ensure_ascii=False), encoding="utf-8")
proc = subprocess.run(
[sys.executable, "-", str(path)],
input=_gate_code(),
text=True,
capture_output=True,
check=False,
)
return proc
def test_success_payload_gives_exit_zero(tmp_path: pathlib.Path) -> None:
proc = _run_gate(tmp_path, {
"ok": True, "state": "DONE", "holes_total": 2, "holes_done": 2,
"relocations": 0, "dry_run": False,
})
assert proc.returncode == 0, proc.stdout + proc.stderr
assert "已装 2/2 孔" in proc.stdout
def test_dry_run_is_marked_in_the_summary(tmp_path: pathlib.Path) -> None:
""""干跑成功"和"真装配成功"必须在输出里分得清 —— 否则会被当成装配过了。"""
proc = _run_gate(tmp_path, {"ok": True, "state": "DONE", "holes_total": 1,
"holes_done": 1, "dry_run": True})
assert proc.returncode == 0
assert "干跑" in proc.stdout
def test_failure_payload_gives_nonzero_and_prints_reasons(tmp_path: pathlib.Path) -> None:
proc = _run_gate(tmp_path, {
"ok": False, "state": "ABORT", "exit_code": 1, "holes_total": 2, "holes_done": 0,
"failures": ["回零服务(/home)—— S01 回 home、S11 复位都靠它"],
"notes": ["跳过 S03:手外相机没标定"],
})
assert proc.returncode != 0
assert "回零服务" in proc.stdout
assert "跳过 S03" in proc.stdout
def test_missing_result_file_is_failure_not_silence(tmp_path: pathlib.Path) -> None:
"""结论文件不存在 = 节点没跑到收尾。**必须**失败并说清,不能静默通过。"""
proc = _run_gate(tmp_path, missing=True)
assert proc.returncode != 0
assert "没有结论文件" in proc.stdout or "没有结论文件" in proc.stderr
def test_broken_result_file_is_failure(tmp_path: pathlib.Path) -> None:
path = tmp_path / "result.json"
path.write_text("{ 这不是 JSON", encoding="utf-8")
proc = subprocess.run(
[sys.executable, "-", str(path)], input=_gate_code(), text=True,
capture_output=True, check=False,
)
assert proc.returncode != 0
assert "读不了" in proc.stdout or "读不了" in proc.stderr
@pytest.mark.parametrize("key", ["ok", "state", "holes_total", "holes_done"])
def test_gate_reads_keys_the_node_actually_writes(key: str) -> None:
"""脚本读的键,必须在节点写的 payload 里(改键名会打出一串 None)。"""
node = (SCRIPT.parents[1] / "src" / "arm_application" / "arm_application"
/ "gauge_assembly_node.py").read_text(encoding="utf-8")
m = re.search(r"payload = \{(.*?)\n \}", node, re.S)
assert m, "节点里找不到 payload 字典(改了写法就要同步改本测试)"
assert f'"{key}"' in m.group(1), f"节点 payload 里没有 {key},但脚本会读它"