diff --git a/.cursor/rules/framework-core.mdc b/.cursor/rules/framework-core.mdc index aca3072..2602b2a 100644 --- a/.cursor/rules/framework-core.mdc +++ b/.cursor/rules/framework-core.mdc @@ -67,6 +67,18 @@ python3 scripts/check_text_only.py 它比对语法树（删 docstring + 抹平字符串常量 + f-string 只留插值）， 不一致就报出来。基准是 `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 节按**症状**列出全部历史反例 diff --git a/README.md b/README.md index 170f0ba..aebeacb 100644 --- a/README.md +++ b/README.md @@ -54,6 +54,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control | 入孔检测 | `scripts/run_hole_detect.sh --arm … --preview` | | Orisys 触觉 | `scripts/run_tactile.sh [--cli]` | | 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` | 89 与 91 **各有** 手眼、相机序列号、锁高、named poses，禁止混用。 diff --git a/config/site.example.yaml b/config/site.example.yaml index 90b6c3f..f9287cd 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -450,6 +450,93 @@ hole_detect: # Orisys 视触觉（夹爪左右指摄像机，不是腕部 D405）。SDK 单独 pip 安装。 # bash ~/ros2/scripts/run_tactile.sh --cli # 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: backend: "orisys" # orisys | mock # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。 diff --git a/docs/FRAMEWORK_RULES.md b/docs/FRAMEWORK_RULES.md index 0fd8fe6..ead4245 100644 --- a/docs/FRAMEWORK_RULES.md +++ b/docs/FRAMEWORK_RULES.md @@ -405,6 +405,53 @@ ROS 构建对非 ASCII 路径会出各种问题。工作目录保持 ASCII（推 （`scripts/build_sim.sh`）。两套并存是刻意的：同一份源码，两个环境的依赖路径不同， 共用 `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. 文档与提交 @@ -511,6 +558,39 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0 **预防**：构建脚本开头应当**断言**当前 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. 改动前的检查清单 @@ -577,6 +657,14 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0 | 服务/话题/TF 全在，但读数全是乱的 | 上一轮的 `sim_gripper_adapter` / `static_transform_publisher` 还活着 | R4.11 | | 命令"开 20 mm"，两指实际 21.75 mm，夹不住工件 | 上游 `` 是线性近似，开口必须走正运动学反解 | R4.4、坑 12 | | 残留进程把旧相机外参按在新仿真上 | `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 | --- diff --git a/docs/GAUGE_ASSEMBLY.md b/docs/GAUGE_ASSEMBLY.md new file mode 100644 index 0000000..ef1c5b5 --- /dev/null +++ b/docs/GAUGE_ASSEMBLY.md @@ -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-.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)：新手从这读起 diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index 46b6e6e..b433894 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -81,7 +81,8 @@ bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联（ | D435 手外相机（S03 粗定位兜底） | ❌ **还没做手眼标定**，链路不可用 | 见 `devices/cameras/README.md` | | 自动启动全部服务（一条命令拉起所有进程） | ❌ 还没做，目前靠三个终端手工起 | 见第 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) | > 这张表是**承诺书**：❌ 的别当成已具备。文档说"已就绪"而实际没有， @@ -175,6 +176,7 @@ D405 YOLO 手眼 T 伺服 ros2_cont | 算法权重（换模型） | `models/<分组>/` + `site.yaml` 的路径 | `scripts/weights.sh list` | [`models/README.md`](../models/README.md) | | 接一个新品牌设备 | `devices/<类别>/<品牌>/` + `registry.yaml` 一行 | `ros2 run arm_interfaces device_check` | [`devices/README.md`](../devices/README.md) | | 命名位姿（回零点等） | `src/arm_control/config/named_poses_89.yaml` / `_91.yaml` | `go_home.sh --arm 89` | [`INTERFACES.md`](INTERFACES.md) | +| 通止规装配的参数（预绕角/力阈值/孔数） | `site.yaml` → `gauge_assembly:`（每个键都有注释） | `run_gauge_assembly.sh --arm 89 --dry-run` | [`GAUGE_ASSEMBLY.md`](GAUGE_ASSEMBLY.md) | | 框架规矩本身 | `docs/FRAMEWORK_RULES.md`，**同时**改 `.cursor/rules/` | `python3 scripts/check_text_only.py` | R6.1 | ### 3.2 改一个现场数值（最高频） @@ -397,6 +399,9 @@ python3 -m pytest src/arm_sim_gazebo/test -q 让人跑去查夹爪代码（R4.12）。 - 上一轮的**残留进程**伪造出"代码写错了"的现场 —— 两个同名适配器抢话题/服务， 残留的 `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 | | 脚本说"已停止"，进程还在 | 进程模式表漏项 | R4.10 | | 孔检测在仿真里怎么都不过 | 图幅与真机不同，像素门槛失效 | 仿真手册 §4.3 | +| `site.yaml` 改了参数，行为一点没变 | 那个键**没接线**（配置表里有、代码里从没读） | R1.2 | +| 流程退出码 1，日志里一个字都没有 | 入口 `os._exit()` 把异常吞了 | R6.9 | +| 同一个手眼 TF，换个脚本跑出来不一样 | 四元数换算被抄了多份 | R5.8 | +| 通止规流程 S00 就停住、说不缺的东西缺了 | 自检的"期望类型"与流程实际调用的类型不一致 | R5.9 | --- diff --git a/docs/INTERFACES.md b/docs/INTERFACES.md index a90591d..0b72fec 100644 --- a/docs/INTERFACES.md +++ b/docs/INTERFACES.md @@ -21,6 +21,7 @@ | `run_dual_gripper.sh` | 两侧 `robots[].gripper` | 双爪 ROS 节点 | | `run_hole_detect.sh --arm …` | `hole_detect.*` `camera` | 入孔像素 | | `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` | | `export_yolo_trt.sh [onnx] [imgsz]` | 权重 | **旧命令**，转发壳 → `weights.sh convert --to engine` | | `show_site_config.sh` | `site.yaml` | 打印解析结果 | @@ -107,6 +108,20 @@ PGEA 单爪节点（`--arm 89` 无 `--cli`）： | `/tactile/left/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` 契约 diff --git a/docs/SITE_CONFIG.md b/docs/SITE_CONFIG.md index 186a511..70f012c 100644 --- a/docs/SITE_CONFIG.md +++ b/docs/SITE_CONFIG.md @@ -50,6 +50,7 @@ bash ~/ros2/scripts/run_handeye.sh --arm 91 teach # 91 必须单独标定 | `hole_detect` | `run_hole_detect.sh`（算法参数也在这一段） | | `tactile` | `run_tactile.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） | | `hole_detect.backend` `hough_*` `conf` | 入孔检测 | | `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。 + +`gauge_assembly:` 段**每个键都有中文注释与取值建议**，直接读 +`config/site.yaml` 那一段即可；流程与判据见 [GAUGE_ASSEMBLY.md](GAUGE_ASSEMBLY.md)。 +键名拼错会**直接报错并给出最相近的候选名**，不会静默退回默认值（R1.2）。 diff --git a/docs/WORKSPACE.md b/docs/WORKSPACE.md index 5f92208..ebe132c 100644 --- a/docs/WORKSPACE.md +++ b/docs/WORKSPACE.md @@ -106,6 +106,7 @@ devices/// | `run_dual_gripper.sh` | 双爪同一 ROS 节点（`arm_msgs`） | | `run_hole_detect.sh --arm … [--preview]` | 入孔检测（只出像素孔心，不插入） | | `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` | | `build_all.sh` | `build.sh`（arm 包）+ `build_real.sh`（Flexiv 硬件包） | | `build.sh` | colcon：`arm_*` + `flexiv_description` | @@ -191,6 +192,7 @@ devices/// | `TACTILE.md` | Orisys 视触觉 | | `INTERFACES.md` | 脚本 / ROS topic / 参数契约 | | `TRAJECTORIES.md` | 关节轨迹回放 | +| `GAUGE_ASSEMBLY.md` | **通止规装配流程**（状态机、判据、E1 掉件处理、怎么调参、验收命令） | --- diff --git a/scripts/README.md b/scripts/README.md index 642f56f..bf174b8 100644 --- a/scripts/README.md +++ b/scripts/README.md @@ -8,6 +8,15 @@ **夹爪：** `run_gripper.sh --arm 89|91` · `run_dual_gripper.sh` **入孔：** `run_hole_detect.sh --arm … [--preview]` **触觉：** `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 convert <文件> --name <模型名>` —— PT→ONNX→TensorRT，自动改名归位 + 固化标签 - 标签：`weights.sh labels <模型名>` 看，`--set "工件,通止规"` 改 diff --git a/scripts/run_gauge_assembly.sh b/scripts/run_gauge_assembly.sh index 6462ad7..3cd1026 100755 --- a/scripts/run_gauge_assembly.sh +++ b/scripts/run_gauge_assembly.sh @@ -24,6 +24,10 @@ # 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看读数量级 # 掉件重放：bash ~/ros2/scripts/run_gauge_assembly.sh 之外另开一个终端 # 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_.json（节点写，本脚本读） set -eo pipefail 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 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 [[ "${DRY_RUN}" == true ]]; then ARGS+=(dry_run:=true); fi @@ -88,3 +96,38 @@ echo "=== launch gauge_assembly（自检不过会自动报缺什么） ===" # 不用 exec：exec 之后 Ctrl-C 直接打给 ros2 launch，这里的清理就没了； # gauge_assembly 自己也会在收到 KeyboardInterrupt 时退到安全高度。 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 diff --git a/src/arm_application/README.md b/src/arm_application/README.md index 7341300..df96274 100644 --- a/src/arm_application/README.md +++ b/src/arm_application/README.md @@ -19,9 +19,23 @@ | `goto_ee_plane` | `goto_ee_plane.py` | 可选：抬高再回 home（默认启动不再调用） | | `demo_named_poses` | `demo_named_poses.py` | mock/实机点位示例 | | `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)。 +## 通止规装配的模块分工（`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/hole_detect.launch.py` | 入孔检测 | | `launch/tactile.launch.py` | 视触觉 | +| `launch/gauge_assembly.launch.py` | 通止规装配（只发布手眼静态 TF；前置服务自己先起） | diff --git a/src/arm_application/arm_application/gauge_assembly_node.py b/src/arm_application/arm_application/gauge_assembly_node.py index 3a40359..c755897 100644 --- a/src/arm_application/arm_application/gauge_assembly_node.py +++ b/src/arm_application/arm_application/gauge_assembly_node.py @@ -55,7 +55,7 @@ import math import os import time 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 rclpy @@ -198,13 +198,24 @@ class GripperClient: 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): self._node.get_logger().error(f"夹爪服务 {tag} 不在（后端 {self.backend}）") return False if self.backend == "contract": req: Any = GripperExecute.Request() 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: req = Trigger.Request() fut = client.call_async(req) @@ -236,7 +247,11 @@ class GripperClient: if self._node.cfg.dry_run: self._node.get_logger().info("[干跑] 夹爪夹紧") 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): @@ -270,6 +285,11 @@ class GaugeAssembly(AssemblyPipeline): self.declare_parameter("dry_run", False) self.declare_parameter("holes_total", -1) # >0 覆盖 site 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() overrides: Dict[str, Any] = {} @@ -340,6 +360,8 @@ class GaugeAssembly(AssemblyPipeline): 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_result_pub = self.create_publisher(String, "/gauge/result", 10) self._gauge_progress_pub = self.create_publisher(String, "/gauge/progress", 10) @@ -433,10 +455,29 @@ class GaugeAssembly(AssemblyPipeline): payload = dict(payload) 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)] + # 先落盘再发话题：落盘是**给脚本/CI 读**的（话题在进程退出后就没了）， + # 顺序反过来的话，"话题发出去了、文件还没写完就被杀掉"会让脚本读不到结论。 + self._write_result_file(payload) msg = String() msg.data = json.dumps(payload, ensure_ascii=False) 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]]: """拿关节7的限位（从 ``/robot_description``）。拿不到返回 None。""" t0 = time.monotonic() @@ -898,8 +939,11 @@ class GaugeAssembly(AssemblyPipeline): f" 做法：bash scripts/run_handeye.sh --mode eye_to_hand（标完再跑本流程）" ) return Event.COARSE_FAIL - if not self._camera_alive(ns): - self.get_logger().warn(f"S03 相机 {ns} 没有图像，回 S04 跟踪") + if not self._wait_coarse_frame(ns): + self.get_logger().warn( + f"S03 相机 {ns} 等了 {self.cfg.coarse_timeout_s:.0f}s 没有新鲜图像" + "（相机没开？），回 S04 跟踪" + ) return Event.COARSE_FAIL xy = self._coarse_workpiece_xy(ns, he) @@ -938,6 +982,19 @@ class GaugeAssembly(AssemblyPipeline): def _camera_alive(self, ns: str) -> bool: 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]]: """手外相机拍一张 → 推理工件框中心 → 反投影到基座 XY。 @@ -1164,7 +1221,15 @@ class GaugeAssembly(AssemblyPipeline): max_depth_m = self.cfg.contact_max_depth_mm / 1000.0 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]) - 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: self.get_logger().warn("S07 单步未完全到位，继续按实测高度判断") try: @@ -1468,10 +1533,19 @@ class GaugeAssembly(AssemblyPipeline): self._safe_retract() 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 = { "ok": ok, "state": str(self.fsm.state), "holes_total": self.cfg.holes_total, + "holes_done": holes_done, "hole_index": self.fsm.ctx.hole_index, "relocations": self.fsm.ctx.relocations, "notes": self.fsm.ctx.notes, @@ -1524,6 +1598,16 @@ def main(args: Optional[Sequence[str]] = None) -> None: node.get_logger().warn("用户中断 —— 按安全高度退出") code = 130 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: try: node._stop_stage1() diff --git a/src/arm_application/arm_application/gauge_config.py b/src/arm_application/arm_application/gauge_config.py index 5241a9c..db10176 100644 --- a/src/arm_application/arm_application/gauge_config.py +++ b/src/arm_application/arm_application/gauge_config.py @@ -37,7 +37,6 @@ _UNIT_SUFFIX: Dict[str, str] = { #: 没有单位又不登记的键，会让"单位自检"变成一句空话（规则 M1）。 _UNITLESS = frozenset( { - "require_preview", "extreme_direction", "grip_check", "coarse_enabled", @@ -54,7 +53,6 @@ _UNITLESS = frozenset( #: 每个键的**类型**（用于类型校验，不做隐式转换：``"20"`` 不当 20 用）。 _TYPES: Dict[str, type] = { "boot_timeout_s": float, - "require_preview": bool, "extreme_angle_deg": float, "extreme_speed_deg_s": float, "extreme_direction": int, @@ -66,12 +64,9 @@ _TYPES: Dict[str, type] = { "coarse_hover_m": float, "coarse_conf": float, "coarse_timeout_s": float, - "track_lost_s": float, - "track_prewarm_s": float, "snap_stable_s": float, "snap_tol_px": float, "snap_min_frames": int, - "hover_m": float, "align_tol_mm": float, "contact_speed_m_s": float, "contact_step_mm": float, @@ -96,7 +91,6 @@ _TYPES: Dict[str, type] = { #: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。 _UNITS: Dict[str, str] = { "boot_timeout_s": "s", - "require_preview": "", "extreme_angle_deg": "deg", "extreme_speed_deg_s": "deg/s", "extreme_direction": "", @@ -108,12 +102,9 @@ _UNITS: Dict[str, str] = { "coarse_hover_m": "m", "coarse_conf": "", "coarse_timeout_s": "s", - "track_lost_s": "s", - "track_prewarm_s": "s", "snap_stable_s": "s", "snap_tol_px": "px", "snap_min_frames": "", - "hover_m": "m", "align_tol_mm": "mm", "contact_speed_m_s": "m/s", "contact_step_mm": "mm", @@ -157,7 +148,6 @@ class GaugeConfigError(ValueError): #: 默认值只是"能跑起来的起点"，必须在 ``describe()`` 里标出来让现场知道要改。 DEFAULTS: Dict[str, Any] = { "boot_timeout_s": 30.0, - "require_preview": True, # S01：逆时针预绕。符号靠 extreme_direction 定，不靠改代码里的负号。 "extreme_angle_deg": 150.0, "extreme_speed_deg_s": 20.0, @@ -172,15 +162,11 @@ DEFAULTS: Dict[str, Any] = { "coarse_hover_m": 0.25, "coarse_conf": 0.35, "coarse_timeout_s": 20.0, - # S04 - "track_lost_s": 3.0, - "track_prewarm_s": 2.0, # S05 "snap_stable_s": 1.0, "snap_tol_px": 2.0, "snap_min_frames": 6, # S06 - "hover_m": 0.05, "align_tol_mm": 1.0, # S07 "contact_speed_m_s": 0.02, @@ -214,7 +200,6 @@ class GaugeConfig: """通止规装配的全部可调量（解析并校验之后）。""" boot_timeout_s: float - require_preview: bool extreme_angle_deg: float extreme_speed_deg_s: float extreme_direction: int @@ -226,12 +211,9 @@ class GaugeConfig: coarse_hover_m: float coarse_conf: float coarse_timeout_s: float - track_lost_s: float - track_prewarm_s: float snap_stable_s: float snap_tol_px: float snap_min_frames: int - hover_m: float align_tol_mm: float contact_speed_m_s: float contact_step_mm: float @@ -283,7 +265,9 @@ class GaugeConfig: 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" 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"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm" "（触觉读数是任意单位增量，必须现场标定）", diff --git a/src/arm_application/launch/gauge_assembly.launch.py b/src/arm_application/launch/gauge_assembly.launch.py index 7980c43..4f2398c 100755 --- a/src/arm_application/launch/gauge_assembly.launch.py +++ b/src/arm_application/launch/gauge_assembly.launch.py @@ -23,7 +23,14 @@ import sys from pathlib import Path 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_ros.actions import Node @@ -86,6 +93,7 @@ def _launch_setup(context, *args, **kwargs): dry_run = _flag("dry_run") holes = LaunchConfiguration("holes_total").perform(context).strip() backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto" + result_file = LaunchConfiguration("result_file").perform(context).strip() nodes = [] frames = [] @@ -116,11 +124,10 @@ def _launch_setup(context, *args, **kwargs): "—— 不发布静态 TF，S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh" ) - nodes.append( - Node( - package="arm_application", - executable="gauge_assembly", - name="gauge_assembly", + gauge_node = Node( + package="arm_application", + executable="gauge_assembly", + name="gauge_assembly", output="screen", parameters=[ { @@ -129,11 +136,12 @@ def _launch_setup(context, *args, **kwargs): ), "dry_run": dry_run, "holes_total": int(holes) if holes else -1, - "gripper_backend": backend, - } - ], - ) + "gripper_backend": backend, + "result_file": result_file, + } + ], ) + nodes.append(gauge_node) return [ LogInfo(msg=f"[gauge] site={path}"), @@ -141,8 +149,16 @@ def _launch_setup(context, *args, **kwargs): LogInfo( 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], *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("holes_total", default_value=""), DeclareLaunchArgument("gripper_backend", default_value="auto"), + # 结论落盘路径（脚本读它判成败：ros2 launch 的退出码恒为 0） + DeclareLaunchArgument("result_file", default_value="/tmp/gauge_assembly_result.json"), OpaqueFunction(function=_launch_setup), ] ) diff --git a/src/arm_application/test/test_gauge_result_gate.py b/src/arm_application/test/test_gauge_result_gate.py new file mode 100644 index 0000000..c1f0834 --- /dev/null +++ b/src/arm_application/test/test_gauge_result_gate.py @@ -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}，但脚本会读它"