应用层修复：S00 自检假阴性、S07 触觉假断流、S07 下探静默卡死
三处都是"看着像硬件问题、其实是判据问题"的假警报：
1. S00 自检假阴性（gauge_boot.py）
探测等待时用 time.sleep 不 spin → 订阅回调根本没机会执行，
于是**健康的硬件也会被判"没有数据"**。改成等待期间持续 spin。
2. S07 触觉假断流（stage3_search_node.py）
判据原本是 |fnormal| > 1.0，但力本来就可能是零或很小（真机轻触、
仿真刚接触）→ 明明在发数据却判"触觉断流"。
改成"数据够新（TACTILE_MAX_AGE_S 内）+ connected" —— 判活看的是
**有没有在发**，不是**数值大不大**。
3. S07 下探静默卡死（gauge_fsm.py / gauge_assembly_node.py）
_goto 的到位容差 ≥ 步长时，每一步都被判成"已在容差内"，臂一步都不走，
而外层只看到"没进度"→ 死循环到超时，日志里看不出原因。
新增 descend_tol_m / descend_iter_budget / descend_progress：
容差从步长推导（不该手工调大），并显式判"连续 N 步零位移"。
容差 ≥ 步长的反例已进 site 回归测试，防止再犯。
4. site 分段化（export_site_env.py）：89/91 两臂的 servo/handeye/点位
各一段，避免"改 91 影响 89"。
另：run_unit_tests.sh 作为统一测试入口（932→933 项），
devices/sim/gazebo/README.md 同步。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -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` 的 `<gravity>` |
|
||||
| 关节前缀 | `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 的物体位姿 + 模型 <visual name="*_ring"> 的局部位置
|
||||
孔像素 = 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. 现状与进度
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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，这里的清理就没了；
|
||||
|
||||
Executable
+44
@@ -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
|
||||
@@ -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("/")
|
||||
"""手外相机的命名空间：``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")
|
||||
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
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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），不依赖检测器"
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
|
||||
@@ -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：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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]:
|
||||
|
||||
@@ -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),
|
||||
]
|
||||
)
|
||||
|
||||
@@ -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,12 +161,55 @@ def _launch_setup(context, *args, **kwargs):
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 感知来源：谁来产生"视觉测量"（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],
|
||||
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(
|
||||
@@ -166,7 +217,7 @@ def _launch_setup(context, *args, **kwargs):
|
||||
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=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="",
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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())
|
||||
|
||||
@@ -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) # 往上跑 ≠ 推进
|
||||
|
||||
@@ -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"] != "未检测到工件"
|
||||
|
||||
@@ -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
|
||||
@@ -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}"
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user