通止规装配:配置段进 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:
@@ -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 节按**症状**列出全部历史反例
|
||||||
|
|||||||
@@ -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,禁止混用。
|
||||||
|
|||||||
@@ -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/ 下的子目录名一致。
|
||||||
|
|||||||
@@ -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 |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|||||||
@@ -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
@@ -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 |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|||||||
@@ -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` 契约
|
||||||
|
|||||||
@@ -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)。
|
||||||
|
|||||||
@@ -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 掉件处理、怎么调参、验收命令) |
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|||||||
@@ -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 "工件,通止规"` 改
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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,8 +124,7 @@ 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",
|
||||||
@@ -130,10 +137,11 @@ 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},但脚本会读它"
|
||||||
Reference in New Issue
Block a user