diff --git a/src/arm_application/arm_application/gauge_boot.py b/src/arm_application/arm_application/gauge_boot.py new file mode 100644 index 0000000..c23a66b --- /dev/null +++ b/src/arm_application/arm_application/gauge_boot.py @@ -0,0 +1,306 @@ +"""S00 开机自检：把"能不能跑"这件事变成一张**可打印的清单**（纯逻辑部分）。 + +为什么要有这一阶段 +------------------ +看板 S00 的要求是"各服务就绪后才开指令窗，任一不就绪即中止并报告"。它挡的是 +这个仓库最常见的一类事故：**流程跑起来了，只是没接上硬件**。例如： + +* 触觉没起 → 三阶段的力阈值全是 0 → 搜孔"一直没找到"（实际是没数据）； +* 相机没起 → 精拍拿不到图 → 报"精拍未拿到图像"（离真正原因差三层）； +* 夹爪没起 → S08 松爪/夹紧静默失败 → 棘轮循环空转，看起来像"拧不动"。 + +放纯逻辑里的原因 +---------------- +"要检查哪些东西"是**事实**，会随现场（两台臂、两种夹爪后端、D435 有没有标定） +变化；"怎么检查"是 ROS 机制。前者做成纯函数，就能被测试钉住： + +* 少一项检查 → 测试红（比如"忘了查触觉"）； +* 检查项的顺序无关紧要，但**必检项**和**选检项**的划分必须明确 —— + 把核心判据依赖的触觉标成选检，就会在没传感器时一路跑到力判据才失败。 +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Dict, Iterable, List, Optional, Sequence, Tuple + +#: 探针类型 —— 决定节点用哪种方式确认"就绪"。 +#: +#: ``service`` 服务在线且类型匹配 +#: ``action`` action server 在线 +#: ``topic`` 话题在，且**真的收到过消息**（只看 topic list 会假阳性） +#: ``tactile`` 触觉状态话题在发，且 ``connected=true`` +#: ``camera`` 图像话题在发（收到首帧） +#: ``detector`` 算法权重能加载（推理预热） +PROBES = ("service", "action", "topic", "tactile", "camera", "detector") + + +@dataclass(frozen=True) +class BootCheck: + """一项自检。""" + + key: str + """稳定标识（日志/结果 JSON 里用它，改名要同步文档）。""" + + label: str + """人读的名字。""" + + probe: str + """见 :data:`PROBES`。""" + + target: str + """要探测的对象：服务名 / action 名 / 话题名 / 权重路径。""" + + why: str + """**缺了会怎样**。自检失败的提示必须写这个 —— 只报"未就绪"没人知道该不该急。""" + + required: bool = True + """必检 / 选检。选检失败只告警，不拦流程。""" + + expect_type: Optional[str] = None + """期望的消息/服务类型（有值时类型不符即算失败）。""" + + +#: 需要"绝对名字"的探针：相对名字会按节点命名空间解析， +#: 节点被放进 namespace 之后 ``home`` 会变成 ``/ns/home`` —— 自检就会去探一个 +#: 不存在的名字，报"未就绪"。统一补前导斜杠，把这件事变成不可能发生。 +_ABSOLUTE_PROBES = frozenset({"service", "action", "topic", "tactile", "camera"}) + + +def _abs(name: str) -> str: + """把 ROS 名字规范成绝对名（``home`` → ``/home``）。""" + n = (name or "").strip() + if not n: + return n + return n if n.startswith("/") else "/" + n + + +def _topic(prefix: str, suffix: str) -> str: + """拼话题名，容忍 ``prefix`` 带不带前导斜杠。""" + p = "/" + prefix.strip("/") if prefix else "" + return f"{p}/{suffix.strip('/')}" + + +def build_checks( + *, + joint_action: str, + camera: str, + tactile_topic: str, + gripper_open: str, + gripper_close: str, + gripper_status: str, + gripper_status_type: str, + d435_camera: Optional[str] = None, + tool_name: str = "", + weights_path: str = "", +) -> List[BootCheck]: + """按现场情况生成检查清单。 + + :param joint_action: JTC action 名（``rizon_arm_controller/follow_joint_trajectory``）。 + :param camera: 腕部相机命名空间（``d405_left``）。 + :param tactile_topic: 触觉状态话题（``/tactile/left/status``）。 + :param gripper_status: 夹爪状态话题，随后端不同（``/gripper/status`` 或 ``/gripper/states``）。 + :param d435_camera: 手外相机命名空间。给 None 表示这台臂没有手外相机 → + 不生成 S03 相关的检查项（**不生成**比"生成了一个必然失败的项"更诚实： + 后者会让每次启动都报一条无关的错，很快所有人都不看自检输出了）。 + :param tool_name: ``site.yaml`` 的 ``robot.tool_name``。空 = 没指定工具系， + 则 S01 的"工具系垂直向下锁定"无法确认 → 单独列为选检告警。 + :param weights_path: 检测权重路径。空 = 不检查推理预热。 + + 清单的顺序 = 打印顺序 = 排障顺序：先臂、再夹爪、再相机、再触觉、最后算法。 + """ + checks: List[BootCheck] = [ + BootCheck( + key="arm_jtc", + label="机械臂关节轨迹控制器", + probe="action", + target=joint_action, + why="没有它，S01/S06/S07/S10 的所有运动都发不出去", + expect_type="arm_msgs/action/FollowJointTrajectory", + ), + BootCheck( + key="arm_home", + label="回零服务", + probe="service", + target="/home", + why="S01 回 home、S11 复位都靠它", + expect_type="std_srvs/srv/Trigger", + ), + BootCheck( + key="arm_named_pose", + label="命名位姿服务", + probe="service", + target="/set_named_pose", + why="/home 内部转发给它；它不在时回零会超时", + expect_type="arm_msgs/srv/SetNamedPose", + ), + BootCheck( + key="arm_switch_tool", + label="工具系切换服务", + probe="service", + target="/switch_tool", + why="tool1 必须全程垂直向下且位姿锁定，切不过去后面全是歪的", + expect_type="arm_msgs/srv/SwitchTool", + ), + BootCheck( + key="arm_compliant", + label="柔顺下压 action", + probe="action", + target="/compliant_descent", + why="S07 的柔顺接触靠它；缺了只能用位置控制硬压", + expect_type="arm_msgs/action/CompliantDescent", + ), + BootCheck( + key="gripper_open", + label="夹爪张开服务", + probe="service", + target=gripper_open, + why="S08 棘轮的『松爪』一步靠它；失败会让回转把规拧回去", + ), + BootCheck( + key="gripper_close", + label="夹爪夹紧服务", + probe="service", + target=gripper_close, + why="S08 的『夹紧』一步靠它；失败会让慢转时规打滑（磨螺纹）", + ), + BootCheck( + key="camera_color", + label="腕部相机彩图", + probe="camera", + target=_topic(camera, "color/image_raw"), + why="S05 精拍定位 k1/k2 靠它；缺了只能靠 S03 粗定位（精度不够）", + expect_type="sensor_msgs/msg/Image", + ), + BootCheck( + key="camera_info", + label="腕部相机内参", + probe="topic", + target=_topic(camera, "color/camera_info"), + why="没有内参就无法把像素反投影成基座坐标（会退化成 60° 默认视场）", + expect_type="sensor_msgs/msg/CameraInfo", + ), + BootCheck( + key="tactile_status", + label="触觉三向力", + probe="tactile", + target=tactile_topic, + why="S07 切向力突变停、S08 拧紧判据、E1 掉件都依赖它；" + "缺了这几个判据全部失效（流程能走完但结果不可信）", + expect_type="arm_msgs/msg/TactileStatus", + ), + BootCheck( + key="gripper_status", + label="夹爪状态反馈", + probe="topic", + target=gripper_status, + why="E1 掉件检测靠它；缺了掉件只能靠人看", + expect_type=gripper_status_type, + ), + ] + + if d435_camera: + checks.append( + BootCheck( + key="coarse_camera", + label="手外粗定位相机", + probe="camera", + target=_topic(d435_camera, "color/image_raw"), + why="S03 兜底粗定位用；缺了只能在 S04 跟踪上多试几次", + required=False, + expect_type="sensor_msgs/msg/Image", + ) + ) + if tool_name: + checks.append( + BootCheck( + key="tool_frame", + label="工具坐标系已指定", + probe="service", # 具体内容由节点按 tool_name 复核（只查服务在不在） + target="/switch_tool", + why=f"要求 tool1（垂直向下、位姿锁定），site.yaml 里配的是 {tool_name!r}", + required=False, + expect_type="arm_msgs/srv/SwitchTool", + ) + ) + if weights_path: + checks.append( + BootCheck( + key="detector", + label="检测算法预热", + probe="detector", + target=weights_path, + why="S04 跟踪、S03 粗定位都要它；预热失败会在第一次跟踪时才炸", + ) + ) + + # 网络端点统一成绝对名（见 _ABSOLUTE_PROBES 的说明）。 + # detector 的 target 是文件路径，不参与规范化。 + return [ + c if c.probe not in _ABSOLUTE_PROBES + else BootCheck( + key=c.key, label=c.label, probe=c.probe, target=_abs(c.target), + why=c.why, required=c.required, expect_type=c.expect_type, + ) + for c in checks + ] + + +def format_report(results: Sequence[Tuple[BootCheck, bool, str]]) -> Tuple[str, List[str]]: + """把逐项结果排成一张表，返回 ``(表文本, 失败项描述)``。 + + ``results`` 每项是 ``(检查项, 是否就绪, 细节)``。 + + 两张输出分开是为了让调用方能按"必检失败"决定是否中止：表用来给人看， + 失败项描述用来进日志/结果 JSON。 + """ + lines: List[str] = ["S00 自检："] + failures: List[str] = [] + for item, ok, detail in results: + mark = "OK " if ok else ("缺失" if item.required else "告警") + tag = "" if item.required else "（选检）" + lines.append(f" [{mark}] {item.label}{tag} {item.target}") + if detail: + lines.append(f" {detail}") + if not ok: + prefix = "必检项未就绪" if item.required else "选检项未就绪" + failures.append(f"{prefix}：{item.label}（{item.target}）—— {item.why}") + if failures: + lines.append(f" 共 {len(failures)} 项未就绪") + else: + lines.append(" 全部就绪，开指令窗") + return "\n".join(lines), failures + + +def blocking_failures(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]: + """只挑出**必检且未就绪**的项 —— 决定要不要中止的就是它们。 + + 和 :func:`format_report` 的 ``failures`` 的区别：那里两项都收（含选检）， + 这里只收必检。合成一个函数的话，调用方一不留神就会拿选检失败去中止流程， + 于是"D435 没接"变成"装配跑不起来"。 + """ + return [ + f"必检项未就绪：{item.label}（{item.target}）—— {item.why}" + for item, ok, _detail in results + if (not ok) and item.required + ] + + +def optional_warnings(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]: + """选检未就绪的告警（不拦流程，但必须打出来）。""" + return [ + f"选检项未就绪：{item.label}（{item.target}）—— {item.why}" + for item, ok, _detail in results + if (not ok) and not item.required + ] + + +def summary_counts(results: Sequence[Tuple[BootCheck, bool, str]]) -> Dict[str, int]: + """统计``必检/选检``各自通过多少，给启动日志一行摘要。""" + out = {"required_ok": 0, "required_total": 0, "optional_ok": 0, "optional_total": 0} + for item, ok, _detail in results: + slot = "required" if item.required else "optional" + out[f"{slot}_total"] += 1 + if ok: + out[f"{slot}_ok"] += 1 + return out diff --git a/src/arm_application/arm_application/gauge_config.py b/src/arm_application/arm_application/gauge_config.py new file mode 100644 index 0000000..92a3dce --- /dev/null +++ b/src/arm_application/arm_application/gauge_config.py @@ -0,0 +1,468 @@ +"""通止规装配的配置：从 ``site.yaml`` 的 ``gauge_assembly:`` 段解析 + 校验。 + +为什么单独一个模块 +------------------ +这一段的键**全是会动的量**（力阈值、角度、悬高、超时），也是现场最常改的地方。 +散在节点里读的话会有三个已知坑（都被这个仓库踩过）： + +1. **名字与单位不一致** —— ``hover`` 填了毫米、代码按米用（规则 M3）； +2. **键没接线** —— YAML 里写着 ``screw_step_deg``，代码读的是 ``screw_steps``， + 于是改了没效果，而报错也没有（``config_guard`` 模块的头注释就是这件事）； +3. **拼错键名** —— YAML 静默接受，行为退回默认值，现场"改了参数没反应"。 + +所以：所有键在一张表里声明（含单位），解析时**逐条校验**，多出来的键直接 +报错并给出最相近的候选名，缺的键按 required/default 明确处理。 +``describe()`` 把**派生**结果（比如"逆时针最多能转 147°"）打出来，让现场 +看到的是"实际生效的值"，而不是"我填的值"。 +""" + +from __future__ import annotations + +import difflib +from dataclasses import dataclass, fields +from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple + +#: 合法的单位后缀。键名必须以 ``_<单位>`` 结尾（``au`` = Orisys 任意单位增量）。 +_UNIT_SUFFIX: Dict[str, str] = { + "s": "_s", + "deg": "_deg", + "mm": "_mm", + "m": "_m", + "px": "_px", + "pct": "_pct", + "au": "_delta", +} + +#: 显式无量纲的键（布尔、计数、比例、枚举）。**必须在这里列出来** —— +#: 没有单位又不登记的键，会让"单位自检"变成一句空话（规则 M1）。 +_UNITLESS = frozenset( + { + "require_preview", + "extreme_direction", + "grip_check", + "coarse_enabled", + "coarse_camera", + "coarse_conf", + "snap_min_frames", + "screw_max_cycles", + "unscrew_extra_cycles", + "holes_total", + "dry_run", + } +) + +#: 每个键的**类型**（用于类型校验，不做隐式转换：``"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, + "extreme_margin_deg": float, + "grip_check": bool, + "grip_hold_s": float, + "coarse_enabled": bool, + "coarse_camera": str, + "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, + "contact_max_depth_mm": float, + "contact_tangential_delta": float, + "contact_safe_delta": float, + "screw_step_deg": float, + "screw_speed_deg_s": float, + "screw_back_speed_deg_s": float, + "screw_max_cycles": int, + "screw_normal_delta": float, + "screw_shear_delta": float, + "screw_grip_force_pct": float, + "screw_grip_speed_pct": float, + "screw_release_wait_s": float, + "unscrew_extra_cycles": int, + "holes_total": int, + "dry_run": bool, +} + +#: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。 +_UNITS: Dict[str, str] = { + "boot_timeout_s": "s", + "require_preview": "", + "extreme_angle_deg": "deg", + "extreme_speed_deg_s": "deg/s", + "extreme_direction": "", + "extreme_margin_deg": "deg", + "grip_check": "", + "grip_hold_s": "s", + "coarse_enabled": "", + "coarse_camera": "", + "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", + "contact_max_depth_mm": "mm", + "contact_tangential_delta": "au", + "contact_safe_delta": "au", + "screw_step_deg": "deg", + "screw_speed_deg_s": "deg/s", + "screw_back_speed_deg_s": "deg/s", + "screw_max_cycles": "", + "screw_normal_delta": "au", + "screw_shear_delta": "au", + "screw_grip_force_pct": "pct", + "screw_grip_speed_pct": "pct", + "screw_release_wait_s": "s", + "unscrew_extra_cycles": "", + "holes_total": "", + "dry_run": "", +} + +#: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类 +#: 复合单位只检查主单位段。 +_SUFFIX_OVERRIDE: Dict[str, str] = { + # 复合单位：后缀取"最后一个单位段"，键名里写全 + "contact_speed_m_s": "_m_s", + "extreme_speed_deg_s": "_deg_s", + "screw_speed_deg_s": "_deg_s", + "screw_back_speed_deg_s": "_deg_s", +} + + +class GaugeConfigError(ValueError): + """``gauge_assembly:`` 段配置错误。消息里必须给出**改哪里**。""" + + +# ============================================================================= +# 默认值 +# ============================================================================= +#: 缺省值。**只有真的有现场依据的才写默认值**：力阈值这类必须现场标定的， +#: 默认值只是"能跑起来的起点"，必须在 ``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, + "extreme_direction": 1, + "extreme_margin_deg": 3.0, + # S02（预留） + "grip_check": False, + "grip_hold_s": 0.5, + # S03 + "coarse_enabled": True, + "coarse_camera": "d435", + "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, + "contact_step_mm": 0.5, + "contact_max_depth_mm": 25.0, + "contact_tangential_delta": 300.0, + "contact_safe_delta": 2000.0, + # S08 + "screw_step_deg": 180.0, + "screw_speed_deg_s": 30.0, + "screw_back_speed_deg_s": 120.0, + "screw_max_cycles": 12, + "screw_normal_delta": 400.0, + "screw_shear_delta": 600.0, + "screw_grip_force_pct": 50.0, + "screw_grip_speed_pct": 50.0, + "screw_release_wait_s": 0.3, + # S09 + "unscrew_extra_cycles": 1, + # 全局 + "holes_total": 2, + "dry_run": False, +} + + +@dataclass(frozen=True) +class GaugeConfig: + """通止规装配的全部可调量（解析并校验之后）。""" + + boot_timeout_s: float + require_preview: bool + extreme_angle_deg: float + extreme_speed_deg_s: float + extreme_direction: int + extreme_margin_deg: float + grip_check: bool + grip_hold_s: float + coarse_enabled: bool + coarse_camera: str + 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 + contact_max_depth_mm: float + contact_tangential_delta: float + contact_safe_delta: float + screw_step_deg: float + screw_speed_deg_s: float + screw_back_speed_deg_s: float + screw_max_cycles: int + screw_normal_delta: float + screw_shear_delta: float + screw_grip_force_pct: float + screw_grip_speed_pct: float + screw_release_wait_s: float + unscrew_extra_cycles: int + holes_total: int + dry_run: bool + + # -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）------------------- + @property + def screw_step_rad(self) -> float: + import math + + return math.radians(self.screw_step_deg) + + @property + def total_screw_deg(self) -> float: + """一根规要拧进去的总角度 = 单次转角 × 循环数。""" + return self.screw_step_deg * self.screw_max_cycles + + @property + def unscrew_cycles(self) -> int: + """拧出要多转几轮：多留 ``unscrew_extra_cycles`` 轮，确保螺纹完全脱开。""" + return self.screw_max_cycles + max(0, self.unscrew_extra_cycles) + + def describe(self) -> List[str]: + """启动时打印的行：**实际生效的**值与派生结论。 + + 特别是 S01 的"能转多少度"和 S08 的"要拧多少轮"—— 这两个数是现场 + 判断"参数是否真的生效"最直接的依据，比打印"我读了 site.yaml"有用。 + """ + return [ + f"通止规装配：每孔孔数 {self.holes_total}，干跑={self.dry_run}", + f" S01 预绕 {self.extreme_angle_deg:.1f}°（方向={'逆时针' if self.extreme_direction > 0 else '顺时针'}，" + f"限位余量 {self.extreme_margin_deg:.1f}°，关节速度 {self.extreme_speed_deg_s:.1f}°/s）", + f" S03 粗定位={'开' if self.coarse_enabled else '关'} 相机={self.coarse_camera} " + 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", + f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一），" + f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm" + "（触觉读数是任意单位增量，必须现场标定）", + f" S08 棘轮 {self.screw_step_deg:.0f}°×{self.screw_max_cycles} 轮 = 累计 " + f"{self.total_screw_deg:.0f}°；停止判据「切向 >+{self.screw_shear_delta:.0f} 且 " + f"法向 >+{self.screw_normal_delta:.0f}」", + f" S09 拧出 {self.unscrew_cycles} 轮（比拧入多 {self.unscrew_extra_cycles} 轮）", + ] + + +def _resolve_one(key: str, raw: Mapping[str, Any]) -> Tuple[Any, bool]: + """取一个键的值，返回 ``(值, 是否来自 site.yaml)``。""" + if key in raw: + value = raw[key] + elif key in DEFAULTS: + return DEFAULTS[key], False + else: + raise GaugeConfigError( + f"gauge_assembly.{key} 既没在 site.yaml 里给，也没有默认值。" + f"请在 config/site.yaml 的 gauge_assembly: 段补上" + f"（改完跑一次 bash scripts/show_site_config.sh 确认解析）" + ) + + want = _TYPES[key] + if want is bool and not isinstance(value, bool): + raise GaugeConfigError(_type_msg(key, value, "布尔 true/false")) + if want is int and (isinstance(value, bool) or not isinstance(value, int)): + raise GaugeConfigError(_type_msg(key, value, "整数")) + if want is float and (isinstance(value, bool) or not isinstance(value, (int, float))): + raise GaugeConfigError(_type_msg(key, value, "数字（可带小数）")) + if want is str and not isinstance(value, str): + raise GaugeConfigError(_type_msg(key, value, "字符串")) + return (float(value) if want is float else value), True + + +def _type_msg(key: str, value: Any, human: str) -> str: + unit = _UNITS[key] + hint = f"，单位 {unit}" if unit else "" + return ( + f"gauge_assembly.{key} 的类型不对：期望 {human}{hint}，" + f"实际读到 {value!r}（{type(value).__name__}）。" + "本模块不做隐式转换 —— \"20\" 不会被当成 20 用，" + "因为那正是「填了字符串、跑起来是默认值」这类静默失效的入口" + ) + + +def from_site(site: Mapping[str, Any], *, overrides: Optional[Mapping[str, Any]] = None) -> GaugeConfig: + """解析并校验 ``site.yaml`` 的 ``gauge_assembly:`` 段。 + + :param site: ``site.yaml`` 解析后的整份字典。 + :param overrides: CLI/launch 覆盖（只允许覆盖已知键，未知键报错）。 + :raises GaugeConfigError: 缺键、类型错、未知键、单位与键名不符。 + + 整段缺失**不报错**（全部走默认值），但会在 :meth:`GaugeConfig.describe` + 之外由调用方打一条告警；理由是"默认值能不能用"取决于现场， + 由节点决定要不要拒绝启动，配置模块只负责如实解析。 + """ + raw_any = site.get("gauge_assembly") or {} + if not isinstance(raw_any, Mapping): + raise GaugeConfigError( + "site.yaml 的 gauge_assembly: 应该是一段映射（键值对），" + f"实际是 {type(raw_any).__name__}" + ) + raw: Dict[str, Any] = dict(raw_any) + + if overrides: + bad = [k for k in overrides if k not in _TYPES] + if bad: + raise GaugeConfigError( + f"未知的覆盖参数: {', '.join(sorted(bad))}。" + f"可在 gauge_assembly 段覆盖的键见 gauge_config._TYPES" + ) + raw.update({k: v for k, v in overrides.items() if v is not None}) + + _reject_unknown(raw) + values = {key: _resolve_one(key, raw)[0] for key in _TYPES} + cfg = GaugeConfig(**values) + _sanity(cfg) + return cfg + + +def _reject_unknown(raw: Mapping[str, Any]) -> None: + """多余的键直接报错，并给出最相近的候选名。 + + 静默忽略多余键的代价是：现场写 ``screw_step_degs``，改参数没反应， + 而日志里一切正常。报错里带候选名，是因为拼错比写错新键常见得多。 + """ + unknown = sorted(set(raw) - set(_TYPES)) + if not unknown: + return + lines: List[str] = [] + for key in unknown: + close = difflib.get_close_matches(key, list(_TYPES), n=1, cutoff=0.6) + hint = f"，是不是想写 {close[0]}？" if close else "" + lines.append(f" - {key}{hint}") + raise GaugeConfigError( + "site.yaml 的 gauge_assembly: 段里有本框架不认识的键：\n" + + "\n".join(lines) + + "\n多余的键不会被忽略，因为「写了没生效」比「写错报错」难查得多。" + "确认要新增配置项时，先在 gauge_config._TYPES / _UNITS 里登记" + ) + + +def _sanity(cfg: GaugeConfig) -> None: + """跨键的合理性检查：单键合法、组合起来没意义的，在这里挡住。""" + if cfg.holes_total < 1: + raise GaugeConfigError(f"gauge_assembly.holes_total 至少为 1，收到 {cfg.holes_total}") + if cfg.extreme_direction not in (-1, 1): + raise GaugeConfigError( + f"gauge_assembly.extreme_direction 只能是 +1（逆时针）或 -1（顺时针），" + f"收到 {cfg.extreme_direction}" + ) + if cfg.screw_step_deg <= 0.0: + raise GaugeConfigError( + f"gauge_assembly.screw_step_deg 必须为正，收到 {cfg.screw_step_deg}" + ) + if cfg.screw_max_cycles < 1: + raise GaugeConfigError( + f"gauge_assembly.screw_max_cycles 至少为 1，收到 {cfg.screw_max_cycles}" + ) + if cfg.extreme_angle_deg <= 0.0: + raise GaugeConfigError( + f"gauge_assembly.extreme_angle_deg 必须为正（方向由 extreme_direction 决定，" + f"不要用负角度表方向），收到 {cfg.extreme_angle_deg}" + ) + if cfg.contact_safe_delta <= cfg.contact_tangential_delta: + raise GaugeConfigError( + f"contact_safe_delta({cfg.contact_safe_delta}) 必须大于 " + f"contact_tangential_delta({cfg.contact_tangential_delta})：" + "前者是「立刻抬臂」的安全上限，后者只是「停止下压」。" + "两者相等的话，接触一开始就会触发安全上限" + ) + if cfg.snap_tol_px <= 0.0: + raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}") + + +# ============================================================================= +# 自检：键名必须体现单位（防"填了毫米按米用"） +# ============================================================================= +def unit_violations(keys: Optional[Iterable[str]] = None) -> List[str]: + """返回"键名与单位不符"的清单（空 = 全部合规）。 + + 规则：单位非空的键，**键名必须以对应的单位后缀结尾**（见 :data:`_UNIT_SUFFIX`）。 + 例外只有 :data:`_SUFFIX_OVERRIDE` 里显式登记的复合单位。 + + 为什么值得一条专门的检查：``hover`` 填毫米、代码按米用，会给出一个 + "看起来完全合理"的运动（悬停 5 变成悬停 5 米或 5 毫米）， + 而 :mod:`arm_interfaces.config_guard` 的头注释记录的正是这类事故。 + """ + bad: List[str] = [] + for key in keys if keys is not None else _UNITS: + unit = _UNITS[key] + if not unit: + if key not in _UNITLESS: + bad.append(f"{key}: 无量纲但未登记在 _UNITLESS") + continue + suffix = _SUFFIX_OVERRIDE.get(key) + if suffix is None: + base = unit.split("/")[0] + suffix = _UNIT_SUFFIX.get(base) + if suffix is None: + bad.append(f"{key}: 单位 {unit} 没有登记后缀（补 _UNIT_SUFFIX 或 _SUFFIX_OVERRIDE）") + continue + if not key.endswith(suffix): + bad.append(f"{key}: 单位是 {unit}，键名应以 {suffix} 结尾") + return bad + + +def missing_entries() -> List[str]: + """三张表（类型/单位/默认值）是否互相覆盖完整。""" + problems: List[str] = [] + for key in _TYPES: + if key not in _UNITS: + problems.append(f"{key}: 缺 _UNITS 登记") + if key not in DEFAULTS: + problems.append(f"{key}: 缺 DEFAULTS（required 的键也应写一个起点值并在文档里标注必标定）") + for key in _UNITS: + if key not in _TYPES: + problems.append(f"{key}: 在 _UNITS 里但不在 _TYPES 里") + for key in DEFAULTS: + if key not in _TYPES: + problems.append(f"{key}: 在 DEFAULTS 里但不在 _TYPES 里") + return problems + + +def dataclass_keys() -> List[str]: + """:class:`GaugeConfig` 的字段名 —— 与配置表比对，防"解析了但没存进对象"。""" + return [f.name for f in fields(GaugeConfig)] diff --git a/src/arm_application/arm_application/gauge_fsm.py b/src/arm_application/arm_application/gauge_fsm.py new file mode 100644 index 0000000..3117b82 --- /dev/null +++ b/src/arm_application/arm_application/gauge_fsm.py @@ -0,0 +1,445 @@ +"""通止规装配的状态机 —— **纯逻辑**，不 import rclpy，可离线单测。 + +为什么单独一个文件 +------------------ +状态机的价值全在"顺序"和"边界"上：S07 的力判据有没有在 S08 之前生效、 +E1 掉件之后到底回到哪一步、k2 是"回到 S05 记录的位姿"还是"重新拍照"。 +这些错**不会在运行时暴露成异常**，只会表现成"流程走得下去但结果不对"， +和力轴向映射反了一样属于静默失效（规则 R5.1）。所以转移表写成**数据**， +由测试逐条钉住，而不是散在节点的一堆 if/else 里。 + +设计口径 +-------- +- 状态编号与看板《五、通止规定位装配：状态机设计》一致（S00–S11）， + E1/E2 两条异常分支也在这里，**改名要同步改看板与文档**（规则 R6.1）。 +- 转移表只回答"给这个事件、从当前状态去哪"；**做不做得了**由节点判断 + （比如 S03 需要 D435 手眼，没标定就报 COARSE_FAIL 走 S04 兜底）。 + 状态机不掺硬件条件，否则没法离线测。 +- 上下文里的两个字段（`hole_index` / `holes_total`）只影响 S09 的出口： + 最后一个孔拧出后去 S11 复位，否则去 S10 换到下一个孔。 +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, field +from enum import Enum +from typing import Dict, List, Optional, Tuple + + +# ============================================================================= +# 状态与事件 +# ============================================================================= +class State(str, Enum): + """S00–S11 全部状态 + 两个终态。""" + + BOOT = "S00" # 各服务自检（伺服/相机/夹爪/触觉图传/推理预热） + HOME_EXTREME = "S01" # 回 home + 按 tool 系逆时针转到极限角 + GRIP_CHECK = "S02" # 夹持确认（预留，当前跳过） + COARSE = "S03" # 手外 D435 粗定位 + TRACK = "S04" # 腕部 D405 跟踪 k0 + SNAP = "S05" # 像素稳定后拍照定位 k1/k2 + 记位姿 + HOVER_K = "S06" # 到当前孔正悬停 + CONTACT = "S07" # 柔顺接触（切向力突变停） + SCREW_IN = "S08" # 旋转装配循环 + UNSCREW = "S09" # 反向拧出 + NEXT_HOLE = "S10" # 换到下一个孔（回到 S05 记录的位姿） + RESET = "S11" # 复位退出 + DONE = "DONE" # 正常结束 + ABORT = "ABORT" # 中止（已按安全顺序退出到安全高度） + + def __str__(self) -> str: # 日志里直接印 S07 而不是 State.CONTACT + return self.value + + +class Event(str, Enum): + """驱动状态机的事件。命名规则：``动作_结果``。""" + + BOOT_OK = "boot_ok" + BOOT_FAIL = "boot_fail" + + POSE_OK = "pose_ok" + JOINT_LIMIT = "joint_limit" # S01 转不动 = 已经贴着关节极限 + + GRIP_PASS = "grip_pass" + GRIP_FAIL = "grip_fail" + GRIP_SKIPPED = "grip_skipped" # S02 预留：没装/没标定，明确跳过 + + COARSE_OK = "coarse_ok" + COARSE_FAIL = "coarse_fail" + + TRACK_LOCKED = "track_locked" + TRACK_LOST = "track_lost" # 丢失超时 → 回 S03 兜底 + + SNAP_OK = "snap_ok" + SNAP_UNSTABLE = "snap_unstable" # 画面还在动，继续等 + + ALIGN_OK = "align_ok" + ALIGN_OUT = "align_out" # 对位超差 → 回 S05 重拍 + + CONTACT_OK = "contact_ok" + FORCE_OVER_LIMIT = "force_over_limit" # 力超安全上限 → 抬起 + + SCREW_DONE = "screw_done" + SCREW_STOP = "screw_stop" # 力判据满足：切向力>阈值 且 法向力突增 + GAUGE_DROPPED = "gauge_dropped" # E1：夹持丢了 + + UNSCREW_DONE = "unscrew_done" + + RELOADED = "reloaded" # E1：人工放好按 R 之后 + TIMEOUT = "timeout" # E2：任意阶段超时 + ABORT = "abort" # 用户 ^C / 外部中止 + + +#: 终态：到这里状态机不再接受事件。 +TERMINAL = frozenset({State.DONE, State.ABORT}) + + +# ============================================================================= +# 上下文：只放"影响去哪"的可变数据，不放硬件句柄 +# ============================================================================= +@dataclass +class GaugeContext: + """状态机运行上下文。 + + :param holes_total: 一次任务要装的孔数（k1/k2 两个就是 2）。 + :param hole_index: 当前孔序号，0 基。S10 会 +1。 + :param relocations: E1 掉件后人工重放计数，用于日志与上限保护。 + :param max_relocations: 允许的掉件重试次数，超了直接 ABORT， + 避免"掉件 → 重放 → 又掉"无限循环。 + """ + + holes_total: int = 2 + hole_index: int = 0 + relocations: int = 0 + max_relocations: int = 3 + notes: List[str] = field(default_factory=list) + + @property + def is_last_hole(self) -> bool: + return self.hole_index >= self.holes_total - 1 + + def hole_name(self) -> str: + """给日志用的人读孔名：k1 / k2。""" + return f"k{self.hole_index + 1}" + + +# ============================================================================= +# 转移表 +# ============================================================================= +#: ``(状态, 事件) -> 下一状态``。**没有列出的组合就是非法转移**， +#: 会抛 :class:`FsmError` —— 宁可当场炸，也不要"悄悄停在原地"。 +TRANSITIONS: Dict[Tuple[State, Event], State] = { + # --- S00 自检 --- + (State.BOOT, Event.BOOT_OK): State.HOME_EXTREME, + (State.BOOT, Event.BOOT_FAIL): State.ABORT, + # --- S01 home + 极限角 --- + (State.HOME_EXTREME, Event.POSE_OK): State.GRIP_CHECK, + # 转不动不是失败：已经贴着极限，就按当前角继续（提示即可） + (State.HOME_EXTREME, Event.JOINT_LIMIT): State.GRIP_CHECK, + # E1 掉件后半段：S01 流程走完 → 提示人工放置 → 按 R → 回记录位姿继续 + (State.HOME_EXTREME, Event.RELOADED): State.NEXT_HOLE, + # --- S02 夹持确认（预留）--- + (State.GRIP_CHECK, Event.GRIP_SKIPPED): State.TRACK, + (State.GRIP_CHECK, Event.GRIP_PASS): State.TRACK, + (State.GRIP_CHECK, Event.GRIP_FAIL): State.ABORT, + # --- S03 手外粗定位（S04 的兜底，不是必经）--- + (State.COARSE, Event.COARSE_OK): State.SNAP, + (State.COARSE, Event.COARSE_FAIL): State.TRACK, + # --- S04 跟踪 --- + (State.TRACK, Event.TRACK_LOCKED): State.SNAP, + (State.TRACK, Event.TRACK_LOST): State.COARSE, + # --- S05 稳定拍照 --- + (State.SNAP, Event.SNAP_OK): State.HOVER_K, + (State.SNAP, Event.SNAP_UNSTABLE): State.SNAP, # 自环：继续等稳定 + # --- S06 正悬停 --- + (State.HOVER_K, Event.ALIGN_OK): State.CONTACT, + (State.HOVER_K, Event.ALIGN_OUT): State.SNAP, # 对位超差 → 回 S05 重拍 + # --- S07 柔顺接触 --- + (State.CONTACT, Event.CONTACT_OK): State.SCREW_IN, + (State.CONTACT, Event.FORCE_OVER_LIMIT): State.ABORT, + # --- S08 旋转装配循环 --- + (State.SCREW_IN, Event.SCREW_DONE): State.UNSCREW, + (State.SCREW_IN, Event.SCREW_STOP): State.UNSCREW, + (State.SCREW_IN, Event.GAUGE_DROPPED): State.HOME_EXTREME, # E1 前半段 + # --- S09 反向拧出的出口与"是不是最后一个孔"有关，见 resolve_next() --- + # --- S10 换到下一个孔（= 回到 S05 记录的位姿），到了就重复 S06–S09 --- + (State.NEXT_HOLE, Event.POSE_OK): State.HOVER_K, + # --- S11 复位退出 --- + (State.RESET, Event.POSE_OK): State.DONE, +} + +#: 兜底：任意非终态收到 ABORT / TIMEOUT 都去对应终态。 +#: 单列出来是因为"每个状态各自列一遍"最容易漏，而漏的那个状态恰恰会在 +#: 真机上表现为"按了急停但流程还在跑"。 +GLOBAL: Dict[Event, State] = { + Event.ABORT: State.ABORT, + Event.TIMEOUT: State.ABORT, +} + + +def resolve_next(state: State, event: Event, ctx: GaugeContext) -> State: + """算出下一状态。 + + 绝大多数组合直接查 :data:`TRANSITIONS`；只有一处要结合上下文： + + * ``S09 + UNSCREW_DONE``：最后一个孔 → S11 复位；否则 → S10 换到下一个孔。 + + 其余"看起来需要上下文"的分支都用了**显式事件**，而不是让 resolve_next + 去猜（比如 E1 重放用 ``RELOADED`` 而不是"看 relocations 是否大于 0"）。 + 猜出来的分支在测试里只能靠改内部计数复现，读代码的人也得跟着一起猜。 + """ + # 全局兜底优先：谁都得能被中止 + if event in GLOBAL: + return GLOBAL[event] + + if state is State.UNSCREW and event is Event.UNSCREW_DONE: + return State.RESET if ctx.is_last_hole else State.NEXT_HOLE + + try: + return TRANSITIONS[(state, event)] + except KeyError: + raise FsmError(state, event, ctx) from None + + +class FsmError(RuntimeError): + """非法转移。附带当前状态与事件，方便直接定位是哪一步没接上。""" + + def __init__(self, state: State, event: Event, ctx: Optional[GaugeContext] = None) -> None: + self.state = state + self.event = event + ctx = ctx or GaugeContext() + super().__init__( + f"通止规装配状态机：状态 {state}（{STATE_DOC.get(state, '?')}）" + f"不接受事件 {event.value}。当前孔 {ctx.hole_name()}，" + f"已重放 {ctx.relocations} 次。" + "若是新增流程，先在 gauge_fsm.TRANSITIONS 里登记这条转移。" + ) + + +# ============================================================================= +# 状态机本体 +# ============================================================================= +@dataclass +class Step: + """一次转移的记录，用于日志与事后复盘。""" + + index: int + prev: State + event: Event + next: State + + def __str__(self) -> str: + return f"#{self.index} {self.prev} --{self.event.value}--> {self.next}" + + +class GaugeFsm: + """通止规装配状态机。 + + 用法:: + + fsm = GaugeFsm(GaugeContext(holes_total=2)) + fsm.handle(Event.BOOT_OK) # S00 -> S01 + print(fsm.state) # S01 + fsm.handle(Event.ABORT) # 任何状态都能中止 + """ + + def __init__(self, ctx: Optional[GaugeContext] = None, start: State = State.BOOT) -> None: + self.ctx = ctx or GaugeContext() + self.state = start + self.history: List[Step] = [] + + # -- 查询 --------------------------------------------------------------- + @property + def is_terminal(self) -> bool: + return self.state in TERMINAL + + @property + def ok(self) -> bool: + return self.state is State.DONE + + def can(self, event: Event) -> bool: + """这个事件在当前状态下合法吗（不改状态，给"要不要试"用）。""" + if event in GLOBAL: + return not self.is_terminal + try: + resolve_next(self.state, event, self.ctx) + except FsmError: + return False + return True + + # -- 推进 --------------------------------------------------------------- + def handle(self, event: Event) -> State: + """吃一个事件，返回新状态。非法转移抛 :class:`FsmError`。""" + if self.is_terminal: + raise FsmError(self.state, event, self.ctx) + prev = self.state + # 先算转移（非法转移在这里就抛，**不留副作用**）—— + # 顺序反过来的话，一次手误的 GAUGE_DROPPED 会白白吃掉一次重放预算， + # 报错信息还说"状态不对"，而现场已经少了一次重试机会。 + nxt = resolve_next(prev, event, self.ctx) + + if event is Event.GAUGE_DROPPED: + self.ctx.relocations += 1 + if self.ctx.relocations > self.ctx.max_relocations: + # **超过上限不抛异常**：掉件 4 次不是"程序非法状态"，是现场问题。 + # 报错文案要指出现场（夹持/螺纹），而不是指着转移表说这条边不存在。 + self.note( + f"通止规已掉 {self.ctx.relocations} 次（上限 " + f"{self.ctx.max_relocations}），不再自动重放，请检查夹持与螺纹" + ) + nxt = State.ABORT + + if nxt is State.NEXT_HOLE and prev is not State.NEXT_HOLE: + # 换孔动作：序号在**进入** S10 时推进；S10 完成就是新孔的 S06。 + self.ctx.hole_index = min(self.ctx.hole_index + 1, max(0, self.ctx.holes_total - 1)) + + self.state = nxt + self.history.append(Step(len(self.history) + 1, prev, event, nxt)) + return nxt + + def note(self, text: str) -> None: + """挂一条人工说明（比如"跳过 S03：D435 未标定"），随结果一起输出。""" + self.ctx.notes.append(text) + + +# ============================================================================= +# 判据（纯函数，被节点与测试共用） +# ============================================================================= +def pixel_stable(samples: List[Tuple[float, float]], tol_px: float, min_frames: int) -> bool: + """S05：像素稳定判据。 + + :param samples: 最近若干帧的 ``(u, v)`` 像素坐标。 + :param tol_px: 允许的抖动半径（像素）。 + :param min_frames: 至少要看多少帧才敢说"稳定"。 + :returns: 样本够多、且所有点到它们的均值的距离都 ≤ ``tol_px``。 + + 为什么不用"两帧之差"：跟踪输出的抖动是**零均值**的，相邻两帧可能恰好 + 很近，于是"稳定"判据会在画面明明在晃的时候早早触发，接着拍出一张糊的 + 定位图 —— 表现为"孔位偶尔偏 1mm"，随机且难复现。用"到均值的最远距离" + 才对得上"画面不动"这个物理事实。 + """ + if len(samples) < max(2, int(min_frames)): + return False + us = [s[0] for s in samples] + vs = [s[1] for s in samples] + cu = sum(us) / len(us) + cv = sum(vs) / len(vs) + return all(math.hypot(u - cu, v - cv) <= tol_px for u, v in samples) + + +def tangential_jump( + shear_now: Tuple[float, float], shear_ref: Tuple[float, float], delta: float +) -> bool: + """S07：切向力突变判据 —— **两个通道都看，任一超阈值即算**。 + + 用户明确过：切向力到底走 X 还是 Y 通道未确认。只监控一个通道的话， + 赌错了就是整段流程静默失效（下发正常、日志正常、就是停不住）。 + 两个都监控的代价只是可能更早停一点点，这个方向的错是安全的。 + + :param shear_now: 当前两路切向读数。 + :param shear_ref: 静止基线的两路切向读数。 + :param delta: 触发阈值（相对基线的增量）。 + """ + return any(abs(n - r) >= delta for n, r in zip(shear_now, shear_ref)) + + +def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool: + """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。 + + 注意是"且"：只看切向力，规刚接触螺纹时会误停；只看法向力，规顶到孔底 + 时会误停。两者同时出现才说明"螺纹咬住了"，这是拧入到位的特征。 + """ + return shear_max > shear_th and normal_delta > normal_th + + +#: S08 的四个原子步，顺序即执行顺序。 +#: **故意不封装成一个"拧螺丝"复合动作**：中途掉件（E1）时，只有每一步都能 +#: 单独调用、单独判断，才可能从任意一步精确恢复，而不是从头再来。 +RATCHET_STEPS: Tuple[str, ...] = ( + "rotate_slow", # 工具系顺时针慢转 step_deg（拧入，带着规一起转） + "release", # 松爪（规留在孔里） + "rotate_back", # 快回转 step_deg（空手回来） + "regrip", # 夹紧 +) + + +def ratchet_cycles(total_deg: float, step_deg: float) -> int: + """要拧 ``total_deg`` 需要多少轮棘轮。向上取整，``step_deg<=0`` 报错。""" + if step_deg <= 0.0: + raise ValueError(f"单次转角必须为正，收到 {step_deg}") + if total_deg <= 0.0: + return 0 + return int(math.ceil(total_deg / step_deg)) + + +def is_joint_limit_error(text: str) -> bool: + """S01：把驱动/规划报的字符串认成"撞到关节极限"。 + + 认得出的理由：这一条在真机上不是异常而是**预期结果**（用户要求"转到不 + 触发关节极限的极限角"，那就一定会在某个角度贴住）。贴住时报 JOINT_LIMIT + 继续流程；认不出来时按普通失败处理（更保守），但要把原文打印出来， + 否则下次还得猜。 + """ + low = (text or "").lower() + return any( + key in low + for key in ( + "joint limit", + "joint_limit", + "超出关节", + "关节极限", + "限位", + "out of range", + "position limit", + "exceed", + ) + ) + + +# ============================================================================= +# 给人看的说明（文档 / 日志共用，避免两处各写一份，规则 M2） +# ============================================================================= +STATE_DOC: Dict[State, str] = { + State.BOOT: "各服务自检（伺服/相机/夹爪/触觉图传/推理预热）", + State.HOME_EXTREME: "回 home + 按工具系逆时针转到极限角", + State.GRIP_CHECK: "夹持确认（预留，当前跳过）", + State.COARSE: "手外 D435 粗定位（S04 的兜底）", + State.TRACK: "腕部 D405 跟踪 k0", + State.SNAP: "像素稳定后拍照定位 k1/k2 并记位姿", + State.HOVER_K: "到当前孔正悬停", + State.CONTACT: "柔顺接触（切向力突变即停）", + State.SCREW_IN: "旋转装配循环（棘轮式拧入）", + State.UNSCREW: "反向拧出", + State.NEXT_HOLE: "换到下一个孔（回 S05 记录的位姿）", + State.RESET: "复位退出", + State.DONE: "正常结束", + State.ABORT: "中止（已退到安全高度）", +} + +EVENT_DOC: Dict[Event, str] = { + Event.BOOT_OK: "全部服务就绪", + Event.BOOT_FAIL: "有服务不就绪", + Event.POSE_OK: "位姿就绪", + Event.JOINT_LIMIT: "已贴住关节极限", + Event.GRIP_PASS: "确认夹着通止规", + Event.GRIP_FAIL: "没夹住/判定不可信", + Event.GRIP_SKIPPED: "跳过夹持确认", + Event.COARSE_OK: "粗定位出坐标", + Event.COARSE_FAIL: "粗定位失败（回跟踪兜底）", + Event.TRACK_LOCKED: "跟踪锁定 k0", + Event.TRACK_LOST: "跟踪丢失超时", + Event.SNAP_OK: "定位到 k1/k2", + Event.SNAP_UNSTABLE: "画面还在动", + Event.ALIGN_OK: "对位在容差内", + Event.ALIGN_OUT: "对位超差", + Event.CONTACT_OK: "已接触工件面", + Event.FORCE_OVER_LIMIT: "力超安全上限", + Event.SCREW_DONE: "达到目标圈数", + Event.SCREW_STOP: "力判据满足（咬住螺纹）", + Event.GAUGE_DROPPED: "通止规掉了", + Event.UNSCREW_DONE: "已拧出", + Event.RELOADED: "人工放好并按 R", + Event.TIMEOUT: "阶段超时", + Event.ABORT: "中止", +} diff --git a/src/arm_application/arm_application/gauge_gripper.py b/src/arm_application/arm_application/gauge_gripper.py new file mode 100644 index 0000000..116875c --- /dev/null +++ b/src/arm_application/arm_application/gauge_gripper.py @@ -0,0 +1,228 @@ +"""夹爪客户端：把"真机今天的接口"和"框架契约里的接口"收敛到一个出入口。 + +为什么要这一层（这不是多余抽象） +-------------------------------- +现场查证（2026-09-24）发现**同一个服务名后面挂着两种类型**： + +=============== ========================== ================================ +服务 A：真机今天跑的 B：框架契约 / 仿真 +=============== ========================== ================================ +``/gripper/open`` ``std_srvs/Trigger`` ``arm_msgs/srv/GripperExecute`` +``/gripper/close`` ``std_srvs/Trigger`` ``arm_msgs/srv/GripperExecute`` +状态 ``/gripper/states`` ``/gripper/status`` + （Float32MultiArray，5 元） （arm_msgs/GripperStatus） +=============== ========================== ================================ + +A 来自 ``arm_application/dh_pgea_node.py``（真机在跑的那个节点，grip_state 直接 +带"咬住/掉落"）；B 来自 ``devices/grippers/dh_pgea/manifest.yaml`` 声明的 +``arm_driver_dh_gripper`` —— **那个包还不存在**（``src/`` 下没有），目前只有 +仿真适配器实现了它。 + +于是有两个错法，都得避免： + +* 只按契约（B）写：真机上服务类型对不上，装配流程一步都走不了， + 而且报错会像是"服务没起"（实际起了，只是类型不同）； +* 只按真机（A）写：仿真跑不了，而且把"临时接口"固化成事实（规则 M2）。 + +所以：**运行时看服务类型自己选**（:func:`pick_backend`），装配节点只依赖 +:class:`GripperClient` 的 ``open/close/wait/is_gripped`` 四个方法。 +等 ``arm_driver_dh_gripper`` 落地后，把 ``gauge_assembly.gripper_backend`` +改回 ``auto`` 即可自动切过去，应用层不用改一行。 +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Dict, List, Mapping, Optional, Sequence + +#: 契约后端用到的服务名（``arm_msgs/srv/GripperExecute``）。 +CONTRACT_TYPE = "arm_msgs/srv/GripperExecute" +#: 真机今天用的后端（``std_srvs/srv/Trigger``）。 +LEGACY_TYPE = "std_srvs/srv/Trigger" + +CONTRACT_OPEN = "/gripper/open" +CONTRACT_CLOSE = "/gripper/close" +CONTRACT_STATUS_TOPIC = "/gripper/status" +LEGACY_STATUS_TOPIC = "/gripper/states" + +#: DH 状态字（与 ``arm_msgs/GripperStatus.msg`` 里的注释一致）。 +GRIP_STATE_MOVING = 0 +GRIP_STATE_REACHED = 1 +GRIP_STATE_CAUGHT = 2 +GRIP_STATE_DROPPED = 3 +GRIP_STATE_NAME = { + GRIP_STATE_MOVING: "运动中", + GRIP_STATE_REACHED: "到位", + GRIP_STATE_CAUGHT: "咬住物体", + GRIP_STATE_DROPPED: "物体掉落", +} + +BACKENDS = ("contract", "legacy") + + +class GripperInterfaceError(RuntimeError): + """夹爪接口无法确定。消息里必须写清"现在是什么状态、怎么修"。""" + + +@dataclass(frozen=True) +class BackendChoice: + """选择结果。""" + + backend: str + """``contract`` 或 ``legacy``。""" + + reason: str + """为什么这么选 —— 要能直接打进启动日志。""" + + status_topic: str + """该后端下状态从哪个话题读。""" + + +def pick_backend( + services: Mapping[str, Sequence[str]], prefer: str = "auto" +) -> BackendChoice: + """按**实际服务类型**决定用哪种夹爪接口。 + + :param services: ``{服务名: [类型, ...]}``，直接来自 + ``Node.get_service_names_and_types()``。 + :param prefer: ``auto`` / ``contract`` / ``legacy``。 + 非 auto 时若实际类型与之不符，**不静默降级**，而是报错 —— + 现场明确指定了后端却跑在另一个上，是最难查的一类"改了没生效"。 + :raises GripperInterfaceError: 认不出来、两种都有、或指定的后端对不上。 + + 用类型而不是名字判断，是因为两种后端的**服务名完全一样**（都是 + ``/gripper/open``）：只看名字必然选不出来，而"随便选一个"的后果是 + 调用一个类型不匹配的服务，报出来的错与真实原因无关（规则 M1）。 + """ + if prefer not in ("auto",) + BACKENDS: + raise GripperInterfaceError( + f"gripper_backend 只能是 auto / {' / '.join(BACKENDS)}，收到 {prefer!r}" + ) + + open_types = list(services.get(CONTRACT_OPEN, ())) + close_types = list(services.get(CONTRACT_CLOSE, ())) + has_contract = CONTRACT_TYPE in open_types and CONTRACT_TYPE in close_types + has_legacy = LEGACY_TYPE in open_types and LEGACY_TYPE in close_types + + if prefer == "contract" and not has_contract: + raise GripperInterfaceError( + f"配置要求 contract 后端，但 {CONTRACT_OPEN} 的类型是 " + f"{open_types or '（服务不存在）'}，不是 {CONTRACT_TYPE}。\n" + " 怎么修：① 起仿真（它有契约服务）或等 arm_driver_dh_gripper 落地；" + "② 改用 gauge_assembly.gripper_backend: legacy（真机今天的接口）；" + "③ 改回 auto 让本函数自己选" + ) + if prefer == "legacy" and not has_legacy: + raise GripperInterfaceError( + f"配置要求 legacy 后端，但 {CONTRACT_OPEN} 的类型是 " + f"{open_types or '（服务不存在）'}，不是 {LEGACY_TYPE}。\n" + " 怎么修：确认真机侧起的是 arm_application 的 dh_pgea 节点" + "（bash scripts/run_gripper.sh --arm 89）" + ) + + if has_contract and has_legacy: + # 同名服务两种类型同时在线：ROS 2 允许，但调用方只能撞运气。 + raise GripperInterfaceError( + f"{CONTRACT_OPEN} 同时存在两种类型：{open_types}。" + "是仿真适配器和真机节点同时起着（或旧的没停干净）。\n" + " 怎么修：只留一个 —— 仿真用 scripts/stop_sim_gz.sh 停干净，" + "真机 pkill -f dh_pgea" + ) + + if prefer == "auto": + if has_contract: + return BackendChoice("contract", f"{CONTRACT_OPEN} 是 {CONTRACT_TYPE}", CONTRACT_STATUS_TOPIC) + if has_legacy: + return BackendChoice( + "legacy", + f"{CONTRACT_OPEN} 是 {LEGACY_TYPE}（真机 dh_pgea 节点；" + "契约驱动 arm_driver_dh_gripper 还没落地）", + LEGACY_STATUS_TOPIC, + ) + raise GripperInterfaceError(_unrecognised_msg(services, open_types)) + + if prefer == "contract": + return BackendChoice("contract", "配置指定 contract", CONTRACT_STATUS_TOPIC) + return BackendChoice("legacy", "配置指定 legacy", LEGACY_STATUS_TOPIC) + + +def _unrecognised_msg( + services: Mapping[str, Sequence[str]], open_types: Sequence[str] +) -> str: + """认不出来时，把"服务不在"和"服务在但类型不对"分开说。 + + 两者都报成"服务没起"是这个仓库反复踩过的坑：现场明明起了夹爪节点， + 却被提示"去起夹爪节点"，然后开始怀疑人生（规则 M1、R5.4）。 + """ + missing = [n for n in (CONTRACT_OPEN, CONTRACT_CLOSE) if n not in services] + if missing: + return ( + f"{' / '.join(missing)} 不在，夹爪节点没起来。\n" + " 怎么修：真机 bash scripts/run_gripper.sh --arm 89；" + "仿真 bash scripts/run_sim_gz.sh（仿真自带契约服务）" + ) + return ( + f"{CONTRACT_OPEN} 在，但类型是 {list(open_types)}，" + f"既不认识 {CONTRACT_TYPE}（契约）也不认识 {LEGACY_TYPE}（真机 dh_pgea）。\n" + " 常见原因：这个服务名被别的节点占用了。查是谁：\n" + " ros2 service list -t | grep gripper\n" + " 怎么修：停掉那个节点，或改 gauge_assembly.gripper_backend 明确指定后端" + ) + + +# ============================================================================= +# 状态解读（纯函数） +# ============================================================================= +def is_gripped_from_state(grip_state: int) -> Optional[bool]: + """DH 状态字 → "爪里有没有东西"。 + + ``2`` 咬住 / ``3`` 掉落 是明确的；``0`` 运动中、``1`` 到位（空爪）都要 + 小心：**"到位"不等于"夹住了"** —— 空爪闭合也会到位。所以只有 2/3 给 + 明确答案，其余返回 None（"不知道"），由调用方决定要不要等。 + + 返回 None 而不是 False 很重要：把"不知道"当成"没夹住"会让 E1 掉件检测 + 在任何一次开合过程中误报。 + """ + if grip_state == GRIP_STATE_CAUGHT: + return True + if grip_state == GRIP_STATE_DROPPED: + return False + return None + + +def is_dropped_from_state(grip_state: int, was_gripped: bool) -> bool: + """E1 掉件判据：**曾经夹住过**，现在状态字变成"掉落"才是掉件。 + + 为什么要 ``was_gripped``：装配前爪里本来就是空的，那时 ``grip_state`` + 可能是 3（掉落/无物），直接判成掉件会让流程一开始就报 E1。 + """ + return was_gripped and grip_state == GRIP_STATE_DROPPED + + +def legacy_states_to_fields(data: Sequence[float]) -> Dict[str, float]: + """``/gripper/states`` 的 5 元素数组 → 具名字段。 + + 超出长度或为空时返回空字典（而不是抛异常）：这是**状态话题**， + 收到畸形数据时应该让上层走"不知道"分支，而不是让整个节点崩掉。 + """ + if not data or len(data) < 5: + return {} + return { + "initialized": float(data[0]), + "grip_state": float(data[1]), + "position": float(data[2]), + "target_position": float(data[3]), + "target_force": float(data[4]), + } + + +def describe_backend_choice(choice: BackendChoice) -> List[str]: + """给启动日志用的多行说明（含"契约驱动还没落地"这类现场事实）。""" + lines = [f"夹爪后端 = {choice.backend}（{choice.reason}）"] + if choice.backend == "legacy": + lines.append( + " 注意：legacy 后端下 /gripper/open|close 是 std_srvs/Trigger，" + "开口是 1000‰ 的百分比语义，不是毫米；装配流程只用它开合，" + "不靠它量开口" + ) + return lines diff --git a/src/arm_application/arm_application/gauge_kin.py b/src/arm_application/arm_application/gauge_kin.py new file mode 100644 index 0000000..d7b7855 --- /dev/null +++ b/src/arm_application/arm_application/gauge_kin.py @@ -0,0 +1,244 @@ +"""通止规装配用的小几何/解析工具 —— **纯函数**，可离线单测。 + +放这里的都是"算错了不会抛异常、只会让动作做错"的东西（规则 M3、R5.1）： + +* ``parse_joint_limits``：从 URDF 文本里抠关节限位，给 S01 的"转到极限角" + 做软夹取。为什么不用 ``urdf_parser_py``：本机没装，多一个依赖就多一个 + "在真机上 import 失败"的机会；限位信息在 URDF 里是极规整的一行 + ````，用标准库解析足够，而且**可控**。 +* ``prewind``：算"逆时针转到不触发关节极限的极限角"到底能转多少度。 +* ``rotation_about_axis_deg``：事后验证那个动作**真的**是绕工具 Z 转的。 + 这条是 S01 的验收判据：不验证的话，"绕错轴"会表现成"转是转了，孔位 + 偏了"，而且要到几个阶段之后才看出来。 +""" + +from __future__ import annotations + +import math +import re +import xml.etree.ElementTree as ET +from dataclasses import dataclass +from typing import Dict, Iterable, Optional, Sequence, Tuple + +import numpy as np + +#: 限位夹取时留的余量（度）。贴到限位上再下发，驱动器会因为"位置超出限位" +#: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。 +DEFAULT_LIMIT_MARGIN_DEG = 3.0 + +_JOINT_RE = re.compile(r" Dict[str, Tuple[float, float]]: + """逐行兜底解析：记住最近一个 ````，紧跟的 ```` 归它。 + + 为什么要"记住"而不是"要求同一行"：正常格式化的 URDF 里关节名和 limit + **不在同一行**，只认可同一行的兜底等于永远不生效（一个不会触发的兜底， + 和没有兜底是一样的）。属性顺序也按出现顺序读，不假设 lower 在 upper 前。 + """ + out: Dict[str, Tuple[float, float]] = {} + current: Optional[str] = None + for line in text.splitlines(): + m = _JOINT_RE.search(line) + if m: + current = m.group(1) + if current is None or " Dict[str, Tuple[float, float]]: + """从 URDF 文本里解析 ``关节名 -> (lower, upper)``（弧度）。 + + 关节名**原样保留**（含 ``Rizon4-063989_`` 前缀），调用方自己决定怎么匹配； + 这里不做"去前缀"之类的猜测 —— 猜测会在换臂时静默失配，然后限位夹取 + 悄悄失效（所有关节都"查不到限位"，于是不夹取，于是贴着限位下发）。 + + 解析失败的关节直接不进结果集：``continuous`` 关节本来就没有限位， + 不该被当成"解析失败"。调用方用"查不到 = 不夹取"来处理。 + """ + out: Dict[str, Tuple[float, float]] = {} + if not urdf_xml or " Optional[Tuple[float, float]]: + """按名字给一串关节名找限位，**要求全部找到**才返回。 + + 只有一部分找到时返回 None 而不是"把找到的那部分用上"：半套限位会让 + "能不能转到极限角"的结论变得不可解释（有些关节夹了、有些没夹）， + 与其给出一个说不清的边界，不如明确地说"这次没有限位依据"。 + """ + found = [limits.get(n) for n in joint_names] + if any(v is None for v in found): + return None + return found[-1] # 最后一个 = 我们要绕的那根轴（调用方传的顺序有意义） + + +def prewind( + q_now_rad: float, + want_deg: float, + direction: int, + limits: Optional[Tuple[float, float]] = None, + margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG, +) -> PrewindResult: + """算 S01 的预绕目标角。 + + :param q_now_rad: 当前关节角 [rad]。 + :param want_deg: 想转的角度（**正值**）[deg]。 + :param direction: ``+1`` = 绕 +Z 逆时针（右手定则）；``-1`` = 顺时针。 + 符号到底对应现场的哪个方向，用真机确认一次后改 ``site.yaml`` 即可， + 不要靠"改代码里的负号"—— 那种改动没人 review 得到（规则 M2）。 + :param limits: ``(lower, upper)`` [rad]。给 None 时不做夹取。 + :param margin_deg: 距离限位保留的余量 [deg]。 + + :returns: :class:`PrewindResult`。**无论有没有限位，都返回目标角**， + 因为"贴到限位再继续转"在现场是预期行为（用户要求转到极限角）， + 驱动器真拒绝时会由 :func:`arm_application.gauge_fsm.is_joint_limit_error` + 认成 JOINT_LIMIT 事件继续流程。 + + 为什么要在这里夹取（而不是直接下发、等驱动器报错）： + 驱动器拒绝一条轨迹时，整个 action 失败，连带把已经转过的那部分**也** + 变成"没转过"的语义（角度实际到位了，但流程认为失败）。夹取到 + "限位 − 余量"能拿到一个明确成功、且确实接近极限的姿态度。 + """ + if want_deg <= 0.0: + raise ValueError(f"预绕角度必须为正，收到 {want_deg}") + if direction not in (-1, 1): + raise ValueError(f"方向只能是 +1 / -1，收到 {direction}") + + signed = direction * abs(want_deg) + target = q_now_rad + math.radians(signed) + has_limits = limits is not None + clamped = False + if limits is not None: + lo = limits[0] + math.radians(margin_deg) + up = limits[1] - math.radians(margin_deg) + if lo > up: # 限位本身比余量还窄（不该发生，但别让它变成静默走偏） + lo = up = 0.5 * (limits[0] + limits[1]) + if target > up: + target, clamped = up, True + elif target < lo: + target, clamped = lo, True + applied = math.degrees(target - q_now_rad) + return PrewindResult( + target_rad=target, + applied_deg=applied, + want_deg=abs(want_deg), + clamped=clamped, + has_limits=has_limits, + ) + + +def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float: + """从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。 + + S01 的验收判据就靠它：绕工具 Z 转 Δ 之后，如果实际绕的是别的轴， + 这个函数给出的角度会与 Δ 明显不符（或者 axis 分量不占优）。 + 做法是把相对旋转投影到 ``axis`` 上： + + ``θ = atan2( axis·(R_b·R_a⁻¹ 的反对称部分) , (trace-1)/2 )`` + + :param axis: **单位**轴（工具 Z 在世界系下的方向 = ``R[:, 2]``）。 + """ + a = np.asarray(axis, dtype=float) + n = float(np.linalg.norm(a)) + if n < 1e-9: + raise ValueError("轴向量长度为 0，无法判定旋转") + a = a / n + + Rb = np.asarray(R_before, dtype=float)[:3, :3] + Ra = np.asarray(R_after, dtype=float)[:3, :3] + Rrel = Ra @ Rb.T + # 反对称部分：sin(θ)·[axis]_× + v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]]) + sin_t = 0.5 * float(np.dot(v, a)) + cos_t = 0.5 * (float(np.trace(Rrel)) - 1.0) + return math.degrees(math.atan2(sin_t, cos_t)) + + +def axis_misalignment_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float: + """相对旋转的转轴与 ``axis`` 的夹角 [deg]。 + + 纯绕轴旋转时这个值≈0；转错轴时它会接近 90°。用作 S01 的判错依据： + 只看"转了多少度"是不够的 —— 绕正确的轴转 90° 和绕错误的轴转 90° + 给出的角度读数一样。 + """ + Rb = np.asarray(R_before, dtype=float)[:3, :3] + Ra = np.asarray(R_after, dtype=float)[:3, :3] + Rrel = Ra @ Rb.T + # 旋转轴 = 反对称部分对应的向量（sinθ 因子不影响方向） + v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]]) + nv = float(np.linalg.norm(v)) + if nv < 1e-9: + return 0.0 # 几乎没转，谈不上"转错轴" + a = np.asarray(axis, dtype=float) + a = a / float(np.linalg.norm(a)) + cos = abs(float(np.dot(v / nv, a))) # 取绝对值：轴线的两个方向等价 + return math.degrees(math.acos(max(-1.0, min(1.0, cos)))) + + +def unit_tool_z(R: np.ndarray) -> np.ndarray: + """工具坐标系的 Z 轴在世界系下的方向（旋转矩阵的第三列）。""" + return np.asarray(R, dtype=float)[:3, 2] + + +def vertical_down_error_deg(R: np.ndarray, world_down: Iterable[float] = (0.0, 0.0, -1.0)) -> float: + """工具 Z 与"垂直向下"的夹角 [deg]。0 = 完全垂直向下。 + + 看板里"工具系全程垂直向下、位姿全局锁定"这句话，只有能算出这个数才算 + 真的在执行 —— 否则每个阶段都得靠人眼看画面判断臂歪没歪。 + """ + z = unit_tool_z(R) + d = np.asarray(list(world_down), dtype=float) + d = d / float(np.linalg.norm(d)) + cos = float(np.dot(z / max(1e-12, float(np.linalg.norm(z))), d)) + return math.degrees(math.acos(max(-1.0, min(1.0, cos)))) diff --git a/src/arm_application/test/test_gauge_boot.py b/src/arm_application/test/test_gauge_boot.py new file mode 100644 index 0000000..1fd1e14 --- /dev/null +++ b/src/arm_application/test/test_gauge_boot.py @@ -0,0 +1,195 @@ +"""``gauge_boot`` 的单元测试：自检清单**内容**的正确性。 + +自检本身很容易变成"每次都过"的仪式（规则 M1）。这里钉住的是它的内容： + +* 核心判据依赖的东西（触觉、相机）必须是**必检** —— 标成选检的话， + 现场不接触觉也能一路跑到 S07 才失败，而那时报的是"力判据没触发"； +* 现场没有的设备（D435）不该生成检查项 —— 生成一个必然失败的项， + 结果是所有人都不再看自检输出了（狼来了）； +* 「为什么需要它」必须写出来，否则现场看到"未就绪"不知道该不该急。 +""" + +from __future__ import annotations + +import pytest + +from arm_application.gauge_boot import ( + PROBES, + BootCheck, + blocking_failures, + build_checks, + format_report, + optional_warnings, + summary_counts, +) + + +def _build(**kw): + base = dict( + joint_action="rizon_arm_controller/follow_joint_trajectory", + camera="d405_left", + tactile_topic="/tactile/left/status", + gripper_open="/gripper/open", + gripper_close="/gripper/close", + gripper_status="/gripper/status", + gripper_status_type="arm_msgs/msg/GripperStatus", + ) + base.update(kw) + return build_checks(**base) + + +def _by_key(checks): + return {c.key: c for c in checks} + + +# ============================================================================= +# 清单内容 +# ============================================================================= +def test_core_checks_are_present_and_required(): + checks = _by_key(_build()) + for key in ( + "arm_jtc", + "arm_home", + "arm_switch_tool", + "arm_compliant", + "gripper_open", + "gripper_close", + "camera_color", + "camera_info", + "tactile_status", + "gripper_status", + ): + assert key in checks, f"自检清单缺 {key}" + assert checks[key].required, f"{key} 是核心判据依赖，必须是必检" + + +def test_tactile_must_not_be_downgraded_to_optional(): + """**负向**：触觉是 S07/S08/E1 三个判据的唯一数据源。 + + 把它标成选检的后果不是"少一项检查"，而是"力判据全部失效但流程照跑"， + 报出来的现象是"拧不进去"，离真正原因差好几层。 + """ + t = _by_key(_build())["tactile_status"] + assert t.required is True + assert t.expect_type == "arm_msgs/msg/TactileStatus" + assert "S07" in t.why and "S08" in t.why + + +def test_camera_checks_point_at_the_right_topics(): + checks = _by_key(_build(camera="d405_left")) + assert checks["camera_color"].target == "/d405_left/color/image_raw" + assert checks["camera_info"].target == "/d405_left/color/camera_info" + + +def test_camera_namespace_with_or_without_slashes_is_the_same(): + a = _by_key(_build(camera="d405_left"))["camera_color"].target + b = _by_key(_build(camera="/d405_left/"))["camera_color"].target + assert a == b == "/d405_left/color/image_raw" + + +def test_no_d435_means_no_make_believe_check(): + """现场没有的设备不生成检查项：一个必然失败的项会训练人忽略输出。""" + keys = _by_key(_build(d435_camera=None)) + assert "coarse_camera" not in keys + with_d435 = _by_key(_build(d435_camera="d435")) + assert "coarse_camera" in with_d435 + assert with_d435["coarse_camera"].required is False, "手外相机是 S03 兜底，不该拦启动" + assert with_d435["coarse_camera"].target == "/d435/color/image_raw" + + +def test_tool_frame_check_only_when_configured(): + assert "tool_frame" not in _by_key(_build(tool_name="")) + with_tool = _by_key(_build(tool_name="tool1")) + assert with_tool["tool_frame"].required is False, "没配工具系是告警，不是缺硬件" + assert "tool1" in with_tool["tool_frame"].why + + +def test_detector_check_only_when_weights_configured(): + assert "detector" not in _by_key(_build(weights_path="")) + with_w = _by_key(_build(weights_path="/home/cc/ros2/models/yolo/detect_best_0.onnx")) + assert with_w["detector"].probe == "detector" + assert with_w["detector"].target.endswith(".onnx") + + +def test_every_check_is_actionable(): + """每项都要有：稳定 key、人读名字、合法探针、明确目标、**为什么**。""" + checks = _build(d435_camera="d435", tool_name="tool1", weights_path="/tmp/x.onnx") + assert len({c.key for c in checks}) == len(checks), "key 必须唯一" + for c in checks: + assert c.label and c.key + assert c.probe in PROBES, f"{c.key} 的探针 {c.probe} 未登记" + # 网络端点必须是绝对名：相对名会随节点命名空间变，自检会去探错名字 + if c.probe != "detector": + assert c.target.startswith("/"), f"{c.key} 的目标应以 / 开头：{c.target}" + assert len(c.why) > 8, f"{c.key} 的 why 太短，缺了会怎样没说清" + + +def test_relative_action_name_is_normalised_to_absolute(): + """stage2 的 ``action_name`` 默认值是相对的（没有前导斜杠）。 + + 自检里必须补成绝对名：否则节点一旦被放进 namespace，自检就会去探 + ``/ns/rizon_arm_controller/...``，报"控制器没起来"而实际好着呢。 + """ + c = _by_key(_build(joint_action="rizon_arm_controller/follow_joint_trajectory"))["arm_jtc"] + assert c.target == "/rizon_arm_controller/follow_joint_trajectory" + + +# ============================================================================= +# 报告与判定 +# ============================================================================= +def _results(**status): + """构造 (检查项, 就绪, 细节) 列表；status 用 key 指定是否就绪。""" + out = [] + for c in _build(d435_camera="d435"): + out.append((c, bool(status.get(c.key, True)), "" if status.get(c.key, True) else "没收到")) + return out + + +def test_report_marks_required_and_optional_differently(): + text, failures = format_report(_results(coarse_camera=False)) + assert "选检" in text + assert "告警" in text + # 选检失败也要列出来（不拦流程，但必须让人看见） + assert any("coarse_camera" in f or "手外" in f for f in failures) + + +def test_only_required_failures_block_the_flow(): + """**负向**：D435 没接不能拦住装配。 + + 区分"必检失败"和"选检失败"是这一层的全部意义：混在一起会让 + "可选设备没接"变成"整个流程跑不起来"。 + """ + results = _results(coarse_camera=False) + assert blocking_failures(results) == [] + assert optional_warnings(results) and "手外" in optional_warnings(results)[0] + + results2 = _results(tactile_status=False) + assert blocking_failures(results2), "触觉缺失必须拦" + assert "触觉" in blocking_failures(results2)[0] + + +def test_report_lists_why_so_the_operator_knows_urgency(): + _text, failures = format_report(_results(camera_color=False)) + assert failures + assert "S05" in failures[0], "失败项必须说明缺了会影响哪一步" + + +def test_counts_summary(): + c = summary_counts(_results(coarse_camera=False)) + assert c["required_total"] == 11 and c["required_ok"] == 11 + assert c["optional_total"] == 1 and c["optional_ok"] == 0 + + +def test_all_ready_says_so(): + text, failures = format_report(_results()) + assert failures == [] + assert "全部就绪" in text + + +def test_bootcheck_defaults_to_required(): + """手工构造检查项时默认必检 —— 默认选检会让漏写 required 变成静默放行。""" + c = BootCheck(key="k", label="l", probe="service", target="/x", why="因为所以要检查它") + assert c.required is True + + with pytest.raises(TypeError): + BootCheck(key="k", label="l", probe="service", target="/x") # type: ignore[call-arg] diff --git a/src/arm_application/test/test_gauge_config.py b/src/arm_application/test/test_gauge_config.py new file mode 100644 index 0000000..d5fb771 --- /dev/null +++ b/src/arm_application/test/test_gauge_config.py @@ -0,0 +1,188 @@ +"""``gauge_config`` 的单元测试。 + +重点不在"能不能读出一个数"，而在**读错了会不会被发现**： + +* 键名拼错 → 必须报错并给候选名（否则现场"改了参数没反应"）； +* 单位与键名不符 → 必须被 ``unit_violations`` 抓到（防"填毫米按米用"）； +* 类型不对 → 必须报错，不许隐式转换； +* 跨键矛盾（安全上限低于停止阈值）→ 必须报错。 +""" + +from __future__ import annotations + +import pytest + +from arm_application.gauge_config import ( + DEFAULTS, + GaugeConfig, + GaugeConfigError, + _TYPES, + _UNITLESS, + _UNITS, + dataclass_keys, + from_site, + missing_entries, + unit_violations, +) + + +# ============================================================================= +# 自检：配置表本身必须自洽 +# ============================================================================= +def test_schema_tables_are_consistent(): + """三张表互相覆盖完整，且 dataclass 字段与键一一对应。 + + 这条挂了通常意味着"新加了一个键，但忘了在另一张表里登记"。 + """ + assert missing_entries() == [] + assert sorted(dataclass_keys()) == sorted(_TYPES), ( + "GaugeConfig 的字段与配置键不一致：" + f"只在 dataclass: {sorted(set(dataclass_keys()) - set(_TYPES))}，" + f"只在配置表: {sorted(set(_TYPES) - set(dataclass_keys()))}" + ) + + +def test_every_key_name_declares_its_unit(): + """**键名必须体现单位**（``_mm`` / ``_deg`` / ``_s`` …）。 + + 这条是"填了毫米当米用"的第一道闸门。挂掉时的修法只有两种： + 改键名带上单位后缀，或（确实无量纲时）登记到 ``_UNITLESS``。 + """ + assert unit_violations() == [] + + +def test_unit_violations_actually_detect_bad_names(): + """**负向**：故意塞一个"名字不体现单位"的键，检查必须报出来。 + + 否则 ``unit_violations`` 可能永远返回空表（检查恒真 = 没有检查，规则 M1）。 + """ + key = "hover" + _UNITS[key] = "m" + try: + bad = unit_violations([key]) + assert bad and "hover" in bad[0], f"应报出 hover 的问题，实际 {bad}" + finally: + del _UNITS[key] + + +def test_unitless_keys_are_explicitly_registered(): + """无量纲键必须显式登记，不能靠"单位为空"蒙混过关。""" + for key, unit in _UNITS.items(): + if not unit: + assert key in _UNITLESS, f"{key} 无量纲却没登记在 _UNITLESS" + + +# ============================================================================= +# 正常解析 +# ============================================================================= +def test_empty_section_uses_defaults_without_error(): + """整段缺失不报错（走默认值）—— 是否可用由节点决定，配置模块只如实解析。""" + cfg = from_site({}) + assert isinstance(cfg, GaugeConfig) + assert cfg.extreme_angle_deg == DEFAULTS["extreme_angle_deg"] + assert cfg.dry_run is False + + +def test_partial_section_merges_defaults(): + cfg = from_site({"gauge_assembly": {"extreme_angle_deg": 120.0, "holes_total": 1}}) + assert cfg.extreme_angle_deg == 120.0 + assert cfg.holes_total == 1 + assert cfg.screw_step_deg == DEFAULTS["screw_step_deg"] + + +def test_int_is_accepted_for_float_key_but_bool_is_not(): + """YAML 里 ``20`` 是 int，填给 float 键要能用；``true`` 不能当 1 用。""" + cfg = from_site({"gauge_assembly": {"extreme_angle_deg": 20}}) + assert cfg.extreme_angle_deg == 20.0 + with pytest.raises(GaugeConfigError) as ei: + from_site({"gauge_assembly": {"extreme_angle_deg": True}}) + assert "类型不对" in str(ei.value) + + +def test_string_number_is_rejected_not_coerced(): + """``"20"`` 必须报错。 + + 隐式转换是"填了字符串、跑起来是默认值"这类静默失效的入口： + YAML 里多写一对引号，现场看不出任何异常。 + """ + with pytest.raises(GaugeConfigError) as ei: + from_site({"gauge_assembly": {"contact_step_mm": "0.5"}}) + msg = str(ei.value) + assert "contact_step_mm" in msg and "mm" in msg and "隐式转换" in msg + + +# ============================================================================= +# 报错要能直接照着改 +# ============================================================================= +def test_unknown_key_is_rejected_with_suggestion(): + with pytest.raises(GaugeConfigError) as ei: + from_site({"gauge_assembly": {"screw_step_degs": 180.0}}) + msg = str(ei.value) + assert "screw_step_degs" in msg + assert "screw_step_deg" in msg, "应给出最相近的候选键名" + assert "gauge_config._TYPES" in msg, "应指出新增配置项要登记在哪" + + +def test_unknown_override_is_rejected(): + with pytest.raises(GaugeConfigError): + from_site({}, overrides={"not_a_key": 1.0}) + + +def test_section_of_wrong_type_reports_clearly(): + with pytest.raises(GaugeConfigError) as ei: + from_site({"gauge_assembly": 12}) + assert "映射" in str(ei.value) + + +def test_cross_key_contradiction_is_rejected(): + """安全上限必须高于停止阈值，否则一接触就报"力超安全上限"。""" + with pytest.raises(GaugeConfigError) as ei: + from_site( + { + "gauge_assembly": { + "contact_tangential_delta": 500.0, + "contact_safe_delta": 500.0, + } + } + ) + assert "contact_safe_delta" in str(ei.value) + + +@pytest.mark.parametrize( + "patch, expect", + [ + ({"holes_total": 0}, "holes_total"), + ({"extreme_direction": 0}, "extreme_direction"), + ({"extreme_angle_deg": -30.0}, "extreme_angle_deg"), + ({"screw_step_deg": 0.0}, "screw_step_deg"), + ({"screw_max_cycles": 0}, "screw_max_cycles"), + ({"snap_tol_px": 0.0}, "snap_tol_px"), + ], +) +def test_bad_values_are_rejected(patch, expect): + with pytest.raises(GaugeConfigError) as ei: + from_site({"gauge_assembly": patch}) + assert expect in str(ei.value) + + +# ============================================================================= +# 派生量：文档与日志共用同一个算法 +# ============================================================================= +def test_derived_totals(): + cfg = from_site( + {"gauge_assembly": {"screw_step_deg": 180.0, "screw_max_cycles": 3, "unscrew_extra_cycles": 1}} + ) + assert cfg.total_screw_deg == 540.0 + assert cfg.unscrew_cycles == 4, "拧出要比拧入多留一轮，确保脱开" + + +def test_describe_prints_effective_values_and_marks_calibration_needed(): + cfg = from_site({}) + text = "\n".join(cfg.describe()) + # 实际生效的值（不是"我读了 site.yaml"） + assert "S01 预绕 150.0°" in text + assert "S08 棘轮 180°×12 轮 = 累计 2160°" in text + # 必须提示这些阈值要现场标定，否则默认值会被当成"调好的参数" + assert "现场标定" in text + # 稳定判据的口径要写出来，避免下次有人改成"相邻帧之差" + assert "到均值的最远距离" in text diff --git a/src/arm_application/test/test_gauge_fsm.py b/src/arm_application/test/test_gauge_fsm.py new file mode 100644 index 0000000..409226b --- /dev/null +++ b/src/arm_application/test/test_gauge_fsm.py @@ -0,0 +1,388 @@ +"""``gauge_fsm`` 的单元测试 —— 通止规装配状态机的**顺序**必须被钉住。 + +这些测试的价值不在覆盖率，而在于：状态机的错不会抛异常，只会让流程 +"走得下去但结果不对"（去错了孔、掉件后从头再来、超时没有出路）。 +所以每条转移、每条判据都要有能报红的断言，包括**负向**的（R5.2）。 +""" + +from __future__ import annotations + +import math + +import pytest + +from arm_application.gauge_fsm import ( + EVENT_DOC, + GLOBAL, + RATCHET_STEPS, + STATE_DOC, + TERMINAL, + TRANSITIONS, + Event, + FsmError, + GaugeContext, + GaugeFsm, + State, + is_joint_limit_error, + pixel_stable, + ratchet_cycles, + resolve_next, + screw_stop, + tangential_jump, +) + + +# ============================================================================= +# 主线：k1 → k2 → 复位 +# ============================================================================= +def test_happy_path_covers_every_state_in_order(): + """一次完整装配的**确切**状态序列。 + + 写成一条断言而不是逐步 if：中间任何一步顺序变了（比如 S05 跑到 S07 + 前面）都要在这里红，而不是等到真机上"孔位偏了 2mm"。 + """ + fsm = GaugeFsm(GaugeContext(holes_total=2)) + seq = [ + (Event.BOOT_OK, State.HOME_EXTREME), + (Event.POSE_OK, State.GRIP_CHECK), + (Event.GRIP_SKIPPED, State.TRACK), + (Event.TRACK_LOCKED, State.SNAP), + (Event.SNAP_OK, State.HOVER_K), + (Event.ALIGN_OK, State.CONTACT), + (Event.CONTACT_OK, State.SCREW_IN), + (Event.SCREW_STOP, State.UNSCREW), + (Event.UNSCREW_DONE, State.NEXT_HOLE), # k1 拧出 → 换 k2 + (Event.POSE_OK, State.HOVER_K), # 已在 k2 正悬停 + (Event.ALIGN_OK, State.CONTACT), + (Event.CONTACT_OK, State.SCREW_IN), + (Event.SCREW_DONE, State.UNSCREW), + (Event.UNSCREW_DONE, State.RESET), # 最后一个孔 → 复位 + (Event.POSE_OK, State.DONE), + ] + for i, (ev, want) in enumerate(seq, start=1): + got = fsm.handle(ev) + assert got is want, f"第 {i} 步（{ev.value}）应到 {want}，实际到 {got}" + assert fsm.is_terminal and fsm.ok + assert [s.next for s in fsm.history] == [want for _ev, want in seq] + + +def test_single_hole_job_goes_straight_to_reset(): + """只有一个孔时，S09 拧出后必须直接 S11，不能去 S10 空转一次。""" + fsm = GaugeFsm(GaugeContext(holes_total=1)) + for ev in ( + Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED, + Event.SNAP_OK, Event.ALIGN_OK, Event.CONTACT_OK, Event.SCREW_STOP, + ): + fsm.handle(ev) + assert fsm.handle(Event.UNSCREW_DONE) is State.RESET + + +# ============================================================================= +# 结构性质：不靠人肉通读转移表 +# ============================================================================= +def test_every_state_can_be_aborted_or_reach_terminal(): + """每个非终态都必须能**走得到**终态。 + + 漏掉的那个状态，在真机上表现为"按了中止，流程还在跑"。 + ABORT/TIMEOUT 是全局兜底，这条断言顺带验证 GLOBAL 真的覆盖了全部状态。 + """ + for state in State: + if state in TERMINAL: + continue + assert state not in TERMINAL + nxt = resolve_next(state, Event.ABORT, GaugeContext()) + assert nxt is State.ABORT, f"{state} 不能中止" + nxt = resolve_next(state, Event.TIMEOUT, GaugeContext()) + assert nxt is State.ABORT, f"{state} 超时没有出路" + + +def test_every_state_is_reachable_from_boot(): + """每个声明出来的状态都要有路径从 S00 走到。 + + 否则就是一个"写在文档里、代码里永远进不去"的阶段 —— 比没有更糟： + 文档说它在跑，现场却在等一个不会发生的事。 + """ + seen = {State.BOOT} + frontier = [State.BOOT] + while frontier: + cur = frontier.pop() + events = [ev for (st, ev) in TRANSITIONS if st is cur] + for ev in events: + nxt = resolve_next(cur, ev, GaugeContext()) + if nxt not in seen: + seen.add(nxt) + frontier.append(nxt) + # S09 的主出口是 UNSCREW_DONE（上下文相关），单独补上 + seen.add(State.UNSCREW) + seen.add(State.NEXT_HOLE) + seen.add(State.RESET) + seen.add(State.DONE) + seen.add(State.ABORT) + missing = {s for s in State} - seen + assert not missing, f"这些状态从 S00 走不到：{sorted(str(s) for s in missing)}" + + +def test_transition_table_has_no_terminal_out_edges(): + """终态不许有出边：有出边就等于"结束了还能继续跑"。""" + for (state, _ev) in TRANSITIONS: + assert state not in TERMINAL, f"{state} 是终态却出现在转移表里" + + +# ============================================================================= +# S05 自环 / S06 回退：不推错孔号 +# ============================================================================= +def test_unstable_snap_keeps_same_hole(): + """S05 等稳定是自环，不能把孔号推走（否则会在 k1 上等，然后去装 k2）。""" + fsm = GaugeFsm(GaugeContext(holes_total=2)) + for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED): + fsm.handle(ev) + assert fsm.ctx.hole_index == 0 + for _ in range(5): + fsm.handle(Event.SNAP_UNSTABLE) + assert fsm.state is State.SNAP and fsm.ctx.hole_index == 0 + assert fsm.ctx.hole_name() == "k1" + + +def test_align_out_goes_back_to_snap_not_to_next_hole(): + """对位超差要**回 S05 重拍**，不是换孔 —— 换孔会静默少装一个孔。""" + fsm = GaugeFsm(GaugeContext(holes_total=2)) + for ev in ( + Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED, + Event.SNAP_OK, + ): + fsm.handle(ev) + assert fsm.handle(Event.ALIGN_OUT) is State.SNAP + assert fsm.ctx.hole_index == 0 + + +# ============================================================================= +# S03/S04 互为兜底 +# ============================================================================= +def test_track_loss_falls_back_to_coarse_and_back(): + """S04 丢失 → S03 粗定位；S03 识别不到 → 回 S04。两条路都要能来回。""" + fsm = GaugeFsm(GaugeContext()) + for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED): + fsm.handle(ev) + assert fsm.state is State.TRACK + assert fsm.handle(Event.TRACK_LOST) is State.COARSE + assert fsm.handle(Event.COARSE_FAIL) is State.TRACK + assert fsm.handle(Event.TRACK_LOST) is State.COARSE + assert fsm.handle(Event.COARSE_OK) is State.SNAP + + +def test_joint_limit_is_not_a_failure(): + """S01 贴住关节极限是**预期结果**（用户要求"转到极限角"），不能当中止。""" + fsm = GaugeFsm(GaugeContext()) + fsm.handle(Event.BOOT_OK) + assert fsm.state is State.HOME_EXTREME + assert fsm.handle(Event.JOINT_LIMIT) is State.GRIP_CHECK + + +# ============================================================================= +# E1 掉件重放 +# ============================================================================= +def _to_screw_in(fsm: GaugeFsm) -> None: + for ev in ( + Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED, + Event.SNAP_OK, Event.ALIGN_OK, Event.CONTACT_OK, + ): + fsm.handle(ev) + + +def test_drop_at_k1_reloads_and_continues_with_k2(): + """E1：张开爪 → S01 流程 → 人工放好按 R → 回记录位姿 → 继续 k2。""" + fsm = GaugeFsm(GaugeContext(holes_total=2)) + _to_screw_in(fsm) + assert fsm.state is State.SCREW_IN and fsm.ctx.hole_index == 0 + + assert fsm.handle(Event.GAUGE_DROPPED) is State.HOME_EXTREME + assert fsm.ctx.relocations == 1 + # 人工重放后直接回记录位姿（等价于换孔动作），坑已跳过，所以是去 k2 + assert fsm.handle(Event.RELOADED) is State.NEXT_HOLE + assert fsm.ctx.hole_index == 1 + assert fsm.handle(Event.POSE_OK) is State.HOVER_K + + +def test_drop_budget_exhaustion_aborts_with_note(): + """掉件超过上限 → ABORT，并留下一条能读懂现场原因的备注。""" + fsm = GaugeFsm(GaugeContext(holes_total=1, max_relocations=2)) + fsm.handle(Event.BOOT_OK) + for attempt in range(3): + fsm.state = State.SCREW_IN # 直接摆到"正在拧"，掉件只可能发生在这一步 + nxt = fsm.handle(Event.GAUGE_DROPPED) + if attempt < 2: + assert nxt is State.HOME_EXTREME, f"第 {attempt + 1} 次掉件应还在预算内" + fsm.handle(Event.RELOADED) + else: + assert nxt is State.ABORT + assert fsm.ctx.relocations == 3 + assert any("掉" in n for n in fsm.ctx.notes) + + +def test_illegal_drop_does_not_consume_reload_budget(): + """**负向**：在错的状态发 GAUGE_DROPPED，不能白吃掉一次重放预算。 + + 实现里"先算转移、再记预算"就是为这条：顺序反了的话，一次手误会让现场 + 少一次重试机会，而报错信息只说"状态不对"，没人会想到预算被扣了。 + """ + fsm = GaugeFsm(GaugeContext(holes_total=1, max_relocations=1)) + assert fsm.state is State.BOOT + with pytest.raises(FsmError): + fsm.handle(Event.GAUGE_DROPPED) + assert fsm.ctx.relocations == 0 + + +# ============================================================================= +# 非法转移要当场炸 +# ============================================================================= +def test_illegal_transition_raises_with_actionable_message(): + fsm = GaugeFsm(GaugeContext()) + with pytest.raises(FsmError) as ei: + fsm.handle(Event.SCREW_DONE) # S00 不可能已经在拧 + msg = str(ei.value) + assert "S00" in msg and "screw_done" in msg + assert "gauge_fsm.TRANSITIONS" in msg # 告诉改代码的人去哪登记 + + +def test_terminal_state_rejects_further_events(): + fsm = GaugeFsm(GaugeContext()) + fsm.handle(Event.BOOT_FAIL) + assert fsm.state is State.ABORT + with pytest.raises(FsmError): + fsm.handle(Event.BOOT_OK) + + +def test_can_does_not_mutate(): + fsm = GaugeFsm(GaugeContext()) + before = (fsm.state, len(fsm.history)) + assert fsm.can(Event.BOOT_OK) is True + assert fsm.can(Event.SCREW_DONE) is False + assert (fsm.state, len(fsm.history)) == before + + +# ============================================================================= +# 判据 +# ============================================================================= +def test_pixel_stable_rejects_zero_mean_jitter(): + """**负向**：相邻两帧很近、但整体在晃，不能被判成"稳定"。 + + 这正是"两帧之差"写法会踩的坑：跟踪抖动的相邻差是小的，于是画面明明 + 在晃，判据早就触发了，然后拍出一张糊图 —— 现场表现为"孔位偶尔偏 1mm"， + 随机且难复现。这里用一条锯齿状的抖动序列把这个坑钉死。 + """ + jitter = [(100.0 + 6.0 * (-1) ** i, 50.0) for i in range(8)] + # 相邻差都是 12px，很大；但下面这条更阴：相邻差 0、整体摆幅 ±6px + sawtooth = [(100.0, 50.0), (100.0, 50.0), (106.0, 50.0), (106.0, 50.0), + (94.0, 50.0), (94.0, 50.0)] + assert not pixel_stable(jitter, tol_px=2.0, min_frames=6) + assert not pixel_stable(sawtooth, tol_px=2.0, min_frames=6) + # 正向：真的不动 + still = [(100.0 + 0.5 * i % 1.0, 50.0) for i in range(8)] + assert pixel_stable(still, tol_px=2.0, min_frames=6) + + +def test_pixel_stable_needs_enough_frames(): + assert not pixel_stable([(100.0, 50.0)], tol_px=5.0, min_frames=6) + assert not pixel_stable([(100.0, 50.0)] * 5, tol_px=5.0, min_frames=6) + assert pixel_stable([(100.0, 50.0)] * 6, tol_px=5.0, min_frames=6) + + +def test_tangential_jump_watches_both_channels(): + """切向力通道未确认 → 两个都监控，任一突变即判定。""" + ref = (100.0, 100.0) + assert not tangential_jump((110.0, 130.0), ref, delta=50.0) + assert tangential_jump((160.0, 100.0), ref, delta=50.0) # X 突变 + assert tangential_jump((100.0, 160.0), ref, delta=50.0) # Y 突变 + # 反向突变（规被顶回来）同样算 + assert tangential_jump((40.0, 100.0), ref, delta=50.0) + # 边界：恰好等于阈值算触发（>=，避免"永远差那么一点"） + assert tangential_jump((150.0, 100.0), ref, delta=50.0) + + +def test_screw_stop_requires_both_conditions(): + """S08 停止判据是**且**：只看一个会误停。""" + # 只有切向力大（规刚蹭到螺纹）→ 不停 + assert not screw_stop(normal_delta=0.0, shear_max=999.0, normal_th=100.0, shear_th=200.0) + # 只有法向力突增（规顶到孔底）→ 不停 + assert not screw_stop(normal_delta=999.0, shear_max=0.0, normal_th=100.0, shear_th=200.0) + # 两者同时满足 → 停 + assert screw_stop(normal_delta=150.0, shear_max=250.0, normal_th=100.0, shear_th=200.0) + + +def test_ratchet_cycle_count_rounds_up(): + assert ratchet_cycles(360.0, 180.0) == 2 + assert ratchet_cycles(361.0, 180.0) == 3 # 差一点也要多做一轮，不能少拧 + assert ratchet_cycles(0.0, 180.0) == 0 + with pytest.raises(ValueError): + ratchet_cycles(360.0, 0.0) + + +def test_ratchet_steps_are_four_separate_atomic_steps(): + """S08 必须是四个可独立调用的原子步，顺序固定。 + + 用户明确要求不能封装成一个"拧螺丝"复合动作：中途掉件时只有每一步都能 + 单独恢复，才可能精确续上（否则只能整段重来，等于少装一个孔）。 + """ + assert RATCHET_STEPS == ("rotate_slow", "release", "rotate_back", "regrip") + assert len(set(RATCHET_STEPS)) == len(RATCHET_STEPS) + + +def test_joint_limit_recognition(): + assert is_joint_limit_error("Joint limit exceeded on joint7") + assert is_joint_limit_error("规划失败：超出关节限位") + assert not is_joint_limit_error("controller not active") + assert not is_joint_limit_error("") + + +# ============================================================================= +# 文档与代码同步 +# ============================================================================= +def test_every_state_and_event_is_documented(): + """每个状态/事件都要有一句人话说明。 + + 没有说明的状态，最后一定会以"日志里只印了个 S07，没人知道 S07 干嘛" + 的形式回到现场。这条断言让"新增状态忘了写说明"当场报红。 + """ + assert set(STATE_DOC) == set(State), ( + f"缺说明的状态: {sorted(str(s) for s in set(State) - set(STATE_DOC))}" + ) + assert set(EVENT_DOC) == set(Event), ( + f"缺说明的事件: {sorted(e.value for e in set(Event) - set(EVENT_DOC))}" + ) + + +def test_global_fallback_covers_only_abort_and_timeout(): + """GLOBAL 只放"谁都该接受"的事件。 + + 一旦有人往里加业务事件，就会悄悄多出一条所有状态都合法的边， + 把"这条转移不存在"这个保护废掉。 + """ + assert set(GLOBAL) == {Event.ABORT, Event.TIMEOUT} + + +def test_context_reports_hole_names(): + ctx = GaugeContext(holes_total=2, hole_index=0) + assert ctx.hole_name() == "k1" + ctx.hole_index = 1 + assert ctx.hole_name() == "k2" + assert ctx.is_last_hole + + +def test_history_index_is_dense_and_monotonic(): + fsm = GaugeFsm(GaugeContext()) + for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED): + fsm.handle(ev) + assert [s.index for s in fsm.history] == [1, 2, 3] + assert str(fsm.history[0]) == "#1 S00 --boot_ok--> S01" + + +def test_math_hypot_precision_is_not_used_loosely(): + """pixel_stable 的容差是闭区间（<=），恰好等于阈值算稳定。 + + 留一条边界断言，是因为改成 ``<`` 会让"刚好卡在容差上"的稳定画面永远 + 等不到 —— 现场表现为"卡在 S05 不动"，而日志只说"还在等稳定"。 + """ + samples = [(100.0, 50.0), (102.0, 50.0), (98.0, 50.0)] + assert math.isclose(2.0, 2.0) + assert pixel_stable(samples, tol_px=2.0, min_frames=3) + assert not pixel_stable(samples, tol_px=1.9, min_frames=3) diff --git a/src/arm_application/test/test_gauge_gripper.py b/src/arm_application/test/test_gauge_gripper.py new file mode 100644 index 0000000..9af824e --- /dev/null +++ b/src/arm_application/test/test_gauge_gripper.py @@ -0,0 +1,176 @@ +"""``gauge_gripper`` 的单元测试：**同名服务、两种类型**怎么选。 + +这一段的价值全在"选错了会报出与真实原因无关的错"上： +真机上只按契约写 → 报"服务没起"（其实起了，只是类型是 Trigger）； +选了 legacy 却在仿真里跑 → 调用类型不匹配的服务。 +所以每条选择路径都要有断言，包括"两种都在线"和"一个都不在"。 +""" + +from __future__ import annotations + +import pytest + +from arm_application.gauge_gripper import ( + CONTRACT_CLOSE, + CONTRACT_OPEN, + CONTRACT_STATUS_TOPIC, + CONTRACT_TYPE, + GRIP_STATE_CAUGHT, + GRIP_STATE_DROPPED, + GRIP_STATE_MOVING, + GRIP_STATE_REACHED, + LEGACY_STATUS_TOPIC, + LEGACY_TYPE, + GripperInterfaceError, + describe_backend_choice, + is_dropped_from_state, + is_gripped_from_state, + legacy_states_to_fields, + pick_backend, +) + + +def _services(open_types, close_types=None, extra=None): + close_types = open_types if close_types is None else close_types + out = {CONTRACT_OPEN: list(open_types), CONTRACT_CLOSE: list(close_types)} + if extra: + out.update(extra) + return out + + +# ============================================================================= +# auto：按实际类型选 +# ============================================================================= +def test_contract_backend_when_services_are_gripper_execute(): + choice = pick_backend(_services([CONTRACT_TYPE])) + assert choice.backend == "contract" + assert CONTRACT_TYPE in choice.reason + assert choice.status_topic == CONTRACT_STATUS_TOPIC + + +def test_legacy_backend_when_services_are_trigger(): + """真机今天的样子：dh_pgea 节点的 Trigger 服务。""" + choice = pick_backend(_services([LEGACY_TYPE])) + assert choice.backend == "legacy" + assert "dh_pgea" in choice.reason + assert choice.status_topic == LEGACY_STATUS_TOPIC + # 理由里要带上"契约驱动还没落地"这个现场事实，否则下次有人以为选错了 + assert "arm_driver_dh_gripper" in choice.reason + + +def test_missing_services_reports_how_to_start_them(): + with pytest.raises(GripperInterfaceError) as ei: + pick_backend({}) + msg = str(ei.value) + assert "run_gripper.sh" in msg and "run_sim_gz.sh" in msg + + +def test_unknown_service_type_is_not_silently_accepted(): + """类型既不是 GripperExecute 也不是 Trigger（比如被别人占用了同名服务）。 + + 这时必须是"认不出来"的明确报错，而不是随便选一个后端然后调用失败。 + """ + with pytest.raises(GripperInterfaceError) as ei: + pick_backend(_services(["std_srvs/srv/SetBool"])) + msg = str(ei.value) + assert "std_srvs/srv/SetBool" in msg + assert "service list -t" in msg, "要给出『是谁占用了这个服务名』的查法" + + +def test_missing_service_and_wrong_type_are_different_messages(): + """**负向**：不能在"服务不在"和"类型不对"两种情况报同一句话。 + + 两者都报"夹爪节点没起来"，现场就会去反复重启一个根本没问题的节点 + （规则 M1：报错信息必须指向真实原因）。 + """ + with pytest.raises(GripperInterfaceError) as absent: + pick_backend({CONTRACT_CLOSE: [CONTRACT_TYPE]}) # open 缺失 + with pytest.raises(GripperInterfaceError) as wrong: + pick_backend(_services(["std_srvs/srv/SetBool"])) + assert "没起来" in str(absent.value) + assert "没起来" not in str(wrong.value) + assert "被别的节点占用" in str(wrong.value) or "占用了" in str(wrong.value) + + +def test_both_backends_online_is_an_error_not_a_coin_flip(): + """仿真和真机节点同时起着（或旧进程没停干净）→ 报错，不许撞运气。""" + with pytest.raises(GripperInterfaceError) as ei: + pick_backend(_services([CONTRACT_TYPE, LEGACY_TYPE])) + msg = str(ei.value) + assert "同时存在两种类型" in msg + assert "stop_sim_gz.sh" in msg + + +def test_open_and_close_types_must_agree(): + """open 是契约、close 是 Trigger → 不算"契约可用"，要按认不出来报错。""" + with pytest.raises(GripperInterfaceError): + pick_backend(_services([CONTRACT_TYPE], [LEGACY_TYPE])) + + +# ============================================================================= +# 显式指定后端：不符就报错，不静默降级 +# ============================================================================= +def test_explicit_contract_on_legacy_hardware_fails_with_fix_list(): + with pytest.raises(GripperInterfaceError) as ei: + pick_backend(_services([LEGACY_TYPE]), prefer="contract") + msg = str(ei.value) + assert "gripper_backend" in msg + assert "legacy" in msg and "auto" in msg + + +def test_explicit_legacy_on_sim_fails_and_points_at_dh_pgea(): + with pytest.raises(GripperInterfaceError) as ei: + pick_backend(_services([CONTRACT_TYPE]), prefer="legacy") + assert "dh_pgea" in str(ei.value) + + +def test_explicit_backend_matching_reality_is_accepted(): + assert pick_backend(_services([CONTRACT_TYPE]), prefer="contract").backend == "contract" + assert pick_backend(_services([LEGACY_TYPE]), prefer="legacy").backend == "legacy" + + +def test_bad_prefer_value_lists_valid_options(): + with pytest.raises(GripperInterfaceError) as ei: + pick_backend(_services([CONTRACT_TYPE]), prefer="whatever") + assert "auto" in str(ei.value) and "contract" in str(ei.value) + + +# ============================================================================= +# 状态解读 +# ============================================================================= +def test_gripped_only_when_state_is_definitive(): + """"到位"不等于"夹住"：空爪闭合也会到位。 + + 把 1（到位）当成"夹住了"会让 E1 掉件检测在正常开合时误报； + 把"不知道"当成"没夹住"会让流程一开始就报错。 + """ + assert is_gripped_from_state(GRIP_STATE_CAUGHT) is True + assert is_gripped_from_state(GRIP_STATE_DROPPED) is False + assert is_gripped_from_state(GRIP_STATE_REACHED) is None + assert is_gripped_from_state(GRIP_STATE_MOVING) is None + + +def test_drop_detection_requires_a_previous_grip(): + """爪里本来就是空的（装配前），不该判成掉件。""" + assert is_dropped_from_state(GRIP_STATE_DROPPED, was_gripped=True) is True + assert is_dropped_from_state(GRIP_STATE_DROPPED, was_gripped=False) is False + assert is_dropped_from_state(GRIP_STATE_CAUGHT, was_gripped=True) is False + assert is_dropped_from_state(GRIP_STATE_MOVING, was_gripped=True) is False + + +def test_legacy_states_parsing_is_field_named_and_defensive(): + f = legacy_states_to_fields([1.0, 2.0, 500.0, 0.0, 50.0]) + assert f["initialized"] == 1.0 and f["grip_state"] == 2.0 + assert f["position"] == 500.0 and f["target_force"] == 50.0 + # 畸形/空数据 → 空字典（上层走"不知道"分支），不抛异常 + assert legacy_states_to_fields([]) == {} + assert legacy_states_to_fields([1.0, 2.0]) == {} + + +def test_describe_mentions_the_unit_trap_for_legacy(): + """legacy 的"开口"是千分比不是毫米，日志里必须写明。 + + 这是最容易误用的地方：拿到 500 以为是 500mm，实际是 10mm 行程的一半。 + """ + text = "\n".join(describe_backend_choice(pick_backend(_services([LEGACY_TYPE])))) + assert "1000‰" in text and "不是毫米" in text diff --git a/src/arm_application/test/test_gauge_kin.py b/src/arm_application/test/test_gauge_kin.py new file mode 100644 index 0000000..9fb8716 --- /dev/null +++ b/src/arm_application/test/test_gauge_kin.py @@ -0,0 +1,209 @@ +"""``gauge_kin`` 的单元测试：S01 的"转到极限角"与"确实绕对了轴"。 + +两个都在测"算错了不会报错、只会做错动作"的东西： +限位夹取算错 → 要么撞限位被驱动器拒绝，要么离极限还差很远（流程看起来一样）； +转轴验证算错 → 无论实际绕哪根轴转都报"没问题"，判据等于没有。 +""" + +from __future__ import annotations + +import math + +import numpy as np +import pytest + +from arm_application.gauge_kin import ( + DEFAULT_LIMIT_MARGIN_DEG, + axis_misalignment_deg, + match_limits, + parse_joint_limits, + prewind, + rotation_about_axis_deg, + unit_tool_z, + vertical_down_error_deg, +) + +URDF = """ + + + + + + + + + + + + + + + + + + +""" + + +# ============================================================================= +# URDF 解析 +# ============================================================================= +def test_parse_limits_keeps_full_joint_names(): + """关节名原样保留（含 SN 前缀）。 + + "顺手去掉前缀"这种清洗会在换臂时静默失配：所有关节都查不到限位， + 于是限位夹取悄悄失效，现象是"贴着限位下发、驱动器拒绝"。 + """ + lim = parse_joint_limits(URDF) + assert lim["Rizon4-063989_joint7"] == (-3.0543, 3.0543) + assert lim["Rizon4-063989_joint6"] == (-1.4835, 4.6251) + # continuous 关节没有 limit，不该被当成解析结果 + assert "Rizon4-063989_roll" not in lim + + +def test_parse_limits_on_garbage_returns_empty_not_partial(): + assert parse_joint_limits("") == {} + assert parse_joint_limits("not xml at all") == {} + # 未展开的 xacro：既不是合法 XML，也没有可用的 limit 行 + assert parse_joint_limits("${bad}") == {} + + +def test_parse_limits_falls_back_to_line_regex_on_malformed_xml(): + """URDF 里有未转义的 ``&`` 时整份 XML 解析会失败，但限位行仍然可读。 + + 降级到逐行正则能救回"拿到的是半成品 URDF"这种常见情况；救不回就返回空表 + （= 不做限位夹取，由调用方告警），**不会**返回半套数据。 + """ + broken = URDF.replace('', '') + lim = parse_joint_limits(broken) + assert lim["Rizon4-063989_joint7"] == (-3.0543, 3.0543) + + +def test_match_limits_is_all_or_nothing(): + lim = parse_joint_limits(URDF) + assert match_limits(lim, ["Rizon4-063989_joint7"]) == (-3.0543, 3.0543) + # 缺一个就整体不可用：半套限位会让"能不能转到极限"的结论无法解释 + assert match_limits(lim, ["Rizon4-063989_joint7", "不存在_joint8"]) is None + assert match_limits({}, ["Rizon4-063989_joint7"]) is None + + +# ============================================================================= +# S01 预绕 +# ============================================================================= +def test_prewind_without_limits_applies_full_angle(): + r = prewind(q_now_rad=0.0, want_deg=150.0, direction=1, limits=None) + assert not r.has_limits and not r.clamped + assert math.isclose(r.applied_deg, 150.0, abs_tol=1e-9) + assert math.isclose(r.target_rad, math.radians(150.0), abs_tol=1e-12) + + +def test_prewind_direction_sign(): + """方向只由 ``extreme_direction`` 决定，不靠代码里的负号。""" + up = prewind(0.0, 90.0, 1, None) + down = prewind(0.0, 90.0, -1, None) + assert up.applied_deg > 0 and down.applied_deg < 0 + assert math.isclose(up.applied_deg, -down.applied_deg, abs_tol=1e-9) + with pytest.raises(ValueError): + prewind(0.0, 90.0, 0, None) + with pytest.raises(ValueError): + prewind(0.0, -90.0, 1, None) + + +def test_prewind_clamps_inside_the_limit_with_margin(): + """上限 3.0543 rad（175°），余量 3° → 最多转到 172°，且报 clamped。""" + r = prewind(q_now_rad=0.0, want_deg=150.0, direction=1, limits=(-3.0543, 3.0543)) + assert math.isclose(r.applied_deg, 150.0, abs_tol=1e-9) + assert not r.clamped + + r2 = prewind(q_now_rad=0.0, want_deg=179.0, direction=1, limits=(-3.0543, 3.0543)) + assert r2.clamped and r2.has_limits + assert r2.applied_deg < 179.0 + # 关键断言：目标必须落在 "上限 − 余量" 之内（余量是给驱动器留的，不是装饰） + upper = 3.0543 - math.radians(DEFAULT_LIMIT_MARGIN_DEG) + assert r2.target_rad <= upper + 1e-12 + assert math.isclose(r2.target_rad, upper, abs_tol=1e-12) + + +def test_prewind_clamps_toward_lower_limit_for_negative_direction(): + r = prewind(q_now_rad=0.0, want_deg=179.0, direction=-1, limits=(-3.0543, 3.0543)) + lower = -3.0543 + math.radians(DEFAULT_LIMIT_MARGIN_DEG) + assert r.clamped and math.isclose(r.target_rad, lower, abs_tol=1e-12) + + +def test_prewind_reports_backwards_motion_when_already_inside_margin(): + """**边界**：已经贴在上限余量之内时，目标会被夹到当前位置**之后**。 + + 这时 ``applied_deg`` 是负的（臂会往回退一点点）。这不是 bug，是"宁可退 + 一点也不要撞限位"；但必须如实报出来，否则日志里会出现"要转 150°、 + 实际转了 -2°"却没人解释。 + """ + q = 3.0543 - math.radians(1.0) # 已经在余量(3°)之内 + r = prewind(q_now_rad=q, want_deg=150.0, direction=1, limits=(-3.0543, 3.0543)) + assert r.clamped and r.applied_deg < 0.0 + assert r.target_rad <= 3.0543 - math.radians(DEFAULT_LIMIT_MARGIN_DEG) + 1e-12 + + +def test_prewind_handles_degenerate_limits(): + """上下限比余量还窄（配置错）时，退到区间中点而不是静默走偏。""" + r = prewind(0.0, 90.0, 1, limits=(-0.01, 0.01), margin_deg=3.0) + assert math.isclose(r.target_rad, 0.0, abs_tol=1e-12) + assert r.clamped + + +# ============================================================================= +# 绕轴旋转的验证 +# ============================================================================= +def _rot_z(deg: float) -> np.ndarray: + t = math.radians(deg) + return np.array( + [[math.cos(t), -math.sin(t), 0.0], [math.sin(t), math.cos(t), 0.0], [0.0, 0.0, 1.0]] + ) + + +def _rot_x(deg: float) -> np.ndarray: + t = math.radians(deg) + return np.array( + [[1.0, 0.0, 0.0], [0.0, math.cos(t), -math.sin(t)], [0.0, math.sin(t), math.cos(t)]] + ) + + +def test_rotation_about_axis_reads_the_signed_angle(): + R0 = np.eye(3) + assert math.isclose(rotation_about_axis_deg(R0, _rot_z(30.0), [0, 0, 1]), 30.0, abs_tol=1e-9) + assert math.isclose(rotation_about_axis_deg(R0, _rot_z(-30.0), [0, 0, 1]), -30.0, abs_tol=1e-9) + # 转了很多圈也要给出同一个角（atan2 值域 ±180°，这里只用来核对单步） + assert math.isclose(rotation_about_axis_deg(R0, _rot_z(179.0), [0, 0, 1]), 179.0, abs_tol=1e-9) + + +def test_rotation_about_axis_ignores_an_orthogonal_rotation_axis(): + """绕 X 转 60°，在 Z 轴上的投影应为 0 —— 这正是"绕错轴"的读数特征。""" + val = rotation_about_axis_deg(np.eye(3), _rot_x(60.0), [0, 0, 1]) + assert abs(val) < 1e-9 + + +def test_axis_misalignment_separates_right_axis_from_wrong_axis(): + """S01 的判错依据：转对轴 ≈0°，转错轴 ≈90°。 + + 只看"转了多少度"不够 —— 绕正确轴转 90° 和绕错误轴转 90° 的角度读数一样。 + """ + R0 = np.eye(3) + assert axis_misalignment_deg(R0, _rot_z(45.0), [0, 0, 1]) < 1e-6 + assert axis_misalignment_deg(R0, _rot_x(45.0), [0, 0, 1]) > 89.0 + # 轴线取反方向应视为同一根轴（cos 取绝对值） + assert axis_misalignment_deg(R0, _rot_z(45.0), [0, 0, -1]) < 1e-6 + # 几乎没转时谈不上"转错轴" + assert axis_misalignment_deg(R0, np.eye(3), [0, 0, 1]) == 0.0 + + +def test_rotation_helpers_reject_zero_axis(): + with pytest.raises(ValueError): + rotation_about_axis_deg(np.eye(3), _rot_z(10.0), [0.0, 0.0, 0.0]) + + +def test_tool_z_and_vertical_down_error(): + """工具 Z 朝世界 -Z = 0°；水平 = 90°；朝上 = 180°。""" + R_down = np.diag([1.0, -1.0, -1.0]) + assert np.allclose(unit_tool_z(R_down), [0.0, 0.0, -1.0]) + assert vertical_down_error_deg(R_down) < 1e-6 + assert math.isclose(vertical_down_error_deg(_rot_x(90.0) @ R_down), 90.0, abs_tol=1e-6) + assert math.isclose(vertical_down_error_deg(_rot_x(180.0) @ R_down), 180.0, abs_tol=1e-6)