From dfc945b7e05b6a1071fb15ac69b4d3999ef77f7c Mon Sep 17 00:00:00 2001 From: arm_dev Date: Thu, 24 Sep 2026 10:12:03 +0800 Subject: [PATCH] =?UTF-8?q?P6=20=E9=80=9A=E6=AD=A2=E8=A7=84=E8=A3=85?= =?UTF-8?q?=E9=85=8D=EF=BC=9A=E4=B8=BB=E6=B5=81=E7=A8=8B=E8=8A=82=E7=82=B9?= =?UTF-8?q?=EF=BC=88S00=E2=80=93S11=20=E8=B7=91=E9=80=9A=EF=BC=89+=20?= =?UTF-8?q?=E5=85=A5=E5=8F=A3=E8=84=9A=E6=9C=AC=20+=20=E7=9C=9F=E6=9C=BA/?= =?UTF-8?q?=E4=BB=BF=E7=9C=9F=E9=AA=8C=E8=AF=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 新增 ---- - arm_application/gauge_assembly_node.py：主流程。每个阶段一个 _step_*， 返回值就是状态机的事件；纯逻辑（状态机/配置/限位/夹爪后端/自检）全部 在 gauge_*.py 里，节点只做"发指令 + 读反馈 + 报错"。 * S00 自检报告带"缺了会怎样"，必检拦住、选检告警； * S01 回 home → 切工具系 → 逆时针预绕，**事后用 TF 实测绕工具 Z 转了多少度** （转轴偏差 >5° 直接中止：以"度"为单位的动作全建立在这条假设上）； * S03 手外粗定位：没有 eye-to-hand 标定文件时明确报"缺哪个文件、怎么生成"， 回 S04 兜底（不做假装成功的反投影）； * S05 稳定判据用"到均值的最远距离"，不用相邻帧之差； * S07 悬停处测基线 → 步进下压 → 两路切向任一突变即停，法向超安全上限抬起中止； * S08 棘轮四步（慢转→松爪→快回转→夹紧），停止判据「切向 >阈值 且 法向突增」， 掉件走 E1 重放；S09 反向拧出（比拧入多一轮）； * E1 重放等 /gauge/reload_ok 人工确认（超时按中止），E2 任何中止都先退安全高度。 - launch/gauge_assembly.launch.py：只做一件事 —— 发布手眼静态 TF（S05 的前提）， 其余判据全交给 S00（判断标准只有一处）。 - scripts/run_gauge_assembly.sh：--arm/--sim/--dry-run/--holes/--backend。 - gauge_assembly 入口（setup.py）+ 站点参数 reload_timeout_s。 - test/test_gauge_step_events.py：**读节点源码 AST**，把每个 _step_* 可能返回的 事件逐个问状态机"合法吗" —— 节点与转移表是两份数据，漏登记要在 pytest 里炸， 而不是在真机上跑到那一步才炸。 顺手修掉的两个真问题 -------------------- 1. 四份 launch 里的四元数换算**已经互相抄歪了**（退化分支的公式各不相同）。 统一收口到 handeye_lib.read_transform_yaml，并加回环测试（真实标定文件 + 180°/270° 等分支切换角度）；实测 4 个 launch 输出与库完全一致。 2. pipeline_node.AssemblyPipeline 支持 node_name 复用（本流程继承它的一阶段 跟踪/二阶段定位链，但在图上必须是另一个节点名）。 验证（都在本机真跑过） ---------------------- pytest（仿真环境） → 349 passed, 14 skipped（缺 onnx 的用例跳过） 干跑全流程 → S00→S01→…→S11→DONE，exit 0 仿真里不干跑（缺 /home 等） → S00 拦住，exit 1，未发任何运动指令 另：仿真环境没有 onnx（只有真机 conda 有），相关用例改为 skip —— 让测试结果 "红就是真红"。 Co-authored-by: Cursor --- scripts/run_gauge_assembly.sh | 90 + .../arm_application/gauge_assembly_node.py | 1550 +++++++++++++++++ .../arm_application/gauge_boot.py | 58 +- .../arm_application/gauge_config.py | 6 + .../arm_application/handeye_lib.py | 65 + .../arm_application/pipeline_node.py | 8 +- .../launch/gauge_assembly.launch.py | 157 ++ src/arm_application/launch/pipeline.launch.py | 52 +- .../launch/stage2_locate.launch.py | 52 +- .../launch/stage3_search.launch.py | 52 +- .../launch/visual_servo.launch.py | 52 +- src/arm_application/setup.py | 2 + src/arm_application/test/test_gauge_boot.py | 47 +- .../test/test_gauge_step_events.py | 159 ++ .../test/test_handeye_lib_roundtrip.py | 146 ++ src/arm_application/test/test_model_labels.py | 5 + .../test/test_weights_layout.py | 5 + 17 files changed, 2335 insertions(+), 171 deletions(-) create mode 100755 scripts/run_gauge_assembly.sh create mode 100644 src/arm_application/arm_application/gauge_assembly_node.py create mode 100755 src/arm_application/launch/gauge_assembly.launch.py create mode 100644 src/arm_application/test/test_gauge_step_events.py create mode 100644 src/arm_application/test/test_handeye_lib_roundtrip.py diff --git a/scripts/run_gauge_assembly.sh b/scripts/run_gauge_assembly.sh new file mode 100755 index 0000000..6462ad7 --- /dev/null +++ b/scripts/run_gauge_assembly.sh @@ -0,0 +1,90 @@ +#!/usr/bin/env bash +# 通止规装配（看板 S00–S11）：自检 → 回 home+极限角 → 跟踪 → 定位 → 悬停 → +# 柔顺接触 → 棘轮拧入 → 反向拧出 → 换孔 → 复位。 +# +# 前置（本脚本**不**替你起这些，只让 S00 自检告诉你缺什么）： +# T1 bash ~/ros2/scripts/run_real.sh --arm 89 # 机械臂控制器 + /home +# T2 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 # 腕部 D405 预览 +# T2 bash ~/ros2/scripts/run_tactile.sh # Orisys 触觉三向力 +# T3 bash ~/ros2/scripts/run_gripper.sh # 夹爪服务（张开/夹紧/状态） +# —— 手眼标定产物 handeye/results/_eye_in_hand.yaml（S05 定位的前提） +# +# 为什么不在脚本里检查/拉起这些：判断标准只能有一处。脚本里查一遍、节点里 +# 再查一遍，就会出现"脚本说没问题、节点说没数据"（发布者不在的假阳性）， +# 而 S00 的报告是打印给人看的，带"缺了会怎样"。 +# +# 用法： +# bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 # 完整装配（2 个孔） +# bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --holes 1 # 只装 k1 +# bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --dry-run # 只验编排/配置，不动臂 +# bash ~/ros2/scripts/run_gauge_assembly.sh --sim --dry-run # 仿真环境跑干跑 +# bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --backend legacy # 强制真机夹爪后端 +# +# 参数接口：config/site.yaml → gauge_assembly（力阈值/角度/轮数/超时全部可调） +# 力阈值标定：先 bash ~/ros2/scripts/run_tactile.sh --cli 看读数量级 +# 掉件重放：bash ~/ros2/scripts/run_gauge_assembly.sh 之外另开一个终端 +# ros2 topic pub -1 /gauge/reload_ok std_msgs/Bool '{data: true}' +set -eo pipefail +ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" + +USE_SIM=false +DRY_RUN=false +HOLES="" +BACKEND="auto" + +# shellcheck disable=SC1091 +source "${ROOT}/scripts/lib/parse_arm_args.sh" +parse_arm_args "$@" +set -- "${PARSE_ARM_REST[@]}" + +while [[ $# -gt 0 ]]; do + case "$1" in + --sim) USE_SIM=true; shift ;; + --dry-run) DRY_RUN=true; shift ;; + --holes) HOLES="${2:?}"; shift 2 ;; + --backend) BACKEND="${2:?}"; shift 2 ;; + -h|--help) sed -n '2,26p' "$0"; exit 0 ;; + *) echo "Unknown: $1" >&2; exit 1 ;; + esac +done + +case "${BACKEND}" in + auto|contract|legacy) ;; + *) echo "--backend 只能是 auto|contract|legacy，收到 ${BACKEND}" >&2; exit 1 ;; +esac + +if [[ "${USE_SIM}" == true ]]; then + echo "=== 仿真环境（install_sim + ROS_DOMAIN_ID=77） ===" + # shellcheck disable=SC1091 + source "${ROOT}/scripts/sim_env.sh" +else + # shellcheck disable=SC1091 + source "${ROOT}/scripts/env.sh" +fi + +if [[ -z "${SITE_ROBOT_SN:-}" ]]; then + echo "ERROR: 没选出机械臂。用 --arm 89 指定，或填 config/site.yaml 的 robot.sn" >&2 + exit 1 +fi + +if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then + export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}" + export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}" +fi + +echo "=== 通止规装配 arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN}) ===" +echo " 孔数=${HOLES:-site} 夹爪后端=${BACKEND} 干跑=${DRY_RUN}" + +# 清掉上一轮可能残留的装配节点（同一时间只允许一个进程在指挥这条臂） +# 不 pkill 整个 arm_application：会连相机/触觉服务一起带走（那些是本脚本的前置） +pkill -f 'lib/arm_application/gauge_assembly' 2>/dev/null || true +sleep 0.5 + +ARGS=(gripper_backend:="${BACKEND}") +if [[ -n "${HOLES}" ]]; then ARGS+=(holes_total:="${HOLES}"); fi +if [[ "${DRY_RUN}" == true ]]; then ARGS+=(dry_run:=true); fi + +echo "=== launch gauge_assembly（自检不过会自动报缺什么） ===" +# 不用 exec：exec 之后 Ctrl-C 直接打给 ros2 launch，这里的清理就没了； +# gauge_assembly 自己也会在收到 KeyboardInterrupt 时退到安全高度。 +ros2 launch arm_application gauge_assembly.launch.py "${ARGS[@]}" diff --git a/src/arm_application/arm_application/gauge_assembly_node.py b/src/arm_application/arm_application/gauge_assembly_node.py new file mode 100644 index 0000000..3a40359 --- /dev/null +++ b/src/arm_application/arm_application/gauge_assembly_node.py @@ -0,0 +1,1550 @@ +#!/usr/bin/env python3 +"""通止规装配主流程（看板 S00–S11 + E1/E2）。 + +一次完整装配做什么 +------------------ +1. **S00** 自检：机械臂伺服 / 相机预览 / 夹爪伺服 / 触觉图传 / 算法权重， + 全部就绪才开指令窗（缺什么、缺了会怎样，逐条打印）； +2. **S01** 回 home，再按工具系逆时针预绕到"不触发关节极限的极限角" —— + 这一步是给 S08 的棘轮留出回转行程，不然后面转两圈就贴限位； +3. **S02** 夹持确认（预留，默认跳过：传感器方案在优化、D435 未标定）； +4. **S04** 腕部 D405 跟踪 k0（S03 手外粗定位只作兜底：跟踪丢了才去， + 今天 D435 还没标定，所以会明确报"缺哪个文件"而不是假装在做）； +5. **S05** 画面稳定 T 秒后精拍定位 k1/k2，并记下当时的位姿； +6. **S06→S09** 对当前孔：正悬停 → 柔顺接触 → 棘轮式拧入 → 反向拧出； +7. **S10** 换到下一个孔（回到 S05 记的位姿），重复 S06–S09； +8. **S11** 复位退出。 + +异常分支：**E1** 掉件（张开爪 → 回 S01 流程 → 提示人工放好 → 按 R 继续）、 +**E2** 任意阶段超时/报错 → 退到安全高度后中止。 + +怎么跑 +------ +:: + + # 真机（先起机械臂与相机：run_real.sh / run_camera_d405.sh） + bash scripts/run_gauge_assembly.sh --arm 89 # 完整流程 + bash scripts/run_gauge_assembly.sh --arm 89 --dry-run # 只验编排，不动臂 + bash scripts/run_gauge_assembly.sh --arm 89 --holes 1 # 只装 k1 + + # 仿真（先起仿真：run_sim_gz.sh） + bash scripts/run_gauge_assembly.sh --sim + +诚实标注（哪些已经能信、哪些还不能） +------------------------------------ +**已经钉住的**（有单测/有验收判据）： + +* S00–S11 的顺序、E1/E2 的落点（``gauge_fsm`` 92 项单测）； +* 配置解析与单位、未知键报错（``gauge_config``）； +* S01 的限位夹取与"真的绕工具 Z 转了"的事后验证（``gauge_kin``，用 TF 实测）； +* 夹爪后端按**服务类型**选择（``gauge_gripper``）； +* S00 的必检/选检划分（``gauge_boot``）。 + +**还不能信的**（必须真机标定/标定后才能用）： + +* S07 的切向阈值、S08 的力判据：全部是"相对静止基线的增量"（任意单位）， + 必须现场标定，代码只给了起点值和打印； +* S03 手外粗定位：D435 还没有 eye-to-hand 标定文件 → 目前必然走兜底； +* S02 夹持确认：预留，默认跳过。 +""" + +from __future__ import annotations + +import json +import math +import os +import time +from pathlib import Path +from typing import Any, Dict, List, Optional, Sequence, Tuple + +import numpy as np +import rclpy +from rclpy.qos import ( + DurabilityPolicy, + QoSProfile, + ReliabilityPolicy, + qos_profile_sensor_data, +) +from sensor_msgs.msg import CameraInfo, Image +from std_msgs.msg import Bool, Float32MultiArray, String +from std_srvs.srv import Trigger +from trajectory_msgs.msg import JointTrajectory + +from arm_msgs.msg import GripperStatus +from arm_msgs.srv import GripperExecute + +from arm_application.gauge_boot import ( + BootCheck, + abs_name, + blocking_failures, + build_checks, + format_report, + optional_warnings, + summary_counts, +) +from arm_application.gauge_config import GaugeConfig, GaugeConfigError, from_site +from arm_application.gauge_fsm import ( + Event, + FsmError, + GaugeContext, + GaugeFsm, + State, + is_joint_limit_error, + pixel_stable, + ratchet_cycles, + screw_stop, + tangential_jump, +) +from arm_application.gauge_gripper import ( + CONTRACT_STATUS_TOPIC, + GripperInterfaceError, + describe_backend_choice, + is_dropped_from_state, + is_gripped_from_state, + legacy_states_to_fields, + pick_backend, +) +from arm_application.gauge_kin import ( + axis_misalignment_deg, + match_limits, + parse_joint_limits, + prewind, + rotation_about_axis_deg, + vertical_down_error_deg, +) +from arm_application.pipeline_node import AssemblyPipeline +from arm_application.handeye_realsense import resolve_realsense_topics +from arm_application.site_io import deep_merge, load_site_dict +# 复用同一份图像解码与轨迹点构造，别在这里再写一份（规则 M2） +from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point + +#: 允许的"绕轴验证"误差 [deg]。超过就说明这根关节轴不是工具 Z 轴， +#: 后面所有以"转多少度"为单位的动作都不成立 —— 必须报错而不是继续。 +AXIS_VERIFY_TOL_DEG = 5.0 +#: 限制"跟踪丢失 → 粗定位 → 跟踪"的来回次数，防死循环。 +MAX_TRACK_RETRIES = 3 + + +class GaugeAbort(RuntimeError): + """阶段内部判定要中止。带上 exit code，方便脚本区分原因。""" + + def __init__(self, message: str, code: int = 6) -> None: + super().__init__(message) + self.code = code + + +class GripperClient: + """夹爪出入口：``open/close/is_gripped`` 三个动作，与后端无关。 + + 后端由 :func:`arm_application.gauge_gripper.pick_backend` 按**服务类型**决定 + （真机今天是 Trigger，契约/仿真 是 GripperExecute），应用层不需要知道。 + """ + + def __init__(self, node: Any, backend: str, status_topic: str) -> None: + self._node = node + self.backend = backend + self._open_cli = None + self._close_cli = None + self._last_gripped: Optional[bool] = None + self._legacy_grip_state: Optional[int] = None + self._ever_gripped = False + + if backend == "contract": + self._open_cli = node.create_client(GripperExecute, "/gripper/open") + self._close_cli = node.create_client(GripperExecute, "/gripper/close") + node.create_subscription(GripperStatus, status_topic, self._on_contract, 10) + else: + self._open_cli = node.create_client(Trigger, "/gripper/open") + self._close_cli = node.create_client(Trigger, "/gripper/close") + node.create_subscription(Float32MultiArray, status_topic, self._on_legacy, 10) + + # -- 状态回调 --------------------------------------------------------- + def _on_contract(self, msg: GripperStatus) -> None: + self._last_gripped = bool(msg.is_gripped) + if self._last_gripped: + self._ever_gripped = True + + def _on_legacy(self, msg: Float32MultiArray) -> None: + fields = legacy_states_to_fields(list(msg.data)) + if not fields: + return + self._legacy_grip_state = int(fields["grip_state"]) + got = is_gripped_from_state(self._legacy_grip_state) + if got is not None: + self._last_gripped = got + if got: + self._ever_gripped = True + + # -- 查询 ------------------------------------------------------------- + def seen(self) -> bool: + """收到过状态帧吗。S00 用它判断"状态话题有没有人发"。 + + 不能用 `topic list` 里有名字代替：话题在、发布者不在，是最常见的 + 假阳性（run_all.sh 踩过）。""" + return self._last_gripped is not None or self._legacy_grip_state is not None + + def is_gripped(self) -> Optional[bool]: + """``True``/``False``/``None``（后端给不出明确答案）。""" + return self._last_gripped + + def dropped(self) -> bool: + """E1 掉件：**曾经夹住过**，现在报"掉落"。 + + 装配前爪里本来就空，那时状态字也可能是"掉落/无物"，直接判会一开场就 + 报 E1（``is_dropped_from_state`` 里解释了这一点）。 + """ + if self.backend == "legacy" and self._legacy_grip_state is not None: + return is_dropped_from_state(self._legacy_grip_state, self._ever_gripped) + return self._ever_gripped and self._last_gripped is False + + # -- 动作 ------------------------------------------------------------- + def _call(self, client: Any, tag: str, timeout_s: float) -> 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 + else: + req = Trigger.Request() + fut = client.call_async(req) + t0 = time.monotonic() + while not fut.done() and time.monotonic() - t0 < timeout_s: + self._node._spin(0.02) + if not fut.done(): + self._node.get_logger().error(f"夹爪 {tag} 超时 {timeout_s:.1f}s") + return False + try: + resp = fut.result() + except Exception as exc: # noqa: BLE001 + self._node.get_logger().error(f"夹爪 {tag} 调用异常：{exc}") + return False + ok = bool(getattr(resp, "success", True)) + if not ok: + self._node.get_logger().error( + f"夹爪 {tag} 失败：{getattr(resp, 'message', '（无消息）')}" + ) + return ok + + def open(self, timeout_s: float = 5.0) -> bool: + if self._node.cfg.dry_run: + self._node.get_logger().info("[干跑] 夹爪张开") + return True + return self._call(self._open_cli, "open", timeout_s) + + def close(self, timeout_s: float = 5.0) -> bool: + if self._node.cfg.dry_run: + self._node.get_logger().info("[干跑] 夹爪夹紧") + return True + return self._call(self._close_cli, "close", timeout_s) + + +class GaugeAssembly(AssemblyPipeline): + """S00–S11 的执行器：状态机的每一步在这类里实现。 + + 继承 :class:`~arm_application.pipeline_node.AssemblyPipeline` 是为了复用 + 已经调通的**跟踪拉起/停止**（``_run_stage1``/``_stop_stage1``/``_track_z``） + 与整条 stage2 定位链（``_run_locate``）—— 这些是阶段式流水线的公共骨架， + 抄一份只会让"改了 A 忘了改 B"变成常态（规则 M2）。 + """ + + #: 状态 → 处理方法名。**顺序即流程**，与 gauge_fsm.TRANSITIONS 一一对应。 + _STEPS: Dict[State, str] = { + State.BOOT: "_step_boot", + State.HOME_EXTREME: "_step_home_extreme", + State.GRIP_CHECK: "_step_grip_check", + State.COARSE: "_step_coarse", + State.TRACK: "_step_track", + State.SNAP: "_step_snap", + State.HOVER_K: "_step_hover_k", + State.CONTACT: "_step_contact", + State.SCREW_IN: "_step_screw_in", + State.UNSCREW: "_step_unscrew", + State.NEXT_HOLE: "_step_next_hole", + State.RESET: "_step_reset", + } + + def __init__(self, node_name: str = "gauge_assembly") -> None: + super().__init__(node_name=node_name) + + self.declare_parameter("dry_run", False) + self.declare_parameter("holes_total", -1) # >0 覆盖 site + self.declare_parameter("gripper_backend", "auto") # auto|contract|legacy + + site = load_site_dict() + overrides: Dict[str, Any] = {} + if bool(self.get_parameter("dry_run").value): + overrides["dry_run"] = True + holes = int(self.get_parameter("holes_total").value) + if holes > 0: + overrides["holes_total"] = holes + try: + self.cfg: GaugeConfig = from_site(site, overrides=overrides or None) + except GaugeConfigError as exc: + raise GaugeAbort(f"gauge_assembly 配置有问题：\n{exc}", code=3) from exc + + self._site = site + self._warn_if_section_missing(site) + + # ---- 自检清单（纯内容 + 本机名字）---- + self._gripper_prefer = str(self.get_parameter("gripper_backend").value or "auto") + # robots[] 里的 tool_name 优先（一台机器一个工具），没有就用全局 robot.tool_name + self.tool_name = str( + self._robot_dict(site).get("tool_name") + or (site.get("robot") or {}).get("tool_name") or "" + ) + self._weights = str((site.get("paths") or {}).get("yolo_weights") or "") + # 夹爪后端要按"服务实际类型"选，所以放到 S00 里做（此时才知道服务在不在） + self._boot_results: List[Tuple[BootCheck, bool, str]] = [] + + # ---- 状态机 ---- + self.fsm = GaugeFsm(GaugeContext(holes_total=self.cfg.holes_total)) + self.grip: Optional[GripperClient] = None + self._coarse_ok = False + self._track_retries = 0 + self._snap_final: Dict[str, Any] = {} + self._snap_pose: Optional[np.ndarray] = None + self._gauge_was_gripped = False + self._axis_verified = False + #: E1（掉件）重放：S08 报掉件后置位，S01 走完要等人工放好并确认 + self._e1_pending = False + self._reload_ok = False + + # ---- 手外相机（S03 粗定位）：stage2 只订阅腕部相机，这里自己订一份 ---- + # 只订不驱动：S03 是兜底路径，平时不读它的帧，不能因为"手外相机没插" + # 就影响主流程（所以 S00 里它只是选检）。 + self._coarse_color: Optional[Image] = None + self._coarse_depth: Optional[Image] = None + self._coarse_info: Optional[CameraInfo] = None + self._coarse_ns = self._d435_ns(site) + if self._coarse_ns: + img_t, info_t = resolve_realsense_topics(self._coarse_ns, image_topic="") + # QoS 必须用 sensor data（BEST_EFFORT）：RealSense 驱动发的是 + # BEST_EFFORT，而 create_subscription 的默认是 RELIABLE —— + # 不匹配时一条消息都收不到，日志里只有一条容易忽略的 + # "offering incompatible QoS" 警告，表现为"相机明明在跑却没图像"。 + self.create_subscription( + Image, img_t, self._on_coarse_color, qos_profile_sensor_data + ) + self.create_subscription( + CameraInfo, info_t, self._on_coarse_info, qos_profile_sensor_data + ) + self.create_subscription( + Image, f"/{self._coarse_ns}/aligned_depth_to_color/image_raw", + self._on_coarse_depth, qos_profile_sensor_data, + ) + + # 人工确认话题：E1 重放时，操作员放好新规后发一条 true + # ros2 topic pub -1 /gauge/reload_ok std_msgs/Bool '{data: true}' + # 用话题而不是服务：一条命令就能发（不必等回包），现场手忙脚乱时更稳。 + self.create_subscription(Bool, "/gauge/reload_ok", self._on_reload_ok, 1) + + # ---- 发布 ---- + 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) + self._robot_description: Optional[str] = None + self.create_subscription( + String, "/robot_description", self._on_robot_description, + QoSProfile( + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ), + ) + + self.get_logger().info( + f"通止规装配 arm={self.prefix.rstrip('_')} " + f"孔数={self.cfg.holes_total} 干跑={self.cfg.dry_run} " + f"工具系={self.tool_name or '(未配置)'} 手外相机={self._coarse_ns or '(无)'}" + ) + for line in self.cfg.describe(): + self.get_logger().info(line) + + # ===================================================================== + # 小工具 + # ===================================================================== + def _warn_if_section_missing(self, site: Dict[str, Any]) -> None: + """整段缺失时明确告警：默认值只是起点，不是调好的参数。""" + if not site.get("gauge_assembly"): + self.get_logger().warn( + "site.yaml 里没有 gauge_assembly: 段 —— 全部走默认值。" + "力阈值类参数必须现场标定，见 config/site.example.yaml 的同名段" + ) + + def _robot_dict(self, site: Dict[str, Any]) -> Dict[str, Any]: + sn = str(self.get_parameter("robot_sn").value or "").strip() + for r in (site.get("robot") or {}).get("robots") or []: + if isinstance(r, dict) and str(r.get("sn") or "").strip() == sn: + return dict(r) + return {} + + def _d435_ns(self, site: Dict[str, Any]) -> str: + """手外相机的命名空间。``role: overhead_eye_to_hand`` 的那个。""" + cams = site.get("cameras") or {} + for _key, cfg in cams.items(): + if not isinstance(cfg, dict): + continue + if str(cfg.get("role") or "").startswith("overhead"): + return str(cfg.get("name") or "").strip("/") + return "" + + def _on_robot_description(self, msg: String) -> None: + self._robot_description = str(msg.data) + + def _on_reload_ok(self, msg: Any) -> None: + if bool(msg.data): + self._reload_ok = True + + def _on_coarse_color(self, msg: Image) -> None: + self._coarse_color = msg + + def _on_coarse_depth(self, msg: Image) -> None: + self._coarse_depth = msg + + def _on_coarse_info(self, msg: CameraInfo) -> None: + self._coarse_info = msg + + def _hole_name(self) -> str: + """孔名 k1/k2/...。直接用状态机的，别在节点里再实现一遍（规则 M2）。""" + return self.fsm.ctx.hole_name() + + def _img_age(self, msg: Optional[Image]) -> float: + """图像从"它的时间戳"到现在过了多久 [s]。拿不到时间戳返回 1e9。""" + if msg is None: + return 1e9 + try: + stamp = float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) * 1e-9 + except Exception: # noqa: BLE001 + return 1e9 + return max(0.0, self.get_clock().now().nanoseconds * 1e-9 - stamp) + + def _state(self, s: str) -> None: # 覆盖：三阶段的状态话题 + 本流程的 + super()._state(s) + msg = String() + msg.data = s + self._gauge_state_pub.publish(msg) + + def _progress(self, text: str) -> None: + msg = String() + msg.data = text + self._gauge_progress_pub.publish(msg) + + def _publish_gauge_result(self, payload: Dict[str, Any]) -> None: + 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)] + msg = String() + msg.data = json.dumps(payload, ensure_ascii=False) + self._gauge_result_pub.publish(msg) + + def _wait_joint_limits(self, timeout_s: float = 3.0) -> Optional[Tuple[float, float]]: + """拿关节7的限位（从 ``/robot_description``）。拿不到返回 None。""" + t0 = time.monotonic() + while self._robot_description is None and time.monotonic() - t0 < timeout_s: + self._spin(0.05) + if not self._robot_description: + self.get_logger().warn( + "拿不到 /robot_description —— S01 不做限位夹取，" + "改为靠驱动器限位报错兜底（真撞上时报 JOINT_LIMIT，流程继续）" + ) + return None + limits = parse_joint_limits(self._robot_description) + got = match_limits(limits, [self.joint_names[-1]]) + if got is None: + self.get_logger().warn( + f"/robot_description 里找不到 {self.joint_names[-1]} 的限位" + f"（解析到 {len(limits)} 个关节）—— 同样不做夹取" + ) + return got + + # ===================================================================== + # S00 自检 + # ===================================================================== + def _boot_checks(self) -> List[BootCheck]: + # 状态话题随后端不同；这里先按"契约"建，S00 内部按**真实服务类型**修正 + return build_checks( + joint_action=str(self.get_parameter("action_name").value), + camera=self._wrist_cam_ns(), + tactile_topic=str( + ((self._site.get("tactile") or {}).get("topic_left")) + or "/tactile/left/status" + ), + gripper_open="/gripper/open", + gripper_close="/gripper/close", + gripper_status=CONTRACT_STATUS_TOPIC, + gripper_status_type="arm_msgs/msg/GripperStatus", + d435_camera=self._coarse_ns or None, + tool_name=self.tool_name, + weights_path=self._weights, + ) + + def _wrist_cam_ns(self) -> str: + """腕部相机命名空间（``d405_left``）。以 site 的 robots[].camera.name 为准。""" + robot = self._robot_dict(self._site) + cam = deep_merge(self._site.get("camera") or {}, robot.get("camera") or {}) + return str(cam.get("name") or "d405").strip().strip("/") + + def _probe(self, item: BootCheck) -> Tuple[bool, str]: + """执行一项自检。返回 ``(就绪, 细节)``。""" + if item.probe == "service": + found = dict(self.get_service_names_and_types()).get(item.target) + if found is None: + return False, "名字不在图上" + if item.expect_type and item.expect_type not in found: + # 类型不符**不算就绪**：这正是"服务在但接不上"的现场 + return False, f"类型是 {list(found)}，期望 {item.expect_type}" + return True, "" + + if item.probe == "action": + # 这套 rclpy **没有** get_action_names_and_types()（实测缺这个方法）， + # 所以 action 只能"建客户端 + wait_for_server"来探。这样反而更实在： + # 名字在图上但服务端没起来（DDS 端点未匹配）也能查出来。 + return self._probe_action(item) + + if item.probe == "detector": + p = Path(item.target) + if not p.is_file(): + return False, f"文件不存在：{p}" + sidecar = p.with_suffix(".labels.yaml") + if not sidecar.is_file(): + return False, f"缺标签文件 {sidecar.name}（weights.sh doctor 可查）" + return True, f"{p.name} + {sidecar.name}" + + # topic / tactile / camera：**要真收到消息**才算就绪。 + # 只看 topic list 是假阳性来源（话题在、没数据），run_all.sh 里踩过。 + deadline = time.monotonic() + min(10.0, self.cfg.boot_timeout_s) + while time.monotonic() < deadline: + detail = self._peek_topic(item) + if detail is None: + return True, "" + time.sleep(0.2) + return False, detail or "等不到消息" + + def _probe_action(self, item: BootCheck) -> Tuple[bool, str]: + """探测 action 服务端是否可连。""" + from rosidl_runtime_py.utilities import get_interface + + if not item.expect_type: + return False, "清单里没写期望的 action 类型，无法探测" + try: + cls = get_interface(item.expect_type) + except Exception as exc: # noqa: BLE001 + return False, f"接口类型 {item.expect_type} 解析失败：{exc}" + # 已经建好的 JTC 客户端优先复用（stage2 的 self._cli 就是它）。 + # 比较前两边都要规范化：清单里的目标被补成了绝对名（``/foo``），而 + # 参数默认值是相对的（``foo``）—— 直接比会不相等，于是这里再建一个 + # **同名的** action 客户端，FastRTPS 会以"同话题名不同类型"报错， + # 报出来的是 create_client 失败，看不出真正原因是漏了一次规范化。 + if abs_name(item.target) == abs_name( + str(self.get_parameter("action_name").value) + ): + ok = self._cli.wait_for_server(timeout_sec=2.0) + return (True, "") if ok else (False, "action 服务端未就绪（wait_for_server 超时）") + from rclpy.action import ActionClient + + cli = ActionClient(self, cls, item.target) + ok = cli.wait_for_server(timeout_sec=2.0) + cli.destroy() + return (True, "") if ok else (False, "action 服务端未就绪（wait_for_server 超时）") + + def _peek_topic(self, item: BootCheck) -> Optional[str]: + """看一眼该话题最近有没有**真数据**。``None`` = 就绪，否则返回原因。 + + 为什么每种都要单独判：`topic list` 里有名字 ≠ 有数据。发布者没起、 + 相机没插、驱动崩了，名字都还在。按名字判会在最需要它的时候（S07 要 + 停力、S08 要判拧紧）才发现没数据。 + """ + if item.probe == "camera": + coarse = self._coarse_ns and f"/{self._coarse_ns}/" in item.target + msg = self._coarse_color if coarse else getattr(self, "_color", None) + age = self._img_age(msg) + if msg is None: + return "还没收到图像（发布者不在？）" + if age > 3.0: + return f"最后一帧已过期 {age:.1f}s" + return None + if item.probe == "tactile": + for tac in (getattr(self, "_tac_l", None), getattr(self, "_tac_r", None)): + if not tac: + continue + t, m = tac + if time.monotonic() - t < 0.5 and bool(getattr(m, "connected", False)): + return None + return "触觉无新鲜帧或 connected=false" + if "camera_info" in item.target: + info = self._coarse_info if (self._coarse_ns and self._coarse_ns in item.target) else getattr(self, "_info", None) + if info is None: + return "没收到内参" + return None + if "gripper" in item.target: + if self.grip is None: + return "夹爪客户端未建立" + if not self.grip.seen(): + return "没收到夹爪状态帧（服务在、反馈不在）" + return None + return None + + def _step_boot(self) -> Event: + """S00：全部必检就绪才开指令窗。""" + self._state("S00_boot") + checks = self._boot_checks() + + # 夹爪后端：按**服务类型**选（真机 Trigger / 契约 GripperExecute）。 + # 选不出来时把这条也算成一项必检失败，而不是抛异常中断整段日志。 + backend_detail = "" + try: + choice = pick_backend(dict(self.get_service_names_and_types()), self._gripper_prefer) + for line in describe_backend_choice(choice): + self.get_logger().info(line) + self.grip = GripperClient(self, choice.backend, choice.status_topic) + checks = [ + c if c.key != "gripper_status" + else BootCheck( + key=c.key, label=c.label, probe=c.probe, target=choice.status_topic, + why=c.why, required=c.required, + expect_type=( + "arm_msgs/msg/GripperStatus" + if choice.backend == "contract" else "std_msgs/msg/Float32MultiArray" + ), + ) + for c in checks + ] + except GripperInterfaceError as exc: + backend_detail = str(exc) + self.get_logger().error(f"夹爪后端无法确定：{exc}") + + # 相机 / 触觉的"真收到消息"探测要先起订阅（stage2/stage3 的 __init__ 已订阅） + results: List[Tuple[BootCheck, bool, str]] = [] + for item in checks: + ok, detail = self._probe(item) + results.append((item, ok, detail)) + + if backend_detail: + # 把"选不出夹爪后端"作为一条必检失败并入报告：一条日志一次看完 + blocker = BootCheck( + key="gripper_backend", label="夹爪后端可确定", probe="service", + target="/gripper/open", why=backend_detail, + ) + results.append((blocker, False, "见上面的详细原因")) + + text, _failures = format_report(results) + for line in text.splitlines(): + self.get_logger().info(line) + counts = summary_counts(results) + self.get_logger().info( + f"S00 摘要：必检 {counts['required_ok']}/{counts['required_total']}，" + f"选检 {counts['optional_ok']}/{counts['optional_total']}" + ) + for line in optional_warnings(results): + self.get_logger().warn(line) + self._boot_results = results + + blockers = blocking_failures(results) + if blockers: + for line in blockers: + self.get_logger().error(line) + if self.cfg.dry_run: + self.get_logger().warn( + "干跑模式：自检没过也继续（只为验证编排与配置，不会真的动臂）" + ) + return Event.BOOT_OK + self._publish_gauge_result({"ok": False, "state": "S00", "failures": blockers}) + return Event.BOOT_FAIL + return Event.BOOT_OK + + # ===================================================================== + # S01 回 home + 逆时针极限角 + # ===================================================================== + def _step_home_extreme(self) -> Event: + self._state("S01_home_extreme") + if self.cfg.dry_run: + self._fsm_dry(Event.POSE_OK, "回 home + 逆时针预绕") + return Event.POSE_OK + + if not self._call_trigger("/home", timeout_s=60.0): + return Event.TIMEOUT + + # 工具系：要求 tool1 垂直向下且位姿锁定。切不过去就不继续 —— + # 后面每个动作的"工具系"含义都建立在它上面。 + if self.tool_name: + if not self._switch_tool(self.tool_name): + return Event.TIMEOUT + else: + self.get_logger().warn( + "site.yaml 里 robot.tool_name 为空 —— 未确认工具系。" + "看板要求 tool1 全程垂直向下并位姿锁定，请填上名字" + ) + + if self._e1_pending: + # E1 重放：先张开爪把残留放掉，再等人工放好新规。 + # 顺序不能反：先确认再张嘴，会把刚放好的规一起掉下去。 + if self.grip is not None and not self.grip.open(): + self.get_logger().error("E1 重放：张开爪失败，无法继续") + return Event.ABORT + if not self._wait_operator_reload(): + return Event.TIMEOUT + self._e1_pending = False + return Event.RELOADED + + q, Tee = self._wait_state(10.0) + if q is None or Tee is None: + self.get_logger().error("拿不到 joint_states/TF，S01 无法继续") + return Event.TIMEOUT + + # 垂直度：这是"工具系垂直向下"的可测版本（不是靠眼看画面） + tilt = vertical_down_error_deg(Tee[:3, :3]) + self.get_logger().info(f"S01 工具 Z 与垂直向下夹角 {tilt:.2f}（要求≈0）") + + want = self.cfg.extreme_angle_deg + if want <= 0.0: + return Event.POSE_OK + + limits = self._wait_joint_limits() + plan = prewind( + q_now_rad=float(q[-1]), want_deg=want, direction=self.cfg.extreme_direction, + limits=limits, margin_deg=self.cfg.extreme_margin_deg, + ) + if plan.clamped: + self.get_logger().info( + f"S01 预绕被限位夹取：想转 {plan.want_deg:.1f}°，" + f"实际转 {plan.applied_deg:.1f}°（限位余量 {self.cfg.extreme_margin_deg:.1f}°）" + ) + return self._rotate_joint_and_verify( + plan.target_rad, tag="S01_extreme", fsm_event=Event.POSE_OK, + speed_deg_s=self.cfg.extreme_speed_deg_s, + ) + + def _wait_operator_reload(self) -> bool: + """E1 重放：等操作员放好通止规并确认。 + + 确认方式：``ros2 topic pub -1 /gauge/reload_ok std_msgs/Bool '{data: true}'`` + 超时（``gauge_assembly.reload_timeout_s``）→ False → 上层 TIMEOUT → 中止。 + 超时不是"再等等"，而是"没人管" —— 臂停在 home，让人来处理比继续跑安全。 + """ + self._reload_ok = False + t0 = time.monotonic() + warn_at = self.cfg.reload_timeout_s * 0.5 + warned = False + self.get_logger().warn( + "E1 重放：请把通止规放回爪内并确认。确认命令：\n" + " ros2 topic pub -1 /gauge/reload_ok std_msgs/Bool '{data: true}'\n" + f" （最多等 {self.cfg.reload_timeout_s:.0f}s，超时按中止处理）" + ) + while time.monotonic() - t0 < self.cfg.reload_timeout_s: + self._spin(0.1) + if self._reload_ok: + self.get_logger().info("E1 重放：已确认，继续流程") + return True + if not warned and time.monotonic() - t0 > warn_at: + self.get_logger().warn("E1 重放：还在等确认（已过半程）") + warned = True + self.get_logger().error( + f"E1 重放：等了 {self.cfg.reload_timeout_s:.0f}s 没等到 /gauge/reload_ok，中止" + ) + return False + + def _call_trigger(self, name: str, timeout_s: float) -> bool: + """``std_srvs/Trigger`` 服务调用（home / 复位都用它）。""" + cli = self.create_client(Trigger, name) + if not cli.wait_for_service(timeout_sec=5.0): + self.get_logger().error(f"{name} 服务不在") + return False + fut = cli.call_async(Trigger.Request()) + t0 = time.monotonic() + while not fut.done() and time.monotonic() - t0 < timeout_s: + self._spin(0.05) + if not fut.done(): + self.get_logger().error(f"{name} 超时 {timeout_s:.0f}s") + return False + resp = fut.result() + if not bool(getattr(resp, "success", False)): + msg = str(getattr(resp, "message", "")) + if is_joint_limit_error(msg): + self.get_logger().warn(f"{name} 报关节极限（S01 视为预期）：{msg}") + return True + self.get_logger().error(f"{name} 失败：{msg}") + return False + return True + + def _switch_tool(self, tool: str) -> bool: + from arm_msgs.srv import SwitchTool + + cli = self.create_client(SwitchTool, "/switch_tool") + if not cli.wait_for_service(timeout_sec=5.0): + self.get_logger().error("/switch_tool 服务不在 —— 无法确认工具系") + return False + req = SwitchTool.Request() + req.tool_name = tool + fut = cli.call_async(req) + t0 = time.monotonic() + while not fut.done() and time.monotonic() - t0 < 30.0: + self._spin(0.05) + if not fut.done(): + self.get_logger().error("切换工具系超时") + return False + resp = fut.result() + if not bool(getattr(resp, "success", False)): + self.get_logger().error(f"切换工具系失败：{getattr(resp, 'message', '')}") + return False + self.get_logger().info(f"工具系已切到 {tool}") + return True + + def _rotate_joint_and_verify( + self, target_rad: float, tag: str, fsm_event: Event, + speed_deg_s: Optional[float] = None, + ) -> Event: + """只动最后一根关节（= 绕工具 Z），**事后用 TF 量它真的转了**。 + + 为什么只动关节 7：工具轴垂直向下时，最后一根关节的轴就是工具 Z 轴， + 绕工具 Z 转 = 动这一根。好处是行程、速度、限位都是关节空间的量， + 不需要管笛卡尔姿态的插补。 + + 为什么必须量回来：如果这台臂的关节 7 轴与工具 Z 不共线（换了工具/ + 装了转接件就可能不共线），"转 180°"就变成了"绕某个斜轴转 180°"， + 规会斜着磨螺纹 —— 而所有日志都显示"成功了"。 + """ + q, Tee = self._wait_state(5.0) + if q is None or Tee is None: + return Event.TIMEOUT + R_before = Tee[:3, :3].copy() + tool_z = R_before[:, 2].copy() + + delta_deg = math.degrees(target_rad - float(q[-1])) + speed = max(1.0, abs(speed_deg_s if speed_deg_s else self.cfg.extreme_speed_deg_s)) + dur = max(0.5, abs(delta_deg) / speed) + traj = JointTrajectory() + traj.joint_names = list(self.joint_names) + q_t = q.copy() + q_t[-1] = float(target_rad) + traj.points = [_joint_point(q, np.zeros(len(q)), 0.2), _joint_point(q_t, np.zeros(len(q)), dur)] + ok = self._send_action(traj, wait_s=dur + 8.0) + if not ok: + self.get_logger().warn(f"{tag}: JTC 未确认完成，按 TF 实测判断") + + time.sleep(0.3) + q2, Tee2 = self._wait_state(5.0) + if q2 is None or Tee2 is None: + return Event.TIMEOUT + got = rotation_about_axis_deg(R_before, Tee2[:3, :3], tool_z) + mis = axis_misalignment_deg(R_before, Tee2[:3, :3], tool_z) + self.get_logger().info( + f"{tag}: 目标 {delta_deg:+.1f}°，实测绕工具 Z {got:+.1f}°" + f"（转轴偏差 {mis:.1f}°，阈值 {AXIS_VERIFY_TOL_DEG:.1f}°）" + ) + if abs(got) < 3.0 and abs(delta_deg) > 10.0: + # 完全没动：最可能是撞上关节极限（用户要求的"转到极限角"就是这种情形） + self.get_logger().warn( + f"{tag}: 指令 {delta_deg:+.1f}° 但几乎没动 —— 判为已贴关节极限" + ) + return Event.JOINT_LIMIT + if mis > AXIS_VERIFY_TOL_DEG: + self.get_logger().error( + f"{tag}: 转轴偏差 {mis:.1f}° 超过阈值 —— 关节7 轴与工具 Z 不共线，" + "以『度』为单位的动作（S01/S08/S09）都不可信，中止。" + "先确认工具系是否垂直向下、有没有装转接件" + ) + return Event.ABORT + if abs(abs(got) - abs(delta_deg)) > max(AXIS_VERIFY_TOL_DEG, 0.1 * abs(delta_deg)): + self.get_logger().error( + f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°，偏差过大，中止" + ) + return Event.ABORT + self._axis_verified = True + return fsm_event + + # ===================================================================== + # S02 夹持确认（预留） + # ===================================================================== + def _step_grip_check(self) -> Event: + self._state("S02_grip_check") + if not self.cfg.grip_check: + self.get_logger().info( + "S02 跳过夹持确认（预留：传感器方案在优化、D435 未标定）。" + "要开：site.yaml 的 gauge_assembly.grip_check: true" + ) + return Event.GRIP_SKIPPED + if self.grip is None: + return Event.GRIP_FAIL + time.sleep(self.cfg.grip_hold_s) + got = self.grip.is_gripped() + if got is None: + self.get_logger().error( + "S02 要求确认夹持，但当前夹爪后端给不出明确状态（状态字是运动中/到位）。" + "把夹爪合到咬住状态，或关掉 gauge_assembly.grip_check" + ) + return Event.GRIP_FAIL + if got: + self._gauge_was_gripped = True + self.get_logger().info("S02 确认：爪里夹着东西") + return Event.GRIP_PASS + self.get_logger().error("S02 判定爪里没有通止规 —— 请放好再跑") + return Event.GRIP_FAIL + + # ===================================================================== + # S03 手外粗定位（S04 的兜底） + # ===================================================================== + def _step_coarse(self) -> Event: + self._state("S03_coarse") + if not self.cfg.coarse_enabled: + self.get_logger().warn("S03 被配置关掉（gauge_assembly.coarse_enabled=false）") + return Event.COARSE_FAIL + ns = self.cfg.coarse_camera + he = self._eye_to_hand_file() + if he is None: + self.get_logger().warn( + "S03 需要手外相机的 eye-to-hand 标定文件，现在没有 —— " + f"按兜底逻辑回 S04 跟踪。\n" + f" 相机 {ns} 的标定产物应放在 handeye/results/<名>_eye_to_hand.yaml；\n" + 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 跟踪") + return Event.COARSE_FAIL + + xy = self._coarse_workpiece_xy(ns, he) + if xy is None: + self.get_logger().warn("S03 推理没找到工件框（或深度无效），回 S04 跟踪") + return Event.COARSE_FAIL + + q, Tee = self._wait_state(5.0) + if q is None or Tee is None: + return Event.TIMEOUT + target = np.array([float(xy[0]), float(xy[1]), self.cfg.coarse_hover_m]) + self.get_logger().info( + f"S03 粗定位：工件中心 ({xy[0]:.3f},{xy[1]:.3f}) → 悬停 " + f"{self.cfg.coarse_hover_m * 1000:.0f}mm" + ) + ok, q, Tee = self._goto( + q, Tee, target, verticalize=True, tol_m=0.005, + tol_tilt=self.vert_tol_rad * 2.0, tag="coarse", + v=self.move_speed, max_hops=10, settle_s=0.25, + ) + if not ok: + self.get_logger().warn("S03 移动到粗定位点未完全到位，仍进 S05 精拍") + self._coarse_ok = True + return Event.COARSE_OK + + def _eye_to_hand_file(self) -> Optional[Path]: + """找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。""" + for cand in ( + Path(f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml"), + Path("handeye/results/eye_to_hand.yaml"), + ): + if cand.is_file(): + return cand + return None + + def _camera_alive(self, ns: str) -> bool: + return self._coarse_color is not None and self._img_age(self._coarse_color) < 3.0 + + def _coarse_workpiece_xy(self, ns: str, he: Path) -> Optional[Tuple[float, float]]: + """手外相机拍一张 → 推理工件框中心 → 反投影到基座 XY。 + + 外参从 **TF** 取（`base_link` ← `<相机>_color_optical_frame`），不再自己 + 解析一份 ``T_4x4``：标定产物已经在启动时以静态 TF 发布了，两份解析 + 必然先漂移一份（而漂移的是哪一份没人知道）。 + + 为什么先查标定文件再进来：没有 eye-to-hand 外参时，"反投影"只能靠 + 猜一个 depth 目标 —— 那会得到一个**看起来合理**的坐标（这正是最坏的 + 情形：动作做对了、位置错了）。所以宁可明确失败。 + """ + img = getattr(self, "_coarse_color", None) + depth_msg = getattr(self, "_coarse_depth", None) + intr = getattr(self, "_coarse_info", None) + if img is None or depth_msg is None or intr is None: + return None + depth = self._depth_from_msg(depth_msg) + if depth is None: + return None + intr_d = { + "fx": float(intr.k[0]), "fy": float(intr.k[4]), + "cx": float(intr.k[2]), "cy": float(intr.k[5]), + } + bgr = self._bgr_from_msg(img) + bbox = self._best_bbox(bgr, conf=self.cfg.coarse_conf) + if bbox is None: + return None + u = 0.5 * (bbox[0] + bbox[2]) + v = 0.5 * (bbox[1] + bbox[3]) + z = float(np.median(depth[int(v) - 2:int(v) + 3, int(u) - 2:int(u) + 3])) + if not math.isfinite(z) or z <= 0.05: + return None + p_cam = self._px_to_cam(u, v, z, intr_d) + T = self._lookup(f"{self.prefix}base_link", f"{ns}_color_optical_frame") + if T is None: + self.get_logger().warn( + f"S03 查不到 {ns}_color_optical_frame 到基座的 TF —— 标定文件 {he.name} " + "在，但没有发布成静态 TF（检查启动时有没有跑 handeye TF 那一步）" + ) + return None + p_base = (T @ np.array([p_cam[0], p_cam[1], p_cam[2], 1.0]))[:3] + return float(p_base[0]), float(p_base[1]) + + def _bgr_from_msg(self, msg: Any) -> np.ndarray: + """``sensor_msgs/Image`` → BGR ndarray（复用 stage2 的同一段解码）。""" + return _bgr_copy(msg) + + def _depth_from_msg(self, msg: Image) -> Optional[np.ndarray]: + """``sensor_msgs/Image``(16UC1) → 米为单位的深度图。""" + try: + arr = np.frombuffer(msg.data, dtype=np.uint16) + if msg.encoding.lower() in ("16uc1", "mono16"): + arr = arr.astype(np.float32) / 1000.0 + else: + arr = arr.astype(np.float32) + return arr.reshape((int(msg.height), int(msg.width))) + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f"深度图解码失败：{exc}") + return None + + # ===================================================================== + # S04 跟踪 + # ===================================================================== + def _step_track(self) -> Event: + self._state("S04_track") + if self.cfg.dry_run: + self._fsm_dry(Event.TRACK_LOCKED, "拉起跟踪并等锁定") + return Event.TRACK_LOCKED + if self._track_retries >= MAX_TRACK_RETRIES: + self.get_logger().error( + f"S04 已连续丢 {self._track_retries} 次（上限 {MAX_TRACK_RETRIES}），" + "不再兜底重试 —— 检查工件是否在视野内、相机是否被挡、权重是否正确" + ) + return Event.ABORT + self._track_retries += 1 + code = self._run_stage1() + if code == 0: + self.get_logger().info(f"S04 跟踪锁定（第 {self._track_retries} 次尝试）") + return Event.TRACK_LOCKED + self.get_logger().warn(f"S04 跟踪未锁定（exit={code}），转 S03 兜底") + return Event.TRACK_LOST + + # ===================================================================== + # S05 稳定拍照 + # ===================================================================== + def _step_snap(self) -> Event: + self._state("S05_snap") + if self.cfg.dry_run: + self._fsm_dry(Event.SNAP_OK, "等画面稳定后精拍定位 k1/k2") + return Event.SNAP_OK + if not self._wait_pixel_stable(): + self.get_logger().warn( + f"S05 画面在 {self.cfg.snap_tol_px:.1f}px 容差内没稳定下来，继续等" + ) + return Event.SNAP_UNSTABLE + + q, Tee = self._wait_state(5.0) + if q is None or Tee is None: + return Event.TIMEOUT + code, q, Tee, final = self._run_locate(q, Tee) + if code != 0 or not final: + self.get_logger().error(f"S05 精拍定位失败 code={code}") + return Event.TIMEOUT + self._snap_final = dict(final) + self._snap_pose = Tee.copy() + self.get_logger().info( + f"S05 定位完成：k1={final.get('k1_base_m')} k2={final.get('k2_base_m')} " + f"残差 {final.get('residual_mm')}mm，已记位姿 Z={Tee[2, 3]:.4f}" + ) + return Event.SNAP_OK + + def _wait_pixel_stable(self) -> bool: + """等画面稳定：连续采 ``snap_stable_s`` 秒的检测框中心，抖动在容差内才算稳。 + + 用"到均值的最远距离"而不是相邻帧之差 —— 跟踪抖动是零均值的， + 相邻差会让"明明在晃"的时刻被判成稳定，然后拍出一张糊图（表现为 + "孔位偶尔偏 1mm"，随机且难复现）。判据实现见 gauge_fsm.pixel_stable。 + """ + deadline = time.monotonic() + 20.0 + while time.monotonic() < deadline: + samples: List[Tuple[float, float]] = [] + t0 = time.monotonic() + while time.monotonic() - t0 < self.cfg.snap_stable_s: + img = getattr(self, "_color", None) + if img is not None: + bbox = self._best_bbox(self._bgr_from_msg(img)) + if bbox is not None: + samples.append( + (0.5 * (bbox[0] + bbox[2]), 0.5 * (bbox[1] + bbox[3])) + ) + self._spin(0.05) + if pixel_stable(samples, self.cfg.snap_tol_px, self.cfg.snap_min_frames): + return True + return False + + # ===================================================================== + # S06 正悬停 + # ===================================================================== + def _step_hover_k(self) -> Event: + hole = self._hole_name() + self._state(f"S06_hover_{hole}") + if self.cfg.dry_run: + self._fsm_dry(Event.ALIGN_OK, f"到 {hole} 正悬停") + return Event.ALIGN_OK + + xy = self._hole_xy(hole) + if xy is None: + self.get_logger().error(f"S06 没有 {hole} 的坐标（S05 没定位到？）") + return Event.TIMEOUT + q, Tee = self._wait_state(5.0) + if q is None or Tee is None: + return Event.TIMEOUT + z = float(self._snap_final.get("hover_z_m") or self.hover_z) + target = np.array([xy[0], xy[1], z]) + ok, q, Tee = self._goto( + q, Tee, target, verticalize=True, tol_m=self.cfg.align_tol_mm / 1000.0, + tol_tilt=self.vert_tol_rad * 1.5, tag=f"S06_hover_{hole}", + v=self.move_speed, max_hops=10, settle_s=0.2, + ) + # 对位残差用**实测**算，不用运动返回值：运动"到点了"和对准了是两件事 + gxy = self._gauge_xy(Tee) + residual_mm = math.hypot(gxy[0] - xy[0], gxy[1] - xy[1]) * 1000.0 + self.get_logger().info( + f"S06 {hole} 悬停：实测残差 {residual_mm:.2f}mm" + f"（容差 {self.cfg.align_tol_mm:.1f}mm）Z={Tee[2, 3]:.4f}" + ) + if residual_mm > self.cfg.align_tol_mm: + self.get_logger().warn("对位超差 → 回 S05 重拍") + return Event.ALIGN_OUT + if not ok: + self.get_logger().warn("运动未完全到位但残差在容差内，继续") + self._progress(f"{hole} 悬停完成，残差 {residual_mm:.2f}mm") + return Event.ALIGN_OK + + def _hole_xy(self, hole: str) -> Optional[Tuple[float, float]]: + key = f"{hole}_base_m" + val = self._snap_final.get(key) + if not val or len(val) < 2: + return None + return float(val[0]), float(val[1]) + + # ===================================================================== + # S07 柔顺接触 + # ===================================================================== + def _shear_axes(self) -> Tuple[float, float]: + """当前两路切向读数（取法向最大的那只手指）。 + + 两路都要，是因为切向力到底走 X 还是 Y 通道未确认：只监控一个通道， + 赌错了就是整段流程静默失效（下发正常、日志正常、就是停不住）。 + """ + best = None + best_fn = -1.0 + for tac in (getattr(self, "_tac_l", None), getattr(self, "_tac_r", None)): + if not tac: + continue + t, m = tac + if time.monotonic() - t > 0.5: + continue + fn = abs(float(getattr(m, "fnormal", 0.0))) + if fn > best_fn: + best_fn, best = fn, m + if best is None: + return (0.0, 0.0) + return (float(getattr(best, "fshearx", 0.0)), float(getattr(best, "fsheary", 0.0))) + + def _step_contact(self) -> Event: + self._state("S07_contact") + if self.cfg.dry_run: + self._fsm_dry(Event.CONTACT_OK, "柔顺下臂到接触") + return Event.CONTACT_OK + q, Tee = self._wait_state(5.0) + if q is None or Tee is None: + return Event.TIMEOUT + + # 基线必须在**还没碰工件**的时候测。S06 结束时悬停在工件上方， + # 正好是测基线的时刻（带载测会把阈值抬飞）。 + ref = self._baseline(samples=8) + self.get_logger().info( + f"S07 基线：切向 ({ref[0]:.1f}, {ref[1]:.1f})、法向 {self._fn_rest:.1f}" + ) + + z0 = float(Tee[2, 3]) + step_m = self.cfg.contact_step_mm / 1000.0 + 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") + if not ok: + self.get_logger().warn("S07 单步未完全到位，继续按实测高度判断") + try: + fn, _fn_max, _shear, _contact = self._tac_fresh(self.tac_timeout) + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f"S07 触觉断流，无法判断接触：{exc}") + self._retract(q, Tee, dz_m=0.01) + return Event.TIMEOUT + now = self._shear_axes() + if tangential_jump(now, ref, self.cfg.contact_tangential_delta): + self.get_logger().info( + f"S07 接触：切向 {now[0]:.1f}/{now[1]:.1f} 相对基线 " + f"{ref[0]:.1f}/{ref[1]:.1f} 超过 +{self.cfg.contact_tangential_delta:.0f}" + f"（法向 {fn:.1f}，下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）" + ) + return Event.CONTACT_OK + if fn - self._fn_rest > self.cfg.contact_safe_delta: + self.get_logger().error( + f"S07 法向力超安全上限：{fn:.1f} - 基线 {self._fn_rest:.1f} > " + f"+{self.cfg.contact_safe_delta:.0f}，抬起并中止" + ) + self._retract(q, Tee, dz_m=0.01) + return Event.FORCE_OVER_LIMIT + self.get_logger().error( + f"S07 下探 {max_depth_m * 1000:.1f}mm 都没触发接触判据。\n" + " 常见原因：① 切向阈值太大（先看上面打印的基线/实测值再调）；" + "② 孔位偏了，规擦着孔壁下去但没接触面；③ 触觉通道不对" + ) + self._retract(q, Tee, dz_m=0.01) + return Event.TIMEOUT + + def _baseline(self, samples: int) -> Tuple[float, float]: + """静止基线的两路切向均值（顺手把法向基线记到 ``self._fn_rest``）。 + + 单侧可用即可：另一只指没插时，同一侧的阈值仍然自洽（相对量）。 + 拿不到任何读数时按 0 处理并**明确告警** —— 阈值会偏保守， + 结果可能是"碰上了没停"，而不是静默地继续跑。 + """ + acc: List[Tuple[float, float]] = [] + fns: List[float] = [] + t0 = time.monotonic() + while len(acc) < samples and time.monotonic() - t0 < 3.0: + sx, sy = self._shear_axes() + if sx != 0.0 or sy != 0.0: + acc.append((sx, sy)) + for tac in (getattr(self, "_tac_l", None), getattr(self, "_tac_r", None)): + if not tac: + continue + tt, mm = tac + if time.monotonic() - tt < 0.5: + fns.append(abs(float(getattr(mm, "fnormal", 0.0)))) + self._spin(0.05) + self._fn_rest = (sum(fns) / len(fns)) if fns else 0.0 + if not acc: + self.get_logger().warn( + "S07 拿不到切向读数，基线按 0 处理（阈值会偏保守：可能碰上了不停）" + ) + return (0.0, 0.0) + return ( + sum(a[0] for a in acc) / len(acc), + sum(a[1] for a in acc) / len(acc), + ) + + # ===================================================================== + # S08 旋转装配循环（棘轮） + # ===================================================================== + def _step_screw_in(self) -> Event: + hole = self._hole_name() + self._state(f"S08_screw_in_{hole}") + if self.cfg.dry_run: + self._fsm_dry(Event.SCREW_DONE, f"棘轮拧入 {hole}") + return Event.SCREW_DONE + + steps = ratchet_cycles( + self.cfg.screw_max_cycles * self.cfg.screw_step_deg, self.cfg.screw_step_deg + ) + self.get_logger().info( + f"S08 开始：最多 {steps} 轮 × {self.cfg.screw_step_deg:.0f}°" + f"（上限 {self.cfg.screw_max_cycles * self.cfg.screw_step_deg:.0f}°），" + f"停止判据「切向 >+{self.cfg.screw_shear_delta:.0f} " + f"且法向 >+{self.cfg.screw_normal_delta:.0f}」" + ) + for cycle in range(self.cfg.screw_max_cycles): + q, Tee = self._wait_state(5.0) + if q is None or Tee is None: + return Event.TIMEOUT + try: + fn_before, _fn_max, _shear, _c = self._tac_fresh(self.tac_timeout) + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f"S08 触觉断流，无法判断拧紧：{exc}") + return Event.ABORT + shear_before = self._shear_axes() + + # --- 四步原子动作，顺序固定（慢转 → 松爪 → 快回转 → 夹紧）--- + # 慢转是**顺时针**：与 S01 的逆时针预绕相反，这样预绕行程就是 + # 这一整段拧入的回转空间（S01 不预绕，转两圈就贴限位）。 + ev = self._rotate_joint_and_verify( + float(q[-1]) - self.cfg.screw_step_rad, tag=f"S08_slow_{cycle}", + fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s, + ) + if ev is not Event.SCREW_DONE: + return ev + # 松爪 → 快回转 → 夹紧 + if self.grip is not None: + if not self.grip.open(): + self.get_logger().error("S08 松爪失败 —— 继续转会把规拧回去") + return Event.ABORT + time.sleep(self.cfg.screw_release_wait_s) + q, Tee = self._wait_state(3.0) + if q is None: + return Event.TIMEOUT + ev = self._rotate_joint_and_verify( + float(q[-1]) + self.cfg.screw_step_rad, tag=f"S08_back_{cycle}", + fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s, + ) + if ev is not Event.SCREW_DONE: + return ev + if not self.grip.close(): + self.get_logger().error("S08 夹紧失败 —— 下一轮慢转会打滑磨螺纹") + return Event.ABORT + else: + self.get_logger().error("S08 没有夹爪客户端，无法松爪/夹紧，中止") + return Event.ABORT + + # --- 停止判据：切向 >阈值 **且** 法向突增 --- + try: + fn_after, _fn_max2, _shear_now, _c2 = self._tac_fresh(self.tac_timeout) + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f"S08 触觉断流，无法判断拧紧：{exc}") + return Event.ABORT + sx, sy = self._shear_axes() + shear_delta = max( + abs(sx - shear_before[0]), abs(sy - shear_before[1]), + ) + normal_delta = fn_after - fn_before + self.get_logger().info( + f"S08 第 {cycle + 1}/{self.cfg.screw_max_cycles} 轮：" + f"切向增量 {shear_delta:.1f}（阈值 {self.cfg.screw_shear_delta:.0f}）" + f" 法向增量 {normal_delta:+.1f}（阈值 {self.cfg.screw_normal_delta:.0f}）" + ) + if screw_stop( + normal_delta, shear_delta, + self.cfg.screw_normal_delta, self.cfg.screw_shear_delta, + ): + self.get_logger().info(f"S08 判据满足，{hole} 拧到位（第 {cycle + 1} 轮）") + self._progress(f"{hole} 拧入到位（{cycle + 1} 轮）") + return Event.SCREW_STOP + if self.grip is not None and self.grip.dropped(): + self.get_logger().error( + "S08 通止规掉了（E1）—— 回 S01 流程：张开爪、提示人工放好，" + "确认后继续（按看板：该孔记为完成，换下一个孔）" + ) + self._e1_pending = True + return Event.GAUGE_DROPPED + self.get_logger().info(f"S08 达到 {self.cfg.screw_max_cycles} 轮上限，进入拧出") + return Event.SCREW_DONE + + # ===================================================================== + # S09 反向拧出 + # ===================================================================== + def _step_unscrew(self) -> Event: + hole = self._hole_name() + self._state(f"S09_unscrew_{hole}") + if self.cfg.dry_run: + self._fsm_dry(Event.UNSCREW_DONE, f"反向拧出 {hole}") + return Event.UNSCREW_DONE + cycles = ratchet_cycles( + (self.cfg.screw_max_cycles + self.cfg.unscrew_extra_cycles) + * self.cfg.screw_step_deg, + self.cfg.screw_step_deg, + ) + self.get_logger().info( + f"S09 拧出 {hole}：{cycles} 轮 × {self.cfg.screw_step_deg:.0f}°" + f"（= 拧入上限 {self.cfg.screw_max_cycles} 轮 + 额外 " + f"{self.cfg.unscrew_extra_cycles} 轮，确保螺纹脱开）" + ) + for cycle in range(cycles): + q, Tee = self._wait_state(5.0) + if q is None: + return Event.TIMEOUT + # 反向：慢转是逆时针（与 S08 相反），空手回转是顺时针 + ev = self._rotate_joint_and_verify( + float(q[-1]) + self.cfg.screw_step_rad, tag=f"S09_slow_{cycle}", + fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s, + ) + if ev is not Event.UNSCREW_DONE: + return ev + if self.grip is None: + return Event.ABORT + if not self.grip.open(): + return Event.ABORT + time.sleep(self.cfg.screw_release_wait_s) + q, Tee = self._wait_state(3.0) + if q is None: + return Event.TIMEOUT + ev = self._rotate_joint_and_verify( + float(q[-1]) - self.cfg.screw_step_rad, tag=f"S09_back_{cycle}", + fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s, + ) + if ev is not Event.UNSCREW_DONE: + return ev + if not self.grip.close(): + return Event.ABORT + self._progress(f"{hole} 已拧出") + return Event.UNSCREW_DONE + + # ===================================================================== + # S10 换孔 / S11 复位 + # ===================================================================== + def _step_next_hole(self) -> Event: + hole = self._hole_name() # 进入 S10 时序号已推进 + self._state(f"S10_to_{hole}") + if self.cfg.dry_run: + self._fsm_dry(Event.POSE_OK, f"换到 {hole}") + return Event.POSE_OK + q, Tee = self._wait_state(5.0) + if q is None or Tee is None: + return Event.TIMEOUT + xy = self._hole_xy(hole) + if xy is None: + self.get_logger().error(f"S10 没有 {hole} 的坐标 —— S05 那次没定位到它") + return Event.TIMEOUT + z = float(self._snap_final.get("hover_z_m") or self.hover_z) + target = np.array([xy[0], xy[1], z]) + self.get_logger().info(f"S10 回 S05 记录的位姿：{hole} ({xy[0]:.4f},{xy[1]:.4f}) Z={z:.4f}") + ok, q, Tee = self._goto( + q, Tee, target, verticalize=True, tol_m=0.002, + tol_tilt=self.vert_tol_rad * 1.5, tag=f"S10_to_{hole}", + v=self.move_speed, max_hops=10, settle_s=0.2, + ) + if not ok: + self.get_logger().warn(f"S10 到 {hole} 未完全到位，S06 会再核残差") + return Event.POSE_OK + + def _step_reset(self) -> Event: + self._state("S11_reset") + if self.cfg.dry_run: + self._fsm_dry(Event.POSE_OK, "复位退出") + return Event.POSE_OK + try: + if self.grip is not None: + self.grip.open() + self._call_trigger("/home", timeout_s=60.0) + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f"S11 复位过程中出错（不再拦住退出）：{exc}") + return Event.POSE_OK + + # ===================================================================== + # 驱动循环 + # ===================================================================== + def _fsm_dry(self, event: Event, what: str) -> None: + self.get_logger().info(f"[干跑] {self.fsm.state}：{what} → {event.value}") + + def run(self) -> int: + """按状态机把 S00–S11 走完。返回进程退出码（0 = 正常）。""" + self._state("start") + self.get_logger().info( + f"通止规装配开始：{len(self._STEPS)} 个阶段，" + f"{self.cfg.holes_total} 个孔，终点 {State.DONE}；" + "状态话题 /gauge/state，结果 /gauge/result" + ) + if not self._cli.wait_for_server(timeout_sec=5.0) and not self.cfg.dry_run: + self.get_logger().error("JTC action 不在 —— 先起机械臂（run_real.sh / run_sim_gz.sh）") + return 2 + + while not self.fsm.is_terminal: + handler_name = self._STEPS.get(self.fsm.state) + if handler_name is None: + self.get_logger().error(f"状态 {self.fsm.state} 没有处理方法（看 _STEPS）") + return 2 + try: + event = getattr(self, handler_name)() + except GaugeAbort as exc: + self.get_logger().error(f"{self.fsm.state} 中止：{exc}") + self._safe_retract() + return exc.code + except FsmError: + raise + except Exception as exc: # noqa: BLE001 + self.get_logger().error( + f"{self.fsm.state} 抛异常：{type(exc).__name__}: {exc}" + ) + self._safe_retract() + return 2 + + try: + nxt = self.fsm.handle(event) + except FsmError as exc: + self.get_logger().error(str(exc)) + return 2 + self.get_logger().info( + f"[fsm] {self.fsm.history[-1]}（{self._hole_name()}）" + ) + if nxt is State.DONE: + break + + if self.fsm.state is State.ABORT and not self.cfg.dry_run: + # E2（中止）落点统一是"退到安全高度"：无论从哪个阶段中止，都不能 + # 把规留在孔里/工件上 —— 下次启动（或人来处理）都假设起点在工件上方。 + self.get_logger().warn("E2 中止：退到安全高度") + self._safe_retract() + + ok = self.fsm.ok + payload = { + "ok": ok, + "state": str(self.fsm.state), + "holes_total": self.cfg.holes_total, + "hole_index": self.fsm.ctx.hole_index, + "relocations": self.fsm.ctx.relocations, + "notes": self.fsm.ctx.notes, + "dry_run": self.cfg.dry_run, + "snap": self._snap_final, + } + self._publish_gauge_result(payload) + done = min(self.fsm.ctx.hole_index + (1 if ok else 0), self.cfg.holes_total) + self.get_logger().info( + f"通止规装配结束：state={self.fsm.state} ok={ok} 已装 {done}/{self.cfg.holes_total} 孔" + + ("（干跑，未动作）" if self.cfg.dry_run else "") + ) + return 0 if ok else 1 + + def _safe_retract(self) -> None: + """任何失败路径都先退到安全高度：不要让臂停在孔里/工件上。 + + 失败时"不动"看起来更安全，实际最危险：下一次调用（或人来处理）都会 + 从"规插在孔里"这个状态开始，而所有逻辑都假设起点在工件上方。 + """ + try: + q, Tee = self._wait_state(3.0) + if q is None or Tee is None: + return + self._retract(q, Tee, dz_m=0.02) + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f"退到安全高度失败（继续退出）：{exc}") + + +def main(args: Optional[Sequence[str]] = None) -> None: + rclpy.init(args=None if args is None else list(args)) + code = 2 + node: Optional[GaugeAssembly] = None + try: + node = GaugeAssembly() + code = node.run() + except GaugeAbort as exc: + print(f"[gauge_assembly] 无法开始：{exc}") + code = exc.code + except Exception: # noqa: BLE001 + # 必须**显式**打印：下面的 finally 里有 os._exit()（为了避开 rclpy 关不掉 + # 的僵尸线程），它会把正在传播的异常连栈一起吞掉 —— 现场表现就是 + # "ROS 说进程退出码 2，屏幕上什么都没有"，完全没法查。 + import traceback + + traceback.print_exc() + code = 2 + except KeyboardInterrupt: + if node is not None: + node.get_logger().warn("用户中断 —— 按安全高度退出") + code = 130 + finally: + if node is not None: + try: + node._stop_stage1() + except Exception: # noqa: BLE001 + pass + try: + node._safe_retract() + node._shutdown_tf() + except Exception: # noqa: BLE001 + pass + try: + node.destroy_node() + except Exception: # noqa: BLE001 + pass + try: + if rclpy.ok(): + rclpy.shutdown() + except Exception: # noqa: BLE001 + pass + os._exit(code) + + +if __name__ == "__main__": + main() diff --git a/src/arm_application/arm_application/gauge_boot.py b/src/arm_application/arm_application/gauge_boot.py index c23a66b..7bc18d8 100644 --- a/src/arm_application/arm_application/gauge_boot.py +++ b/src/arm_application/arm_application/gauge_boot.py @@ -67,8 +67,12 @@ class BootCheck: _ABSOLUTE_PROBES = frozenset({"service", "action", "topic", "tactile", "camera"}) -def _abs(name: str) -> str: - """把 ROS 名字规范成绝对名（``home`` → ``/home``）。""" +def abs_name(name: str) -> str: + """把 ROS 名字规范成绝对名（``home`` → ``/home``）。 + + 对外公开：调用方（节点）也要用同一套规范化去比较名字，否则会出现 + "清单里是 /foo、参数里是 foo，比较不相等"这类只在运行时暴露的错。 + """ n = (name or "").strip() if not n: return n @@ -116,7 +120,12 @@ def build_checks( probe="action", target=joint_action, why="没有它，S01/S06/S07/S10 的所有运动都发不出去", - expect_type="arm_msgs/action/FollowJointTrajectory", + # 类型必须是 **control_msgs**：本流程（和 stage2/stage3/pipeline 一样） + # 直接把轨迹发给 ros2_control 的 JTC 服务端，不经过 arm_control 的 + # 轨迹适配器（那个用的是 arm_msgs/action/FollowJointTrajectory）。 + # 写成 arm_msgs 的会得到一个"永远探不到服务端"的自检 —— 假阴性， + # 而且看起来像控制器没起来。 + expect_type="control_msgs/action/FollowJointTrajectory", ), BootCheck( key="arm_home", @@ -134,20 +143,16 @@ def build_checks( 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 的柔顺接触靠它；缺了只能用位置控制硬压", + # 选检：S07 现在走节点内的"步进 + 力突变停"（与这个 action 内部 + # 同一套做法），不依赖它；有它时驱动侧原语更顺，属于加分项。 + # 标成必检会让仿真（arm_driver_flexiv 不参与）永远过不了自检。 + why="有它时 S07 用驱动侧柔顺原语；没有则用节点内步进（不影响正确性）", + required=False, expect_type="arm_msgs/action/CompliantDescent", ), BootCheck( @@ -223,6 +228,33 @@ def build_checks( expect_type="arm_msgs/srv/SwitchTool", ) ) + # 配了工具系就必须能切：S01 切不过去会直接 TIMEOUT 中止， + # 所以在自检里按**必检**拦下（早报比跑到 S01 再报少一次上电动作）。 + checks.append( + BootCheck( + key="arm_switch_tool", + label="工具系切换服务", + probe="service", + target="/switch_tool", + why=f"site.yaml 指定了工具 {tool_name!r}，切不过去后面全是歪的", + expect_type="arm_msgs/srv/SwitchTool", + ) + ) + else: + # 没配工具系：S01 只会告警不会切，所以这条只算选检 —— + # 真机没配工具系是**配置问题**（看板要求 tool1），但仿真里根本没有 + # 工具系切换服务，标必检会让仿真永远过不去。 + checks.append( + BootCheck( + key="arm_switch_tool", + label="工具系切换服务", + probe="service", + target="/switch_tool", + why="本机没配 robot.tool_name：真机应配 tool1 并确认这个服务在；仿真不需要", + required=False, + expect_type="arm_msgs/srv/SwitchTool", + ) + ) if weights_path: checks.append( BootCheck( @@ -239,7 +271,7 @@ def build_checks( return [ c if c.probe not in _ABSOLUTE_PROBES else BootCheck( - key=c.key, label=c.label, probe=c.probe, target=_abs(c.target), + key=c.key, label=c.label, probe=c.probe, target=abs_name(c.target), why=c.why, required=c.required, expect_type=c.expect_type, ) for c in checks diff --git a/src/arm_application/arm_application/gauge_config.py b/src/arm_application/arm_application/gauge_config.py index 92a3dce..5241a9c 100644 --- a/src/arm_application/arm_application/gauge_config.py +++ b/src/arm_application/arm_application/gauge_config.py @@ -90,6 +90,7 @@ _TYPES: Dict[str, type] = { "unscrew_extra_cycles": int, "holes_total": int, "dry_run": bool, + "reload_timeout_s": float, } #: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。 @@ -131,6 +132,7 @@ _UNITS: Dict[str, str] = { "unscrew_extra_cycles": "", "holes_total": "", "dry_run": "", + "reload_timeout_s": "s", } #: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类 @@ -201,6 +203,9 @@ DEFAULTS: Dict[str, Any] = { # 全局 "holes_total": 2, "dry_run": False, + # E1（掉件）重放：等操作员放好并确认 /gauge/reload_ok 的最长时间。 + # 超时 = 中止（臂在 home，人不在旁边时"继续等"比"停"更危险）。 + "reload_timeout_s": 180.0, } @@ -245,6 +250,7 @@ class GaugeConfig: unscrew_extra_cycles: int holes_total: int dry_run: bool + reload_timeout_s: float # -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）------------------- @property diff --git a/src/arm_application/arm_application/handeye_lib.py b/src/arm_application/arm_application/handeye_lib.py index ecf23d4..5ce396c 100644 --- a/src/arm_application/arm_application/handeye_lib.py +++ b/src/arm_application/arm_application/handeye_lib.py @@ -7,8 +7,10 @@ from __future__ import annotations +import math from collections import deque from dataclasses import dataclass +from pathlib import Path from typing import Deque, List, Optional, Sequence, Tuple import numpy as np @@ -32,6 +34,69 @@ class CameraIntrinsics: dist: np.ndarray +def R_to_quat_xyzw(R: np.ndarray) -> Tuple[float, float, float, float]: + """3×3 旋转 → ``(x, y, z, w)`` 四元数（Shepperd 法，按最大分量选分支）。 + + 为什么放在库里而不是各个 launch 里：这段换算**已经在两份 launch 里各抄了 + 一遍**，而且抄错过（对角分支的公式被写歪，症状是手眼 TF 差了 30°， + 定位看起来"总能到附近"但永远差一点）。换算只有一份才可能被测试： + ``test_handeye_lib_roundtrip.py`` 拿真实标定文件验证 + ``quat_xyzw_to_R(R_to_quat_xyzw(R)) == R``。 + """ + R = np.asarray(R, dtype=float).reshape(3, 3) + tr = float(R[0, 0] + R[1, 1] + R[2, 2]) + if tr > 0.0: + s = math.sqrt(tr + 1.0) * 2.0 + w = 0.25 * s + x = (R[2, 1] - R[1, 2]) / s + y = (R[0, 2] - R[2, 0]) / s + z = (R[1, 0] - R[0, 1]) / s + elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: + s = math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0 + w = (R[2, 1] - R[1, 2]) / s + x = 0.25 * s + y = (R[0, 1] + R[1, 0]) / s + z = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0 + w = (R[0, 2] - R[2, 0]) / s + x = (R[0, 1] + R[1, 0]) / s + y = 0.25 * s + z = (R[1, 2] + R[2, 1]) / s + else: + s = math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0 + w = (R[1, 0] - R[0, 1]) / s + x = (R[0, 2] + R[2, 0]) / s + y = (R[1, 2] + R[2, 1]) / s + z = 0.25 * s + n = math.sqrt(x * x + y * y + z * z + w * w) + if n <= 0.0: + raise ValueError("旋转矩阵无法转成四元数（矩阵为零？）") + return x / n, y / n, z / n, w / n + + +def read_transform_yaml(path: str | Path) -> Tuple[np.ndarray, Tuple[float, float, float, float]]: + """读手眼标定产物 → ``(xyz_m, quat_xyzw)``。 + + 文件格式见 :func:`format_transform_yaml`：``T_4x4`` 是 4 行 4 列的列表。 + 读取时**必须校验形状**：手眼文件是现场人手拷来拷去的，形状不对时 + 直接报错，比"取到一堆 nan 然后发了个全零 TF"好得多（后者会让定位 + 永远差一个常量，而且看起来像相机标定不准）。 + """ + import yaml # 局部导入：本模块其它函数不需要 yaml（标定计算本身不依赖） + + with open(path, "r", encoding="utf-8") as f: + data = yaml.safe_load(f) or {} + T = data.get("T_4x4") + if T is None: + raise ValueError(f"{path}: 缺少 T_4x4 字段") + arr = np.asarray(T, dtype=float) + if arr.shape != (4, 4): + raise ValueError(f"{path}: T_4x4 应为 4x4，实际 {arr.shape}") + xyz = np.array([arr[0, 3], arr[1, 3], arr[2, 3]], dtype=float) + return xyz, R_to_quat_xyzw(arr[:3, :3]) + + def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray: x, y, z, w = [float(v) for v in q] n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12 diff --git a/src/arm_application/arm_application/pipeline_node.py b/src/arm_application/arm_application/pipeline_node.py index 587beef..9c26e48 100644 --- a/src/arm_application/arm_application/pipeline_node.py +++ b/src/arm_application/arm_application/pipeline_node.py @@ -25,8 +25,12 @@ from arm_application.stage3_search_node import Stage3Search class AssemblyPipeline(Stage3Search): - def __init__(self) -> None: - super().__init__(node_name="assembly_pipeline") + def __init__(self, node_name: str = "assembly_pipeline") -> None: + # node_name 可覆盖：通止规装配（gauge_assembly_node）继承本类以复用 + # 一阶段跟踪的拉起/拆分与二阶段定位链，但它是**另一条流程**， + # 在图上必须是另一个节点名（否则 ros2 node list 里两个同名的节点， + # 出问题时连"是哪条流程在发指令"都分不清）。 + super().__init__(node_name=node_name) self.declare_parameter("skip_stage1", False) self.declare_parameter("skip_stage2", False) self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth diff --git a/src/arm_application/launch/gauge_assembly.launch.py b/src/arm_application/launch/gauge_assembly.launch.py new file mode 100755 index 0000000..7980c43 --- /dev/null +++ b/src/arm_application/launch/gauge_assembly.launch.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 +"""启动通止规装配主流程（看板 S00–S11）。 + +与 stage2/stage3 的 launch 差别：**这里不检查任何服务**。 +"所有服务是否就绪"是 S00 的职责（``gauge_boot`` + 节点里的探测）， +在 launch/shell 里再查一遍就会出现两处判据 —— 一处改了另一处没改， +现场表现为"脚本说没问题、节点说没数据"。 + +唯一在这里做的是**发布手眼静态 TF**：腕部相机的 eye-in-hand 外参是主流程 +（S05 精拍定位）算坐标的前提，而它只能由标定产物提供（site.yaml → +paths.handeye_file）。文件不存在时不发 TF，节点会在 S05 报"查不到 TF"。 + +用法（一般由 scripts/run_gauge_assembly.sh 调用）:: + + ros2 launch arm_application gauge_assembly.launch.py + ros2 launch arm_application gauge_assembly.launch.py dry_run:=true holes_total:=1 +""" + +from __future__ import annotations + +import os +import sys +from pathlib import Path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def _ws_root() -> Path: + """工作区根目录：优先环境变量，其次向上找 config/site.yaml。""" + for key in ("ROS2_WS", "SITE_ROS2_ROOT"): + v = os.environ.get(key, "").strip() + if v: + return Path(v) + here = Path(__file__).resolve() + for i in range(2, 8): + p = here.parents[i] + if (p / "config" / "site.yaml").is_file(): + return p + return Path.home() / "ros2" + + +def _handeye_xyz_xyzw(path: str): + """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。 + + **不要在这里再写一遍四元数换算**：这段换算在别的 launch 里已各抄了一遍， + 抄错过的症状是"手眼 TF 差了 30°"（定位总能到附近，永远差一点）。 + 实现只有一份在 ``arm_application.handeye_lib``，且被回环测试钉住。 + """ + from arm_application.handeye_lib import read_transform_yaml + + xyz, quat = read_transform_yaml(path) + return ( + float(xyz[0]), float(xyz[1]), float(xyz[2]), + float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]), + ) + + +def _launch_setup(context, *args, **kwargs): + root = _ws_root() + sys.path.insert(0, str(root / "scripts" / "lib")) + from export_site_env import apply_active_arm, load_site, site_path # type: ignore + + cfg = os.environ.get("SITE_CONFIG", "").strip() + path = Path(cfg).expanduser() if cfg else site_path() + if not path.is_file(): + path = root / "config" / "site.yaml" + data = load_site(path) + merged, robot = apply_active_arm(data) + + ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange") + handeye = str((merged.get("paths") or {}).get("handeye_file") or "").strip() + if not handeye: + handeye = str((robot.get("paths") or {}).get("handeye_file") or "").strip() + cam_cfg = merged.get("camera") or {} + cam = str(cam_cfg.get("name") or "d405").strip().strip("/") + cam_frame = f"{cam}_color_optical_frame" + + def _flag(name: str) -> bool: + return LaunchConfiguration(name).perform(context).strip().lower() in ( + "1", "true", "yes", "on", + ) + + dry_run = _flag("dry_run") + holes = LaunchConfiguration("holes_total").perform(context).strip() + backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto" + + nodes = [] + frames = [] + if os.path.isfile(handeye): + try: + tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye) + except Exception as exc: # noqa: BLE001 + frames.append(f"[gauge] 手眼文件读不了（{handeye}）：{exc}") + else: + frames.append(f"[gauge] 手眼 TF {ee_frame} ← {cam_frame}（{Path(handeye).name}）") + nodes.append( + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="gauge_handeye_static_tf", + arguments=[ + "--x", str(tx), "--y", str(ty), "--z", str(tz), + "--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw), + "--frame-id", ee_frame, + "--child-frame-id", cam_frame, + ], + output="screen", + ) + ) + else: + frames.append( + f"[gauge] 没有手眼文件（{handeye or 'paths.handeye_file 为空'}）" + "—— 不发布静态 TF，S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh" + ) + + nodes.append( + Node( + package="arm_application", + executable="gauge_assembly", + name="gauge_assembly", + output="screen", + parameters=[ + { + "robot_sn": str( + robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or "" + ), + "dry_run": dry_run, + "holes_total": int(holes) if holes else -1, + "gripper_backend": backend, + } + ], + ) + ) + + return [ + LogInfo(msg=f"[gauge] site={path}"), + LogInfo(msg=f"[gauge] arm={robot.get('label') or robot.get('sn')} cam={cam}"), + LogInfo( + msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} backend={backend}" + ), + *[LogInfo(msg=m) for m in frames], + *nodes, + ] + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription( + [ + DeclareLaunchArgument("dry_run", default_value="false"), + DeclareLaunchArgument("holes_total", default_value=""), + DeclareLaunchArgument("gripper_backend", default_value="auto"), + OpaqueFunction(function=_launch_setup), + ] + ) diff --git a/src/arm_application/launch/pipeline.launch.py b/src/arm_application/launch/pipeline.launch.py index e309c77..5cdd355 100644 --- a/src/arm_application/launch/pipeline.launch.py +++ b/src/arm_application/launch/pipeline.launch.py @@ -7,12 +7,10 @@ from __future__ import annotations -import math import os import sys from pathlib import Path -import yaml from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction from launch.actions import EmitEvent, RegisterEventHandler @@ -34,43 +32,21 @@ def _ws_root() -> Path: return Path.home() / "ros2" -def _quat_from_R(R): - tr = float(R[0][0] + R[1][1] + R[2][2]) - if tr > 0: - s = math.sqrt(tr + 1.0) * 2.0 - w = 0.25 * s - x = (R[2][1] - R[1][2]) / s - y = (R[0][2] - R[2][0]) / s - z = (R[1][0] - R[0][1]) / s - elif R[0][0] > R[1][1] and R[0][0] > R[2][2]: - s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0 - w = (R[2][1] - R[1][2]) / s - x = 0.25 * s - y = (R[0][1] + R[1][0]) / s - z = (R[0][2] - R[2][1]) / s - elif R[1][1] > R[2][2]: - s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0 - w = (R[0][2] - R[2][1]) / s - x = (R[0][1] + R[1][0]) / s - y = 0.25 * s - z = (R[1][2] - R[2][1]) / s - else: - s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0 - w = (R[1][2] - R[0][1]) / s - x = (R[0][2] - R[0][1]) / s - y = (R[1][2] - R[2][0]) / s - z = 0.25 * s - return x, y, z, w - - def _handeye_xyz_xyzw(path: str): - with open(path, "r", encoding="utf-8") as f: - data = yaml.safe_load(f) - T = data["T_4x4"] - tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3]) - R = [[float(T[i][j]) for j in range(3)] for i in range(3)] - qx, qy, qz, qw = _quat_from_R(R) - return tx, ty, tz, qx, qy, qz, qw + """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。 + + 实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``）， + 并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。 + 这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 —— + 手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。 + """ + from arm_application.handeye_lib import read_transform_yaml + + xyz, quat = read_transform_yaml(path) + return ( + float(xyz[0]), float(xyz[1]), float(xyz[2]), + float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]), + ) def _as_bool(s: str) -> bool: diff --git a/src/arm_application/launch/stage2_locate.launch.py b/src/arm_application/launch/stage2_locate.launch.py index 60adcdb..fcdb997 100644 --- a/src/arm_application/launch/stage2_locate.launch.py +++ b/src/arm_application/launch/stage2_locate.launch.py @@ -7,12 +7,10 @@ from __future__ import annotations -import math import os import sys from pathlib import Path -import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction @@ -32,43 +30,21 @@ def _ws_root() -> Path: return Path.home() / "ros2" -def _quat_from_R(R): - tr = float(R[0][0] + R[1][1] + R[2][2]) - if tr > 0: - s = math.sqrt(tr + 1.0) * 2.0 - w = 0.25 * s - x = (R[2][1] - R[1][2]) / s - y = (R[0][2] - R[2][0]) / s - z = (R[1][0] - R[0][1]) / s - elif R[0][0] > R[1][1] and R[0][0] > R[2][2]: - s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0 - w = (R[2][1] - R[1][2]) / s - x = (R[0][1] + R[1][0]) / s - y = (R[1][0] + R[0][1]) / s - z = (R[0][2] - R[2][1]) / s - elif R[1][1] > R[2][2]: - s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0 - w = (R[0][2] - R[2][1]) / s - x = (R[0][1] + R[1][0]) / s - y = (R[1][2] + R[2][1]) / s - z = (R[1][2] - R[2][1]) / s - else: - s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0 - w = (R[1][2] - R[0][1]) / s - x = (R[0][2] - R[0][1]) / s - y = (R[1][2] - R[2][0]) / s - z = (R[1][2] - R[2][1]) / s - return x, y, z, w - - def _handeye_xyz_xyzw(path: str): - with open(path, "r", encoding="utf-8") as f: - data = yaml.safe_load(f) - T = data["T_4x4"] - tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3]) - R = [[float(T[i][j]) for j in range(3)] for i in range(3)] - qx, qy, qz, qw = _quat_from_R(R) - return tx, ty, tz, qx, qy, qz, qw + """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。 + + 实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``）， + 并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。 + 这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 —— + 手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。 + """ + from arm_application.handeye_lib import read_transform_yaml + + xyz, quat = read_transform_yaml(path) + return ( + float(xyz[0]), float(xyz[1]), float(xyz[2]), + float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]), + ) def _launch_setup(context, *args, **kwargs): diff --git a/src/arm_application/launch/stage3_search.launch.py b/src/arm_application/launch/stage3_search.launch.py index e743b59..58e0cdd 100644 --- a/src/arm_application/launch/stage3_search.launch.py +++ b/src/arm_application/launch/stage3_search.launch.py @@ -8,12 +8,10 @@ from __future__ import annotations -import math import os import sys from pathlib import Path -import yaml from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction from launch.substitutions import LaunchConfiguration @@ -32,43 +30,21 @@ def _ws_root() -> Path: return Path.home() / "ros2" -def _quat_from_R(R): - tr = float(R[0][0] + R[1][1] + R[2][2]) - if tr > 0: - s = math.sqrt(tr + 1.0) * 2.0 - w = 0.25 * s - x = (R[2][1] - R[1][2]) / s - y = (R[0][2] - R[2][0]) / s - z = (R[1][0] - R[0][1]) / s - elif R[0][0] > R[1][1] and R[0][0] > R[2][2]: - s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0 - w = (R[2][1] - R[1][2]) / s - x = (R[0][1] + R[1][0]) / s - y = (R[1][0] + R[0][1]) / s - z = (R[1][2] - R[2][1]) / s - elif R[1][1] > R[2][2]: - s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0 - w = (R[0][2] - R[2][1]) / s - x = (R[1][0] + R[0][1]) / s - y = (R[1][2] + R[2][1]) / s - z = (R[1][2] - R[2][1]) / s - else: - s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0 - w = (R[1][2] - R[0][1]) / s - x = (R[0][2] - R[0][1]) / s - y = (R[1][2] + R[2][1]) / s - z = (R[1][2] - R[2][1]) / s - return x, y, z, w - - def _handeye_xyz_xyzw(path: str): - with open(path, "r", encoding="utf-8") as f: - data = yaml.safe_load(f) - T = data["T_4x4"] - tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3]) - R = [[float(T[i][j]) for j in range(3)] for i in range(3)] - qx, qy, qz, qw = _quat_from_R(R) - return tx, ty, tz, qx, qy, qz, qw + """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。 + + 实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``）， + 并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。 + 这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 —— + 手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。 + """ + from arm_application.handeye_lib import read_transform_yaml + + xyz, quat = read_transform_yaml(path) + return ( + float(xyz[0]), float(xyz[1]), float(xyz[2]), + float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]), + ) def _launch_setup(context, *args, **kwargs): diff --git a/src/arm_application/launch/visual_servo.launch.py b/src/arm_application/launch/visual_servo.launch.py index a7928fe..939e9b5 100644 --- a/src/arm_application/launch/visual_servo.launch.py +++ b/src/arm_application/launch/visual_servo.launch.py @@ -6,12 +6,10 @@ from __future__ import annotations -import math import os import sys from pathlib import Path -import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction @@ -54,43 +52,21 @@ def _load_site(): return merged, path, perception_overrides(data), servo_overrides(data), robot -def _quat_from_R(R): - tr = float(R[0][0] + R[1][1] + R[2][2]) - if tr > 0: - s = math.sqrt(tr + 1.0) * 2.0 - w = 0.25 * s - x = (R[2][1] - R[1][2]) / s - y = (R[0][2] - R[2][0]) / s - z = (R[1][0] - R[0][1]) / s - elif R[0][0] > R[1][1] and R[0][0] > R[2][2]: - s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0 - w = (R[2][1] - R[1][2]) / s - x = 0.25 * s - y = (R[0][1] + R[1][0]) / s - z = (R[0][2] + R[2][0]) / s - elif R[1][1] > R[2][2]: - s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0 - w = (R[0][2] - R[2][0]) / s - x = (R[0][1] + R[1][0]) / s - y = 0.25 * s - z = (R[1][2] + R[2][1]) / s - else: - s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0 - w = (R[1][0] - R[0][1]) / s - x = (R[0][2] + R[2][0]) / s - y = (R[1][2] + R[2][1]) / s - z = 0.25 * s - return x, y, z, w - - def _handeye_xyz_xyzw(path: str): - with open(path, "r", encoding="utf-8") as f: - data = yaml.safe_load(f) - T = data["T_4x4"] - tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3]) - R = [[float(T[i][j]) for j in range(3)] for i in range(3)] - qx, qy, qz, qw = _quat_from_R(R) - return tx, ty, tz, qx, qy, qz, qw + """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。 + + 实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``）， + 并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。 + 这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 —— + 手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。 + """ + from arm_application.handeye_lib import read_transform_yaml + + xyz, quat = read_transform_yaml(path) + return ( + float(xyz[0]), float(xyz[1]), float(xyz[2]), + float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]), + ) def _launch_setup(context, *args, **kwargs): diff --git a/src/arm_application/setup.py b/src/arm_application/setup.py index e15c5ef..78f2846 100644 --- a/src/arm_application/setup.py +++ b/src/arm_application/setup.py @@ -78,6 +78,8 @@ setup( 'stage3_search = arm_application.stage3_search_node:main', # 三段式装配流水线（跟踪 → 定位 → 找孔，单进程） 'assembly_pipeline = arm_application.pipeline_node:main', + # 通止规装配（看板 S00–S11，含自检/换孔/掉件重放） + 'gauge_assembly = arm_application.gauge_assembly_node:main', 'tactile = arm_application.tactile_node:main', 'tactile_cli = arm_application.tactile_cli:main', ], diff --git a/src/arm_application/test/test_gauge_boot.py b/src/arm_application/test/test_gauge_boot.py index 1fd1e14..9767c56 100644 --- a/src/arm_application/test/test_gauge_boot.py +++ b/src/arm_application/test/test_gauge_boot.py @@ -46,12 +46,13 @@ def _by_key(checks): # 清单内容 # ============================================================================= def test_core_checks_are_present_and_required(): - checks = _by_key(_build()) + """核心判据依赖的东西必须是**必检**（配了工具系的那种机器）。""" + checks = _by_key(_build(tool_name="tool1")) for key in ( "arm_jtc", "arm_home", + "arm_named_pose", "arm_switch_tool", - "arm_compliant", "gripper_open", "gripper_close", "camera_color", @@ -63,6 +64,36 @@ def test_core_checks_are_present_and_required(): assert checks[key].required, f"{key} 是核心判据依赖，必须是必检" +def test_jtc_check_expects_the_type_this_flow_actually_calls(): + """JTC 自检的类型必须是 control_msgs（本流程直接发给 ros2_control）。 + + 写成 arm_msgs/action/FollowJointTrajectory（arm_control 适配器那一套）会 + 让自检**永远探不到服务端**：报"控制器没起来"，而控制器明明好着。 + """ + c = _by_key(_build())["arm_jtc"] + assert c.expect_type == "control_msgs/action/FollowJointTrajectory" + + +def test_switch_tool_required_only_when_a_tool_is_configured(): + """配了工具系 → 切不过去 S01 会中止，所以按必检拦；没配 → 选检。 + + 没配工具系的机器上（比如仿真）标必检，会让"仿真里根本不存在 + /switch_tool 服务"变成每次启动的一条红字，人很快就不看自检了。 + """ + assert _by_key(_build(tool_name="tool1"))["arm_switch_tool"].required is True + assert _by_key(_build(tool_name=""))["arm_switch_tool"].required is False + + +def test_compliant_descent_is_optional(): + """S07 不依赖柔顺 action（节点内步进 + 力突变停是同一套做法）。 + + 标必检的后果：仿真里永远过不了自检，而 S07 其实跑得挺好。 + """ + c = _by_key(_build())["arm_compliant"] + assert c.required is False + assert "S07" in c.why + + def test_tactile_must_not_be_downgraded_to_optional(): """**负向**：触觉是 S07/S08/E1 三个判据的唯一数据源。 @@ -175,9 +206,17 @@ def test_report_lists_why_so_the_operator_knows_urgency(): def test_counts_summary(): + """数量刻意写死：加/减一条自检项时必须来这里改一行并想清楚它属于哪一类。 + + 当前（未配工具系、有手外相机、无权重）： + 必检 9 = arm_jtc, arm_home, arm_named_pose, gripper_open, gripper_close, + camera_color, camera_info, tactile_status, gripper_status + 选检 3 = coarse_camera, arm_switch_tool, arm_compliant + """ 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 + assert c["required_total"] == 9 and c["required_ok"] == 9 + # 选检里只有手外相机没起来 → 3 项里 2 项通过（工具系/柔顺下压按通过算） + assert c["optional_total"] == 3 and c["optional_ok"] == 2 def test_all_ready_says_so(): diff --git a/src/arm_application/test/test_gauge_step_events.py b/src/arm_application/test/test_gauge_step_events.py new file mode 100644 index 0000000..7570c4c --- /dev/null +++ b/src/arm_application/test/test_gauge_step_events.py @@ -0,0 +1,159 @@ +"""主流程节点与状态机的**接缝**测试（不需要 ROS、不需要真机）。 + +为什么要有这一层 +---------------- +``gauge_assembly_node`` 的每个 ``_step_*`` 方法会返回一个 :class:`Event`， +由 ``gauge_fsm.TRANSITIONS`` 决定它去哪。这两份东西是**分开写的**： + +* 状态机里没有的转移 → 运行到那一步抛 :class:`FsmError`（好：当场炸）； +* 但如果是**最危险的那一步**才炸（力超限、掉件、关节极限），代价就大了。 + +所以这里直接读节点源码的 AST，把"每个阶段可能返回哪些事件"抽出来， +逐个问状态机"这一步合法吗"。这样新增/修改流程时，忘了登记转移会在 +``pytest`` 里就报出来，而不是等到真机上。 +""" + +from __future__ import annotations + +import ast +import pathlib +from typing import Dict, Set + +import pytest + +from arm_application.gauge_fsm import Event, GaugeContext, GaugeFsm, State + +NODE_SRC = ( + pathlib.Path(__file__).resolve().parents[1] + / "arm_application" / "gauge_assembly_node.py" +) + + +def _node_ast() -> ast.Module: + return ast.parse(NODE_SRC.read_text(encoding="utf-8")) + + +def _step_map(tree: ast.Module) -> Dict[str, str]: + """``State.X -> "_step_y"``（读节点的 ``_STEPS`` 表）。""" + for node in ast.walk(tree): + # 表是带类型标注的（``_STEPS: Dict[State, str] = {...}``）→ AnnAssign + target = None + if isinstance(node, ast.AnnAssign) and isinstance(node.target, ast.Name): + target = node.target.id + elif isinstance(node, ast.Assign) and len(node.targets) == 1: + tgt = node.targets[0] + target = tgt.id if isinstance(tgt, ast.Name) else None + if target != "_STEPS": + continue + value = node.value + assert isinstance(value, ast.Dict), "_STEPS 必须是字面量字典（测试要读它）" + out: Dict[str, str] = {} + for k, v in zip(value.keys, value.values): + assert isinstance(k, ast.Attribute) and isinstance(v, ast.Constant) + out[k.attr] = str(v.value) + return out + raise AssertionError("节点里找不到 _STEPS 表") + + +def _events_in(node: ast.AST) -> Set[str]: + """表达式里出现的所有 ``Event.XXX`` 名字。""" + return { + n.attr + for n in ast.walk(node) + if isinstance(n, ast.Attribute) + and isinstance(n.value, ast.Name) + and n.value.id == "Event" + } + + +def _returned_events(func: ast.FunctionDef) -> Set[str]: + """一个 ``_step_*`` 方法可能返回的事件。 + + 收集两处，别的地方不算（比如异常处理里顺手记一条 ``Event.ABORT`` 做日志， + 那不是"返回"）： + 1. 所有 ``return `` 里的 ``Event.X``； + 2. 所有 ``fsm_event=Event.X`` 关键字实参（喂给 ``_rotate_joint_and_verify``， + 它会把成功事件原样返回）。 + """ + out: Set[str] = set() + for node in ast.walk(func): + if isinstance(node, ast.Return) and node.value is not None: + out |= _events_in(node.value) + elif isinstance(node, ast.Call): + for kw in node.keywords: + if kw.arg == "fsm_event": + out |= _events_in(kw.value) + return out + + +def _func_by_name(tree: ast.Module, name: str) -> ast.FunctionDef: + for node in ast.walk(tree): + if isinstance(node, ast.FunctionDef) and node.name == name: + return node + raise AssertionError(f"节点里找不到方法 {name}") + + +def test_steps_table_covers_every_non_terminal_state() -> None: + """每个非终态都要有处理方法 —— 漏一个就是"流程走到这儿不知道干什么"。""" + tree = _node_ast() + steps = _step_map(tree) + for st in State: + if st in (State.DONE, State.ABORT): + continue + assert st.name in steps, f"{st} 没有登记处理方法" + assert len(steps) == len(set(steps.values())), "一个方法不该被两个状态复用" + + +def test_every_returned_event_is_legal_in_that_state() -> None: + """本阶段可能返回的每个事件，在状态机里必须合法。 + + 这是本文件的主要断言：它把"节点写的事件"和"状态机登记的转移"对齐， + 免得在真机上跑到那一步才抛 FsmError。 + """ + tree = _node_ast() + problems = [] + for state_name, method in _step_map(tree).items(): + state = State[state_name] + func = _func_by_name(tree, method) + for ev_name in sorted(_returned_events(func)): + event = Event[ev_name] + # 每个事件都在"干净上下文 + 该状态"下问一次状态机 + fsm = GaugeFsm(GaugeContext(holes_total=2), start=state) + if not fsm.can(event): + problems.append(f"{state.value} 的 {method} 会返回 {ev_name}，但状态机不接受") + assert not problems, "节点与状态机不一致：\n " + "\n ".join(problems) + + +def test_s08_drop_goes_to_home_extreme() -> None: + """E1 的落点必须还是 S01：它要在那里张开爪、等人工放好。 + + 这条单列出来，是因为"掉件后去哪"很容易被顺手改成 S11（"反正收工"）， + 那样掉在孔里的规就没人管了。 + """ + fsm = GaugeFsm(GaugeContext(holes_total=2), start=State.SCREW_IN) + assert fsm.handle(Event.GAUGE_DROPPED) is State.HOME_EXTREME + assert fsm.handle(Event.RELOADED) is State.NEXT_HOLE + # 掉件后孔序号推进（看板：该孔记为完成，换下一个孔） + assert fsm.ctx.hole_index == 1 + + +def test_contact_force_over_limit_aborts_not_retries() -> None: + """法向力超上限 → 中止，不是"抬起来再来一次"。 + + 超限说明已经压进去了（位置下限或规没对准），原地重试会把工件压变形。 + """ + fsm = GaugeFsm(GaugeContext(holes_total=2), start=State.CONTACT) + assert fsm.handle(Event.FORCE_OVER_LIMIT) is State.ABORT + + +@pytest.mark.parametrize("state_name", ["SNAP", "HOVER_K"]) +def test_snap_and_hover_have_a_back_edge(state_name: str) -> None: + """S05/S06 必须能退回去：不稳定 → 再等；对位超差 → 重拍。 + + 没有回边的结果是：判据没过就一路往下走（去接触、去拧）， + 而这是"孔没对准"最危险的表达方式。 + """ + fsm = GaugeFsm(GaugeContext(holes_total=2), start=State[state_name]) + back = Event.SNAP_UNSTABLE if state_name == "SNAP" else Event.ALIGN_OUT + assert fsm.can(back) + assert fsm.handle(back) is State.SNAP diff --git a/src/arm_application/test/test_handeye_lib_roundtrip.py b/src/arm_application/test/test_handeye_lib_roundtrip.py new file mode 100644 index 0000000..2b7508d --- /dev/null +++ b/src/arm_application/test/test_handeye_lib_roundtrip.py @@ -0,0 +1,146 @@ +"""手眼文件读取与四元数换算的回环测试（用真实标定产物）。 + +为什么值得单测这个 +------------------ +``R_to_quat_xyzw`` 这段换算在仓库里被抄过好几遍，并且**抄错过**：某个分支的 +公式写歪之后，四元数还是"像模像样的数字"，静态 TF 也照发。症状是手眼外参 +差了 30°：定位总能落到工件附近（因为平移是对的），但永远差一点， +看起来像"相机标定不准"，很难查到是 launch 里的一行除法。 + +所以判据必须是**数学回环**（``quat → R`` 要能还原 ``R``），而不是"看数字合不合理"。 +真实标定文件（``handeye/results/*.yaml``）也一起测：文件是人手拷来拷去的， +形状/字段被改坏时，这里直接报错比发一个全零 TF 好。 +""" + +from __future__ import annotations + +import math +import pathlib + +import numpy as np +import pytest + +from arm_application.handeye_lib import ( + R_to_quat_xyzw, + quat_xyzw_to_R, + read_transform_yaml, +) + + +def _repo_root() -> pathlib.Path: + here = pathlib.Path(__file__).resolve() + for p in here.parents: + if (p / "config" / "site.yaml").is_file(): + return p + raise AssertionError("找不到仓库根（config/site.yaml）") + + +def _rot(axis: str, deg: float) -> np.ndarray: + a = math.radians(deg) + c, s = math.cos(a), math.sin(a) + if axis == "x": + return np.array([[1, 0, 0], [0, c, -s], [0, s, c]], dtype=float) + if axis == "y": + return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], dtype=float) + return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], dtype=float) + + +@pytest.mark.parametrize("axis", ["x", "y", "z"]) +@pytest.mark.parametrize("deg", [0.0, 1.0, 45.0, 90.0, 179.0, 180.0, 181.0, 270.0]) +def test_quaternion_roundtrip_by_axis(axis: str, deg: float) -> None: + """绕单轴转任意角度：``quat → R`` 必须还原原矩阵。 + + 覆盖 0/180/270 这些"某个分量恰好为零"的角度 —— 四元数换算的分支 + 正是在这些角度上切换的，抄错分支的公式只会在这些角度露出来。 + """ + R = _rot(axis, deg) + q = R_to_quat_xyzw(R) + assert abs(math.sqrt(sum(v * v for v in q)) - 1.0) < 1e-9, "四元数未归一化" + R2 = quat_xyzw_to_R(q) + assert np.allclose(R, R2, atol=1e-9), f"轴 {axis} 转角 {deg}：回环不一致" + + +def test_quaternion_roundtrip_on_composite_rotations() -> None: + """复合旋转（三个分支都会被走到）+ 随机旋转。""" + mats = [ + _rot("x", 30.0) @ _rot("y", -70.0) @ _rot("z", 120.0), + _rot("z", 179.9) @ _rot("x", 90.0), + _rot("y", 180.0) @ _rot("x", 180.0), + ] + rng = np.random.default_rng(20260924) + for _ in range(50): + A = rng.normal(size=(3, 3)) + Q, _R = np.linalg.qr(A) + if np.linalg.det(Q) < 0: + Q[:, 0] *= -1.0 + mats.append(Q) + for R in mats: + assert np.allclose(R, quat_xyzw_to_R(R_to_quat_xyzw(R)), atol=1e-8) + + +def test_read_real_handeye_files_match_their_own_matrix() -> None: + """真实标定文件：读出来的 xyz/quat 必须和文件里的 T_4x4 自洽。 + + 这条断言的价值在于"文件 + 读取代码 + 换算"三者一旦有一个变味就会报。 + """ + results = _repo_root() / "handeye" / "results" + files = sorted(results.glob("*_eye_in_hand.yaml")) + if not files: + pytest.skip("仓库里没有手眼标定产物（还没标定过）") + import yaml + + for f in files: + xyz, quat = read_transform_yaml(f) + # 直接拿文件里的 T_4x4 当基准（不手写解析：解析器本身就是被测对象之一， + # 用手写解析比对等于让两份解析互相印错） + T_file = np.asarray(yaml.safe_load(f.read_text(encoding="utf-8"))["T_4x4"], dtype=float) + assert T_file.shape == (4, 4), f"{f.name} 的 T_4x4 形状不对" + R = quat_xyzw_to_R(quat) + assert np.allclose(xyz, T_file[:3, 3], atol=1e-6), f"{f.name} 平移不一致" + assert np.allclose(R, T_file[:3, :3], atol=1e-6), f"{f.name} 旋转不一致" + + +def test_read_transform_yaml_rejects_broken_files(tmp_path: pathlib.Path) -> None: + """缺字段 / 形状不对 → 明确报错，而不是回一堆 nan 去发 TF。""" + (tmp_path / "no_field.yaml").write_text("translation_m: [0, 0, 0]\n", encoding="utf-8") + with pytest.raises(ValueError, match="T_4x4"): + read_transform_yaml(tmp_path / "no_field.yaml") + + (tmp_path / "bad_shape.yaml").write_text( + "T_4x4:\n - [1, 0, 0, 0]\n - [0, 1, 0, 0]\n", encoding="utf-8" + ) + with pytest.raises(ValueError, match="4x4"): + read_transform_yaml(tmp_path / "bad_shape.yaml") + + +def test_identical_rotation_gives_same_quaternion_up_to_sign() -> None: + """同一个姿态的四元数允许差一个整体符号（q 与 -q 同一个旋转）。 + + 写这条是因为很容易顺手加一句"四元数必须逐位相等"的断言，然后在某次 + 标定重跑后莫名其妙地红 —— 那不是回归，是符号约定。 + """ + R = _rot("z", 100.0) + q1 = np.array(R_to_quat_xyzw(R)) + q2 = np.array(R_to_quat_xyzw(R)) + assert np.allclose(q1, q2) + assert np.allclose(quat_xyzw_to_R(-q1), quat_xyzw_to_R(q1)) + + +def test_launch_files_do_not_reimplement_the_quaternion_math() -> None: + """launch 文件里不许再出现自己写的四元数换算。 + + 这段换算曾在 4 个 launch 里各抄一份，而且**四份已经抄得不完全一样** + （退化分支的公式各不相同）。共用的那一份在 ``handeye_lib`` 里，有回环测试； + 自己再写一份就等于加了一处没人测的数学。要用就 ``read_transform_yaml``。 + """ + launch_dir = _repo_root() / "src" / "arm_application" / "launch" + bad = [ + f.name + for f in sorted(launch_dir.glob("*.launch.py")) + if "_quat_from_R" in f.read_text(encoding="utf-8") + ] + assert not bad, ( + "这些 launch 自己实现了四元数换算：" + + ", ".join(bad) + + "；请改用 arm_application.handeye_lib.read_transform_yaml" + ) diff --git a/src/arm_application/test/test_model_labels.py b/src/arm_application/test/test_model_labels.py index 73bd8ae..e37e678 100644 --- a/src/arm_application/test/test_model_labels.py +++ b/src/arm_application/test/test_model_labels.py @@ -39,6 +39,11 @@ from arm_application.model_labels import ( def _make_onnx(path, names_json=None, nc=None, metadata=None): """造一个最小可读的 ONNX，用于测试 metadata 读取。""" + # onnx 只在真机环境（ros_flexiv）里装了，仿真环境（ros_gz_sim）没有。 + # 缺它时**跳过**而不是失败：仿真环境里每次 pytest 都多十几条红字， + # 很快就没有人看测试结果了（"狼来了"），而真正需要 onnx 的用例 + # 本来也没法在没有 onnx 的环境里验证。 + pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）") import onnx from onnx import TensorProto, helper diff --git a/src/arm_application/test/test_weights_layout.py b/src/arm_application/test/test_weights_layout.py index e575ba2..fe341b5 100644 --- a/src/arm_application/test/test_weights_layout.py +++ b/src/arm_application/test/test_weights_layout.py @@ -45,6 +45,11 @@ REPO = Path(__file__).resolve().parents[3] def _make_onnx(path: Path, names_json: str | None = None) -> Path: + # onnx 只在真机环境（ros_flexiv）里装了，仿真环境（ros_gz_sim）没有。 + # 缺它时**跳过**而不是失败：仿真环境里每次 pytest 都多十几条红字， + # 很快就没有人看测试结果了（"狼来了"），而真正需要 onnx 的用例 + # 本来也没法在没有 onnx 的环境里验证。 + pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）") import onnx from onnx import TensorProto, helper