diff --git a/devices/sim/gazebo/README.md b/devices/sim/gazebo/README.md index d4b3b64..c96058f 100644 --- a/devices/sim/gazebo/README.md +++ b/devices/sim/gazebo/README.md @@ -246,6 +246,7 @@ UserCommands 系统）会让**所有**模型都 spawn 不进去（见 [坑 2](# | 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致告警 | | 相机图幅（影响像素尺度） | `config/cameras.yaml` 的 `width`/`height` **且** SDF 的 `image_width/height` | ✔ 启动时自动比对（这是最容易漏的一处，见 4.3） | | 触觉标度/轴向/基线 | `config/tactile.yaml` | ✔ 适配器每次启动都读 | +| 感知来源（一阶段目标点 + 二阶段孔位） | `site.yaml` → `perception.source` | ✔ 仿真那台写 `truth`：一阶段由 `sim_truth_perception` 按场景位姿算目标点，二阶段由 `arm_sim_gazebo.truth_holes` 按场景几何投影孔位（见 §5 坑 13） | | 装配的悬高/速度 | **复用真机那套**（`servo.*` / `hole_detect.stage2.*`） | ✔ 有意共用 | | 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 的 `` | | 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。改要动三处：`urdf/arm.sim.urdf.xacro` 的 `arm_prefix`、`config/controllers.yaml` 的 `sim_joint1..7`、本项 | @@ -471,6 +472,64 @@ mimic 只能是 revolute"，挂在 `unused_finger_width_link` 上（那个 link 适配器的做法：反馈取"与主关节直接相连、|multiplier| 最大的那个从动关节" （GN01 是 `left_outer_knuckle_joint`），再按耦合关系反推开口。 +### 坑 13：二阶段的孔，在仿真里"每拍都丢" + +现象（`log_sim/gauge_flow.log`）： + +``` +一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段 +[pipeline] === S05_snap === +精拍 5帧中值图 L=(265.2,320.4) R=(529.7,288.8) +WARN 两孔基座间距 107.9mm 不像 k1/k2（期望 32–85mm），丢弃本拍 +...（同样一行重复 8 次） +ERROR S05 精拍定位失败 code=4 +``` + +一阶段（S04）完全正常：真值目标锁定、跟踪到位、悬停到 0.632m。所以看起来像 +"孔检测的门槛配错了"。真因有两段，**第二段才是关键**： + +1. 工件检测模型在合成图上出不了框（实测最高分 0.017，见 `models/` 的说明）→ + `hole_detect_hough` 拿不到 ROI，于是按调用方的允许退到**整幅画面**当 ROI。 + 这一步本身是设计好的退路（`detect_holes(allow_full_frame_roi=True)`）， + 真机上"权重不匹配但孔看得见"时就靠它。 +2. 整幅画面里还站着**夹爪和通止规**（它们就在工件上方、比工件更近）。 + HoughCircles 在它们身上找到一对"看起来对称的圆"，间距 108mm —— 真值 + 58mm。于是每一拍都被间距门槛丢掉，丢满 8 拍超时。 + +**为什么不能靠"把门槛放宽"解决**：107.9mm 离 58mm 不算远，放宽一点就会让这对 +**假孔**通过，然后机械臂去对准一个错的点 —— 症状推迟到 S07 才爆（拧不进 / +掉件重放），比现在难查得多。宁可现在"每拍都丢"。 + +修法：仿真里不要用检测器，用**真值孔位** —— 与一阶段的 `sim_truth_perception` +同一套思路（都是"场景里本来就知道，何必去推"）： + +``` +孔世界点 = scene.yaml 的物体位姿 + 模型 的局部位置 +孔像素 = K · T_cam_world · 孔世界点 （T 由 TF 查，K 来自 camera_info） +``` + +* 实现在 `arm_sim_gazebo/truth_holes.py`：**纯几何**，没有 ROS 依赖， + 所以在没有 Gazebo 的机器上也能测（`test_truth_holes.py`，13 条）。 +* 由 `stage2_locate_node` 在 `perception.source == truth` 时接管 + `_detect_holes_img` / `_best_bbox`（`gauge_assembly_node` 继承这两个方法， + 所以 S05 的整条链一起生效）。 +* 真机（`source: yolo`）那条路**一行都没改**。 +* 为什么用 `perception.source` 而不是新加一个"仿真孔位"开关：这个问题本来就是 + "谁产生视觉测量"，一阶段的目标点与二阶段的孔位只是它的两个消费者 —— + 拆成两个开关，迟早出现"一阶段走真值、二阶段走模型"的半仿状态。 + +顺带被这条坑钉住的两个行为： + +* 孔在**相机背后**时真值路径返回"不可用"而不是硬给一个点（给伺服幻影目标 + 会让机械臂朝错误方向走，看起来像"标定坏了"）； +* 模型里嵌件亮环不是**正好 2 个**时直接报"有几个环"，不猜。 + +验收： + +``` +python3 -m pytest src/arm_sim_gazebo/test/test_truth_holes.py -q +``` + --- ## 6. 现状与进度 diff --git a/scripts/lib/export_site_env.py b/scripts/lib/export_site_env.py index 42aa072..f69606d 100644 --- a/scripts/lib/export_site_env.py +++ b/scripts/lib/export_site_env.py @@ -80,7 +80,9 @@ _SIDE_WORDS = { "右臂": "right", } -_ARM_SECTION_KEYS = ("camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo") +_ARM_SECTION_KEYS = ( + "camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo", "perception", +) def arm_short_id(sn: str) -> str: diff --git a/scripts/run_gauge_assembly.sh b/scripts/run_gauge_assembly.sh index 3cd1026..c7be3b9 100755 --- a/scripts/run_gauge_assembly.sh +++ b/scripts/run_gauge_assembly.sh @@ -36,6 +36,15 @@ DRY_RUN=false HOLES="" BACKEND="auto" +# 用户**这一条命令**里有没有显式指定臂？要看原始 argv： +# `parse_arm_args` 只设 ROBOT_SN，而调用者的 shell 可能已经 export 了 ROBOT_SN +#（比如先 source 过 env.sh）—— 那种情况下"仿真要不要默认成 sim 台"就分不清了。 +_HAD_ARM=false +for _a in "$@"; do + if [[ "${_a}" == "--arm" || "${_a}" == "--sn" ]]; then _HAD_ARM=true; fi +done +unset _a + # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/parse_arm_args.sh" parse_arm_args "$@" @@ -57,6 +66,15 @@ case "${BACKEND}" in *) echo "--backend 只能是 auto|contract|legacy，收到 ${BACKEND}" >&2; exit 1 ;; esac +if [[ "${USE_SIM}" == true && "${_HAD_ARM}" == false ]]; then + # 仿真默认选「仿真台」——就是 site.yaml 里 robots[] 的那条 `sn: sim` + #（TF/关节名是 sim_* 前缀，相机是 sim_d405）。**必须在 source sim_env.sh 之前**： + # 那里会把 site.yaml 导成 SITE_*，之后再改就晚了。 + # 显式 --arm 仍然优先：想用仿真核对某台真机的点位时会这么用。 + export ROBOT_SN=sim + export ROBOT_ARM=sim +fi + if [[ "${USE_SIM}" == true ]]; then echo "=== 仿真环境（install_sim + ROS_DOMAIN_ID=77） ===" # shellcheck disable=SC1091 @@ -77,7 +95,7 @@ if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then fi echo "=== 通止规装配 arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN}) ===" -echo " 孔数=${HOLES:-site} 夹爪后端=${BACKEND} 干跑=${DRY_RUN}" +echo " 孔数=${HOLES:-site} 夹爪后端=${BACKEND} 干跑=${DRY_RUN} 仿真时钟=${USE_SIM}" # 清掉上一轮可能残留的装配节点（同一时间只允许一个进程在指挥这条臂） # 不 pkill 整个 arm_application：会连相机/触觉服务一起带走（那些是本脚本的前置） @@ -91,6 +109,34 @@ rm -f "${RESULT_FILE}" ARGS=(gripper_backend:="${BACKEND}" result_file:="${RESULT_FILE}") if [[ -n "${HOLES}" ]]; then ARGS+=(holes_total:="${HOLES}"); fi if [[ "${DRY_RUN}" == true ]]; then ARGS+=(dry_run:=true); fi +# 仿真必须 use_sim_time:=true：本流程的"帧新不新"和 TF 查询都按节点时钟比对。 +# 忘了带的现场是"相机 13Hz 明明在发，S00 却报腕部相机无数据（过期 1.79e9s）" —— +# 报出来的原因是假的，人只会去查相机。 +if [[ "${USE_SIM}" == true ]]; then ARGS+=(use_sim_time:=true); fi +# 仿真里手眼是"URDF 的真值导出成文件"（不标定）。没有这份文件，S04 的 +# visual_servo 会直接拒绝启动（"手眼标定结果缺失"），整条流程卡在 S04↔S03 循环。 +# 所以这里先把它刷成当前真值（已是最新则什么都不做）。 +# 手眼静态 TF 由 URDF 提供 → 本流程**不**发（否则同一对父子两个发布者）。 +if [[ "${USE_SIM}" == true ]]; then + ARGS+=(publish_handeye_tf:=false) + echo "=== 刷新仿真手眼真值（sim_eye_in_hand.yaml / sim_d435_eye_to_hand.yaml） ===" + if ! python3 "${ROOT}/scripts/gen_sim_handeye.py" --arm "${ROBOT_SN:-sim}"; then + echo "ERROR: 仿真手眼真值导出失败（上面有原因）。仿真栈起来了吗？" >&2 + echo " bash scripts/run_sim_gz.sh 之后重试。" >&2 + exit 1 + fi + # 仿真没有"人把规塞进夹爪"这一步：不装规，整条流程会在 S07 才暴露 + #（下了 25mm 什么也没碰到，看着像力阈值问题）。所以这里上前先上料， + # 装不上就直接停 —— 不允许带着空夹爪往下跑。 + # 真机不需要：现场由人装夹，S02 的夹持确认是预留跳过项（grip_check）。 + echo "=== 仿真上料：把通止规装进夹爪（真机由人工装夹） ===" + if ! python3 "${ROOT}/scripts/sim_load_gauge.py"; then + echo "ERROR: 仿真上料失败（上面有原因）。可以单独重跑：" >&2 + echo " python3 scripts/sim_load_gauge.py # 装规" >&2 + echo " python3 scripts/sim_load_gauge.py --dry-run # 只看算出来的位姿" >&2 + exit 1 + fi +fi echo "=== launch gauge_assembly（自检不过会自动报缺什么） ===" # 不用 exec：exec 之后 Ctrl-C 直接打给 ros2 launch，这里的清理就没了； diff --git a/scripts/run_unit_tests.sh b/scripts/run_unit_tests.sh new file mode 100755 index 0000000..91c82b9 --- /dev/null +++ b/scripts/run_unit_tests.sh @@ -0,0 +1,44 @@ +#!/usr/bin/env bash +# 跑全仓库的单元测试（纯逻辑：不需要硬件、不需要 Gazebo、不发运动指令）。 +# +# 为什么要有这个脚本，而不是让人手敲 pytest +# ---------------------------------------- +# 测试都按**包名**导入（arm_application.* / arm_sim_gazebo.* / arm_kinematics.*）， +# 所以 PYTHONPATH 必须同时包含 src/ 下所有包目录。手敲很容易漏一个： +# 漏了 src/arm_kinematics，`from arm_application.pipeline_node import ...` 会在 +# 收集阶段就 ModuleNotFoundError —— 现象是"测试跑了 0 个 / 1 个 error"， +# 很容易被读成"测试环境坏了"，而不是"少了一个路径"。 +# +# 用法： +# bash scripts/run_unit_tests.sh # 全部 +# bash scripts/run_unit_tests.sh -k gauge # 只跑名字带 gauge 的 +# bash scripts/run_unit_tests.sh src/arm_sim_gazebo/test/test_sim_scene.py -x +# +# 环境：真机 conda 环境或仿真环境都能跑；缺可选依赖的用例自己会 skip +#（例如 onnx/ultralytics 只在真机环境有 —— 见各测试里的 pytest.importorskip）。 +set -euo pipefail + +ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +PY="${PYTHON:-python3}" + +# 把所有 src/* 目录挂到 PYTHONPATH 最前面（包名不冲突，顺序无关）。 +paths=() +for d in "${ROOT}"/src/*/; do + paths+=("${d%/}") +done +export PYTHONPATH="$(IFS=:; echo "${paths[*]}")${PYTHONPATH:+:${PYTHONPATH}}" + +test_dirs=() +for d in "${ROOT}"/src/*/test; do + [ -d "$d" ] && test_dirs+=("$d") +done +if [ "${#test_dirs[@]}" -eq 0 ]; then + echo "没有找到任何测试目录（src/*/test）—— 仓库不完整？" >&2 + exit 1 +fi + +cd "${ROOT}" +if [ "$#" -gt 0 ]; then + exec "${PY}" -m pytest "${test_dirs[@]}" -q "$@" +fi +exec "${PY}" -m pytest "${test_dirs[@]}" -q diff --git a/src/arm_application/arm_application/gauge_assembly_node.py b/src/arm_application/arm_application/gauge_assembly_node.py index c755897..4907395 100644 --- a/src/arm_application/arm_application/gauge_assembly_node.py +++ b/src/arm_application/arm_application/gauge_assembly_node.py @@ -50,12 +50,13 @@ from __future__ import annotations +import dataclasses import json import math import os import time from pathlib import Path -from typing import Any, Dict, List, Mapping, Optional, Sequence, Tuple +from typing import Any, Dict, Iterable, List, Mapping, Optional, Sequence, Tuple import numpy as np import rclpy @@ -74,6 +75,9 @@ from arm_msgs.msg import GripperStatus from arm_msgs.srv import GripperExecute from arm_application.gauge_boot import ( + dependency_advice, + missing_modules, + perception_source_of, BootCheck, abs_name, blocking_failures, @@ -82,15 +86,25 @@ from arm_application.gauge_boot import ( optional_warnings, summary_counts, ) -from arm_application.gauge_config import GaugeConfig, GaugeConfigError, from_site +from arm_application.gauge_config import ( + GaugeConfig, + GaugeConfigError, + from_site, + snap_feature_of, +) from arm_application.gauge_fsm import ( + DESCEND_STALL_STEPS, Event, FsmError, GaugeContext, GaugeFsm, State, + descend_iter_budget, + descend_progress, + descend_tol_m, is_joint_limit_error, pixel_stable, + points_stable, ratchet_cycles, screw_stop, tangential_jump, @@ -114,7 +128,7 @@ from arm_application.gauge_kin import ( ) 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 +from arm_application.site_io import deep_merge, load_site_dict, ros2_root # 复用同一份图像解码与轨迹点构造，别在这里再写一份（规则 M2） from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point @@ -317,6 +331,8 @@ class GaugeAssembly(AssemblyPipeline): # 夹爪后端要按"服务实际类型"选，所以放到 S00 里做（此时才知道服务在不在） self._boot_results: List[Tuple[BootCheck, bool, str]] = [] + self._warn_if_sim_time_missing() + # ---- 状态机 ---- self.fsm = GaugeFsm(GaugeContext(holes_total=self.cfg.holes_total)) self.grip: Optional[GripperClient] = None @@ -337,6 +353,26 @@ class GaugeAssembly(AssemblyPipeline): self._coarse_depth: Optional[Image] = None self._coarse_info: Optional[CameraInfo] = None self._coarse_ns = self._d435_ns(site) + # 手外相机名的**唯一数据源**是 site.yaml 的 cameras 注册表（上面那行）。 + # 配置段里的 gauge_assembly.coarse_camera 是"注册表里没登记手外相机"时的 + # 兜底；两者不一致时以注册表为准，但要**出声** —— 否则改了一处没改另一处， + # 表现是"话题在发、S03 却拿到另一台相机/另一个 frame"，静默错。 + if self._coarse_ns and self.cfg.coarse_camera != self._coarse_ns: + self.get_logger().warn( + f"手外相机名不一致：cameras 注册表说 {self._coarse_ns}，" + f"gauge_assembly.coarse_camera 写的是 {self.cfg.coarse_camera}。" + f"以注册表为准（{self._coarse_ns}）。" + "site.yaml 里那处可以删掉了（留着只会误导下一个人）" + ) + # GaugeConfig 是 frozen 的：要改只能造一个新的（replace）。 + # 直接赋值会抛 FrozenInstanceError，而且是在 __init__ 里 —— + # 表现为"节点起不来"，看不出跟相机名有关。 + self.cfg = dataclasses.replace(self.cfg, coarse_camera=self._coarse_ns) + #: 各相机的**收到时刻**（单调钟）。判"有没有活的数据"用它，不用 + #: header.stamp —— 两者可能不在同一个时钟域（见 `_img_age` 的注释）。 + self._t_color: Optional[float] = None + self._t_coarse_color: Optional[float] = None + self._skew_warned = False if self._coarse_ns: img_t, info_t = resolve_realsense_topics(self._coarse_ns, image_topic="") # QoS 必须用 sensor data（BEST_EFFORT）：RealSense 驱动发的是 @@ -401,14 +437,51 @@ class GaugeAssembly(AssemblyPipeline): 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 "" + """手外相机的命名空间：``role: overhead*`` 且**属于当前这台臂**的那一个。 + + 为什么必须按 ``arm`` 过滤：仿真那台也有手外相机（``sim_d435``）， + 只按 role 扫会同时命中两台，而"命中哪一台"取决于字典顺序 —— + 同一份配置在仿真/真机之间**错而不报**地互串。 + + 规则（确定性的三级）： + 1. ``arm`` 与当前臂相同的优先（仿真：sim_d435 / 真机若两台各有相机也适用）； + 2. 都没有 ``arm``（= 双臂共用的那台老相机）时回落到它； + 3. 还是多个 → 出声告警并取第一个，让人去配置里区分。 + """ + cams = [ + cfg + for cfg in (site.get("cameras") or {}).values() + if isinstance(cfg, dict) + and str(cfg.get("role") or "").startswith("overhead") + ] + if not cams: + return "" + arm = str(self._robot_dict(site).get("arm") or "").strip() + mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else [] + hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams + if len(hits) > 1: + names = ", ".join(str(c.get("name") or "?") for c in hits) + self.get_logger().warn( + f"有 {len(hits)} 台手外相机都符合当前臂（{names}）—— 取第一个，" + "但请给 site.yaml 的 cameras.* 补上 arm 字段区分，否则选哪台看运气" + ) + return str(hits[0].get("name") or "").strip("/") + + def _eye_to_hand_file(self) -> Optional[Path]: + """找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。 + + 路径按**仓库根**解析而不是当前工作目录：相对 CWD 的路径会在"换个目录 + 启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。 + """ + root = ros2_root() + for rel in ( + f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml", + "handeye/results/eye_to_hand.yaml", + ): + cand = root / rel + if cand.is_file(): + return cand + return None def _on_robot_description(self, msg: String) -> None: self._robot_description = str(msg.data) @@ -417,8 +490,16 @@ class GaugeAssembly(AssemblyPipeline): if bool(msg.data): self._reload_ok = True + def _on_color(self, msg: Image) -> None: + # 腕部相机：父类（stage2）负责存帧，这里只补"收到时刻"并顺带查时钟域。 + super()._on_color(msg) + self._t_color = time.monotonic() + self._note_stamp_skew(msg, "腕部相机") + def _on_coarse_color(self, msg: Image) -> None: self._coarse_color = msg + self._t_coarse_color = time.monotonic() + self._note_stamp_skew(msg, "手外相机") def _on_coarse_depth(self, msg: Image) -> None: self._coarse_depth = msg @@ -430,15 +511,65 @@ class GaugeAssembly(AssemblyPipeline): """孔名 k1/k2/...。直接用状态机的，别在节点里再实现一遍（规则 M2）。""" return self.fsm.ctx.hole_name() - def _img_age(self, msg: Optional[Image]) -> float: - """图像从"它的时间戳"到现在过了多久 [s]。拿不到时间戳返回 1e9。""" - if msg is None: + def _img_age(self, t_recv: Optional[float]) -> float: + """图像从**收到的那一刻**到现在过了多久 [s]；从没收到返回 1e9。 + + 为什么不用 `header.stamp`（原来就是那么写的，出过事）： + 节点时钟和消息时钟可能是**两个时钟域** —— 仿真里忘了 `use_sim_time` + 时，`/clock` 从 0 开始、节点却用系统时间，于是"按时间戳算年龄"会算出 + `1790217444.7s` 这种数，把"图像 13Hz 好好地流着"报成 + "腕部相机无数据"，而人只会去怀疑相机。收到时刻用单调钟记， + 与两个时钟域都无关，判"有没有活数据"永远是对的。 + """ + if t_recv is None: return 1e9 + return max(0.0, time.monotonic() - t_recv) + + def _stamp_skew(self, msg: Image) -> Optional[float]: + """消息时间戳与节点时钟的差 [s]；取不到时间戳返回 None。""" 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) + return None + return self.get_clock().now().nanoseconds * 1e-9 - stamp + + def _note_stamp_skew(self, msg: Image, where: str) -> None: + """时钟域不一致时说一次，别每帧都刷。 + + > 1 小时就认为是两个时钟域（仿真差的是整段时间基准， + > 不可能只差几十秒；真机上的时延是毫秒级）。 + """ + if self._skew_warned: + return + skew = self._stamp_skew(msg) + if skew is None or abs(skew) < 3600.0: + return + self._skew_warned = True + self.get_logger().warn( + f"{where}的时间戳与节点时钟差 {skew:.0f}s：**不是数据问题，是时钟域不同**。" + "仿真里给本节点加 use_sim_time:=true（run_gauge_assembly.sh --sim 已带）。" + "不修的话 TF 查询也会全线失败 —— 因为 TF 是按时间戳比对的。" + ) + + def _warn_if_sim_time_missing(self) -> None: + """看起来在仿真里，却没开 use_sim_time → 出声（否则症状极难定位）。 + + `/clock` 有发布者 = 有人在做仿真（真机上没有 `/clock`）。这时如果 + `use_sim_time` 还是 false，所有 header.stamp 比对、TF lookup 都会错， + 而报出来的却是"相机没数据""TF 查不到"这类**假原因**。 + """ + if bool(self.get_parameter("use_sim_time").value): + return + try: + n = self.count_publishers("/clock") + except Exception: # noqa: BLE001 + return + if n > 0: + self.get_logger().warn( + "检测到 /clock 有发布者（在仿真里），但本节点 use_sim_time=false：" + "时间戳比对与 TF 查询都会失败。请加 use_sim_time:=true" + "（scripts/run_gauge_assembly.sh --sim 会自动带）。" + ) def _state(self, s: str) -> None: # 覆盖：三阶段的状态话题 + 本流程的 super()._state(s) @@ -517,6 +648,24 @@ class GaugeAssembly(AssemblyPipeline): d435_camera=self._coarse_ns or None, tool_name=self.tool_name, weights_path=self._weights, + perception_source=self._perception_source(), + ) + + def _perception_source(self) -> str: + """感知来源（site.yaml 顶层 perception.source，按当前臂覆盖后）；未知值报错。 + + 两件事在这里必须做对： + + 1. **按当前臂覆盖**：本节点读的是文件里的原始 site.yaml，机器人级 + 覆盖（``robots[].perception.source``）要自己合进来 —— 仿真那条 + ``source: truth`` 就写在 robots[`sim`] 下。忘了合的表现是 + "仿真里明明配了 truth，S00 还是去查 ultralytics"。 + 2. **只有一处读它**：launch 文件（决定起哪个感知节点）和 S00（决定查 + 不查推理依赖）必须看同一个值，否则输出会自相矛盾。 + """ + robot = self._robot_dict(self._site) + return perception_source_of( + deep_merge(self._site.get("perception") or {}, robot.get("perception") or {}) ) def _wrist_cam_ns(self) -> str: @@ -525,10 +674,35 @@ class GaugeAssembly(AssemblyPipeline): cam = deep_merge(self._site.get("camera") or {}, robot.get("camera") or {}) return str(cam.get("name") or "d405").strip().strip("/") + def _service_snapshot( + self, names: Iterable[str] = () + ) -> Dict[str, List[str]]: + """等服务发现稳定下来，再取服务表。 + + 为什么不能"查一次就算"：`get_service_names_and_types()` 返回的是 + **当前已发现** 的那部分图。本节点是刚加入 DDS 的新参与者，起来那一瞬间 + 别人的服务很可能还没被发现 —— 于是"服务明明在"被判成"名字不在图上"， + 同一份代码时好时坏。实测现场：仿真栈全绿时 S00 仍报 `/home`、 + `/set_named_pose`、`/gripper/status` 缺失，而 2 秒后手工 `ros2 service list` + 三个都在。这类"看起来像配置错"的抖动最难查，所以这里等到名字齐了再返回。 + + 等待期间必须 **spin**：发现事件要靠执行器回调处理，`time.sleep()` 只是干等。 + """ + want = {n for n in names if n} + deadline = time.monotonic() + max(0.5, min(5.0, self.cfg.boot_timeout_s)) + snap: Dict[str, List[str]] = {} + while True: + snap = dict(self.get_service_names_and_types()) + if not (want - set(snap)): + return snap + if time.monotonic() >= deadline: + return snap + self._spin(0.2) + def _probe(self, item: BootCheck) -> Tuple[bool, str]: """执行一项自检。返回 ``(就绪, 细节)``。""" if item.probe == "service": - found = dict(self.get_service_names_and_types()).get(item.target) + found = self._service_snapshot([item.target]).get(item.target) if found is None: return False, "名字不在图上" if item.expect_type and item.expect_type not in found: @@ -549,6 +723,11 @@ class GaugeAssembly(AssemblyPipeline): sidecar = p.with_suffix(".labels.yaml") if not sidecar.is_file(): return False, f"缺标签文件 {sidecar.name}（weights.sh doctor 可查）" + # 文件在 ≠ 能推理：缺 torch/ultralytics 时这里必须失败， + # 否则会一路跑到 S04 才以"跟踪没锁定"的形式暴露（见 gauge_boot 的注释）。 + miss = missing_modules() + if miss: + return False, dependency_advice(miss) return True, f"{p.name} + {sidecar.name}" # topic / tactile / camera：**要真收到消息**才算就绪。 @@ -558,7 +737,11 @@ class GaugeAssembly(AssemblyPipeline): detail = self._peek_topic(item) if detail is None: return True, "" - time.sleep(0.2) + # 必须 **spin** 而不是 sleep：订阅回调只在 executor 转起来时才被调用， + # `time.sleep()` 期间一条消息都进不来。这个坑的现场是"相机/触觉/夹爪 + # 明明都在发数据，S00 却逐项报缺失"（真机和仿真都会中招）， + # 而人只会去怀疑硬件。 + self._spin(0.2) return False, detail or "等不到消息" def _probe_action(self, item: BootCheck) -> Tuple[bool, str]: @@ -598,9 +781,18 @@ class GaugeAssembly(AssemblyPipeline): 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: + t_recv = self._t_coarse_color if coarse else self._t_color + if msg is None or t_recv is None: return "还没收到图像（发布者不在？）" + # 时钟域不对时**必须说清**：否则同一现象有两种完全不同的原因， + # 而报出来的话术会把人引到错的方向（先查这个，再查数据）。 + skew = self._stamp_skew(msg) + if skew is not None and abs(skew) > 3600.0: + return ( + f"图像时间戳与节点时钟差 {skew:.0f}s：数据在流，是**时钟域**不同 —— " + "仿真里要给节点 use_sim_time:=true（不加的话 TF 也会查不到）" + ) + age = self._img_age(t_recv) if age > 3.0: return f"最后一帧已过期 {age:.1f}s" return None @@ -628,13 +820,32 @@ class GaugeAssembly(AssemblyPipeline): def _step_boot(self) -> Event: """S00：全部必检就绪才开指令窗。""" self._state("S00_boot") - checks = self._boot_checks() + try: + checks = self._boot_checks() + except ValueError as exc: + # 配置错（例如 perception.source 拼错）在这里拦下：继续跑下去只会在 + # S04 以"一阶段不锁定"的形式暴露，那时没人会想到是自检配置写错了。 + self.get_logger().error(f"S00 自检清单建不起来（配置有问题）：{exc}") + return Event.BOOT_FAIL + src = self._perception_source() + if src != "yolo": + self.get_logger().info( + f"S00 自检：感知来源 perception.source = {src} —— 不查推理依赖" + + ("（仿真真值节点随 S04 的 visual_servo 一起起）" if src == "truth" + else "（测量由外部节点按 visual_servo/filtered_point 提供）") + + "；它只验控制与接口，不验检测算法" + ) # 夹爪后端：按**服务类型**选（真机 Trigger / 契约 GripperExecute）。 # 选不出来时把这条也算成一项必检失败，而不是抛异常中断整段日志。 backend_detail = "" try: - choice = pick_backend(dict(self.get_service_names_and_types()), self._gripper_prefer) + # 夹爪后端按"服务实际类型"选：同样要等发现齐了（见 _service_snapshot）。 + # 两种后端都有 /gripper/open、/gripper/close，等这两个就够。 + choice = pick_backend( + self._service_snapshot(["/gripper/open", "/gripper/close"]), + self._gripper_prefer, + ) for line in describe_backend_choice(choice): self.get_logger().info(line) self.grip = GripperClient(self, choice.backend, choice.status_topic) @@ -969,18 +1180,8 @@ class GaugeAssembly(AssemblyPipeline): 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 + return self._img_age(self._t_coarse_color) < 3.0 def _wait_coarse_frame(self, ns: str) -> bool: """等手外相机的**新鲜**图像，最多 ``coarse_timeout_s`` 秒。 @@ -1106,28 +1307,63 @@ class GaugeAssembly(AssemblyPipeline): ) return Event.SNAP_OK + def _snap_feature(self) -> str: + """S05 稳定判据用哪个特征：``bbox``（检测框像素）还是 ``motion``（法兰 mm）。 + + ``auto`` 按感知来源选：真机（yolo）用检测框 —— 保持原有行为不变； + 仿真真值/外部感知没有可用的工件检测器（合成图过不了真机训练出来的模型， + 实测最高分 0.017），用检测框只会永远"不稳定"地空转，所以改用法兰位移。 + """ + return snap_feature_of(self.cfg.snap_stable_feature, self._perception_source()) + + def _snap_sample(self, feature: str) -> Optional[Tuple[float, ...]]: + """采一帧"画面特征"（单位随 feature 变：bbox=像素，motion=毫米）。""" + if feature == "bbox": + img = getattr(self, "_color", None) + if img is None: + return None + bbox = self._best_bbox(self._bgr_from_msg(img)) + if bbox is None: + return None + return (0.5 * (bbox[0] + bbox[2]), 0.5 * (bbox[1] + bbox[3])) + # motion：法兰（= 相机）在世界系里的位置。相机装在法兰上，法兰不动， + # 画面就不会动 —— 比"检测框不动"更直接，也不依赖任何模型。 + _q, Tee = self._wait_state(0.5) + if Tee is None: + return None + return (float(Tee[0, 3]) * 1000.0, float(Tee[1, 3]) * 1000.0, float(Tee[2, 3]) * 1000.0) + def _wait_pixel_stable(self) -> bool: - """等画面稳定：连续采 ``snap_stable_s`` 秒的检测框中心，抖动在容差内才算稳。 + """等画面稳定：连续采 ``snap_stable_s`` 秒的特征，抖动在容差内才算稳。 用"到均值的最远距离"而不是相邻帧之差 —— 跟踪抖动是零均值的， 相邻差会让"明明在晃"的时刻被判成稳定，然后拍出一张糊图（表现为 "孔位偶尔偏 1mm"，随机且难复现）。判据实现见 gauge_fsm.pixel_stable。 """ + feature = self._snap_feature() + tol = self.cfg.snap_tol_px if feature == "bbox" else self.cfg.snap_tol_mm + got_any = False deadline = time.monotonic() + 20.0 while time.monotonic() < deadline: - samples: List[Tuple[float, float]] = [] + samples: List[Tuple[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])) - ) + s = self._snap_sample(feature) + if s is not None: + samples.append(s) + got_any = True self._spin(0.05) - if pixel_stable(samples, self.cfg.snap_tol_px, self.cfg.snap_min_frames): + if points_stable(samples, tol, self.cfg.snap_min_frames): return True + if not got_any and feature == "bbox": + # 一条都采不到 = 检测器根本没出框。这跟"画面在晃"是**两件事**， + # 分开报才找得到人（真机上这是"权重与图像不匹配"的典型症状）。 + self.get_logger().error( + "S05 等稳定时工件检测器一直没出框（0 帧样本）——" + "不是画面在晃，是没检测到工件。查：权重是否与这台相机匹配" + "（models/yolo/README.md）、或把 gauge_assembly.snap_stable_feature " + "设成 motion 改用法兰位移判据" + ) return False # ===================================================================== @@ -1219,19 +1455,66 @@ class GaugeAssembly(AssemblyPipeline): 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 + # 这一步的到位容差**必须小于步长**（见 gauge_fsm.descend_tol_m 的说明： + # 容差≥步长时 _goto 判"已在容差内"直接返回，一步都不走 → 静默自旋）。 + tol_step_m = descend_tol_m(step_m) + budget = descend_iter_budget(max_depth_m, step_m) + self.get_logger().info( + f"S07 步进下压：步长 {step_m * 1000:.2f}mm、到位容差 {tol_step_m * 1000:.3f}mm、" + f"最多 {max_depth_m * 1000:.1f}mm / {budget} 步" + ) + steps = 0 + stalls = 0 + z_prev = z0 while float(Tee[2, 3]) - z0 > -max_depth_m: + if steps >= budget: + self.get_logger().error( + f"S07 下压步数用尽（{budget} 步 ≈ {max_depth_m * 1000:.1f}mm）仍未触发接触判据。\n" + " 这不是正常路径：正常应该是「接触→CONTACT_OK」或「到底→超深失败」。\n" + " 走到这里通常意味着臂**没在动**（控制器死区/被夹住/孔位偏得擦不到面）。" + ) + self._retract(q, Tee, dz_m=0.01) + return Event.TIMEOUT + steps += 1 target = np.array([float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]) - step_m]) # 不用 _step_to：它把速度写死成 search_speed，而这**一步**必须慢 # （接触判据要在"碰到"的瞬间停住，快了就是一个冲击）。速度取 # gauge_assembly.contact_speed_m_s，其余参数与 _step_to 对齐。 ok, q, Tee = self._goto( - q, Tee, target, verticalize=True, tol_m=0.0005, + q, Tee, target, verticalize=True, tol_m=tol_step_m, tol_tilt=self.vert_tol_rad * 2.5, tag="S07_step", v=self.cfg.contact_speed_m_s, max_hops=2, settle_s=max(0.06, self.settle3 * 0.3), min_dur=0.16, ) if not ok: self.get_logger().warn("S07 单步未完全到位，继续按实测高度判断") + # 实测位移 = 唯一的推进证据。指令发出去不等于臂动了（R5.1 静默失效）。 + z_now = float(Tee[2, 3]) + if descend_progress(z_prev, z_now, step_m): + stalls = 0 + else: + stalls += 1 + self.get_logger().warn( + f"S07 第 {steps} 步实测只走了 {(z_prev - z_now) * 1000:+.3f}mm" + f"（要求 ≈{step_m * 1000:.2f}mm），连续 {stalls}/{DESCEND_STALL_STEPS} 步没动" + ) + if stalls >= DESCEND_STALL_STEPS: + self.get_logger().error( + f"S07 连续 {stalls} 步**实测零位移**，臂根本没往下走。\n" + " 常见原因：① 这一步的容差 ≥ 步长（_goto 判「已在容差内」直接返回）；\n" + " ② 控制器死区/增益太小，0.x mm 的微步被吃掉；\n" + " ③ 工件/夹具把臂顶住了（此时应报接触，却没达到切向阈值）；\n" + " ④ 孔位偏了，规擦着孔壁下去但没接触面。" + ) + self._retract(q, Tee, dz_m=0.01) + return Event.TIMEOUT + z_prev = z_now + if steps % 5 == 0: + now_dbg = self._shear_axes() + self.get_logger().info( + f"S07 已下压 {abs(z_now - z0) * 1000:.2f}mm" + f"（{steps}/{budget} 步）切向 ({now_dbg[0]:.1f}, {now_dbg[1]:.1f})" + ) try: fn, _fn_max, _shear, _contact = self._tac_fresh(self.tac_timeout) except Exception as exc: # noqa: BLE001 diff --git a/src/arm_application/arm_application/gauge_boot.py b/src/arm_application/arm_application/gauge_boot.py index 7bc18d8..59bd620 100644 --- a/src/arm_application/arm_application/gauge_boot.py +++ b/src/arm_application/arm_application/gauge_boot.py @@ -22,7 +22,7 @@ from __future__ import annotations from dataclasses import dataclass -from typing import Dict, Iterable, List, Optional, Sequence, Tuple +from typing import Dict, Iterable, List, Mapping, Optional, Sequence, Tuple #: 探针类型 —— 决定节点用哪种方式确认"就绪"。 #: @@ -34,6 +34,68 @@ from typing import Dict, Iterable, List, Optional, Sequence, Tuple #: ``detector`` 算法权重能加载（推理预热） PROBES = ("service", "action", "topic", "tactile", "camera", "detector") +#: ``detector`` 自检除了"权重文件在"，还要求这些 Python 模块能 import。 +#: 为什么必须查：权重文件在 ≠ 能推理。实测现场 —— 仿真环境（ros_gz_sim）里 +#: 没有 torch/ultralytics，S00 的"检测算法预热"照样报 OK，然后 S04 空转 150s +#: 才报"没锁定"，看起来像算法或标定问题（真正的错在很远的另一处）。 +#: ``ultralytics`` 会连带把 torch 拉进来，查它就够；真要细分再看 yolo_detector。 +DETECTOR_MODULES = ("ultralytics",) + +#: 感知来源（``site.yaml`` 顶层 ``perception.source``）—— 谁产生"视觉测量"。 +#: +#: ``yolo`` 本体 perception_node（要 torch/ultralytics） +#: ``truth`` 仿真真值：按场景里已知的物体位姿直接算目标点，不跑推理 +#: ``external`` 外部节点按同一话题（``visual_servo/filtered_point``）提供 +#: +#: ⚠️ 别和 ``yolo.backend`` 混：那个是"**怎么**推理"（tensorrt/onnx）。 +PERCEPTION_SOURCES = ("yolo", "truth", "external") + + +def perception_source_of(perception_cfg: Optional[Mapping] = None) -> str: + """读 ``perception.source`` 并校验。**未知值直接报错，不默默当 yolo**。 + + 默默退回 yolo 的后果：仿真里 perception_node 起不来，S00 又去查 + "推理依赖"，报出来的错跟真正的原因（source 拼错）隔着两层。 + """ + src = str(((perception_cfg or {}) or {}).get("source") or "yolo").strip().lower() + if src not in PERCEPTION_SOURCES: + raise ValueError( + f"site.yaml 的 perception.source = {src!r} 不认识。" + f"只支持 {' / '.join(PERCEPTION_SOURCES)}" + "（yolo=本体推理，truth=仿真真值，external=外部节点提供）。" + ) + return src + + +def missing_modules(modules: Sequence[str] = DETECTOR_MODULES) -> List[str]: + """哪些模块当前解释器里 import 不到（纯逻辑，可离线测试）。""" + import importlib.util + + out: List[str] = [] + for m in modules: + try: + if importlib.util.find_spec(m) is None: + out.append(m) + except (ImportError, ValueError): + # find_spec 对"父包不存在/名字非法"会抛，这同样等于"用不了" + out.append(m) + return out + + +def dependency_advice(missing: Sequence[str]) -> str: + """缺依赖时给**两条**出路：装它、或换掉这个后端。 + + 只写"请安装 X"是不够的：仿真环境常常是离线的（这台机器实测 pip 直接 + SSL 失败），那样的话现场需要的是"换一条不依赖它的路"，而不是反复重试安装。 + """ + mods = ", ".join(missing) + return ( + f"缺推理依赖 {mods} —— 权重文件在也没用，导入不了就跑不了推理。两条路：\n" + f" 1) 装上：pip install {' '.join(missing)}（需要联网；本机仿真环境离线时做不到）\n" + " 2) 换后端：仿真里可以不用 YOLO —— 由外部按同一话题/同一类型提供感知结果\n" + " （见 devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节）" + ) + @dataclass(frozen=True) class BootCheck: @@ -97,6 +159,7 @@ def build_checks( d435_camera: Optional[str] = None, tool_name: str = "", weights_path: str = "", + perception_source: str = "yolo", ) -> List[BootCheck]: """按现场情况生成检查清单。 @@ -110,6 +173,10 @@ def build_checks( :param tool_name: ``site.yaml`` 的 ``robot.tool_name``。空 = 没指定工具系， 则 S01 的"工具系垂直向下锁定"无法确认 → 单独列为选检告警。 :param weights_path: 检测权重路径。空 = 不检查推理预热。 + :param perception_source: ``site.yaml`` 的 ``perception.source``。**只有 + ``yolo`` 才查推理预热**：truth/external 根本不跑推理，查了必然是 + 假阴性（"缺 ultralytics"），而那是预期之内的。这里刻意**不生成** + 这一项，而不是生成一个永远 OK 的项 —— 后者会让人以为"感知也验过了"。 清单的顺序 = 打印顺序 = 排障顺序：先臂、再夹爪、再相机、再触觉、最后算法。 """ @@ -255,7 +322,7 @@ def build_checks( expect_type="arm_msgs/srv/SwitchTool", ) ) - if weights_path: + if weights_path and perception_source == "yolo": checks.append( BootCheck( key="detector", diff --git a/src/arm_application/arm_application/gauge_config.py b/src/arm_application/arm_application/gauge_config.py index db10176..97b97eb 100644 --- a/src/arm_application/arm_application/gauge_config.py +++ b/src/arm_application/arm_application/gauge_config.py @@ -22,6 +22,28 @@ import difflib from dataclasses import dataclass, fields from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple +from arm_application.gauge_fsm import descend_tol_m + +#: S05 稳定判据可用的特征来源。``auto`` 按 ``perception.source`` 自动选。 +#: +#: 为什么需要有"motion"这一路：判据原本是**工件检测框中心**的抖动（像素）， +#: 那要求检测器真的出框。仿真里合成图过不了真机训练出来的模型（实测最高分 +#: 0.017），于是 S05 会永远"不稳定"地空转 —— 而真正该等的是"相机不再动"。 +#: 用法兰位移（mm）当特征既直接又不依赖模型，且判据本身（到均值的最远距离）不变。 +SNAP_STABLE_FEATURES = ("auto", "bbox", "motion") + +def snap_feature_of(want: str, perception_source: str) -> str: + """把 ``snap_stable_feature``（可能是 ``auto``）+ 感知来源 → 实际用哪个特征。 + + 抽成纯函数是为了能脱离 ROS 节点测：这个选择错了，S05 会在真机上"永远不稳定" + 或者在仿真里空转到超时，而两种表现都像是"机械臂在晃"，很难往配置上想。 + """ + want = str(want or "auto") + if want != "auto": + return want + return "bbox" if perception_source == "yolo" else "motion" + + #: 合法的单位后缀。键名必须以 ``_<单位>`` 结尾（``au`` = Orisys 任意单位增量）。 _UNIT_SUFFIX: Dict[str, str] = { "s": "_s", @@ -43,6 +65,7 @@ _UNITLESS = frozenset( "coarse_camera", "coarse_conf", "snap_min_frames", + "snap_stable_feature", "screw_max_cycles", "unscrew_extra_cycles", "holes_total", @@ -66,7 +89,9 @@ _TYPES: Dict[str, type] = { "coarse_timeout_s": float, "snap_stable_s": float, "snap_tol_px": float, + "snap_tol_mm": float, "snap_min_frames": int, + "snap_stable_feature": str, "align_tol_mm": float, "contact_speed_m_s": float, "contact_step_mm": float, @@ -104,7 +129,9 @@ _UNITS: Dict[str, str] = { "coarse_timeout_s": "s", "snap_stable_s": "s", "snap_tol_px": "px", + "snap_tol_mm": "mm", "snap_min_frames": "", + "snap_stable_feature": "", "align_tol_mm": "mm", "contact_speed_m_s": "m/s", "contact_step_mm": "mm", @@ -165,7 +192,9 @@ DEFAULTS: Dict[str, Any] = { # S05 "snap_stable_s": 1.0, "snap_tol_px": 2.0, + "snap_tol_mm": 0.4, "snap_min_frames": 6, + "snap_stable_feature": "auto", # S06 "align_tol_mm": 1.0, # S07 @@ -213,7 +242,9 @@ class GaugeConfig: coarse_timeout_s: float snap_stable_s: float snap_tol_px: float + snap_tol_mm: float snap_min_frames: int + snap_stable_feature: str align_tol_mm: float contact_speed_m_s: float contact_step_mm: float @@ -263,14 +294,19 @@ class GaugeConfig: f"限位余量 {self.extreme_margin_deg:.1f}°，关节速度 {self.extreme_speed_deg_s:.1f}°/s）", f" S03 粗定位={'开' if self.coarse_enabled else '关'} 相机={self.coarse_camera} " f"悬高 {self.coarse_hover_m * 1000:.0f}mm 置信度≥{self.coarse_conf:.2f}", - f" S05 稳定 {self.snap_stable_s:.1f}s / 抖动≤{self.snap_tol_px:.1f}px / 至少 {self.snap_min_frames} 帧" + f" S05 稳定 {self.snap_stable_s:.1f}s / 至少 {self.snap_min_frames} 帧 / " + f"特征={self.snap_stable_feature}" + f"（检测框 ≤{self.snap_tol_px:.1f}px、法兰位移 ≤{self.snap_tol_mm:.2f}mm）" "（稳定判据用「到均值的最远距离」，不是相邻帧之差）", # 悬停高度**不在这里**：它取自 servo.hole_hover_z_m（stage2 那一层）， # 这里只印本段的容差 —— 一段配置里出现"另一个段的键"会让人改错地方。 f" S06 对位容差 {self.align_tol_mm:.1f}mm（悬停高取自 servo.hole_hover_z_m）", - f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一），" - f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm" - "（触觉读数是任意单位增量，必须现场标定）", + f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一，现场标定），" + f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm", + f" S07 步进 {self.contact_step_mm:.2f}mm @ {self.contact_speed_m_s * 1000:.0f}mm/s，" + f"到位容差 {descend_tol_m(self.contact_step_mm / 1000.0) * 1000:.3f}mm" + "（=步长的 40%，**推导出来的、不是可调项**：容差 ≥ 步长会让每一步都被判成" + "「已在容差内」，臂一步都不走）", f" S08 棘轮 {self.screw_step_deg:.0f}°×{self.screw_max_cycles} 轮 = 累计 " f"{self.total_screw_deg:.0f}°；停止判据「切向 >+{self.screw_shear_delta:.0f} 且 " f"法向 >+{self.screw_normal_delta:.0f}」", @@ -402,6 +438,16 @@ def _sanity(cfg: GaugeConfig) -> None: ) if cfg.snap_tol_px <= 0.0: raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}") + if cfg.snap_tol_mm <= 0.0: + raise GaugeConfigError(f"gauge_assembly.snap_tol_mm 必须为正，收到 {cfg.snap_tol_mm}") + if cfg.snap_stable_feature not in SNAP_STABLE_FEATURES: + raise GaugeConfigError( + f"gauge_assembly.snap_stable_feature={cfg.snap_stable_feature!r} 不认识，" + f"只支持 {sorted(SNAP_STABLE_FEATURES)}。\n" + f" auto = 按 perception.source 自动选（真机 yolo→bbox、仿真/外部→motion）\n" + f" bbox = 用工件检测框中心（像素），需要检测器真的出框\n" + f" motion = 用法兰位移（mm），不依赖检测器" + ) # ============================================================================= diff --git a/src/arm_application/arm_application/gauge_fsm.py b/src/arm_application/arm_application/gauge_fsm.py index 3117b82..2125c1b 100644 --- a/src/arm_application/arm_application/gauge_fsm.py +++ b/src/arm_application/arm_application/gauge_fsm.py @@ -24,7 +24,7 @@ from __future__ import annotations import math from dataclasses import dataclass, field from enum import Enum -from typing import Dict, List, Optional, Tuple +from typing import Dict, List, Optional, Sequence, Tuple # ============================================================================= @@ -306,6 +306,26 @@ class GaugeFsm: # ============================================================================= # 判据（纯函数，被节点与测试共用） # ============================================================================= +def points_stable(samples: List[Sequence[float]], tol: float, min_frames: int) -> bool: + """稳定判据（任意维）：样本够多、且所有点到均值距离都 ≤ ``tol``。 + + 与 :func:`pixel_stable` 是同一套判据，只是不限定二维。为什么需要： + S05 的"画面稳了再拍"在仿真里用不了检测框（合成图过不了真机训练出来的 + 工件模型），于是改用**法兰位移**（mm）当特征 —— 判据本身一模一样， + 只是量纲从像素变成毫米，所以抽出来共用，而不是各写一份。 + """ + if len(samples) < max(2, int(min_frames)): + return False + dim = len(samples[0]) + means = [sum(float(s[i]) for s in samples) / len(samples) for i in range(dim)] + if any(len(s) != dim for s in samples): + return False + for s in samples: + if math.sqrt(sum((float(s[i]) - means[i]) ** 2 for i in range(dim))) > tol: + return False + return True + + def pixel_stable(samples: List[Tuple[float, float]], tol_px: float, min_frames: int) -> bool: """S05：像素稳定判据。 @@ -319,13 +339,7 @@ def pixel_stable(samples: List[Tuple[float, float]], tol_px: float, min_frames: 定位图 —— 表现为"孔位偶尔偏 1mm"，随机且难复现。用"到均值的最远距离" 才对得上"画面不动"这个物理事实。 """ - if len(samples) < max(2, int(min_frames)): - return False - us = [s[0] for s in samples] - vs = [s[1] for s in samples] - cu = sum(us) / len(us) - cv = sum(vs) / len(vs) - return all(math.hypot(u - cu, v - cv) <= tol_px for u, v in samples) + return points_stable(samples, tol_px, min_frames) def tangential_jump( @@ -344,6 +358,56 @@ def tangential_jump( return any(abs(n - r) >= delta for n, r in zip(shear_now, shear_ref)) +#: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。 +#: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、 +#: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内" +#: → 臂原地不动、循环条件永远成立 → **静默自旋**（日志停在基线那行、CPU 跑满、 +#: 既不报错也不前进）。2026-09-24 在仿真里踩到过一次，真机同参数同结果。 +DESCEND_TOL_FRAC: float = 0.4 +#: 容差下限 [m]：再小就追不动了（伺服死区/噪声会让臂永远"差一点点"）。 +DESCEND_TOL_FLOOR_M: float = 2.0e-5 +#: 判定"这一步真的往下走了"的位移下限，占步长的比例。 +DESCEND_PROGRESS_FRAC: float = 0.2 +#: 允许连续多少步毫无位移后中止（防"没接触也一直转"的静默自旋）。 +DESCEND_STALL_STEPS: int = 5 + + +def descend_tol_m(step_m: float) -> float: + """S07 单步下压该给 ``_goto`` 的到位容差 [m]。 + + 取步长的 40%，但不超过步长的一半、不低于 ``DESCEND_TOL_FLOOR_M``。 + 这样"走满这一步"才是循环推进的**唯一**出口 —— 容差比步长小，`_goto` + 就不可能因为"已经在容差内"而空转（那正是上面注释里那个静默自旋）。 + """ + if step_m <= 0.0: + raise ValueError(f"下压步长必须为正，收到 {step_m}") + return max(step_m * DESCEND_TOL_FRAC, min(DESCEND_TOL_FLOOR_M, step_m * 0.5)) + + +def descend_iter_budget(max_depth_m: float, step_m: float, slack: int = 8) -> int: + """S07 下压循环的**硬**迭代上限：够走完整个下探深度，再多给几步余量。 + + 为什么要有硬上限：循环条件写的是"实测高度还没到最深"，一旦臂因为死区、 + 被夹住、孔位偏了而**实际没动**，这个条件就恒真。上限不是用来"早点失败" + 的（那是 ``contact_max_depth_mm`` 的活），而是**兜住静默自旋**： + 宁可报"S07 没走完也没接触"，也不能让节点无声无息地空转到天亮。 + """ + if step_m <= 0.0: + raise ValueError(f"下压步长必须为正，收到 {step_m}") + if max_depth_m <= 0.0: + return 0 + return int(math.ceil(max_depth_m / step_m)) + max(1, int(slack)) + + +def descend_progress(z_prev: float, z_now: float, step_m: float) -> bool: + """这一步是否真的往下走了（法兰 z 向下为负）。 + + 判据是**实测**位移，不是"指令发出去了"：``_goto`` 因为容差而空转、 + 或步长被控制器死区吞掉时，实测位移≈0 而指令日志一切正常。 + """ + return (float(z_prev) - float(z_now)) >= step_m * DESCEND_PROGRESS_FRAC + + def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool: """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。 diff --git a/src/arm_application/arm_application/hole_detect_hough.py b/src/arm_application/arm_application/hole_detect_hough.py index b3581b6..3bc34e4 100644 --- a/src/arm_application/arm_application/hole_detect_hough.py +++ b/src/arm_application/arm_application/hole_detect_hough.py @@ -2,6 +2,8 @@ from __future__ import annotations +import logging + from pathlib import Path from typing import Any @@ -23,6 +25,9 @@ def load_config(path: str | Path | None = None) -> dict[str, Any]: return data +logger = logging.getLogger(__name__) + + def _fail( msg: str, draw_img: np.ndarray, @@ -383,12 +388,20 @@ def detect_holes( skip_center_gate: bool = False, depth_m: np.ndarray | None = None, intr: dict | None = None, + allow_full_frame_roi: bool = False, ) -> dict[str, Any]: """ 二阶段检测螺纹孔中心。 - detector: 需提供 detect(image_bgr, conf=..., iou=..., imgsz=...) -> list[DetBox] - （YOLODetector / FasterRCNNDetector 均可） + detector: 需提供 detect(image_bgr, conf=..., iou=..., imgsz=...) -> list[Detection] + （YOLODetector / FasterRCNNDetector / ONNX 均可） + + :param allow_full_frame_roi: 检测器**没给出工件框**时，是否退回"整幅画面当 ROI"。 + 默认 False（保持原行为：没有框就不做，报「未检测到工件」）。 + 置 True 的场景是**调用方明确要试一把**：知道检测器可能漏检，但孔本身 + 在画面里已经看得见（例如工件检测模型与当前相机/画面不匹配、或工件贴着 + 画面中心）。此时 ROI 的约束由孔自身的判据接手 —— 环半径、两孔间距、 + 居中门控都还在，所以不会凭空多出"随便一个圆"。 返回: status, msg, hole_points [[cx,cy],...], hole_radii [r1,r2], bbox, draw_img(BGR) @@ -413,6 +426,14 @@ def detect_holes( # 1) 检测器定位工件 bbox（最多 1 个）— 兼容 YOLO / Faster R-CNN dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz) bbox = _parse_best_bbox(dets, conf) + if bbox is None and allow_full_frame_roi: + # 退回整幅画面：ROI 从"检测器给的框"换成"整幅"，孔自身的判据 + #（环半径区间、两孔间距、居中门控）继续把关。 + bbox = (0, 0, int(w - 1), int(h - 1)) + logger.info( + "工件检测器没给框，按调用方要求用整幅画面当 ROI 继续找孔" + "（允许的原因见 allow_full_frame_roi 的说明）" + ) if bbox is None: return _fail("未检测到工件", draw_img) diff --git a/src/arm_application/arm_application/pipeline_node.py b/src/arm_application/arm_application/pipeline_node.py index 9c26e48..d4eab0a 100644 --- a/src/arm_application/arm_application/pipeline_node.py +++ b/src/arm_application/arm_application/pipeline_node.py @@ -19,11 +19,56 @@ from typing import Optional import numpy as np import rclpy +from geometry_msgs.msg import PointStamped +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy from std_msgs.msg import Bool, String from arm_application.stage3_search_node import Stage3Search +#: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。 +#: +#: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清 +#: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再 +#: 小幅下降（只需 5cm 以内的行程）。 +#: +#: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对 +#: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套 +#: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。 +TRACK_CLEARANCE_M = 0.05 + + +def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str: + """一阶段超时了，到底是"感知没给点"还是"给了点但没收敛"。 + + 这两种情况的排查方向完全相反，而现象长得一模一样（都是"没锁定"）： + + * **一个点都没收到** → 感知侧的事：yolo 后端多半缺 ultralytics/torch、 + truth 后端真值节点没起来、external 后端没人发。跟标定、跟工件在不在 + 视野里**都没关系**。 + * **收到了点但没收敛** → 控制/几何侧的事：工件不在视野、手眼不对、 + 目标超出工作空间、增益太小。 + + 以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node + 因为缺依赖早退了"被读成"算法不收敛"，白查半天。 + """ + if points_seen <= 0: + return ( + f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**" + "（visual_servo/filtered_point 上 0 条）—— 这是感知没起来，" + "不是标定/视野问题。按 perception.source 对号入座：\n" + " yolo → 多半缺推理依赖（ultralytics/torch），" + f"看感知进程退出前几行：{log_path}\n" + " truth → 仿真真值节点没起来，或场景/物体名读不出来" + "（它退出前会打印原因）\n" + " external → 外部节点没按这个话题/这个类型发" + ) + return ( + f"一阶段 {timeout_s:.0f}s 没锁定（收到 {points_seen} 个目标点但没收敛）：" + f"工件不在视野 / 手眼不对 / 目标超出工作空间。看 {log_path}" + ) + + class AssemblyPipeline(Stage3Search): def __init__(self, node_name: str = "assembly_pipeline") -> None: # node_name 可覆盖：通止规装配（gauge_assembly_node）继承本类以复用 @@ -40,6 +85,28 @@ class AssemblyPipeline(Stage3Search): self._s1: Optional[subprocess.Popen] = None self._locked = False self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10) + # 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。 + # 只计数，不参与控制 —— 它绝不能影响流程本身的判断。 + # + # ⚠️ QoS 必须与感知发布端**逐项一致**（BEST_EFFORT + KEEP_LAST(5)， + # 见 perception_node / sim_truth_perception）。用默认的 RELIABLE 订阅 + # BEST_EFFORT 的话题：不报错、话题列表里也在，但**一条都收不到** —— + # 于是超时归因会一口咬定"感知没起来"，把人往完全错误的方向带 + #（真实原因可能只是这里 QoS 不匹配）。这类"诊断自己坏了"最难查， + # 所以宁可多写这几行。 + self.declare_parameter("diag_point_topic", "visual_servo/filtered_point") + self._diag_point_topic = str(self.get_parameter("diag_point_topic").value) + self._point_msgs = 0 + self.create_subscription( + PointStamped, + self._diag_point_topic, + self._on_point_seen, + QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=5, + reliability=ReliabilityPolicy.BEST_EFFORT, + ), + ) self._pipe_pub = self.create_publisher(String, "/pipeline/state", 10) self._pipe_result = self.create_publisher(String, "/pipeline/result", 10) @@ -62,6 +129,10 @@ class AssemblyPipeline(Stage3Search): if bool(msg.data): self._locked = True + def _on_point_seen(self, _msg: PointStamped) -> None: + """只记"收到过目标点"，用于超时后的归因（见 stage1_timeout_reason）。""" + self._point_msgs += 1 + def _kill_old_stage1(self) -> None: for pat in ( "visual_servo.launch", @@ -92,8 +163,8 @@ class AssemblyPipeline(Stage3Search): self._kill_old_stage1() def _track_z(self) -> float: - """跟踪高度 = 孔悬停高 + 5cm，通止规离开工件，二阶段只需小幅下降。""" - return float(self.hover_z) + 0.05 + """跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。""" + return float(self.hover_z) + TRACK_CLEARANCE_M def _ensure_clearance(self) -> None: """上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。""" @@ -125,12 +196,17 @@ class AssemblyPipeline(Stage3Search): log_path = "/tmp/pipeline_stage1.log" logf = open(log_path, "w", encoding="utf-8") lock_z = self._track_z() + # use_sim_time 必须**原样传下去**：本节点在仿真里是 true，子 launch 若是 + # false，perception/servo 就跑在系统时间上，TF 全是仿真时刻的戳 → + # "看得见图、算得出点，就是查不到变换"，一阶段永远不收敛。 + use_sim = bool(self.get_parameter("use_sim_time").value) cmd = [ "ros2", "launch", "arm_application", "visual_servo.launch.py", "enable_control:=true", "publish_handeye_tf:=false", f"ee_z_m:={lock_z:.3f}", "lock_hold_sec:=0.6", + f"use_sim_time:={str(use_sim).lower()}", ] self.get_logger().info( f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）" @@ -139,6 +215,7 @@ class AssemblyPipeline(Stage3Search): cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=True, ) timeout = float(self.get_parameter("stage1_timeout_sec").value) + self._point_msgs = 0 t0 = time.monotonic() while time.monotonic() - t0 < timeout: self._spin(0.1) @@ -150,7 +227,7 @@ class AssemblyPipeline(Stage3Search): else: self._stop_stage1() self.get_logger().error( - f"一阶段 {timeout:.0f}s 没锁定（视野里没工件？看 {log_path}）" + stage1_timeout_reason(self._point_msgs, timeout, log_path) ) return 1 self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段") diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index e06108d..667265b 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -46,6 +46,7 @@ from arm_application.geom_ik import ( quat_xyzw_to_R, ) from arm_application.handeye_realsense import resolve_realsense_topics +from arm_application.gauge_boot import perception_source_of from arm_application.hole_detect_hough import detect_holes from arm_application.hole_detector import config_from_site, weights_from_site from arm_application.site_io import deep_merge, load_site_dict @@ -153,6 +154,9 @@ class Stage2Locate(Node): self.declare_parameter("hover_z_m", -1.0) # 大于 0 → 覆盖 site 里的值 self.declare_parameter("preview", True) self.declare_parameter("action_name", "rizon_arm_controller/follow_joint_trajectory") + # 真值孔位（perception.source=truth）要把 scene.yaml 的物体位姿投影成像， + # 场景位姿在 Gazebo 的 world 系里，所以要有世界系的名字。 + self.declare_parameter("world_frame", "world") # ---------------- site.yaml 解析（和脚本同一套） ---------------- site = load_site_dict() @@ -194,6 +198,14 @@ class Stage2Locate(Node): servo.get("hover_offset_ee_xy_m") or [0.0, 0.0], dtype=float ).reshape(2) + # ---- 感知来源：本节点的孔位是"检测出来的"还是"场景真值算出来的" ---- + # 与 gauge_assembly_node 用**同一个解析函数**（gauge_boot.perception_source_of）： + # 两处各自读一遍 site，迟早会出现"流程以为在用真值、检测节点还在跑模型"。 + perception = deep_merge(site.get("perception") or {}, robot.get("perception") or {}) + self.perception_source = perception_source_of(perception) + self._truth_object = str((perception.get("truth") or {}).get("object") or "") + self._truth_warn_t = 0.0 + self.target_hole = ( str(self.get_parameter("target_hole").value or "").strip().lower() or str(stage2.get("target_hole") or "k1").strip().lower() @@ -854,8 +866,77 @@ class Stage2Locate(Node): if fb is not None: yield fb + def _truth_enabled(self) -> bool: + """本节点该不该走"真值孔位"。 + + 只看 ``perception.source``（site.yaml 的 robots[].perception.source 也会 + 合进来）：``truth`` = 仿真真值；``yolo``/``external`` = 老老实实跑检测器。 + 真机上永远是后者 —— 这条分支在真机不可达。 + """ + return str(getattr(self, "perception_source", "yolo")) == "truth" + + def _truth_hole_result(self, bgr: np.ndarray, depth_m=None, intr=None) -> Optional[dict]: + """真值孔位（像素 + ROI 框）。拿不到就返回 status=False 的结果并说明原因。 + + 为什么失败也**不**退回检测器：既然明确配了 ``perception.source=truth``， + 说明这台机器上"检测器给不出可信结果"（仿真里合成图过不了真机模型）。 + 退回检测器的后果是**用整幅画面当 ROI**，于是 HoughCircles 在夹爪/通规/ + 台面上找出一对"假孔"（实测间距 108mm，真值 58mm），再被门槛丢掉 —— + 现象是"每拍都丢、最后超时"，比直接说"TF 没到"难查得多。 + """ + try: + from arm_sim_gazebo import truth_holes + except ImportError: + # 真机上没有仿真包，perception.source 也不会是 truth —— 到这里只可能是 + # 有人把 source 配成 truth 却没装仿真包，直接报清楚。 + return None + T = self._lookup(self.cam_frame, str(self.get_parameter("world_frame").value or "world")) + if intr is None and self._info is not None: + k = self._info.k + intr = {"fx": float(k[0]), "fy": float(k[4]), "cx": float(k[2]), "cy": float(k[5])} + i3 = None + if intr is not None: + i3 = (intr.get("fx"), intr.get("fy"), intr.get("cx"), intr.get("cy")) + res = truth_holes.locate_holes( + T, i3, (int(bgr.shape[0]), int(bgr.shape[1])), + object_name=self._truth_object, root=None, + ) + if not res.ok: + now = time.monotonic() + if now - self._truth_warn_t > 5.0: + self._truth_warn_t = now + self.get_logger().warn(f"真值孔位不可用：{res.why}") + return { + "status": False, + "msg": f"真值孔位不可用：{res.why}", + "hole_points": [], + "hole_radii": [], + "bbox": None, + "draw_img": truth_holes.draw(bgr, res), + } + return { + "status": True, + "msg": f"真值孔位（{res.note}）", + "hole_points": [[float(p[0]), float(p[1])] for p in res.hole_points], + "hole_radii": [float(r) for r in res.hole_radii], + "bbox": [int(v) for v in res.bbox], + "label": "workpiece(truth)", + "draw_img": truth_holes.draw(bgr, res), + } + def _detect_holes_img(self, bgr: np.ndarray, *, skip_center_gate: bool, depth_m: np.ndarray = None, intr: dict = None) -> dict: + """跑一次孔检测。第二次调用（``skip_center_gate=True``）允许整幅画面当 ROI。 + + 为什么第二次要放宽到整幅：走到那一步时调用方的处境是"**没找到工件框**， + 但可能已经看得见孔了"。不放宽的话会停在"未检测到工件"上 —— 而这恰恰是 + "模型没出框、孔明明在画面里"的情形（换相机/换画面域、或模型漏检时会出现）。 + 放宽不等于乱认：环半径区间、两孔间距、居中门控这些判据都还在。 + """ + if self._truth_enabled(): + res = self._truth_hole_result(bgr, depth_m=depth_m, intr=intr) + if res is not None: + return res last = {} for det in self._iter_dets(): res = detect_holes( @@ -868,7 +949,15 @@ class Stage2Locate(Node): if (not res.get("status")) and res.get("gate_block") and not skip_center_gate: res = detect_holes( bgr, det, config=self._hole_cfg, skip_center_gate=True, - depth_m=depth_m, intr=intr, + depth_m=depth_m, intr=intr, allow_full_frame_roi=True, + ) + last = res + if res.get("status"): + return res + elif (not res.get("status")) and skip_center_gate: + res = detect_holes( + bgr, det, config=self._hole_cfg, skip_center_gate=True, + depth_m=depth_m, intr=intr, allow_full_frame_roi=True, ) last = res if res.get("status"): @@ -876,6 +965,14 @@ class Stage2Locate(Node): return last def _best_bbox(self, bgr: np.ndarray, conf: Optional[float] = None) -> Optional[Tuple[int, int, int, int]]: + if self._truth_enabled(): + # 真值模式：工件框由场景几何投出来。**必须走这条路**，不能落到检测器上： + # 检测器在仿真里出不了框，S05 的"粗居中"就会拿整幅画面当工件， + # 于是把"画面中心 ≠ 工件中心"算成 58.8mm 偏差，白白横移。 + res = self._truth_hole_result(bgr) + if res is not None and res.get("status") and res.get("bbox"): + return tuple(int(v) for v in res["bbox"]) # type: ignore[return-value] + return None thr = float(conf if conf is not None else min(0.12, float(self._hole_cfg.get("conf", 0.25)))) min_area = 4000.0 best = None diff --git a/src/arm_application/arm_application/stage3_search_node.py b/src/arm_application/arm_application/stage3_search_node.py index e4477aa..28867dc 100644 --- a/src/arm_application/arm_application/stage3_search_node.py +++ b/src/arm_application/arm_application/stage3_search_node.py @@ -33,7 +33,7 @@ import json import math import time from pathlib import Path -from typing import Any, Dict, List, Optional, Tuple +from typing import Any, Dict, List, Optional, Sequence, Tuple import numpy as np import rclpy @@ -58,6 +58,52 @@ from trajectory_msgs.msg import JointTrajectory # ==================== 纯逻辑（可离线单测） ==================== +#: 触觉帧"够新"的窗口 [s]：比它更久没更新就认为这一侧没数据。 +TACTILE_MAX_AGE_S = 0.4 + + +def live_tactile( + now: float, pairs: Sequence[Any], max_age_s: float = TACTILE_MAX_AGE_S +) -> List[Any]: + """挑出这一轮**可用**的触觉帧。``pairs`` 是每侧一个 ``(收到时刻, msg)``。 + + 判据只有两条：**够新** + ``connected=true``。 + + ⚠️ 不要拿力的**幅值**当"传感器活着"的代理（曾经写的是 ``|fnormal| > 1.0``）： + 静止时读数本来就是 0 —— 空手、还没接触、或者读数本身就是形变任意单位 + （Orisys 的量纲）—— 于是 S07 一进柔顺下压就报"触觉断流（左右指都无新鲜 + 数据）"，而话题其实以 19Hz 在发、``connected`` 也是 true。 + 上下游的契约在 ``sim_tactile_adapter`` 里写得很清楚： + "没数据"发零 **且** ``connected=false``；"零力"是 ``connected=true``。 + """ + out: List[Any] = [] + for pair in pairs or (): + if pair is None: + continue + t, msg = pair + if now - float(t) < max_age_s and bool(getattr(msg, "connected", False)): + out.append(msg) + return out + + +def tactile_timeout_reason(pairs: Sequence[Any], timeout_s: float) -> str: + """触觉等超时了 → 给一句**照着就能修**的话。 + + 两种"没数据"现象一样（都是等不到可用帧），修法却完全不同： + · 有帧到、但 ``connected=false`` → 驱动在跑，设备/SDK 那边的问题； + · 一帧都没到 / 帧全过期 → 话题或驱动根本没在发。 + 以前共用一句"断流（左右指都无新鲜数据）"，于是"设备没连上"被读成 + "话题没起来"，查错方向。 + """ + seen = [p for p in (pairs or ()) if p is not None] + if seen and not any(bool(getattr(m, "connected", False)) for _t, m in seen): + return ( + f"触觉帧有到但 connected=false（驱动在跑、设备没连上；已等 {timeout_s:.1f}s）" + f"——看 /tmp/stage3_tactile.log，或先跑 " + f"bash ~/ros2/scripts/run_tactile.sh --cli 验证" + ) + return f"触觉话题断流 >{timeout_s:.1f}s（左右指都无新鲜数据）" + def spiral_points( cx: float, cy: float, pitch_mm: float, step_mm: float, r_max_mm: float ) -> List[Tuple[float, float]]: @@ -230,32 +276,33 @@ class Stage3Search(Stage2Locate): def _tac_fresh(self, timeout_s: float = 1.0) -> Tuple[float, float, float, bool]: """等新鲜触觉帧 → (fn_avg, fn_max, shear_max, contact_any)。 - 单侧可用即可（另一指掉线/未插时用健在侧继续，阈值相对同侧基线仍自洽）。""" + 单侧可用即可（另一指掉线/未插时用健在侧继续，阈值相对同侧基线仍自洽）。 + + "可用"= **帧到了（0.4s 内）且 connected=true**，与幅值无关。 + 曾经这里写的是 ``|fnormal| > 1.0``（拿力的大小当"传感器活着"的代理）， + 那条判据在**静止读数≈0** 时必然误判：空手、还没接触、或者读数本身就是 + 形变任意单位（Orisys 的量纲）—— 于是 S07 一进柔顺下压就报 + "触觉断流 >3.0s（左右指都无新鲜数据）"，而话题其实以 19Hz 在发、 + ``connected`` 也是 true。仿真适配器早就把这两个概念分开了 + （没数据时发零 + ``connected=false``，见 sim_tactile_adapter）， + 下游必须按同一条契约判"有没有数据"。 + """ t0 = time.monotonic() while time.monotonic() - t0 < timeout_s: self._spin(0.02) - now = time.monotonic() - live = [] - for tac in (self._tac_l, self._tac_r): - if tac is None: - continue - t, m = tac - if now - t < 0.4: - live.append(m) + live = live_tactile(time.monotonic(), (self._tac_l, self._tac_r)) if live: - live_ok = [m for m in live if abs(float(m.fnormal)) > 1.0] - if not live_ok: - continue - fns = [float(m.fnormal) for m in live_ok] + fns = [float(m.fnormal) for m in live] shear = max( - math.hypot(float(m.fshearx), float(m.fsheary)) for m in live_ok + math.hypot(float(m.fshearx), float(m.fsheary)) for m in live ) return ( sum(fns) / len(fns), max(fns), shear, - any(bool(m.contact) for m in live_ok), + any(bool(m.contact) for m in live), ) + # 超时了 —— 归因写在纯函数里（两条判据、两种修法，能被测试钉住） raise SearchAbort( - f"触觉话题断流 >{timeout_s:.1f}s（左右指都无新鲜数据）", code=7 + tactile_timeout_reason((self._tac_l, self._tac_r), timeout_s), code=7 ) def _ft_mag(self) -> Optional[float]: diff --git a/src/arm_application/launch/gauge_assembly.launch.py b/src/arm_application/launch/gauge_assembly.launch.py index 4f2398c..f14c872 100755 --- a/src/arm_application/launch/gauge_assembly.launch.py +++ b/src/arm_application/launch/gauge_assembly.launch.py @@ -92,12 +92,20 @@ def _launch_setup(context, *args, **kwargs): dry_run = _flag("dry_run") holes = LaunchConfiguration("holes_total").perform(context).strip() + # 仿真里必须 true：本节点所有 header.stamp 比对和 TF 查询都按这个时钟走。 + # 忘了开的话症状是"相机没数据/TF 查不到"，看不出真正原因（脚本 --sim 会带）。 + use_sim_time = _flag("use_sim_time") backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto" result_file = LaunchConfiguration("result_file").perform(context).strip() + # 是否由本 launch 发手眼静态 TF。真机必须发（相机系与法兰之间除了标定值 + # 没有别的来源）；仿真**不能**发 —— 仿真里相机在 URDF 里就挂在法兰上， + # 再发一条同父子关系的静态 TF 会出现两个父节点（TF_MULTIPLE_AUTHORITIES）， + # 查出来的位姿在两条之间跳。脚本 --sim 会传 false。 + publish_handeye_tf = _flag("publish_handeye_tf") nodes = [] frames = [] - if os.path.isfile(handeye): + if publish_handeye_tf and os.path.isfile(handeye): try: tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye) except Exception as exc: # noqa: BLE001 @@ -118,6 +126,11 @@ def _launch_setup(context, *args, **kwargs): output="screen", ) ) + elif not publish_handeye_tf: + frames.append( + f"[gauge] 按参数不发手眼静态 TF（publish_handeye_tf=false）：" + f"相机系 {cam_frame} 由 URDF/驱动给出（仿真就是这条路径）" + ) else: frames.append( f"[gauge] 没有手眼文件（{handeye or 'paths.handeye_file 为空'}）" @@ -138,6 +151,7 @@ def _launch_setup(context, *args, **kwargs): "holes_total": int(holes) if holes else -1, "gripper_backend": backend, "result_file": result_file, + "use_sim_time": use_sim_time, } ], ) @@ -147,7 +161,8 @@ def _launch_setup(context, *args, **kwargs): 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}" + msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} " + f"backend={backend} use_sim_time={use_sim_time}" ), LogInfo(msg=f"[gauge] 结论文件 {result_file}"), *[LogInfo(msg=m) for m in frames], @@ -170,6 +185,10 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("gripper_backend", default_value="auto"), # 结论落盘路径（脚本读它判成败：ros2 launch 的退出码恒为 0） DeclareLaunchArgument("result_file", default_value="/tmp/gauge_assembly_result.json"), + # 仿真必须 true（真机 false）。理由见 _launch_setup 里的注释。 + DeclareLaunchArgument("use_sim_time", default_value="false"), + # 真机 true（相机系只能由标定值给出）；仿真 false（URDF 已给出）。 + DeclareLaunchArgument("publish_handeye_tf", default_value="true"), OpaqueFunction(function=_launch_setup), ] ) diff --git a/src/arm_application/launch/visual_servo.launch.py b/src/arm_application/launch/visual_servo.launch.py index 939e9b5..47236ed 100644 --- a/src/arm_application/launch/visual_servo.launch.py +++ b/src/arm_application/launch/visual_servo.launch.py @@ -12,7 +12,15 @@ from pathlib import Path from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + LogInfo, + OpaqueFunction, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -153,20 +161,63 @@ def _launch_setup(context, *args, **kwargs): except ValueError: pass - perception = Node( - package="arm_application", - executable="perception_node", - name="perception_node", - output="screen", - parameters=[detect_yaml, perc_params], + # ------------------------------------------------------------------ + # 感知来源：谁来产生"视觉测量"（site.yaml 的 perception.source） + # + # yolo 本体 perception_node（要 torch/ultralytics） + # truth 仿真真值节点（按场景已知位姿直接算目标点，不跑推理） + # external 外部节点按同一话题提供，本 launch 不起感知进程 + # + # 为什么要有这个开关：仿真环境（ros_gz_sim conda）没有 torch/ultralytics + # 且离线装不上，perception_node 一启动就死；而仿真里工件位姿本来就是 + # 已知的。切到 truth 后，流程能在仿真里验"控制与接口"那一半。 + # 值写错**直接报错**，不要默默当 yolo —— 那会退回到"一阶段永远不锁定"。 + # ------------------------------------------------------------------ + # 允许值与校验只有一处实现（gauge_boot.perception_source_of），S00 自检用的 + # 也是它 —— 两边各写一份的话，会出现"起了真值节点、S00 却去查 ultralytics"。 + from arm_application.gauge_boot import perception_source_of + + perc_src = perception_source_of(site.get("perception")) + # 仿真时间必须**一路传下去**：这个 launch 由装配流程（pipeline_node）在 + # S04 里以子进程拉起，父节点在仿真里带 use_sim_time=true，这里若漏传， + # perception/servo 会跑在系统时间上 —— TF 全是仿真时刻的戳，于是 + # "看得见图、算得出点，就是查不到变换"，表现为一阶段永远不收敛。 + use_sim = LaunchConfiguration("use_sim_time").perform(context).lower() in ( + "1", "true", "yes", ) + perception = None + if perc_src == "yolo": + perception = Node( + package="arm_application", + executable="perception_node", + name="perception_node", + output="screen", + parameters=[detect_yaml, perc_params, {"use_sim_time": use_sim}], + ) + elif perc_src == "truth": + truth_cfg = (site.get("perception") or {}).get("truth") or {} + perception = Node( + package="arm_sim_gazebo", + executable="sim_truth_perception", + name="sim_truth_perception", + output="screen", + parameters=[ + { + "camera_frame": cam_frame, + "target_object": str(truth_cfg.get("object") or ""), + "offset_m": [float(v) for v in (truth_cfg.get("offset_m") or [0.0, 0.0, 0.0])], + "use_sim_time": use_sim, + } + ], + ) + servo = Node( package="arm_application", executable="servo_node", name="servo_node", output="screen", - parameters=[servo_yaml, servo_params], + parameters=[servo_yaml, servo_params, {"use_sim_time": use_sim}], ) weights = perc_params.get("weights", "") @@ -176,12 +227,31 @@ def _launch_setup(context, *args, **kwargs): msg=f"[arm] {robot.get('label') or robot.get('sn')} " f"handeye={handeye}" ), - LogInfo(msg=f"[site] YOLO weights: {weights}"), - LogInfo( - msg="节点：perception_node（YOLO）| servo_node（100Hz）—— " - "各自独立进程，话题 /visual_servo/filtered_point" - ), + LogInfo(msg=f"[site] 感知来源 perception.source = {perc_src}"), ] + if perc_src == "yolo": + nodes.append(LogInfo(msg=f"[site] YOLO weights: {weights}")) + nodes.append( + LogInfo( + msg="节点：perception_node（YOLO）| servo_node（100Hz）—— " + "各自独立进程，话题 /visual_servo/filtered_point" + ) + ) + elif perc_src == "truth": + nodes.append( + LogInfo( + msg="节点：sim_truth_perception（仿真真值，不跑推理）| servo_node（100Hz）\n" + " 真值只验**控制与接口**：像素噪声/遮挡/算法错判它都没有。" + "能不能在真机上看到孔，要看真机那一次（见 devices/sim/gazebo/README.md）" + ) + ) + else: + nodes.append( + LogInfo( + msg="节点：servo_node（100Hz）。感知由**外部**节点按 " + "visual_servo/filtered_point 提供（本 launch 不起感知进程）" + ) + ) pub_tf = LaunchConfiguration("publish_handeye_tf").perform(context).lower() in ( "1", "true", "yes", ) @@ -190,7 +260,31 @@ def _launch_setup(context, *args, **kwargs): LogInfo(msg=f"静态手眼 {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]") ) nodes.append(static_tf) - nodes.extend([perception, servo]) + if perception is not None: + nodes.append(perception) + nodes.append(servo) + # 感知进程一死就整段退出（truth 后端同样适用 —— 它可能因为配置/场景读 + # 不出来而退出，那时 servo 还活着，调用方只能等满 stage1_timeout_sec）。 + # 为什么必须显式写：yolo 后端是最需要 GPU/torch 的那个（缺 ultralytics/torch + # 时**启动即死**，而 servo_node 还活着继续跑）。没有这个 handler，launch 不会 + # 结束，调用方（装配流程的 S04）只能等满 stage1_timeout_sec 才报"没锁定"—— + # 现场看到的是"算法不收敛"，真正原因是感知进程早就不在了。 + if perception is not None: + nodes.append( + RegisterEventHandler( + OnProcessExit( + target_action=perception, + on_exit=[ + LogInfo( + msg=f"[visual_servo] ✘ 感知进程（{perc_src}）退出了 —— 停止本段（" + "看它退出前最后几行：yolo 多为缺推理依赖，" + "truth 多为场景/参数读不出来）" + ), + EmitEvent(event=Shutdown(reason="perception exited")), + ], + ) + ) + ) return nodes @@ -220,6 +314,15 @@ def generate_launch_description() -> LaunchDescription: "publish_handeye_tf", default_value="true", ), + # 仿真时间开关。**仿真里必须传 true**：/clock 由 Gazebo 发， + # 节点若跑在系统时间上，TF/图像时间戳全对不上（症状是"看得见图、 + # 就是查不到变换，一阶段永远不锁定"）。 + # 装配流程在仿真里跑时由 pipeline_node 自动传 true（见 + # pipeline_node._run_stage1），手工跑时自己加上。 + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + ), DeclareLaunchArgument( "ee_z_m", default_value="", diff --git a/src/arm_application/test/test_gauge_boot.py b/src/arm_application/test/test_gauge_boot.py index 9767c56..00d28bc 100644 --- a/src/arm_application/test/test_gauge_boot.py +++ b/src/arm_application/test/test_gauge_boot.py @@ -14,12 +14,14 @@ from __future__ import annotations import pytest from arm_application.gauge_boot import ( + PERCEPTION_SOURCES, PROBES, BootCheck, blocking_failures, build_checks, format_report, optional_warnings, + perception_source_of, summary_counts, ) @@ -232,3 +234,64 @@ def test_bootcheck_defaults_to_required(): with pytest.raises(TypeError): BootCheck(key="k", label="l", probe="service", target="/x") # type: ignore[call-arg] + +def test_missing_modules_detects_a_module_that_cannot_exist(): + """依赖检查要能真的报缺（否则 detector 自检又会变成假阳性）。""" + from arm_application.gauge_boot import missing_modules + + assert missing_modules(["json"]) == [] + assert missing_modules(["__it_cannot_exist__"]) == ["__it_cannot_exist__"] + # 父包不存在时 find_spec 会抛，这同样算"缺" + assert missing_modules(["__no_such_pkg__.sub"]) == ["__no_such_pkg__.sub"] + + +def test_dependency_advice_offers_a_way_out_without_network(): + """提示必须给两条路：装它、或换掉这个后端。 + + 只写"请 pip install"在离线机器上等于没写（本机仿真环境实测 pip 直接 SSL 失败）。 + """ + from arm_application.gauge_boot import dependency_advice + + msg = dependency_advice(["ultralytics"]) + assert "ultralytics" in msg + assert "pip install" in msg + assert "真值" in msg or "外部" in msg + + +# ============================================================================= +# 感知来源（perception.source） +# ============================================================================= +def test_detector_check_only_exists_for_the_yolo_backend(): + """只有 yolo 才查推理预热；truth/external 不跑推理，查了必然是假阴性。 + + 反面做法（生成一个"永远 OK"的项）更糟：它会让人以为"感知也验过了"。 + """ + yolo = _by_key(_build(weights_path="/tmp/w.onnx", perception_source="yolo")) + assert "detector" in yolo + + for src in ("truth", "external"): + checks = _by_key(_build(weights_path="/tmp/w.onnx", perception_source=src)) + assert "detector" not in checks, f"{src} 后端不该查推理依赖" + + +def test_no_weights_path_still_means_no_detector_check(): + """没给权重路径（比如没配检测）就不生成这一项 —— 老行为不能丢。""" + assert "detector" not in _by_key(_build(weights_path="")) + + +def test_perception_source_defaults_to_yolo_when_unset(): + assert perception_source_of(None) == "yolo" + assert perception_source_of({}) == "yolo" + assert perception_source_of({"source": ""}) == "yolo" + assert perception_source_of({"source": " TRUTH "}) == "truth" + + +def test_perception_source_rejects_typo_with_the_allowed_values(): + """拼错必须报错：默默当 yolo 会让人去查推理依赖，而真因是键值写错。""" + with pytest.raises(ValueError) as e: + perception_source_of({"source": "yolo2"}) + msg = str(e.value) + assert "yolo2" in msg + for s in PERCEPTION_SOURCES: + assert s in msg + assert "site.yaml" in msg diff --git a/src/arm_application/test/test_gauge_config.py b/src/arm_application/test/test_gauge_config.py index d5fb771..27d49c9 100644 --- a/src/arm_application/test/test_gauge_config.py +++ b/src/arm_application/test/test_gauge_config.py @@ -10,6 +10,8 @@ from __future__ import annotations +from pathlib import Path + import pytest from arm_application.gauge_config import ( @@ -21,9 +23,11 @@ from arm_application.gauge_config import ( _UNITS, dataclass_keys, from_site, + snap_feature_of, missing_entries, unit_violations, ) +from arm_application.gauge_fsm import descend_iter_budget, descend_tol_m # ============================================================================= @@ -186,3 +190,78 @@ def test_describe_prints_effective_values_and_marks_calibration_needed(): assert "现场标定" in text # 稳定判据的口径要写出来，避免下次有人改成"相邻帧之差" assert "到均值的最远距离" in text + + +# ============================================================================= +# S05 稳定判据的特征来源 +# ============================================================================= +def test_snap_feature_auto_follows_perception_source(): + """auto：真机（yolo）用检测框，仿真真值/外部感知用法兰位移。 + + 为什么必须分开：检测框那一路上游是工件模型，仿真合成图过不了真机训练出来的 + 模型（实测最高分 0.017）——选错的话 S05 会永远"不稳定"地空转到超时， + 而日志看起来像"机械臂一直在晃"。 + """ + assert snap_feature_of("auto", "yolo") == "bbox" + assert snap_feature_of("auto", "truth") == "motion" + assert snap_feature_of("auto", "external") == "motion" + + +def test_snap_feature_explicit_override_wins(): + assert snap_feature_of("bbox", "truth") == "bbox" + assert snap_feature_of("motion", "yolo") == "motion" + + +def test_snap_feature_rejects_unknown_value(): + with pytest.raises(GaugeConfigError) as e: + from_site({"gauge_assembly": {"snap_stable_feature": "detect"}}) + assert "snap_stable_feature" in str(e.value) + + +def test_snap_tolerance_must_be_positive(): + with pytest.raises(GaugeConfigError): + from_site({"gauge_assembly": {"snap_tol_mm": 0.0}}) + + +def test_snap_keys_are_declared_with_units(): + """新键必须登记单位/类型，否则"单位自检"对它们就是空话。""" + assert _UNITS["snap_tol_mm"] == "mm" + assert _UNITS["snap_stable_feature"] == "" + assert "snap_stable_feature" in _UNITLESS + assert _TYPES["snap_tol_mm"] is float + assert _TYPES["snap_stable_feature"] is str + + +# ============================================================================= +# 出厂配置的 S07：步长与到位容差不能相等 +# ============================================================================= +def test_shipped_site_yaml_s07_step_cannot_deadlock(): + """**回归锚点**（2026-09-24 在仿真里踩到，真机同参数同结果）。 + + ``contact_step_mm: 0.5`` 配上一个写死的 0.5mm 到位容差时，S07 的"往下走 + 一步"会被 ``_goto`` 判成"已在容差内"→ 直接返回、一动不动；循环条件 + （实测高度还没到最深）恒真 → 静默自旋：日志停在基线那行、CPU 跑满、 + 不报错也不前进。 + + 容差现在由 ``gauge_fsm.descend_tol_m`` 从步长推出来。这条测试盯住 + **出厂 site.yaml 的真实取值**（不是默认值字典），因为现场改的正是它： + 只要容差 ≥ 步长，这里必须报红。 + """ + import yaml + + root = Path(__file__).resolve().parents[3] + doc = yaml.safe_load((root / "config" / "site.yaml").read_text(encoding="utf-8")) + sec = doc["gauge_assembly"] + cfg = from_site(doc) + + step_m = float(sec["contact_step_mm"]) / 1000.0 + assert step_m > 0.0 + assert descend_tol_m(step_m) < step_m, ( + f"出厂配置 S07 步长 {sec['contact_step_mm']}mm 的到位容差 ≥ 步长：" + "臂会一步都不走（静默自旋）" + ) + # 步数上限要够走完整个下探深度，否则"没接触"会被误报成"步数用尽" + depth_m = float(sec["contact_max_depth_mm"]) / 1000.0 + assert descend_iter_budget(depth_m, step_m) >= depth_m / step_m + # 顺带钉住"出厂配置的派生值印得出来"（describe 里会打印容差） + assert "S07" in "\n".join(cfg.describe()) diff --git a/src/arm_application/test/test_gauge_fsm.py b/src/arm_application/test/test_gauge_fsm.py index 409226b..6b7364a 100644 --- a/src/arm_application/test/test_gauge_fsm.py +++ b/src/arm_application/test/test_gauge_fsm.py @@ -23,8 +23,12 @@ from arm_application.gauge_fsm import ( GaugeContext, GaugeFsm, State, + descend_iter_budget, + descend_progress, + descend_tol_m, is_joint_limit_error, pixel_stable, + points_stable, ratchet_cycles, resolve_next, screw_stop, @@ -386,3 +390,86 @@ def test_math_hypot_precision_is_not_used_loosely(): assert math.isclose(2.0, 2.0) assert pixel_stable(samples, tol_px=2.0, min_frames=3) assert not pixel_stable(samples, tol_px=1.9, min_frames=3) + + +def test_points_stable_works_in_three_dimensions(): + """S05 在仿真里改用**法兰位移**（三维、毫米）判稳定，判据必须支持三维。 + + 同一套"到均值最远距离"的判据：换成毫米只是量纲变了，逻辑不该重写一份。 + """ + still = [(1.0, 2.0, 3.05) for _ in range(6)] + assert points_stable(still, tol=0.4, min_frames=6) + # 零均值抖动：相邻帧很近，整体在 ±5mm 之间摆 —— 必须判为不稳 + sawtooth = [(0.0, 0.0, 0.0), (0.0, 0.0, 0.0), (5.0, 0.0, 0.0), + (5.0, 0.0, 0.0), (-5.0, 0.0, 0.0), (-5.0, 0.0, 0.0)] + assert not points_stable(sawtooth, tol=0.4, min_frames=6) + + +def test_points_stable_rejects_ragged_samples(): + """样本维数不一致时必须判不稳（否则会按第一个样本的维数静默比较）。 + + 真实来源是"两种特征混进同一个窗口"：比如特征从 bbox 切成 motion 时， + 残留的二维样本会被当成三维来算 —— 与其猜，不如直接判不稳重采。 + """ + assert not points_stable([(1.0, 2.0), (1.0, 2.0, 3.0)] * 3, tol=1.0, min_frames=6) + + +def test_points_stable_needs_enough_frames(): + assert not points_stable([(0.0, 0.0, 0.0)] * 5, tol=10.0, min_frames=6) + assert points_stable([(0.0, 0.0, 0.0)] * 6, tol=10.0, min_frames=6) + + +# ============================================================================= +# S07 步进下压：容差 < 步长（否则 _goto 空转，一步都不走） +# ============================================================================= +@pytest.mark.parametrize("step_mm", [0.05, 0.1, 0.5, 1.0, 2.0, 4.0]) +def test_descend_tol_is_strictly_smaller_than_the_step(step_mm): + """**回归测试**（2026-09-24 仿真踩到过）： + + 原实现把 ``_goto`` 的到位容差写死 0.5mm，而 ``contact_step_mm`` 也是 0.5mm。 + ``_goto`` 的第一件事就是"已落在容差内 → 直接返回、不动"，于是 S07 + 每次"往下走一步"都变成原地不动：循环条件恒真、日志停在基线那行、 + CPU 跑满、不报错也不前进 —— 典型的静默自旋（规则 R5.1）。 + + 纪律：单步下压的容差必须**严格小于**步长，这是循环能推进的前提， + 不是调参风格的偏好。 + """ + step_m = step_mm / 1000.0 + assert descend_tol_m(step_m) < step_m + + +def test_descend_tol_has_a_floor_for_tiny_steps(): + """步长极小时容差不能退化成 0：伺服追不到 0，会永远"差一点点"。""" + tol = descend_tol_m(0.00001) # 10µm 的步长（比任何现实步长都小） + assert 0.0 < tol <= 0.00001 * 0.5 + + +def test_descend_tol_rejects_nonpositive_step(): + with pytest.raises(ValueError): + descend_tol_m(0.0) + with pytest.raises(ValueError): + descend_tol_m(-0.0005) + + +def test_descend_iter_budget_covers_the_whole_depth(): + """步数上限必须够走完整个下探深度（否则会在"还没到底"时被上限截断）。""" + step_m, depth_m = 0.0005, 0.025 + assert descend_iter_budget(depth_m, step_m) >= math.ceil(depth_m / step_m) + # 但也不能是无限：这正是"臂没动时不要把节点转成电暖器"的那道闸 + assert descend_iter_budget(depth_m, step_m) < 200 + + +def test_descend_iter_budget_zero_depth_means_no_iterations(): + assert descend_iter_budget(0.0, 0.0005) == 0 + with pytest.raises(ValueError): + descend_iter_budget(0.025, 0.0) + + +def test_descend_progress_only_counts_real_downward_motion(): + """推进的证据是**实测位移**：指令发出去不等于臂动了。""" + step_m = 0.0005 + assert descend_progress(0.6316, 0.6311, step_m) # 走满一步 + assert descend_progress(0.6316, 0.63146, step_m) # 只走了 0.14mm 也算动了 + assert not descend_progress(0.6316, 0.63159, step_m) # 只走了 0.01mm：被死区吃掉 + assert not descend_progress(0.6316, 0.6316, step_m) # 完全没动（空转） + assert not descend_progress(0.6316, 0.6317, step_m) # 往上跑 ≠ 推进 diff --git a/src/arm_application/test/test_hole_detector.py b/src/arm_application/test/test_hole_detector.py index 94f4ae3..6d42475 100644 --- a/src/arm_application/test/test_hole_detector.py +++ b/src/arm_application/test/test_hole_detector.py @@ -84,3 +84,56 @@ def test_default_model_path_is_canonical_name(): def test_default_model_path_is_absolute(): """必须是绝对路径：相对路径会随 CWD 漂移，节点从不同目录启动就找不到。""" assert _default_model_path().is_absolute() + + +# --------------------------------------------------------------------------- +# detect_holes：没有工件框时，调用方可以要求"整幅画面当 ROI" +# --------------------------------------------------------------------------- +class _NoDetector: + """一个什么都检测不到的"检测器"（模型漏检 / 与画面域不匹配时的替身）。""" + + def detect(self, *_a, **_k): + return [] + + +def test_detect_holes_needs_a_bbox_unless_asked_otherwise(): + """默认行为不变：没有工件框就直接报「未检测到工件」。 + + 这条是给真机路径兜底的：默认不能被"整幅画面当 ROI"悄悄改掉 —— ROI 一放宽， + 画面里别的圆（工装、夹爪上的特征）都有可能被当成孔。 + """ + import cv2 + import numpy as np + + from arm_application.hole_detect_hough import detect_holes, load_config + + blank = np.full((200, 200, 3), 120, np.uint8) + res = detect_holes( + blank, _NoDetector(), config=load_config(), skip_center_gate=True + ) + assert res["status"] is False + assert res["msg"] == "未检测到工件" + + +def test_detect_holes_full_frame_roi_gets_past_the_bbox_gate(): + """``allow_full_frame_roi=True`` 时，没框也要继续往下走（走到找孔那一步）。 + + 判据用"失败原因变了"：从「未检测到工件」（卡在 ROI）变成「HoughCircles + 未检测到圆孔」（已经真的找过孔了）。用空白图是有意的 —— 它保证结论只 + 来自"是否跳过了 ROI 要求"，不掺杂检测器本身好不好用。 + """ + import cv2 + import numpy as np + + from arm_application.hole_detect_hough import detect_holes, load_config + + blank = np.full((200, 200, 3), 120, np.uint8) + res = detect_holes( + blank, + _NoDetector(), + config=load_config(), + skip_center_gate=True, + allow_full_frame_roi=True, + ) + assert res["status"] is False + assert res["msg"] != "未检测到工件" diff --git a/src/arm_application/test/test_pipeline_stage1_diag.py b/src/arm_application/test/test_pipeline_stage1_diag.py new file mode 100644 index 0000000..56c565c --- /dev/null +++ b/src/arm_application/test/test_pipeline_stage1_diag.py @@ -0,0 +1,42 @@ +"""一阶段超时的**归因**：是"感知没给点"还是"给了点没收敛"。 + +两者现象完全一样（都是"没锁定"），排查方向却相反： + +* 一个点都没收到 → 感知侧（缺依赖 / 真值节点没起 / 没人发）； +* 收到了点没收敛 → 控制与几何侧（工件不在视野、手眼、工作空间）。 + +以前共用一句"没锁定（视野里没工件？）"，于是"perception_node 缺 +ultralytics 早退了"被读成"算法不收敛"，白查半天。这里把这句话钉住。 +""" + +from __future__ import annotations + +import pytest + +from arm_application.pipeline_node import stage1_timeout_reason + + +def test_zero_points_blames_perception_not_calibration(): + msg = stage1_timeout_reason(0, 150.0, "/tmp/pipeline_stage1.log") + assert "一个视觉目标点都没收到" in msg + # 必须逐后端给出对号入座的查法，否则人还是不知道下一步做什么 + assert "yolo" in msg and "truth" in msg and "external" in msg + assert "ultralytics" in msg # yolo 后端最常见的原因 + assert "perception.source" in msg # 指明这个开关怎么用 + # 不能把人往"标定/视野"上带 + assert "不是标定" in msg + + +def test_points_seen_but_no_convergence_blames_geometry(): + msg = stage1_timeout_reason(123, 150.0, "/tmp/pipeline_stage1.log") + assert "123" in msg + assert "没收敛" in msg + assert "手眼" in msg + # 这时**不该**再提感知依赖 + assert "ultralytics" not in msg + + +def test_timeout_seconds_and_log_path_are_in_the_message(): + msg = stage1_timeout_reason(0, 42.0, "/tmp/x.log") + assert "42s" in msg + assert "/tmp/x.log" in msg diff --git a/src/arm_application/test/test_site_arm_sections.py b/src/arm_application/test/test_site_arm_sections.py new file mode 100644 index 0000000..29f32af --- /dev/null +++ b/src/arm_application/test/test_site_arm_sections.py @@ -0,0 +1,96 @@ +"""site.yaml 的**机器人级覆盖**必须登记在案。 + +site.yaml 有两层：顶层是站点配置，``robots[]`` 里每台臂可以覆盖若干**段** +（camera/paths/servo/…）。合并只发生在 ``apply_active_arm`` 的 +``_ARM_SECTION_KEYS`` 上 —— 没登记进那张表的段，**覆盖会静默失效**： +文件里写着 ``source: truth``，合出来还是 yolo。 + +这个坑踩过两次（yolo、perception），两次的现象都是"配置明明改了，行为没变"， +而且不报错。所以这里钉一条结构性的规矩： + + robots[] 里任何**嵌套映射**（段）都必须出现在 _ARM_SECTION_KEYS 里。 + +标量键（sn/arm/label/ip/tool_name/tactile…）由代码逐个读，不参与合并，不受此限。 +""" + +from __future__ import annotations + +import sys +from pathlib import Path +from typing import Iterable, List, Mapping + +import pytest +import yaml + +# 测试在 src/<包>/test/ 下 → 仓库根是 parents[3]（[0]=test,[1]=包,[2]=src） +ROOT = Path(__file__).resolve().parents[3] +SCRIPTS_LIB = ROOT / "scripts" / "lib" + + +def robot_entries(doc: Mapping) -> List[Mapping]: + """site.yaml 里的机器人列表（``robot.robots``）。 + + ⚠️ 路径是 **robot.robots**，不是顶层的 robots。写成 ``doc.get("robots")`` + 会拿到 None → 循环一次都不跑 → 测试"永远通过"（这个假通过本文件就踩过一次）。 + 所以下面那个函数一律经过这里取，并且断言列表非空。 + """ + entries = ((doc.get("robot") or {}).get("robots")) or [] + assert entries, "site.yaml 里 robot.robots 是空的 —— 键路径写错了？" + return list(entries) + + +def unregistered_sections(robots: Iterable[Mapping], keys: Iterable[str]) -> List[str]: + """哪些机器人级的"段"没登记进合并表（返回 ``SN.段名``）。""" + known = set(keys) + bad: List[str] = [] + for robot in robots: + for key, val in dict(robot).items(): + if isinstance(val, dict) and key not in known: + bad.append(f"{robot.get('sn')}.{key}") + return bad + + +@pytest.fixture(scope="module") +def arm_section_keys() -> tuple[str, ...]: + sys.path.insert(0, str(SCRIPTS_LIB)) + import export_site_env # type: ignore + + return tuple(export_site_env._ARM_SECTION_KEYS) + + +def test_every_robot_level_section_is_registered_for_merging(arm_section_keys): + doc = yaml.safe_load((ROOT / "config" / "site.yaml").read_text(encoding="utf-8")) + bad = unregistered_sections(robot_entries(doc), arm_section_keys) + assert not bad, ( + "这些机器人级段没登记进 _ARM_SECTION_KEYS，覆盖会**静默失效**：" + f"{bad}\n" + " 修法：scripts/lib/export_site_env.py 的 _ARM_SECTION_KEYS 里加上它们。" + ) + + +def test_robot_entries_refuses_a_wrong_key_path(): + """取错键路径必须**报错**，不能静默返回空列表（那会让自检永远通过）。""" + with pytest.raises(AssertionError): + robot_entries({"robots": [{"sn": "x"}]}) # 少了外层 robot: + + +def test_the_predicate_can_actually_fail(arm_section_keys): + """判据本身要能报错：否则上面那条可能只是"永远通过"。""" + bad = unregistered_sections( + [{"sn": "x", "camera": {"name": "c"}, "brand_new_section": {"a": 1}}], + arm_section_keys, + ) + assert bad == ["x.brand_new_section"] + + +def test_sim_robot_carries_the_truth_backend_override(arm_section_keys): + """仿真那条 ``perception.source: truth`` 是真的会被合并进去的。 + + 这条同时钉住"两份配置别漂移"：site.yaml 里删掉了它， + 仿真就会退回 yolo 去跑推理（然后因为缺依赖起不来）。 + """ + doc = yaml.safe_load((ROOT / "config" / "site.yaml").read_text(encoding="utf-8")) + sim = [r for r in robot_entries(doc) if r.get("sn") == "sim"] + assert sim, "site.yaml 里没有 sim 机器人 —— 仿真流程会挑不到配置" + src = (sim[0].get("perception") or {}).get("source") + assert src == "truth", f"sim 的 perception.source 应为 truth，现在是 {src!r}" diff --git a/src/arm_application/test/test_stage3_tactile_live.py b/src/arm_application/test/test_stage3_tactile_live.py new file mode 100644 index 0000000..627d02a --- /dev/null +++ b/src/arm_application/test/test_stage3_tactile_live.py @@ -0,0 +1,79 @@ +"""触觉"有没有数据"的判据：为什么不能拿力的大小当传感器活着的代理。 + +背景（真实踩到）：S07 一进柔顺下压就报 + + ERROR S07 触觉断流，无法判断接触：触觉话题断流 >3.0s（左右指都无新鲜数据） + +而同一时刻 ``ros2 topic hz /tactile/left/status`` 是 19Hz、``connected: true``、 +``fnormal ≈ 1.3e-05``。旧判据是 ``|fnormal| > 1.0`` —— 把"力小"当成了"没数据"， +于是**空手/未接触/形变任意单位**这几种正常情形全都判成断流。 + +契约（``sim_tactile_adapter`` 里写明、真机 ``tactile_node`` 同款）： + + 没数据 → 发零 **且** connected=false + 零 力 → connected=true + fnormal≈0 + +所以判据只能是"够新 + connected"，与幅值无关。 +""" + +from __future__ import annotations + +import time +from dataclasses import dataclass + +from arm_application.stage3_search_node import ( + TACTILE_MAX_AGE_S, + live_tactile, + tactile_timeout_reason, +) + + +@dataclass +class _Msg: + fnormal: float = 0.0 + connected: bool = True + + +def test_zero_force_but_connected_is_live_data(): + """静止读数≈0 是**有效数据**，不是断流。""" + now = time.monotonic() + live = live_tactile(now, [(now, _Msg(fnormal=1.3e-05, connected=True)), None]) + assert len(live) == 1 + assert live[0].fnormal == 1.3e-05 + + +def test_disconnected_frames_are_not_live_even_if_they_keep_arriving(): + """帧一直在到、但 connected=false（设备没连上）→ 不算可用。""" + now = time.monotonic() + assert live_tactile(now, [(now, _Msg(connected=False)), (now, _Msg(connected=False))]) == [] + + +def test_stale_frames_are_not_live(): + now = time.monotonic() + old = now - TACTILE_MAX_AGE_S - 0.01 + assert live_tactile(now, [(old, _Msg(connected=True))]) == [] + + +def test_both_sides_live_keeps_both(): + now = time.monotonic() + live = live_tactile(now, [(now, _Msg(fnormal=3.0)), (now, _Msg(fnormal=5.0))]) + assert [m.fnormal for m in live] == [3.0, 5.0] + + +def test_timeout_reason_separates_device_problem_from_no_publisher(): + """两种"没数据"必须报**不同**的话：一个查驱动/设备，一个查谁在发话题。""" + now = time.monotonic() + dev = tactile_timeout_reason([(now, _Msg(connected=False)), None], 3.0) + assert "connected=false" in dev + assert "run_tactile.sh" in dev # 给出下一步做什么 + assert "断流" not in dev # 不能把人往"话题没起来"上带 + + none = tactile_timeout_reason([None, None], 3.0) + assert "断流" in none and "3.0" in none + + +def test_timeout_reason_still_reports_stall_when_only_stale_frames_exist(): + """帧过期（驱动卡住）仍然算"断流"，不该被说成设备没连上。""" + old = time.monotonic() - 10.0 + msg = tactile_timeout_reason([(old, _Msg(connected=True)), None], 3.0) + assert "断流" in msg