通止规装配:配置段进 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:
@@ -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;前置服务自己先起) |
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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"
|
||||
"(触觉读数是任意单位增量,必须现场标定)",
|
||||
|
||||
@@ -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),
|
||||
]
|
||||
)
|
||||
|
||||
@@ -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