Update workstation UI, ROS task control and device integration
This commit is contained in:
cheng
Binary file not shown.
|
Before Width: | Height: | Size: 354 KiB After Width: | Height: | Size: 1016 KiB |
@@ -0,0 +1,4 @@
|
||||
# Project-local ROS discovery defaults. Apply by sourcing scripts/env.sh.
|
||||
# Keep this project off the network-wide default domain 0.
|
||||
ORISYS_ROS_DOMAIN_ID=42
|
||||
ORISYS_ROS_LOCALHOST_ONLY=1
|
||||
+42
-38
@@ -378,9 +378,9 @@ camera:
|
||||
name: "d405"
|
||||
serial: "352122274408" # 全局缺省=左腕；真机以 robots[].camera.serial 为准
|
||||
# USB3 推荐 848x480@30；USB2 自动降到 640x480@15，深度/右目仍开
|
||||
width: 848
|
||||
height: 480 # 改宽高/帧率必须重启相机：bash ~/ros2/scripts/run_camera_d405.sh --preview
|
||||
fps: 30
|
||||
width: 1280
|
||||
height: 720
|
||||
fps: 15
|
||||
enable_depth: true
|
||||
enable_infra: true # 同一 SDK 进程开左右红外；失败则自动退回彩图+深度
|
||||
preview: true
|
||||
@@ -401,8 +401,9 @@ cameras:
|
||||
role: left_wrist
|
||||
arm: left
|
||||
label: 左腕D405（89 平行夹爪）
|
||||
width: 848 # 相机分辨率以本段为准（全局 camera: 段为旧路径，P2 收敛）
|
||||
height: 480
|
||||
width: 1280
|
||||
height: 720
|
||||
fps: 15
|
||||
emitter_enabled: false # 腕部必须关投射器，红外点阵会污染跟踪图像
|
||||
d405_right:
|
||||
model: d405
|
||||
@@ -411,8 +412,9 @@ cameras:
|
||||
role: right_wrist
|
||||
arm: right
|
||||
label: 右腕D405（91 旋转夹爪）
|
||||
width: 848
|
||||
height: 480
|
||||
width: 1280
|
||||
height: 720
|
||||
fps: 15
|
||||
emitter_enabled: false
|
||||
# >>> 仿真那两台（arm: sim）<<<
|
||||
# 为什么仿真也要在这里登记：应用是"按当前臂"去认相机的 —— 腕部相机取
|
||||
@@ -447,11 +449,10 @@ cameras:
|
||||
role: overhead_eye_to_hand
|
||||
label: 双臂中间手外D435
|
||||
name: d435
|
||||
# 本机 D435 与 D405 挂在同一个 5Gbps 外接 hub 上：D435 跑 1280x720@30 时
|
||||
# D405 只剩 4~6fps 并反复断流重连（二阶段 snap_coarse_1 因此超时）；
|
||||
# 停掉 D435 后 D405 立即稳定 30fps。D435 只做一次性顶视，848x480@15 足够。
|
||||
width: 848
|
||||
height: 480
|
||||
# 彩色 1080p/15，原生深度由采集器选择兼容的 720p/15 并对齐。
|
||||
# 与 D405 共用 5Gbps Hub；两路同时运行时需核对实际帧率与断流日志。
|
||||
width: 1920
|
||||
height: 1080
|
||||
fps: 15
|
||||
auto_exposure: true
|
||||
ae_limit_us: 8000
|
||||
@@ -772,6 +773,7 @@ hole_detect:
|
||||
once_ff_look_per_m: 0.0
|
||||
final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY
|
||||
# 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
|
||||
hole_entry_mm: 1.0 # 同步上游：对孔收尾规尖进入上表面下 1mm
|
||||
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
|
||||
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
|
||||
# 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
|
||||
@@ -799,7 +801,7 @@ hole_detect:
|
||||
max_depth_m: 0.60
|
||||
linger_sec: 8.0 # 完成后继续发布结果/叠图的时长
|
||||
|
||||
# ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
|
||||
# ===== 旧 stage3_search_node 的力阈值；首页旋转已使用独立 a7_rotation =====
|
||||
# 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。
|
||||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
|
||||
stage3:
|
||||
@@ -919,8 +921,10 @@ gauge_assembly:
|
||||
# ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
|
||||
# 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
|
||||
extreme_angle_deg: 150.0 # 想转多少度（按工具系逆时针）
|
||||
extreme_speed_deg_s: 20.0 # 关节角速度 [deg/s]，慢一点防冲击
|
||||
extreme_direction: 1 # +1=逆时针 / -1=顺时针（改符号，别去改代码）
|
||||
extreme_speed_deg_s: 10.0 # 与上游 A7 首轮顺时针进孔工序一致
|
||||
extreme_direction: -1 # 上游本机实测：A7 角度减小为逆时针
|
||||
unwind_open_permille: 400 # 每轮松爪开度，千分比
|
||||
unwind_lift_mm: 3.0 # 每轮微抬，降回使用本轮实测值
|
||||
extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
|
||||
# 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"
|
||||
|
||||
@@ -1085,30 +1089,21 @@ gauge_assembly:
|
||||
# 不灵敏（压很深才停）→ 调小；同时看启动日志里的基线/实测值再定。
|
||||
|
||||
tactile:
|
||||
backend: "orisys" # orisys | mock
|
||||
# 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
|
||||
model: "dd03"
|
||||
# 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
|
||||
config_file: "devices/sensors/orisys/models/dd03/config.json"
|
||||
# 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
|
||||
# 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
|
||||
left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0"
|
||||
right_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.2:1.0-video-index0"
|
||||
cuda: false # 两路进程抢 GPU 上下文会互相拖慢；CPU 解算仅 ~9ms/帧，足够
|
||||
isstitch: true
|
||||
infer_backend: "cpu" # cupy | opencl | cpu；双路并行用 cpu 最稳
|
||||
baseline_seconds: 2.0
|
||||
filter_alpha: 0.25
|
||||
motion_threshold: 0.0
|
||||
backend: "station_flow"
|
||||
arm_id: "89"
|
||||
# Only low-level optical tactile SDK; no imports from the example gripper app.
|
||||
sdk_python_path: "/home/cheng/orisys-sdk/src"
|
||||
config_file: "src/arm_application/config/tactile/ctag2f90d.json"
|
||||
# auto only selects Sunplus 1bcf tactile UVC cameras; explicit by-path recommended.
|
||||
# Change these two values to stable device paths to fix the physical left/right order.
|
||||
left_video: "auto"
|
||||
right_video: "auto"
|
||||
target_fps: 30.0
|
||||
publish_hz: 20.0
|
||||
# 视触觉图像与力矢量场的发布频率。0 = 关闭（默认）。
|
||||
# 三力走 30fps 的轻量队列；图像每帧几百 KB，单独走一条 maxsize=1 的队列，
|
||||
# 打开它不会影响三力的时序。需要看形变图/矢量场时设成 5~10 即可。
|
||||
image_hz: 0.0
|
||||
rotation_config: "" # 左右共用；有分指文件则用下面两项
|
||||
left_rotation: "" # 例：orisys SDK 的 rotation_config_0.json
|
||||
right_rotation: ""
|
||||
publish_hz: 30.0
|
||||
preview_transport: "compressed_flow"
|
||||
motion_threshold: 0.0
|
||||
cuda: false
|
||||
force_units: "raw"
|
||||
|
||||
go_home:
|
||||
pose: "home"
|
||||
@@ -1144,3 +1139,12 @@ sim:
|
||||
world: "devices/sim/gazebo/worlds/assembly.world"
|
||||
physics_enable: true
|
||||
joint_prefix: "sim_" # 仿真关节前缀，必须与真实臂前缀区分
|
||||
|
||||
# 仅 Qt 预览读取远端相机；机械臂/夹爪/传感器继续使用本地控制域。
|
||||
remote_camera_preview:
|
||||
enabled: true
|
||||
host: "192.168.2.10"
|
||||
interface_address: "192.168.2.50"
|
||||
domain: 0
|
||||
cameras: [d405_left, d435]
|
||||
transport: raw # 远端当前没有 JPEG 预览话题
|
||||
|
||||
@@ -0,0 +1,61 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Offline STEP -> triangle cache for Qt OpenGL. No robot/ROS/device operations.
|
||||
|
||||
Requires gmsh (OpenCascade). Runtime GUI needs only numpy and the generated NPZ.
|
||||
"""
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
import gmsh
|
||||
import numpy as np
|
||||
|
||||
|
||||
def convert(source, output):
|
||||
source=Path(source).resolve();output=Path(output).resolve()
|
||||
gmsh.initialize(['step-converter','-nopopup'])
|
||||
try:
|
||||
gmsh.option.setNumber('General.Terminal',0)
|
||||
gmsh.option.setString('Geometry.OCCTargetUnit','MM')
|
||||
gmsh.model.add('gripper')
|
||||
gmsh.model.occ.importShapes(str(source));gmsh.model.occ.synchronize()
|
||||
gmsh.option.setNumber('Mesh.MeshSizeMin',.8)
|
||||
gmsh.option.setNumber('Mesh.MeshSizeMax',2.)
|
||||
gmsh.option.setNumber('Mesh.MeshSizeFromCurvature',18)
|
||||
gmsh.option.setNumber('Mesh.ElementOrder',1)
|
||||
gmsh.model.mesh.generate(2)
|
||||
tags,coords,_=gmsh.model.mesh.getNodes()
|
||||
order=np.argsort(tags);sorted_tags=tags[order]
|
||||
nodes=np.asarray(coords).reshape(-1,3)[order]
|
||||
parts=[];colors=[];labels=[]
|
||||
surfaces=gmsh.model.getEntities(2)
|
||||
for part,(_,tag) in enumerate(surfaces):
|
||||
kinds,_,element_nodes=gmsh.model.mesh.getElements(2,tag)
|
||||
r,g,b,a=gmsh.model.getColor(2,tag)
|
||||
base=np.array([r,g,b],float)/255.
|
||||
if np.max(base)-np.min(base)<.03: base=np.array([.76,.79,.83])
|
||||
for kind,indices in zip(kinds,element_nodes):
|
||||
if kind!=2: raise ValueError('Only first order triangles are supported')
|
||||
indices=np.asarray(indices).reshape(-1,3)
|
||||
triangles=nodes[np.searchsorted(sorted_tags,indices)]
|
||||
parts.append(triangles/1000.)
|
||||
colors.append(np.tile(base,(len(triangles),1)))
|
||||
labels.append(np.full(len(triangles),part,np.int32))
|
||||
triangles=np.concatenate(parts).astype(np.float32)
|
||||
rgb=np.concatenate(colors).astype(np.float32)
|
||||
if not np.isfinite(triangles).all() or not len(triangles):raise ValueError('Invalid mesh')
|
||||
lo=triangles.min(axis=(0,1));hi=triangles.max(axis=(0,1))
|
||||
output.parent.mkdir(parents=True,exist_ok=True)
|
||||
np.savez_compressed(output,triangles=triangles,colors=rgb,surface=np.concatenate(labels))
|
||||
info=dict(source=source.name,source_sha256=hashlib.sha256(source.read_bytes()).hexdigest(),
|
||||
units='m',source_units='mm',triangles=len(triangles),surfaces=len(surfaces),
|
||||
solids=len(gmsh.model.getEntities(3)),bounds_m=[lo.tolist(),hi.tolist()],
|
||||
dimensions_mm=((hi-lo)*1000).tolist(),articulated=False,
|
||||
note='Original assembled STEP pose; no inferred joints or live finger animation.')
|
||||
output.with_suffix('.json').write_text(json.dumps(info,ensure_ascii=False,indent=2)+'\n')
|
||||
print(json.dumps(info,ensure_ascii=False))
|
||||
finally:gmsh.finalize()
|
||||
|
||||
if __name__=='__main__':
|
||||
parser=argparse.ArgumentParser();parser.add_argument('source');parser.add_argument('output')
|
||||
args=parser.parse_args();convert(args.source,args.output)
|
||||
@@ -18,15 +18,34 @@ ACK = Path("/tmp/flexiv_movel_ack")
|
||||
DONE = Path("/tmp/flexiv_movel_done")
|
||||
|
||||
|
||||
def format_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> str:
|
||||
return (
|
||||
def format_request(
|
||||
seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
|
||||
zone: str = "", toler: int = -1,
|
||||
) -> str:
|
||||
"""一行请求。不带 ``zone`` 时仍是 6 个数，插孔和粗定位保持厂家默认到位。
|
||||
|
||||
微升降要带过渡区和到位等级。厂家默认 ``Z50``（50 mm）加等级 3，
|
||||
几毫米的直线会在还差约 0.5 mm 时就报到位。合法的最细过渡区是 ``ZFine``，
|
||||
最严且仍会检查到位的等级是 1（0 表示不检查）。
|
||||
"""
|
||||
if bool(zone) != (int(toler) >= 0):
|
||||
raise ValueError("MoveL 过渡区和到位等级必须一起给")
|
||||
if zone and (zone != "ZFine" or int(toler) != 1):
|
||||
raise ValueError(f"MoveL 微升降只接受 ZFine 和到位等级 1，收到 {zone} {toler}")
|
||||
line = (
|
||||
f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
|
||||
f"{1 if keep_z else 0} {float(vel):.4f}\n"
|
||||
f"{1 if keep_z else 0} {float(vel):.4f}"
|
||||
)
|
||||
if zone:
|
||||
line += f" {zone} {int(toler)}"
|
||||
return line + "\n"
|
||||
|
||||
|
||||
def write_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> None:
|
||||
text = format_request(seq, x, y, z, keep_z, vel)
|
||||
def write_request(
|
||||
seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
|
||||
zone: str = "", toler: int = -1,
|
||||
) -> None:
|
||||
text = format_request(seq, x, y, z, keep_z, vel, zone, toler)
|
||||
tmp = REQ.with_name(REQ.name + ".tmp")
|
||||
tmp.write_text(text, encoding="utf-8")
|
||||
tmp.replace(REQ)
|
||||
|
||||
+31
-42
@@ -1,48 +1,37 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# scripts/lib/ros_domain.sh —— 真机与仿真的 DDS 域隔离
|
||||
#
|
||||
# 为什么要单独抽一层
|
||||
# ----------------
|
||||
# 仿真侧（scripts/sim_env.sh）**强制** ROS_DOMAIN_ID=77，并且注释里写明了
|
||||
# "隔离不能靠『环境里恰好没设』"。真机侧当时没有对应保护，于是同一个坑换个
|
||||
# 方向又踩了一次（2026-09-28 实测）：
|
||||
#
|
||||
# 先 source 过 sim_env.sh 的 shell（ROS_DOMAIN_ID=77 还 export 着）
|
||||
# 再起真机栈 → 真机栈落到**仿真的域**上 → 两边节点名撞车
|
||||
# （/controller_manager、/robot_state_publisher、/arm_status_node…）、
|
||||
# /joint_states 与 /tf 各有两个来源互相盖。
|
||||
#
|
||||
# 现象极具误导性：控制器 feedback 与 reference 完全一致（控制器没坏），
|
||||
# 但 TF 显示末端**一动不动** —— 顺着现象查会一路查到 IK / TF / 时钟上，
|
||||
# 全是无辜的。
|
||||
#
|
||||
# 真机侧有两个入口（scripts/env.sh 给交互 shell 用；scripts/run_real.sh 自己
|
||||
# 起 conda + install），两处都得有这个纠正，所以放在这里共用一份。
|
||||
#
|
||||
# 用法（在 `set -uo pipefail` 或 `set -eo pipefail` 之后 source）：
|
||||
# source ".../lib/ros_domain.sh"
|
||||
# ros_domain_force_real
|
||||
# =============================================================================
|
||||
|
||||
# 仿真的域。改这里要同步 scripts/sim_env.sh（那边是硬编码的 77）。
|
||||
# One explicit discovery policy for the station and every child launch.
|
||||
# The inherited generic ROS_DOMAIN_ID is never silently adopted.
|
||||
ROS_SIM_DOMAIN=77
|
||||
|
||||
# 把 ROS_DOMAIN_ID 纠正到"真机该用的域"。
|
||||
# · REAL_ROS_DOMAIN_ID 显式设了 → 用它（要换域就走这个，名字里带 REAL，
|
||||
# 不会被仿真环境的 export 撞上）；
|
||||
# · 环境里是 77（仿真的域）→ **喊出来**再改回 0；
|
||||
# · 环境里没设 → 显式写 0，让人一眼能从 `env` 里看出真机跑在哪个域。
|
||||
ros_domain_force_real() {
|
||||
if [ -n "${REAL_ROS_DOMAIN_ID:-}" ]; then
|
||||
export ROS_DOMAIN_ID="${REAL_ROS_DOMAIN_ID}"
|
||||
elif [ "${ROS_DOMAIN_ID:-0}" = "${ROS_SIM_DOMAIN}" ]; then
|
||||
echo "[domain] ⚠ 环境里的 ROS_DOMAIN_ID=${ROS_SIM_DOMAIN} 是**仿真**用的域" >&2
|
||||
echo "[domain] （scripts/sim_env.sh 强制那个值）。已改回 0 起真机栈 ——" >&2
|
||||
echo "[domain] 真机与仿真同域会让节点名/话题互相盖，现象是'臂没动'" >&2
|
||||
echo "[domain] 而控制器一切正常。要另指定请设 REAL_ROS_DOMAIN_ID。" >&2
|
||||
export ROS_DOMAIN_ID=0
|
||||
elif [ -z "${ROS_DOMAIN_ID:-}" ]; then
|
||||
export ROS_DOMAIN_ID=0
|
||||
local runtime_file requested_domain requested_localhost previous_domain
|
||||
runtime_file="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)/config/ros_runtime.env"
|
||||
requested_domain="${REAL_ROS_DOMAIN_ID:-}"
|
||||
requested_localhost="${REAL_ROS_LOCALHOST_ONLY:-}"
|
||||
if [[ -f "${runtime_file}" ]]; then
|
||||
source "${runtime_file}"
|
||||
fi
|
||||
requested_domain="${requested_domain:-${ORISYS_ROS_DOMAIN_ID:-42}}"
|
||||
requested_localhost="${requested_localhost:-${ORISYS_ROS_LOCALHOST_ONLY:-1}}"
|
||||
if [[ ! "${requested_domain}" =~ ^[0-9]+$ ]] || (( 10#${requested_domain} > 232 )); then
|
||||
echo "[domain] ERROR: ROS domain must be an integer in 0..232" >&2
|
||||
return 2
|
||||
fi
|
||||
if [[ "${requested_localhost}" != 0 && "${requested_localhost}" != 1 ]]; then
|
||||
echo "[domain] ERROR: REAL_ROS_LOCALHOST_ONLY must be 0 or 1" >&2
|
||||
return 2
|
||||
fi
|
||||
if (( 10#${requested_domain} == ROS_SIM_DOMAIN )); then
|
||||
echo "[domain] ERROR: domain 77 is reserved for simulation" >&2
|
||||
return 2
|
||||
fi
|
||||
previous_domain="${ROS_DOMAIN_ID:-unset}"
|
||||
export ROS_DOMAIN_ID="$((10#${requested_domain}))"
|
||||
export ROS_LOCALHOST_ONLY="${requested_localhost}"
|
||||
# All nested launch scripts inherit exactly the same selection.
|
||||
export REAL_ROS_DOMAIN_ID="${ROS_DOMAIN_ID}"
|
||||
export REAL_ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY}"
|
||||
if [[ "${previous_domain}" != "${ROS_DOMAIN_ID}" ]]; then
|
||||
echo "[domain] orisys_arm: ${previous_domain} -> ${ROS_DOMAIN_ID}; localhost_only=${ROS_LOCALHOST_ONLY}" >&2
|
||||
fi
|
||||
}
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
# arm_application
|
||||
|
||||
工位 ROS 2 业务节点包。日常入口是本仓库的 `station/run_station.sh`；本地 Qt 界面位于 `station/src/station`，不在本包内。
|
||||
|
||||
## 上游同步
|
||||
|
||||
2026-10-20 从 `http://git.orisys-tech.cn/orisys/ros_contorl.git` 的 `exp/hole-keypoint-once` 分支同步，提交 `561d7da21f20d3e63030f81510ae2f29eed01d46`。
|
||||
|
||||
同步内容：视觉感知/跟踪、二阶段孔定位、流水线与 A7 工序、通止规装配状态机、夹爪串口修复、双夹爪节点、手眼标定 ROS 工具，以及对应算法、配置、launch、轨迹和节点测试。
|
||||
|
||||
未引入远程的 `operator_*` Qt/网页控制台、`realsense_view` Qt 预览、独立权重整理工具、整站 shell 入口和根目录现场配置。相应界面/脚本测试不随本包同步；混合测试文件中的远程整站检查有明确 skip 原因。本地工位与配置继续使用现有版本。
|
||||
|
||||
保留本地相机高清采集与压缩预览、每指独立触觉节点和力标定、工位夹爪节点、进程树清理和短结果发布周期；保留 `gauge_pass.py`，本地工位仍依赖它。
|
||||
|
||||
## 主要 ROS 入口
|
||||
|
||||
- 感知与相机：`realsense_color_pub`、`perception_node`、`servo_node`、`hole_detect`。
|
||||
- 任务：`stage2_locate`、`stage3_search`、`assembly_pipeline`、`gauge_assembly`、`a7_rotation`。
|
||||
- 本地设备驱动：`tactile_flow`（每指一个进程）、`station_gripper`（每臂独立命名空间）。
|
||||
- 显式兼容工具：`dh_pgea`、`dual_gripper`、`tactile` 及对应 CLI。不与本地设备驱动同时占用同一串口/传感器。
|
||||
- 标定：`handeye_teach`、`handeye_waypoints`、`handeye_calibrate`。
|
||||
|
||||
注册入口以 `setup.py` 为准。导入模块不会自动运行这些动作。
|
||||
|
||||
## 本地兼容修复
|
||||
|
||||
- 一阶段清理只停止自己启动的子进程树，禁止按名称 `pkill` 其他节点。
|
||||
- 父类机器人描述回调独立命名，避免被装配节点遮蔽。
|
||||
- A7 失败返回失败；pipeline 子节点非零退出时，launch 同样失败。
|
||||
- 二阶段 Action 只把 SUCCEEDED 且 error_code=0 判为成功；接单超时不重发并取消迟到任务，执行超时取消并中止流程。
|
||||
- 保留夹爪 `GripperExecute` 服务及类型化状态，兼容远程六元素旧状态帧的在线标志。
|
||||
- `run_a7_after_insert` 保留上游工序开关；本地工位在定位进程中显式传 false；“旋转”使用独立 a7_rotation 节点，“开始全部流程”在定位成功后显式进入该节点。对孔采用新版配置下降，不再强制 no_descent。
|
||||
- 同步 `scripts/lib/flexiv_movel_ipc.py` 的可选 ZFine/到位等级参数，并在本地硬件接口补充对应解析。原先六字段请求保持兼容，本地停止/重新使能位置指令保护保留。
|
||||
|
||||
## 验证
|
||||
|
||||
在 `ros_flexiv` 环境和已构建的 ROS 工作空间中运行本包 pytest；需要 X11 的工位回归使用 `bash station/run_station.sh --self-test`。测试使用模拟驱动/独立 DDS 域，不下发真实设备指令。
|
||||
|
||||
同步后应在现场任务停止时重启工位与驱动，使新节点入口、后端和硬件库一起生效；这次同步没有重启现有进程，也没有执行实机动作。
|
||||
@@ -1 +1,32 @@
|
||||
"""arm_application：从旧工位抽出的节点。"""
|
||||
"""arm_application —— Flexiv Rizon + D405 + DH 夹爪的现场节点包。
|
||||
|
||||
对外可直接运行的 ROS 程序（见 ``setup.py`` 的 ``console_scripts``）
|
||||
------------------------------------------------------------------
|
||||
相机： realsense_color_pub
|
||||
感知： perception_node
|
||||
伺服： servo_node
|
||||
手眼： handeye_teach、handeye_waypoints、handeye_calibrate
|
||||
夹爪： dh_pgea、dh_pgea_cli、dual_gripper、dual_gripper_cli
|
||||
入孔检测： hole_detect
|
||||
触觉： tactile、tactile_cli
|
||||
辅助： goto_ee_plane、demo_named_poses、run_joint_trajectory
|
||||
|
||||
可复用的 Python 模块（可以在你自己的节点里 import）
|
||||
------------------------------------------------------------------
|
||||
``yolo_detector`` YOLO / TensorRT 推理 + ``pick_best`` 挑最优框
|
||||
``box_tracker`` 像素平面内的匀速模型框预测（丢帧时"滑行"用）
|
||||
``plane_feature`` 在 YOLO 框内取立体平面的质心
|
||||
``dh_pgea`` 大寰 PGEA 的官方 Modbus RTU 客户端
|
||||
``dh_official`` ``dual_gripper`` 用的 DH Modbus 客户端
|
||||
``dual_gripper`` 左 PGEA + 右 RGI
|
||||
``hole_detector`` yolo / hough / two_stage 三种孔心检测
|
||||
``orisys_tactile`` Orisys 视触觉适配器
|
||||
``geom_ik`` Rizon 几何雅可比 / DLS / 手眼加载
|
||||
``handeye_lib`` 棋盘格 + OpenCV ``calibrateHandEye``
|
||||
``handeye_realsense`` D405 的话题 / 序列号辅助
|
||||
``pid_controller`` 带积分分离的 PID
|
||||
``kalman_filter`` 三维匀速模型卡尔曼滤波
|
||||
|
||||
本仓库日常操作使用 station 工位与 config/site.yaml；远程 Qt/网页控制台不随节点同步。
|
||||
接口契约见 ``docs/INTERFACES.md``。
|
||||
"""
|
||||
|
||||
@@ -0,0 +1,104 @@
|
||||
"""Explicit A7 stage, reusing upstream rounds with station-owned typed gripper IO.
|
||||
|
||||
Starts at the current aligned pose: no tracking, clearance lift or hole search.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
import math
|
||||
import os
|
||||
import time
|
||||
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
from arm_application.pipeline_node import AssemblyPipeline, close_ready, main as pipeline_main
|
||||
|
||||
|
||||
class A7Rotation(AssemblyPipeline):
|
||||
def __init__(self):
|
||||
super().__init__(node_name='orisys_a7_rotation')
|
||||
self._grip_feedback=None
|
||||
self._stroke=float(os.environ.get('SITE_GRIP_STROKE_MM','0'))
|
||||
self._force=float(os.environ.get('SITE_GRIP_FORCE','33'))
|
||||
self._speed=float(os.environ.get('SITE_GRIP_SPEED','25'))
|
||||
if not math.isfinite(self._stroke) or self._stroke<=0:
|
||||
raise ValueError('未配置有效夹爪行程')
|
||||
if not 20<=self._force<=100 or not 1<=self._speed<=100:
|
||||
raise ValueError('夹爪力或速度参数无效')
|
||||
aid=self._arm_short_id()
|
||||
if not aid:raise ValueError('未配置当前机械臂号')
|
||||
endpoint=f'/station/grippers/arm_{aid}'
|
||||
self._grip_client=self.create_client(GripperExecute,endpoint+'/execute')
|
||||
self.create_subscription(GripperStatus,endpoint+'/status',self._on_rotation_grip,10)
|
||||
|
||||
def _on_rotation_grip(self,msg):
|
||||
self._grip_feedback=(time.monotonic(),msg)
|
||||
|
||||
def _fresh_grip(self,after=0.):
|
||||
reading=self._grip_feedback
|
||||
if not reading or reading[0]<=after or time.monotonic()-reading[0]>.75:
|
||||
return None
|
||||
return reading[1] if reading[1].connected else None
|
||||
|
||||
def _gripper_cli(self,action,value=''):
|
||||
"""One typed command; await new feedback, never launch a shell or resend."""
|
||||
target=0. if action=='close' else float(value) if action=='pos' else float('nan')
|
||||
if not math.isfinite(target) or not 0<=target<=1000:
|
||||
raise ValueError('A7 夹爪只接受 close 或 pos 0–1000')
|
||||
before=self._fresh_grip()
|
||||
if before is None or before.is_moving:
|
||||
raise RuntimeError('夹爪离线、反馈过期或仍在运动，旋转流程中止')
|
||||
if not self._grip_client.wait_for_service(timeout_sec=2.):
|
||||
raise RuntimeError('本站夹爪 execute 服务不可用')
|
||||
req=GripperExecute.Request()
|
||||
req.command.command='move';req.command.position=target/1000*self._stroke
|
||||
req.command.force=self._force;req.command.speed=self._speed
|
||||
req.command.wait_complete=False;req.command.timeout=8.
|
||||
self.get_logger().info(f'旋转夹爪：{action}，目标开度 {target:.0f}/1000')
|
||||
sent=time.monotonic();future=self._grip_client.call_async(req)
|
||||
while not future.done() and time.monotonic()-sent<5.:self._spin(.02)
|
||||
if not future.done():raise RuntimeError('夹爪命令响应超时，结果未知；中止且不重发')
|
||||
response=future.result()
|
||||
if not response.success:raise RuntimeError('夹爪拒绝命令：'+response.message)
|
||||
while time.monotonic()-sent<12.:
|
||||
self._spin(.02)
|
||||
state=self._fresh_grip(after=sent)
|
||||
if state is None:continue
|
||||
if state.is_moving:continue
|
||||
pos=float(state.finger_position_per_mille)
|
||||
reached=close_ready(state.gripper_state,pos) if action=='close' else state.gripper_state==1 and abs(pos-target)<=40.
|
||||
if reached:
|
||||
self.get_logger().info(f'旋转夹爪已到位：{pos:.0f}/1000')
|
||||
return True
|
||||
raise RuntimeError('夹爪未收到新鲜到位反馈，旋转流程中止且不重发')
|
||||
|
||||
def run(self):
|
||||
if not self._cli.wait_for_server(timeout_sec=5.):
|
||||
raise RuntimeError('机械臂轨迹服务不可用')
|
||||
if not self._grip_client.wait_for_service(timeout_sec=3.):
|
||||
raise RuntimeError('请先连接本站平行夹爪')
|
||||
self._spin(.2)
|
||||
grip=self._fresh_grip()
|
||||
if grip is None or grip.is_moving:
|
||||
raise RuntimeError('夹爪未就绪，不能开始 A7 工序')
|
||||
if not close_ready(grip.gripper_state,float(grip.finger_position_per_mille)):
|
||||
raise RuntimeError('夹爪未夹紧，请先夹爪闭合再开始旋转')
|
||||
q,tee=self._wait_state(5.)
|
||||
if q is None or tee is None:raise RuntimeError('没有机械臂实时位姿')
|
||||
# The new stage must not turn at the former 15mm observation height.
|
||||
if not self.tool_tip_below_flange_m>0 or not self.surface_z_m>0:
|
||||
raise RuntimeError('缺少规长或工件面几何，不能确认旋转起点')
|
||||
gap=float(tee[2,3])-self.tool_tip_below_flange_m-self.surface_z_m
|
||||
if not math.isfinite(gap) or not math.isfinite(self.insert_gap_m) or abs(gap-self.insert_gap_m)>.002:
|
||||
raise RuntimeError(f'当前位置离对孔收尾高度过远（间隙 {gap*1000:.1f}mm），请先完成新版对孔')
|
||||
if not math.isfinite(float(tee[2,2])) or float(tee[2,2])>-.98:
|
||||
raise RuntimeError('工具轴未垂直向下，请先完成对孔')
|
||||
self._state('stage3_a7_rotation')
|
||||
ok=self._run_a7_seat()
|
||||
self._state('stage3_a7_done' if ok else 'stage3_a7_failed')
|
||||
return 0 if ok else 2
|
||||
|
||||
|
||||
def main(args=None):
|
||||
pipeline_main(args=args,node_factory=A7Rotation)
|
||||
|
||||
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,10 @@
|
||||
"""Choose only enumerated camera modes; color and native depth may differ."""
|
||||
|
||||
|
||||
def depth_profile(color, available):
|
||||
available=set(available)
|
||||
if color in available: return color
|
||||
width,height,fps=color
|
||||
matches=[p for p in available if p[2]==fps and p[0]<=width and p[1]<=height]
|
||||
if not matches: raise ValueError(f'No native depth profile at {fps} fps for color {color}')
|
||||
return max(matches,key=lambda p:p[0]*p[1])
|
||||
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python3
|
||||
"""演示：通过 ``/set_named_pose`` 循环切换命名位姿（对任何后端都适用）。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_msgs.srv import SetNamedPose
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
class DemoNamedPoses(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('demo_named_poses')
|
||||
self.declare_parameter(
|
||||
'poses', ['home', 'ready', 'inspect', 'pos_left', 'home']
|
||||
)
|
||||
self.declare_parameter('velocity_scale', 0.5)
|
||||
self.declare_parameter('dwell_sec', 0.3)
|
||||
self.declare_parameter('loops', 1)
|
||||
self.poses = list(
|
||||
self.get_parameter('poses').get_parameter_value().string_array_value
|
||||
or ['home', 'ready', 'inspect', 'home']
|
||||
)
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
self.dwell_sec = float(self.get_parameter('dwell_sec').value)
|
||||
self.loops = int(self.get_parameter('loops').value)
|
||||
self._cli = self.create_client(SetNamedPose, 'set_named_pose')
|
||||
|
||||
def run(self) -> int:
|
||||
if not self._cli.wait_for_service(timeout_sec=15.0):
|
||||
self.get_logger().error('``/set_named_pose`` 服务不可用')
|
||||
return 1
|
||||
for loop in range(max(self.loops, 1)):
|
||||
self.get_logger().info(f'第 {loop + 1}/{self.loops} 轮')
|
||||
for pose in self.poses:
|
||||
req = SetNamedPose.Request()
|
||||
req.pose_name = pose
|
||||
req.velocity_scale = self.velocity_scale
|
||||
req.acceleration_scale = self.velocity_scale
|
||||
fut = self._cli.call_async(req)
|
||||
deadline = time.monotonic() + 60.0
|
||||
while rclpy.ok() and not fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error(f'{pose} 调用超时')
|
||||
return 2
|
||||
res = fut.result()
|
||||
if res is None or not res.success:
|
||||
self.get_logger().error(
|
||||
f'{pose} 执行失败：{res.message if res else "无响应"}'
|
||||
)
|
||||
return 3
|
||||
self.get_logger().info(f'{pose}：{res.message}')
|
||||
time.sleep(self.dwell_sec)
|
||||
self.get_logger().info('演示结束')
|
||||
return 0
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = DemoNamedPoses()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,286 @@
|
||||
"""DH-Robotics 官方 Modbus RTU 控制（非 Flexiv RDK）。
|
||||
|
||||
覆盖两类设备：
|
||||
- PGE/PGEA 平行夹爪：初始化 0x0100，力 0x0101，位置 0x0103（0~1000‰），速度 0x0104。
|
||||
- RGI 旋转夹爪：在平行夹爪寄存器基础上，增加旋转角度 0x0105、旋转速度 0x0107、
|
||||
旋转力 0x0108；反馈增加旋转角度 0x0208、旋转初始化状态 0x020A、旋转状态 0x020B。
|
||||
|
||||
说明：
|
||||
- RGI 手册示例中负角度使用“反码/one's complement”：+360 -> 0x0168，-360 -> 0xFE97。
|
||||
因此这里默认按 one's complement 编解码；若后续厂家确认为 two's complement，只改
|
||||
`_encode_signed` / `_decode_signed` 两处即可。
|
||||
- 写位置/角度寄存器后会立即运动，没有 ChangingTek SDK 的 0x0108 触发寄存器。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from typing import Iterable
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# DH-Robotics Modbus 寄存器映射
|
||||
# =============================================================================
|
||||
|
||||
# 基础控制寄存器（保持寄存器，功能码 03/06/10）
|
||||
REG_INIT = 0x0100 # 0x01 初始化，0xA5 完全初始化
|
||||
REG_FORCE = 0x0101 # 夹爪闭合力，20~100 %
|
||||
REG_POSITION = 0x0103 # 手指参考位置，0~1000 千分比
|
||||
REG_SPEED = 0x0104 # 手指开合速度，1~100 %
|
||||
|
||||
# RGI 旋转控制寄存器
|
||||
REG_ROTATION_ANGLE = 0x0105 # 目标旋转角度，有符号角度值
|
||||
REG_ROTATION_SPEED = 0x0107 # 旋转速度，1~100 %
|
||||
REG_ROTATION_FORCE = 0x0108 # 旋转力/力矩，20~100 %
|
||||
|
||||
# 反馈寄存器
|
||||
REG_INIT_STATE = 0x0200 # 0 未初始化，1 已初始化，2 初始化中
|
||||
REG_GRIPPER_STATE = 0x0201 # 0 运动中，1 已到位，2 已夹到物体，3 物体掉落
|
||||
REG_CURRENT_POSITION = 0x0202 # 当前手指位置，千分比
|
||||
REG_ROTATION_ANGLE_FB = 0x0208 # 当前旋转角度，有符号角度值
|
||||
REG_ROTATION_INIT_STATE = 0x020A # 0 未初始化，1 已初始化，2 初始化中
|
||||
REG_ROTATION_STATE = 0x020B # 0 运动中，1 已到位，2 受阻，3 曾经受阻
|
||||
|
||||
# 配置寄存器
|
||||
REG_SAVE_PARAMS = 0x0300 # 写入 1 可把所有参数保存到 flash
|
||||
|
||||
|
||||
class DHModbusError(RuntimeError):
|
||||
"""DH Modbus 通信或协议错误。"""
|
||||
|
||||
|
||||
def _clamp_int(value: float | int, lo: int, hi: int) -> int:
|
||||
return int(max(lo, min(hi, int(round(float(value))))))
|
||||
|
||||
|
||||
def _encode_signed(value: float | int) -> int:
|
||||
"""把有符号角度值编码为 DH 的反码（one's complement）寄存器值。
|
||||
|
||||
RGI 手册示例：+360 -> 0x0168，-360 -> 0xFE97。
|
||||
"""
|
||||
value = _clamp_int(value, -32767, 32767)
|
||||
if value >= 0:
|
||||
return value & 0xFFFF
|
||||
return (~abs(value)) & 0xFFFF
|
||||
|
||||
|
||||
def _decode_signed(raw: int) -> int:
|
||||
"""把 DH 的反码（one's complement）寄存器值解码为有符号整数。"""
|
||||
raw = int(raw) & 0xFFFF
|
||||
if raw & 0x8000:
|
||||
mag = (~raw) & 0x7FFF
|
||||
return -mag
|
||||
return raw & 0x7FFF
|
||||
|
||||
|
||||
class DHModbusClient:
|
||||
"""面向 DH-Robotics 夹爪的最小线程安全 Modbus RTU 客户端。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str,
|
||||
slave_id: int = 1,
|
||||
baudrate: int = 115200,
|
||||
timeout: float = 0.2,
|
||||
*,
|
||||
bytesize: int = 8,
|
||||
parity: str = "N",
|
||||
stopbits: int = 1,
|
||||
) -> None:
|
||||
try:
|
||||
import minimalmodbus
|
||||
import serial
|
||||
except ImportError as exc: # pragma: no cover - 取决于运行时环境
|
||||
raise DHModbusError(
|
||||
"缺少 minimalmodbus/pyserial。请先安装：pip install minimalmodbus pyserial"
|
||||
) from exc
|
||||
|
||||
parity_map = {
|
||||
"N": serial.PARITY_NONE,
|
||||
"E": serial.PARITY_EVEN,
|
||||
"O": serial.PARITY_ODD,
|
||||
}
|
||||
self.instrument = minimalmodbus.Instrument(port, int(slave_id))
|
||||
self.instrument.serial.baudrate = int(baudrate)
|
||||
self.instrument.serial.bytesize = int(bytesize)
|
||||
self.instrument.serial.parity = parity_map.get(str(parity).upper(), serial.PARITY_NONE)
|
||||
self.instrument.serial.stopbits = int(stopbits)
|
||||
self.instrument.serial.timeout = float(timeout)
|
||||
self.instrument.mode = minimalmodbus.MODE_RTU
|
||||
self.instrument.clear_buffers_before_each_transaction = True
|
||||
try:
|
||||
self.instrument.serial.exclusive = True
|
||||
except Exception:
|
||||
pass
|
||||
self._lock = threading.Lock()
|
||||
self.port = port
|
||||
|
||||
def close(self) -> None:
|
||||
with self._lock:
|
||||
try:
|
||||
self.instrument.serial.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 底层寄存器读写
|
||||
# ------------------------------------------------------------------
|
||||
def write_register(self, address: int, value: int) -> None:
|
||||
value = _clamp_int(value, 0, 0xFFFF)
|
||||
last_exc: Exception | None = None
|
||||
for attempt in range(3):
|
||||
try:
|
||||
with self._lock:
|
||||
self.instrument.write_register(address, value, functioncode=6)
|
||||
return
|
||||
except Exception as exc:
|
||||
last_exc = exc
|
||||
time.sleep(0.05 * (attempt + 1))
|
||||
raise DHModbusError(f"写寄存器 0x{address:04X}={value} 失败: {last_exc}") from last_exc
|
||||
|
||||
def read_register(self, address: int) -> int:
|
||||
last_exc: Exception | None = None
|
||||
for attempt in range(3):
|
||||
try:
|
||||
with self._lock:
|
||||
return int(self.instrument.read_register(address, functioncode=3))
|
||||
except Exception as exc:
|
||||
last_exc = exc
|
||||
time.sleep(0.05 * (attempt + 1))
|
||||
hint = ""
|
||||
text = str(last_exc).lower()
|
||||
if "multiple access" in text or "no data" in text:
|
||||
hint = f"。串口 {getattr(self, 'port', '')} 可能被其它进程占用，先执行: fuser -k {getattr(self, 'port', '/dev/ttyUSB*')}"
|
||||
raise DHModbusError(f"读寄存器 0x{address:04X} 失败: {last_exc}{hint}") from last_exc
|
||||
|
||||
def write_registers(self, address: int, values: Iterable[int]) -> None:
|
||||
vals = [_clamp_int(v, 0, 0xFFFF) for v in values]
|
||||
with self._lock:
|
||||
self.instrument.write_registers(address, vals)
|
||||
|
||||
def read_registers(self, address: int, count: int) -> list[int]:
|
||||
with self._lock:
|
||||
return [int(v) for v in self.instrument.read_registers(address, count, functioncode=3)]
|
||||
|
||||
def write_signed_register(self, address: int, value: float | int) -> None:
|
||||
self.write_register(address, _encode_signed(value))
|
||||
|
||||
def read_signed_register(self, address: int) -> int:
|
||||
return _decode_signed(self.read_register(address))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# PGE/PGEA/RGI 通用控制
|
||||
# ------------------------------------------------------------------
|
||||
def initialize(self, *, full: bool = False, rotation: bool = False, timeout: float = 10.0) -> None:
|
||||
"""初始化/回零夹爪，并等待其就绪。"""
|
||||
self.write_register(REG_INIT, 0xA5 if full else 0x01)
|
||||
start = time.monotonic()
|
||||
while time.monotonic() - start < timeout:
|
||||
init_state = self.read_register(REG_INIT_STATE)
|
||||
rot_ok = True
|
||||
if rotation:
|
||||
rot_ok = self.read_register(REG_ROTATION_INIT_STATE) == 1
|
||||
if init_state == 1 and rot_ok:
|
||||
return
|
||||
time.sleep(0.1)
|
||||
raise DHModbusError(f"夹爪初始化超时（>{timeout:.1f}s）")
|
||||
|
||||
def save_parameters(self) -> None:
|
||||
self.write_register(REG_SAVE_PARAMS, 1)
|
||||
|
||||
def set_force_pct(self, force_pct: float | int) -> int:
|
||||
value = _clamp_int(force_pct, 20, 100)
|
||||
self.write_register(REG_FORCE, value)
|
||||
return value
|
||||
|
||||
def set_speed_pct(self, speed_pct: float | int) -> int:
|
||||
value = _clamp_int(speed_pct, 1, 100)
|
||||
self.write_register(REG_SPEED, value)
|
||||
return value
|
||||
|
||||
def set_position_per_mille(self, position: float | int) -> int:
|
||||
value = _clamp_int(position, 0, 1000)
|
||||
# 与 dh_pgea.latched_position_break 同一份：命令已是目标时先写当前位置，
|
||||
# 读回之后再写最终值。60ms 后写回旧命令，固件采样不到，手指不动。
|
||||
from arm_application.dh_pgea import latched_position_break
|
||||
|
||||
try:
|
||||
cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
|
||||
except Exception:
|
||||
cmd = value
|
||||
try:
|
||||
cur = self.get_position_per_mille()
|
||||
except Exception:
|
||||
cur = value
|
||||
bridge = latched_position_break(cmd, cur, value)
|
||||
if bridge is not None:
|
||||
self.write_register(REG_POSITION, bridge)
|
||||
deadline = time.monotonic() + 0.6
|
||||
echoed = False
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
if _clamp_int(self.read_register(REG_POSITION), 0, 1000) == bridge:
|
||||
echoed = True
|
||||
break
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.05)
|
||||
if not echoed:
|
||||
raise DHModbusError(
|
||||
f"位置命令中间值 {bridge} 没有读回，不再写入最终目标 {value}"
|
||||
)
|
||||
time.sleep(0.15)
|
||||
self.write_register(REG_POSITION, value)
|
||||
return value
|
||||
|
||||
def get_position_per_mille(self) -> int:
|
||||
return _clamp_int(self.read_register(REG_CURRENT_POSITION), 0, 1000)
|
||||
|
||||
def get_gripper_state(self) -> int:
|
||||
return self.read_register(REG_GRIPPER_STATE)
|
||||
|
||||
def get_init_state(self) -> int:
|
||||
return self.read_register(REG_INIT_STATE)
|
||||
|
||||
def hold_current_position(self) -> None:
|
||||
"""在没有专用停止寄存器的 DH 夹爪上尽力停止/保持。"""
|
||||
self.set_position_per_mille(self.get_position_per_mille())
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# RGI 旋转控制
|
||||
# ------------------------------------------------------------------
|
||||
def set_rotation_speed_pct(self, speed_pct: float | int) -> int:
|
||||
value = _clamp_int(speed_pct, 1, 100)
|
||||
self.write_register(REG_ROTATION_SPEED, value)
|
||||
return value
|
||||
|
||||
def set_rotation_force_pct(self, force_pct: float | int) -> int:
|
||||
value = _clamp_int(force_pct, 20, 100)
|
||||
self.write_register(REG_ROTATION_FORCE, value)
|
||||
return value
|
||||
|
||||
def set_rotation_angle_deg(self, angle_deg: float | int) -> int:
|
||||
angle = _clamp_int(angle_deg, -32767, 32767)
|
||||
self.write_signed_register(REG_ROTATION_ANGLE, angle)
|
||||
return angle
|
||||
|
||||
def get_rotation_angle_raw(self) -> int:
|
||||
return self.read_register(REG_ROTATION_ANGLE_FB) & 0xFFFF
|
||||
|
||||
def get_rotation_angle_deg(self) -> int:
|
||||
return self.read_signed_register(REG_ROTATION_ANGLE_FB)
|
||||
|
||||
def get_rotation_init_state(self) -> int:
|
||||
return self.read_register(REG_ROTATION_INIT_STATE)
|
||||
|
||||
def get_rotation_state(self) -> int:
|
||||
return self.read_register(REG_ROTATION_STATE)
|
||||
|
||||
def hold_current_rotation(self) -> None:
|
||||
"""重新下发当前反馈角度来停止旋转。"""
|
||||
self.write_register(REG_ROTATION_ANGLE, self.get_rotation_angle_raw())
|
||||
|
||||
def hold_current_all(self) -> None:
|
||||
self.hold_current_position()
|
||||
self.hold_current_rotation()
|
||||
@@ -21,11 +21,15 @@ Flexiv RDK 的 ``Gripper`` 驱动不了这个夹爪：它不是 Studio Device。
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import fcntl
|
||||
import select
|
||||
import struct
|
||||
import sys
|
||||
import termios
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
from typing import Callable, Optional
|
||||
|
||||
try:
|
||||
import serial
|
||||
@@ -104,6 +108,21 @@ INIT_STATE_NAME = {
|
||||
}
|
||||
|
||||
|
||||
def wait_tx_empty(queued: Callable[[], int], deadline: float, pause=time.sleep) -> bool:
|
||||
"""等到发送缓冲里的字节数变成 0。到 ``deadline`` 还没空就返回 False。
|
||||
|
||||
不要改成 ``Serial.flush()`` / ``tcdrain``。USB 串口不收数据时那个调用
|
||||
不看写超时，会把夹爪节点的回调一直堵住。2026-10-20 合爪：命令 10:40:16
|
||||
已经交给话题，节点 120s 后才打出 ``ctrl pos=0``，流水线 8s 时手指还停在 400。
|
||||
"""
|
||||
while True:
|
||||
if int(queued()) == 0:
|
||||
return True
|
||||
if time.monotonic() >= deadline:
|
||||
return False
|
||||
pause(0.01)
|
||||
|
||||
|
||||
def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
|
||||
"""命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
|
||||
|
||||
@@ -204,6 +223,22 @@ class DhPgeaError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
class DhPgeaBusError(DhPgeaError):
|
||||
"""串口本身坏了（EIO / 设备被拔 / USB 掉线），不是夹爪没应答。
|
||||
|
||||
2026-10-20 之前 ``_transact`` 让 ``termios.error: (5, 'Input/output error')``
|
||||
直接冒到 timer 回调外，整个夹爪节点带着 traceback 退出。这两种失败必须分开：
|
||||
没应答可以继续重试；串口坏了要先关再重开。
|
||||
"""
|
||||
|
||||
|
||||
#: 连续这么多拍读不到状态就重开串口。
|
||||
#: 2026-10-20 实测：写命令有回显（``ctrl pos=400`` 有记录、手指也动了），
|
||||
#: 读 ``0x0201`` 却连续 264 次无应答；重启节点立刻恢复。说明串口会话坏了，
|
||||
#: 光在同一个 fd 上重试没用。按失败拍数（不是秒）计数，是因为每拍耗时随重试变长。
|
||||
BUS_REOPEN_AFTER_FAILURES = 30
|
||||
|
||||
|
||||
WIRING_HINT = (
|
||||
"PC 已发出 Modbus，夹爪 0 字节返回。不是力/速度参数问题。按顺序查：\n"
|
||||
" 1) 夹爪 24 V DC 已上电（指示灯亮）\n"
|
||||
@@ -362,22 +397,56 @@ class DhPgeaGripper:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
raise DhPgeaError("串口未打开")
|
||||
bit_s = 10.0 / max(1, self.baudrate)
|
||||
try:
|
||||
self._set_tx(True)
|
||||
if self.rs485_rts:
|
||||
time.sleep(2.0 * bit_s)
|
||||
self._ser.reset_input_buffer()
|
||||
self._ser.write(frame)
|
||||
self._ser.flush()
|
||||
# 不用 flush/tcdrain。发送缓冲排不空就在 timeout_s 内失败，回调才能返回。
|
||||
# 读应答另给一个 timeout_s，不能和排空共用同一个截止时间。
|
||||
if not wait_tx_empty(self._tx_queued, time.monotonic() + self.timeout_s):
|
||||
raise DhPgeaError(
|
||||
f"{self.port} 串口发送缓冲 {self.timeout_s:.2f}s 内没排空"
|
||||
)
|
||||
if self.rs485_rts:
|
||||
time.sleep(len(frame) * bit_s + 0.0008)
|
||||
self._set_tx(False)
|
||||
deadline = time.monotonic() + self.timeout_s
|
||||
buf = self._read_reply(expect, time.monotonic() + self.timeout_s)
|
||||
except OSError as exc:
|
||||
raise DhPgeaBusError(
|
||||
f"{self.port} 串口读写失败（{exc}）。"
|
||||
"设备被拔掉或 USB 掉线时要先关串口再重开，不是夹爪没应答。"
|
||||
) from exc
|
||||
return bytes(buf) if buf else None
|
||||
|
||||
def _tx_queued(self) -> int:
|
||||
"""内核发送缓冲里还没推出去的字节数。"""
|
||||
raw = fcntl.ioctl(self._ser.fileno(), termios.TIOCOUTQ, struct.pack("I", 0))
|
||||
return struct.unpack("I", raw)[0]
|
||||
|
||||
def _read_reply(self, expect: int, deadline: float) -> bytes:
|
||||
"""读应答。单次 ``read`` 不许越过 ``deadline``。
|
||||
|
||||
pyserial 的超时在 USB 卡住时不可靠；``select`` 到点就停。
|
||||
"""
|
||||
fd = self._ser.fileno()
|
||||
buf = bytearray()
|
||||
while time.monotonic() < deadline and len(buf) < expect:
|
||||
remain = deadline - time.monotonic()
|
||||
ready, _, _ = select.select([fd], [], [], remain)
|
||||
if not ready:
|
||||
break
|
||||
old = self._ser.timeout
|
||||
self._ser.timeout = 0
|
||||
try:
|
||||
chunk = self._ser.read(expect - len(buf))
|
||||
if chunk:
|
||||
finally:
|
||||
self._ser.timeout = old
|
||||
if not chunk:
|
||||
break
|
||||
buf.extend(chunk)
|
||||
return bytes(buf) if buf else None
|
||||
return bytes(buf)
|
||||
|
||||
def initialization(self) -> bool:
|
||||
return self.write_register(REG_INIT, INIT_CMD)
|
||||
|
||||
@@ -6,8 +6,9 @@
|
||||
data: [initialize, position, force, speed]
|
||||
initialize>0.5 → 写 0xA5；否则设置力/速度/位置
|
||||
/gripper/states std_msgs/Float32MultiArray
|
||||
data: [initialized, grip_state, position, target_pos, target_force]
|
||||
/gripper/joint_states sensor_msgs/JointState
|
||||
data: [initialized, grip_state, position, target_pos, target_force, online]
|
||||
online=1 这一帧刚从总线读回来；0=读不到（前 5 个是上一次的值）
|
||||
/gripper/joint_states sensor_msgs/JointState（只在读到状态时发）
|
||||
|
||||
服务（std_srvs/Trigger）：
|
||||
/gripper/initialize /gripper/open /gripper/close
|
||||
@@ -17,25 +18,29 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
import threading
|
||||
import time
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
import math
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Float32, Float32MultiArray, MultiArrayDimension, MultiArrayLayout
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from arm_application.dh_pgea import (
|
||||
BUS_REOPEN_AFTER_FAILURES,
|
||||
DhPgeaBusError,
|
||||
DhPgeaError,
|
||||
DhPgeaGripper,
|
||||
GripperState,
|
||||
WIRING_HINT,
|
||||
)
|
||||
from arm_application.gauge_gripper import STATES_ONLINE_INDEX
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
|
||||
@@ -48,14 +53,14 @@ def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
|
||||
|
||||
class DhPgeaNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("dh_pgea", namespace="/gripper")
|
||||
super().__init__("dh_pgea")
|
||||
self.declare_parameter("port", "/dev/ttyUSB0")
|
||||
self.declare_parameter("baud", 115200)
|
||||
self.declare_parameter("slave_id", 1)
|
||||
self.declare_parameter("stroke_mm", 20.0)
|
||||
self.declare_parameter("force_pct", 50)
|
||||
self.declare_parameter("speed_pct", 50)
|
||||
self.declare_parameter("auto_init", False)
|
||||
self.declare_parameter("auto_init", True)
|
||||
self.declare_parameter("state_hz", 20.0)
|
||||
self.declare_parameter("open_position", 1000)
|
||||
self.declare_parameter("close_position", 0)
|
||||
@@ -67,8 +72,6 @@ class DhPgeaNode(TolerantNode):
|
||||
self._baud = int(self.get_parameter("baud").value)
|
||||
self._slave = int(self.get_parameter("slave_id").value)
|
||||
self._stroke = float(self.get_parameter("stroke_mm").value)
|
||||
if not math.isfinite(self._stroke) or self._stroke <= 0:
|
||||
raise DhPgeaError("夹爪行程必须是正数")
|
||||
self._force = int(self.get_parameter("force_pct").value)
|
||||
self._speed = int(self.get_parameter("speed_pct").value)
|
||||
_auto = self.get_parameter("auto_init").value
|
||||
@@ -89,17 +92,19 @@ class DhPgeaNode(TolerantNode):
|
||||
)
|
||||
self._state = GripperState(stroke_mm=self._stroke)
|
||||
self._cmd_lock = threading.Lock()
|
||||
self._last_reopen = 0.0
|
||||
self._fail_streak = 0
|
||||
|
||||
self._pub_state = self.create_publisher(Float32MultiArray, "states", 10)
|
||||
self._pub_state = self.create_publisher(Float32MultiArray, "/gripper/states", 10)
|
||||
self._pub_typed = self.create_publisher(GripperStatus, "status", 10)
|
||||
self.create_service(GripperExecute, "execute", self._srv_execute)
|
||||
self._pub_js = self.create_publisher(JointState, "joint_states", 10)
|
||||
self.create_subscription(Float32MultiArray, "ctrl", self._on_ctrl, 10)
|
||||
self.create_subscription(Float32, "set_position", self._on_set_pos, 10)
|
||||
self._pub_js = self.create_publisher(JointState, "/gripper/joint_states", 10)
|
||||
self.create_subscription(Float32MultiArray, "/gripper/ctrl", self._on_ctrl, 10)
|
||||
self.create_subscription(Float32, "/gripper/set_position", self._on_set_pos, 10)
|
||||
|
||||
self.create_service(Trigger, "initialize", self._srv_init)
|
||||
self.create_service(Trigger, "open", self._srv_open)
|
||||
self.create_service(Trigger, "close", self._srv_close)
|
||||
self.create_service(Trigger, "/gripper/initialize", self._srv_init)
|
||||
self.create_service(Trigger, "/gripper/open", self._srv_open)
|
||||
self.create_service(Trigger, "/gripper/close", self._srv_close)
|
||||
|
||||
self.get_logger().info(
|
||||
f"DH {self._model} SN={self._sn or '?'} "
|
||||
@@ -154,6 +159,11 @@ class DhPgeaNode(TolerantNode):
|
||||
|
||||
def _on_ctrl(self, msg: Float32MultiArray) -> None:
|
||||
initialize, position, force, speed = _as_ctrl_array(msg)
|
||||
# 写在执行之前。串口堵住时，只有这一行能看出命令是何时进来的。
|
||||
self.get_logger().info(
|
||||
f"收到夹爪命令 initialize={initialize:.0f} pos={position:.0f} "
|
||||
f"force={force:.0f} speed={speed:.0f}"
|
||||
)
|
||||
try:
|
||||
if initialize > 0.5:
|
||||
self._grip.initialize()
|
||||
@@ -164,14 +174,14 @@ class DhPgeaNode(TolerantNode):
|
||||
info = self._apply_move(int(position), f, s)
|
||||
self.get_logger().info(f"ctrl {info}")
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
self._note_bus_error(exc)
|
||||
|
||||
def _on_set_pos(self, msg: Float32) -> None:
|
||||
try:
|
||||
info = self._apply_move(int(msg.data))
|
||||
self.get_logger().info(f"set_position {info}")
|
||||
except DhPgeaError as exc:
|
||||
self.get_logger().error(str(exc))
|
||||
self._note_bus_error(exc)
|
||||
|
||||
def _srv_init(self, _req, resp):
|
||||
try:
|
||||
@@ -179,29 +189,63 @@ class DhPgeaNode(TolerantNode):
|
||||
resp.success = True
|
||||
resp.message = self._grip.get_run_states().summary()
|
||||
except DhPgeaError as exc:
|
||||
self._note_bus_error(exc)
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
|
||||
def _srv_open(self, _req, resp):
|
||||
return self._srv_goto(_req, resp, self._open_pos, self._force, "未全开")
|
||||
|
||||
def _srv_close(self, _req, resp):
|
||||
# 20–33% 力碰到阻力会提前停，固件仍报到位；全闭至少 80%。
|
||||
return self._srv_goto(_req, resp, self._close_pos, max(int(self._force), 80), "未闭合到位")
|
||||
|
||||
def _srv_goto(self, _req, resp, position, force, fail_prefix):
|
||||
from arm_application.dh_pgea import grip_motion_settled
|
||||
try:
|
||||
info = self._apply_move(self._open_pos)
|
||||
resp.success = True
|
||||
resp.message = info
|
||||
info = self._apply_move(int(position), int(force), self._speed)
|
||||
st = self._grip.wait_settle(int(position), timeout_s=8.0)
|
||||
if not grip_motion_settled(st.grip_state, st.position, int(position), True):
|
||||
self._apply_move(int(position), int(force), self._speed)
|
||||
st = self._grip.wait_settle(int(position), timeout_s=6.0)
|
||||
self._state = st
|
||||
ok = grip_motion_settled(st.grip_state, st.position, int(position), True)
|
||||
resp.success = bool(ok)
|
||||
resp.message = st.summary() if ok else f"{fail_prefix} {st.summary()}（{info}）"
|
||||
except DhPgeaError as exc:
|
||||
self._note_bus_error(exc)
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
|
||||
def _srv_close(self, _req, resp):
|
||||
def _reopen_serial(self) -> None:
|
||||
"""串口 EIO 后先关再重开。1 秒最多一次，避免把 USB 打爆。"""
|
||||
now = time.monotonic()
|
||||
if now - self._last_reopen < 1.0:
|
||||
return
|
||||
self._last_reopen = now
|
||||
try:
|
||||
info = self._apply_move(self._close_pos)
|
||||
resp.success = True
|
||||
resp.message = info
|
||||
except DhPgeaError as exc:
|
||||
resp.success = False
|
||||
resp.message = str(exc)
|
||||
return resp
|
||||
self._grip.close()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
self._grip.open()
|
||||
self._grip.probe()
|
||||
except (DhPgeaError, OSError) as exc:
|
||||
self.get_logger().error(
|
||||
f"夹爪串口重开失败：{exc}", throttle_duration_sec=10.0
|
||||
)
|
||||
return
|
||||
self._online = True
|
||||
self.get_logger().info("夹爪串口已重开，总线恢复应答")
|
||||
|
||||
def _note_bus_error(self, exc: Exception) -> None:
|
||||
"""命令回调里的失败也走同一套：标离线，串口坏了就重开。"""
|
||||
self._online = False
|
||||
self.get_logger().error(str(exc))
|
||||
if isinstance(exc, DhPgeaBusError):
|
||||
self._reopen_serial()
|
||||
|
||||
def _typed_status(self, error=""):
|
||||
state = self._state
|
||||
@@ -223,6 +267,7 @@ class DhPgeaNode(TolerantNode):
|
||||
msg.error_msg = error
|
||||
return msg
|
||||
|
||||
|
||||
def _srv_execute(self, request, response):
|
||||
command = request.command
|
||||
try:
|
||||
@@ -255,28 +300,56 @@ class DhPgeaNode(TolerantNode):
|
||||
response.status = self._typed_status("" if response.success else response.message)
|
||||
return response
|
||||
|
||||
|
||||
def _on_timer(self) -> None:
|
||||
online = True
|
||||
try:
|
||||
st = self._grip.get_run_states(retries=2 if self._online else 1)
|
||||
self._state = st
|
||||
self._fail_streak = 0
|
||||
if not self._online:
|
||||
self._online = True
|
||||
self.get_logger().info("夹爪总线已恢复")
|
||||
except DhPgeaBusError as exc:
|
||||
self._online = False
|
||||
self._fail_streak = 0
|
||||
self.get_logger().warn(str(exc), throttle_duration_sec=5.0)
|
||||
self._reopen_serial()
|
||||
online = False
|
||||
except DhPgeaError as exc:
|
||||
self._online = False
|
||||
self._fail_streak += 1
|
||||
self.get_logger().warn(str(exc), throttle_duration_sec=5.0)
|
||||
self._pub_typed.publish(self._typed_status(str(exc)))
|
||||
return
|
||||
if self._fail_streak >= BUS_REOPEN_AFTER_FAILURES:
|
||||
# 写有回显、读一直无应答时，重试同一个串口会话没用，必须重开。
|
||||
self.get_logger().warn(
|
||||
f"连续 {self._fail_streak} 拍读不到状态，先重开串口再继续重试"
|
||||
)
|
||||
self._fail_streak = 0
|
||||
self._reopen_serial()
|
||||
online = False
|
||||
self._publish_state(online)
|
||||
|
||||
self._pub_typed.publish(self._typed_status())
|
||||
def _publish_state(self, online: bool) -> None:
|
||||
"""离线也要发一帧，前 5 个是上一次的值、最后一位是 0。
|
||||
|
||||
不发的话上层只看到"没有 /gripper/states"，会把"总线读不到"当成
|
||||
"夹爪没到位"。只有 5 个元素的老消费者照旧能用。
|
||||
"""
|
||||
self._pub_typed.publish(self._typed_status("" if online else "夹爪总线离线"))
|
||||
st = self._state
|
||||
n = STATES_ONLINE_INDEX + 1
|
||||
arr = Float32MultiArray()
|
||||
arr.layout = MultiArrayLayout(
|
||||
dim=[MultiArrayDimension(label="dh_pgea", size=5, stride=5)],
|
||||
dim=[MultiArrayDimension(label="dh_pgea", size=n, stride=n)],
|
||||
data_offset=0,
|
||||
)
|
||||
arr.data = list(st.as_tuple())
|
||||
arr.data = list(st.as_tuple()) + [1.0 if online else 0.0]
|
||||
self._pub_state.publish(arr)
|
||||
|
||||
# 关节状态没有"离线"位：宁可不发，也不要把上一次的开口当成现在的。
|
||||
if not online:
|
||||
return
|
||||
js = JointState()
|
||||
js.header.stamp = self.get_clock().now().to_msg()
|
||||
js.name = ["gripper_finger1_joint"]
|
||||
|
||||
@@ -0,0 +1,803 @@
|
||||
"""双臂夹爪统一控制架构（DH-Robotics 官方 Modbus RTU，非 Flexiv RDK）。
|
||||
|
||||
设计目标：
|
||||
- 左臂（Rizon4-063989）：大寰 PGEA 平行夹爪，控制行程/夹持力/速度。
|
||||
- 右臂（Rizon4-063991）：大寰 RGI-100-30-O-S-M1 旋转夹爪，控制手指夹持与旋转角度/圈数/速度。
|
||||
- 统一接口，便于后续扩展到 ROS2 节点和新项目脚本。
|
||||
|
||||
注意：DH 官方协议是“写位置/角度寄存器后立即运动”，没有 ChangingTek SDK 的 0x0108 触发寄存器。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from abc import ABC, abstractmethod
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, auto
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.dh_official import DHModbusClient, DHModbusError
|
||||
|
||||
|
||||
class GripperType(Enum):
|
||||
PARALLEL = auto() # 平行夹爪
|
||||
ROTARY = auto() # 旋转夹爪
|
||||
VACUUM = auto() # 真空吸盘（预留）
|
||||
|
||||
|
||||
class GripperState(Enum):
|
||||
IDLE = auto()
|
||||
MOVING = auto()
|
||||
GRIPPING = auto()
|
||||
RELEASING = auto()
|
||||
ERROR = auto()
|
||||
|
||||
|
||||
@dataclass
|
||||
class GripperStatus:
|
||||
"""夹爪状态反馈。"""
|
||||
|
||||
state: GripperState = GripperState.IDLE
|
||||
position: float = 0.0 # 平行：mm；旋转：角度 deg
|
||||
force: float = 0.0 # 最近一次命令力/力矩 %
|
||||
velocity: float = 0.0 # 最近一次命令速度 %
|
||||
is_moving: bool = False
|
||||
is_gripped: bool = False # 是否夹持到物体
|
||||
connected: bool = False
|
||||
initialized: bool = False
|
||||
finger_position_per_mille: float = 0.0
|
||||
rotation_deg: float = 0.0
|
||||
gripper_state: int = -1 # DH 0x0201：0 moving,1 reached,2 caught,3 dropped
|
||||
rotation_state: int = -1 # RGI 0x020B：0 moving,1 reached,2 blocking,3 blocked
|
||||
error_msg: str = ""
|
||||
timestamp: float = field(default_factory=time.time)
|
||||
|
||||
|
||||
@dataclass
|
||||
class GripperCommand:
|
||||
"""夹爪控制指令。"""
|
||||
|
||||
target_position: float | None = None # 平行：mm；旋转：deg
|
||||
target_force: float | None = None # 目标力/力矩 %
|
||||
target_velocity: float | None = None # 目标速度 %
|
||||
wait_complete: bool = True # 是否等待完成
|
||||
timeout: float = 10.0 # 超时时间
|
||||
|
||||
|
||||
class BaseGripper(ABC):
|
||||
"""夹爪基类，定义统一接口。"""
|
||||
|
||||
name: str = "base_gripper"
|
||||
gripper_type: GripperType = GripperType.PARALLEL
|
||||
|
||||
def __init__(self, config: dict[str, Any] | None = None) -> None:
|
||||
self.cfg = config or {}
|
||||
self._connected = False
|
||||
self._status = GripperStatus()
|
||||
|
||||
@abstractmethod
|
||||
def connect(self) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def disconnect(self) -> None: ...
|
||||
|
||||
@abstractmethod
|
||||
def execute(self, cmd: GripperCommand) -> GripperStatus: ...
|
||||
|
||||
@abstractmethod
|
||||
def get_status(self) -> GripperStatus: ...
|
||||
|
||||
def is_connected(self) -> bool:
|
||||
return self._connected
|
||||
|
||||
def open(self, **kwargs) -> GripperStatus:
|
||||
raise NotImplementedError
|
||||
|
||||
def close(self, **kwargs) -> GripperStatus:
|
||||
raise NotImplementedError
|
||||
|
||||
def stop(self) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 大寰平行夹爪（PGE/PGEA 系列）
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class ParallelGripperConfig:
|
||||
"""大寰平行夹爪配置。"""
|
||||
|
||||
port: str = "/dev/ttyUSB0"
|
||||
slave_id: int = 1
|
||||
baudrate: int = 115200
|
||||
timeout: float = 0.2
|
||||
|
||||
# PGEA-100-40 行程为 40mm；若实际是 PGEA-100-26，请把 stroke_max 改成 26.0。
|
||||
stroke_min: float = 0.0
|
||||
stroke_max: float = 40.0
|
||||
|
||||
# 默认参数（DH 百分比）
|
||||
default_speed: int = 50
|
||||
default_force: int = 50
|
||||
force_min: int = 20
|
||||
force_max: int = 100
|
||||
|
||||
# 控制行为
|
||||
auto_initialize: bool = True
|
||||
init_timeout: float = 10.0
|
||||
position_tolerance_per_mille: int = 15
|
||||
status_poll_interval: float = 0.05
|
||||
|
||||
|
||||
class ParallelGripper(BaseGripper):
|
||||
"""大寰 PGE/PGEA 平行夹爪。"""
|
||||
|
||||
name = "dh_pgea_parallel"
|
||||
gripper_type = GripperType.PARALLEL
|
||||
|
||||
def __init__(self, config: ParallelGripperConfig | None = None) -> None:
|
||||
super().__init__()
|
||||
self.cfg = config or ParallelGripperConfig()
|
||||
self._client: DHModbusClient | None = None
|
||||
self._last_position_mm = float(self.cfg.stroke_min)
|
||||
self._last_force = int(self.cfg.default_force)
|
||||
self._last_speed = int(self.cfg.default_speed)
|
||||
|
||||
def connect(self) -> None:
|
||||
if self._client is not None:
|
||||
self.disconnect()
|
||||
self._client = DHModbusClient(
|
||||
self.cfg.port,
|
||||
slave_id=self.cfg.slave_id,
|
||||
baudrate=self.cfg.baudrate,
|
||||
timeout=self.cfg.timeout,
|
||||
)
|
||||
self._connected = True
|
||||
self._status.connected = True
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.error_msg = ""
|
||||
if self.cfg.auto_initialize:
|
||||
self.initialize()
|
||||
|
||||
def disconnect(self) -> None:
|
||||
if self._client is not None:
|
||||
self._client.close()
|
||||
self._client = None
|
||||
self._connected = False
|
||||
self._status.connected = False
|
||||
self._status.state = GripperState.IDLE
|
||||
|
||||
def initialize(self, full: bool = False) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._client.initialize(full=full, rotation=False, timeout=self.cfg.init_timeout)
|
||||
self._status.initialized = True
|
||||
|
||||
def _require_client(self) -> None:
|
||||
if not self._connected or self._client is None:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "平行夹爪未连接"
|
||||
raise DHModbusError(self._status.error_msg)
|
||||
|
||||
def _mm_to_per_mille(self, position_mm: float) -> int:
|
||||
lo = float(self.cfg.stroke_min)
|
||||
hi = float(self.cfg.stroke_max)
|
||||
if hi <= lo:
|
||||
hi = lo + 1.0
|
||||
ratio = (float(position_mm) - lo) / (hi - lo)
|
||||
return int(np.clip(round(ratio * 1000.0), 0, 1000))
|
||||
|
||||
def _per_mille_to_mm(self, position: float | int) -> float:
|
||||
lo = float(self.cfg.stroke_min)
|
||||
hi = float(self.cfg.stroke_max)
|
||||
return lo + (float(position) / 1000.0) * (hi - lo)
|
||||
|
||||
def execute(self, cmd: GripperCommand) -> GripperStatus:
|
||||
try:
|
||||
self._require_client()
|
||||
except DHModbusError:
|
||||
return self._status
|
||||
assert self._client is not None
|
||||
|
||||
position_mm = self._last_position_mm if cmd.target_position is None else float(cmd.target_position)
|
||||
position_mm = float(np.clip(position_mm, self.cfg.stroke_min, self.cfg.stroke_max))
|
||||
speed = int(np.clip(cmd.target_velocity or self.cfg.default_speed, 1, 100))
|
||||
force = int(np.clip(cmd.target_force or self.cfg.default_force, self.cfg.force_min, self.cfg.force_max))
|
||||
|
||||
self._client.set_speed_pct(speed)
|
||||
self._client.set_force_pct(force)
|
||||
target_per_mille = self._client.set_position_per_mille(self._mm_to_per_mille(position_mm))
|
||||
|
||||
self._last_position_mm = position_mm
|
||||
self._last_speed = speed
|
||||
self._last_force = force
|
||||
self._status.state = GripperState.MOVING
|
||||
self._status.error_msg = ""
|
||||
|
||||
if cmd.wait_complete:
|
||||
self._wait_complete(target_per_mille, cmd.timeout)
|
||||
return self.get_status()
|
||||
|
||||
def _wait_complete(self, target_per_mille: int, timeout: float) -> None:
|
||||
assert self._client is not None
|
||||
start = time.monotonic()
|
||||
time.sleep(0.05)
|
||||
while time.monotonic() - start < timeout:
|
||||
state = self._client.get_gripper_state()
|
||||
pos = self._client.get_position_per_mille()
|
||||
self._status.gripper_state = state
|
||||
self._status.finger_position_per_mille = float(pos)
|
||||
self._status.position = self._per_mille_to_mm(pos)
|
||||
self._status.is_moving = state == 0
|
||||
self._status.is_gripped = state == 2
|
||||
if state in (1, 2):
|
||||
self._status.state = GripperState.GRIPPING if state == 2 else GripperState.IDLE
|
||||
return
|
||||
if state == 3:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "夹持后物体掉落"
|
||||
return
|
||||
if abs(pos - target_per_mille) <= self.cfg.position_tolerance_per_mille and state != 0:
|
||||
self._status.state = GripperState.IDLE
|
||||
return
|
||||
time.sleep(self.cfg.status_poll_interval)
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = f"平行夹爪运动超时（>{timeout:.1f}s）"
|
||||
|
||||
def get_status(self) -> GripperStatus:
|
||||
if not self._connected or self._client is None:
|
||||
self._status.connected = False
|
||||
return self._status
|
||||
try:
|
||||
pos = self._client.get_position_per_mille()
|
||||
state = self._client.get_gripper_state()
|
||||
init_state = self._client.get_init_state()
|
||||
self._status.connected = True
|
||||
self._status.initialized = init_state == 1
|
||||
self._status.finger_position_per_mille = float(pos)
|
||||
self._status.position = self._per_mille_to_mm(pos)
|
||||
self._status.velocity = float(self._last_speed)
|
||||
self._status.force = float(self._last_force)
|
||||
self._status.gripper_state = state
|
||||
self._status.is_moving = state == 0
|
||||
self._status.is_gripped = state == 2
|
||||
if self._status.state != GripperState.ERROR:
|
||||
self._status.state = GripperState.MOVING if state == 0 else (
|
||||
GripperState.GRIPPING if state == 2 else GripperState.IDLE
|
||||
)
|
||||
self._status.timestamp = time.time()
|
||||
except Exception as exc:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = str(exc)
|
||||
self._status.connected = False
|
||||
return self._status
|
||||
|
||||
def open(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
|
||||
# DH 千分比：0‰=闭合，1000‰=张开，对应行程 0mm -> stroke_max。
|
||||
return self.execute(GripperCommand(
|
||||
target_position=float(self.cfg.stroke_max),
|
||||
target_force=force or self.cfg.default_force,
|
||||
target_velocity=speed or self.cfg.default_speed,
|
||||
**kwargs,
|
||||
))
|
||||
|
||||
def close(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
|
||||
return self.execute(GripperCommand(
|
||||
target_position=float(self.cfg.stroke_min),
|
||||
target_force=force or self.cfg.default_force,
|
||||
target_velocity=speed or self.cfg.default_speed,
|
||||
**kwargs,
|
||||
))
|
||||
|
||||
def move_to(self, position_mm: float, force: int | None = None, speed: int | None = None,
|
||||
**kwargs) -> GripperStatus:
|
||||
return self.execute(GripperCommand(
|
||||
target_position=position_mm,
|
||||
target_force=force or self.cfg.default_force,
|
||||
target_velocity=speed or self.cfg.default_speed,
|
||||
**kwargs,
|
||||
))
|
||||
|
||||
def set_force(self, force_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_force = self._client.set_force_pct(force_pct)
|
||||
|
||||
def set_speed(self, speed_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_speed = self._client.set_speed_pct(speed_pct)
|
||||
|
||||
def stop(self) -> None:
|
||||
if self._client is not None:
|
||||
self._client.hold_current_position()
|
||||
self._status.state = GripperState.IDLE
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 大寰旋转夹爪（RGI-100-30-O-S-M1）
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class RotaryGripperConfig:
|
||||
"""大寰 RGI 旋转夹爪配置。"""
|
||||
|
||||
port: str = "/dev/ttyUSB1"
|
||||
slave_id: int = 1
|
||||
baudrate: int = 115200
|
||||
timeout: float = 0.2
|
||||
|
||||
# RGI 旋转角度寄存器是 signed 16bit；为避免 one's-complement 负零歧义，默认限制在 ±32767。
|
||||
angle_min: float = -32767.0
|
||||
angle_max: float = 32767.0
|
||||
max_step_deg: float = 30000.0 # rotate_revolutions 分段旋转的最大单步角度
|
||||
|
||||
# DH 手册：手指位置 0~1000‰。0=闭合，1000=张开。RGI-100 行程约 70mm。
|
||||
finger_open_per_mille: int = 1000
|
||||
finger_close_per_mille: int = 0
|
||||
|
||||
# 默认参数（DH 百分比）
|
||||
default_speed: int = 50 # 旋转速度 %
|
||||
default_torque: int = 50 # 旋转力/力矩 %
|
||||
default_finger_speed: int = 50 # 手指速度 %
|
||||
default_finger_force: int = 50 # 手指夹持力 %
|
||||
torque_min: int = 20
|
||||
torque_max: int = 100
|
||||
|
||||
# 控制行为
|
||||
auto_initialize: bool = True
|
||||
init_timeout: float = 15.0 # RGI 初始化包含旋转找零，时间更长
|
||||
angle_tolerance_deg: float = 1.0
|
||||
status_poll_interval: float = 0.05
|
||||
|
||||
|
||||
class RotaryGripper(BaseGripper):
|
||||
"""大寰 RGI-100-30-O-S-M1 旋转夹爪。"""
|
||||
|
||||
name = "dh_rgi_rotary"
|
||||
gripper_type = GripperType.ROTARY
|
||||
|
||||
def __init__(self, config: RotaryGripperConfig | None = None) -> None:
|
||||
super().__init__()
|
||||
self.cfg = config or RotaryGripperConfig()
|
||||
self._client: DHModbusClient | None = None
|
||||
self._last_angle = 0.0
|
||||
self._last_torque = int(self.cfg.default_torque)
|
||||
self._last_speed = int(self.cfg.default_speed)
|
||||
self._last_finger_force = int(self.cfg.default_finger_force)
|
||||
self._last_finger_speed = int(self.cfg.default_finger_speed)
|
||||
self._last_finger_per_mille = int(self.cfg.finger_open_per_mille)
|
||||
|
||||
def connect(self) -> None:
|
||||
if self._client is not None:
|
||||
self.disconnect()
|
||||
self._client = DHModbusClient(
|
||||
self.cfg.port,
|
||||
slave_id=self.cfg.slave_id,
|
||||
baudrate=self.cfg.baudrate,
|
||||
timeout=self.cfg.timeout,
|
||||
)
|
||||
self._connected = True
|
||||
self._status.connected = True
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.error_msg = ""
|
||||
if self.cfg.auto_initialize:
|
||||
self.initialize()
|
||||
|
||||
def disconnect(self) -> None:
|
||||
if self._client is not None:
|
||||
self._client.close()
|
||||
self._client = None
|
||||
self._connected = False
|
||||
self._status.connected = False
|
||||
self._status.state = GripperState.IDLE
|
||||
|
||||
def initialize(self, full: bool = False) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._client.initialize(full=full, rotation=True, timeout=self.cfg.init_timeout)
|
||||
self._status.initialized = True
|
||||
|
||||
def _require_client(self) -> None:
|
||||
if not self._connected or self._client is None:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "旋转夹爪未连接"
|
||||
raise DHModbusError(self._status.error_msg)
|
||||
|
||||
def execute(self, cmd: GripperCommand) -> GripperStatus:
|
||||
"""执行旋转指令；cmd.target_position 单位为度。"""
|
||||
return self.rotate_to(
|
||||
self._last_angle if cmd.target_position is None else float(cmd.target_position),
|
||||
speed=cmd.target_velocity,
|
||||
torque=cmd.target_force,
|
||||
wait_complete=cmd.wait_complete,
|
||||
timeout=cmd.timeout,
|
||||
)
|
||||
|
||||
def _wait_complete(self, target_angle: float, timeout: float) -> None:
|
||||
assert self._client is not None
|
||||
start = time.monotonic()
|
||||
start_angle = float(self._client.get_rotation_angle_deg())
|
||||
expected = abs(float(target_angle) - start_angle) + 30.0
|
||||
settled = 0
|
||||
last_angle = start_angle
|
||||
time.sleep(0.05)
|
||||
while time.monotonic() - start < timeout:
|
||||
rot_state = self._client.get_rotation_state()
|
||||
angle = float(self._client.get_rotation_angle_deg())
|
||||
self._status.rotation_state = rot_state
|
||||
self._status.rotation_deg = angle
|
||||
self._status.position = angle
|
||||
self._status.is_moving = rot_state == 0
|
||||
if abs(angle - start_angle) > expected:
|
||||
self._client.hold_current_rotation()
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = (
|
||||
f"旋转超程保护：目标 {target_angle:.1f}°，从 {start_angle:.1f}° 跑到 {angle:.1f}°，已发停止"
|
||||
)
|
||||
return
|
||||
if rot_state in (1, 2):
|
||||
self._status.state = GripperState.IDLE if rot_state == 1 else GripperState.GRIPPING
|
||||
if rot_state == 2:
|
||||
self._status.error_msg = "旋转受阻（blocking）"
|
||||
return
|
||||
if rot_state == 3:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "旋转曾受阻且未到目标位"
|
||||
return
|
||||
if abs(angle - float(target_angle)) <= self.cfg.angle_tolerance_deg:
|
||||
settled += 1
|
||||
if settled >= 3:
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.is_moving = False
|
||||
return
|
||||
elif abs(angle - last_angle) <= self.cfg.angle_tolerance_deg:
|
||||
settled += 1
|
||||
if settled >= 8 and abs(angle - float(target_angle)) <= 15.0:
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.is_moving = False
|
||||
return
|
||||
else:
|
||||
settled = 0
|
||||
last_angle = angle
|
||||
time.sleep(self.cfg.status_poll_interval)
|
||||
self._client.hold_current_rotation()
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = f"旋转运动超时（>{timeout:.1f}s），已发停止"
|
||||
|
||||
def get_status(self) -> GripperStatus:
|
||||
if not self._connected or self._client is None:
|
||||
self._status.connected = False
|
||||
return self._status
|
||||
try:
|
||||
finger_pos = self._client.get_position_per_mille()
|
||||
finger_state = self._client.get_gripper_state()
|
||||
init_state = self._client.get_init_state()
|
||||
rot_angle = self._client.get_rotation_angle_deg()
|
||||
rot_state = self._client.get_rotation_state()
|
||||
rot_init = self._client.get_rotation_init_state()
|
||||
|
||||
self._status.connected = True
|
||||
self._status.initialized = init_state == 1 and rot_init == 1
|
||||
self._status.finger_position_per_mille = float(finger_pos)
|
||||
self._status.rotation_deg = float(rot_angle)
|
||||
self._status.position = float(rot_angle)
|
||||
self._status.velocity = float(self._last_speed)
|
||||
self._status.force = float(self._last_torque)
|
||||
self._status.gripper_state = finger_state
|
||||
self._status.rotation_state = rot_state
|
||||
self._status.is_moving = finger_state == 0 or rot_state == 0
|
||||
self._status.is_gripped = finger_state == 2
|
||||
if self._status.state != GripperState.ERROR:
|
||||
if finger_state == 2 or rot_state == 2:
|
||||
self._status.state = GripperState.GRIPPING
|
||||
elif finger_state == 0 or rot_state == 0:
|
||||
self._status.state = GripperState.MOVING
|
||||
else:
|
||||
self._status.state = GripperState.IDLE
|
||||
self._status.timestamp = time.time()
|
||||
except Exception as exc:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = str(exc)
|
||||
self._status.connected = False
|
||||
return self._status
|
||||
|
||||
# -----------------------------
|
||||
# 手指夹持
|
||||
# -----------------------------
|
||||
def move_fingers(self, position_per_mille: int, force: int | None = None,
|
||||
speed: int | None = None, wait_complete: bool = True,
|
||||
timeout: float = 10.0) -> GripperStatus:
|
||||
try:
|
||||
self._require_client()
|
||||
except DHModbusError:
|
||||
return self._status
|
||||
assert self._client is not None
|
||||
target = int(np.clip(position_per_mille, 0, 1000))
|
||||
f = int(np.clip(force or self.cfg.default_finger_force, 20, 100))
|
||||
s = int(np.clip(speed or self.cfg.default_finger_speed, 1, 100))
|
||||
self._client.set_speed_pct(s)
|
||||
self._client.set_force_pct(f)
|
||||
self._client.set_position_per_mille(target)
|
||||
self._last_finger_per_mille = target
|
||||
self._last_finger_force = f
|
||||
self._last_finger_speed = s
|
||||
if wait_complete:
|
||||
start = time.monotonic()
|
||||
time.sleep(0.05)
|
||||
while time.monotonic() - start < timeout:
|
||||
state = self._client.get_gripper_state()
|
||||
pos = self._client.get_position_per_mille()
|
||||
self._status.gripper_state = state
|
||||
self._status.finger_position_per_mille = float(pos)
|
||||
if state in (1, 2):
|
||||
break
|
||||
if state == 3:
|
||||
self._status.state = GripperState.ERROR
|
||||
self._status.error_msg = "夹持后物体掉落"
|
||||
break
|
||||
time.sleep(self.cfg.status_poll_interval)
|
||||
return self.get_status()
|
||||
|
||||
def open_fingers(self, **kwargs) -> GripperStatus:
|
||||
return self.move_fingers(self.cfg.finger_open_per_mille, **kwargs)
|
||||
|
||||
def close_fingers(self, **kwargs) -> GripperStatus:
|
||||
return self.move_fingers(self.cfg.finger_close_per_mille, **kwargs)
|
||||
|
||||
# -----------------------------
|
||||
# 旋转
|
||||
# -----------------------------
|
||||
def rotate_to(self, angle_deg: float, speed: int | None = None, torque: int | None = None,
|
||||
wait_complete: bool = True, timeout: float = 10.0, **kwargs) -> GripperStatus:
|
||||
try:
|
||||
self._require_client()
|
||||
except DHModbusError:
|
||||
return self._status
|
||||
assert self._client is not None
|
||||
|
||||
angle = float(np.clip(angle_deg, self.cfg.angle_min, self.cfg.angle_max))
|
||||
s = int(np.clip(speed or self.cfg.default_speed, 1, 100))
|
||||
t = int(np.clip(torque or self.cfg.default_torque, self.cfg.torque_min, self.cfg.torque_max))
|
||||
|
||||
self._client.set_rotation_speed_pct(s)
|
||||
self._client.set_rotation_force_pct(t)
|
||||
target = self._client.set_rotation_angle_deg(angle)
|
||||
|
||||
self._last_angle = float(target)
|
||||
self._last_speed = s
|
||||
self._last_torque = t
|
||||
self._status.state = GripperState.MOVING
|
||||
self._status.error_msg = ""
|
||||
|
||||
if wait_complete:
|
||||
self._wait_complete(float(target), timeout)
|
||||
return self.get_status()
|
||||
|
||||
def rotate_by(self, delta_deg: float, **kwargs) -> GripperStatus:
|
||||
current = self.get_status().rotation_deg
|
||||
return self.rotate_to(current + float(delta_deg), **kwargs)
|
||||
|
||||
def rotate_revolutions(self, revolutions: float, **kwargs) -> GripperStatus:
|
||||
"""旋转指定圈数；超过单步角度范围时自动分段，并按实际反馈角度结算。"""
|
||||
remaining = float(revolutions) * 360.0
|
||||
status = self.get_status()
|
||||
max_step = abs(float(self.cfg.max_step_deg)) or 30000.0
|
||||
while abs(remaining) > 1e-6:
|
||||
before = float(self.get_status().rotation_deg)
|
||||
step = float(np.clip(remaining, -max_step, max_step))
|
||||
status = self.rotate_to(before + step, **kwargs)
|
||||
if status.state == GripperState.ERROR:
|
||||
return status
|
||||
after = float(status.rotation_deg)
|
||||
achieved = after - before
|
||||
if abs(achieved) < max(1.0, abs(step) * 0.2):
|
||||
status.state = GripperState.ERROR
|
||||
status.error_msg = (
|
||||
"旋转未达到预期步进，可能超出角度寄存器范围或反馈异常："
|
||||
f"step={step:.3f}, before={before:.3f}, after={after:.3f}"
|
||||
)
|
||||
return status
|
||||
remaining -= achieved
|
||||
return status
|
||||
|
||||
def home_rotation(self, **kwargs) -> GripperStatus:
|
||||
return self.rotate_to(0.0, **kwargs)
|
||||
|
||||
def set_rotation_speed(self, speed_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_speed = self._client.set_rotation_speed_pct(speed_pct)
|
||||
|
||||
def set_rotation_torque(self, torque_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_torque = self._client.set_rotation_force_pct(torque_pct)
|
||||
|
||||
def set_finger_speed(self, speed_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_finger_speed = self._client.set_speed_pct(speed_pct)
|
||||
|
||||
def set_finger_force(self, force_pct: int) -> None:
|
||||
self._require_client()
|
||||
assert self._client is not None
|
||||
self._last_finger_force = self._client.set_force_pct(force_pct)
|
||||
|
||||
def open(self, **kwargs) -> GripperStatus:
|
||||
"""对于 RGI，open 定义为张开手指；旋转请用 rotate_to/rotate_revolutions。"""
|
||||
return self.open_fingers(**kwargs)
|
||||
|
||||
def close(self, **kwargs) -> GripperStatus:
|
||||
"""对于 RGI，close 定义为闭合手指；旋转请用 rotate_to/rotate_revolutions。"""
|
||||
return self.close_fingers(**kwargs)
|
||||
|
||||
def stop(self) -> None:
|
||||
if self._client is not None:
|
||||
self._client.hold_current_all()
|
||||
self._status.state = GripperState.IDLE
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 双臂夹爪管理器
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class DualArmGripperConfig:
|
||||
"""双臂夹爪配置。
|
||||
|
||||
这里**没有** ``left_arm_sn`` / ``right_arm_sn``：它们曾经存在但从来没有被读过
|
||||
（只写不读的死字段，R5.5），而且默认值是本机的 SN —— 换台臂就会误导读者。
|
||||
哪个侧别是哪台臂，由 ``load_from_site()`` 从 site.yaml 的 ``robots[].arm`` 决定。
|
||||
"""
|
||||
|
||||
left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig)
|
||||
right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig)
|
||||
|
||||
|
||||
class DualArmGripperManager:
|
||||
"""双臂夹爪管理器：统一控制左右臂夹爪。"""
|
||||
|
||||
def __init__(self, config: DualArmGripperConfig | None = None) -> None:
|
||||
self.cfg = config or DualArmGripperConfig()
|
||||
self._left: ParallelGripper | None = None
|
||||
self._right: RotaryGripper | None = None
|
||||
|
||||
@property
|
||||
def left(self) -> ParallelGripper:
|
||||
if self._left is None:
|
||||
self._left = ParallelGripper(self.cfg.left_gripper)
|
||||
return self._left
|
||||
|
||||
@property
|
||||
def right(self) -> RotaryGripper:
|
||||
if self._right is None:
|
||||
self._right = RotaryGripper(self.cfg.right_gripper)
|
||||
return self._right
|
||||
|
||||
def connect_all(self) -> None:
|
||||
self.left.connect()
|
||||
self.right.connect()
|
||||
|
||||
def disconnect_all(self) -> None:
|
||||
if self._left:
|
||||
self._left.disconnect()
|
||||
if self._right:
|
||||
self._right.disconnect()
|
||||
|
||||
def initialize_all(self, full: bool = False) -> None:
|
||||
self.left.initialize(full=full)
|
||||
self.right.initialize(full=full)
|
||||
|
||||
def get_all_status(self) -> dict[str, GripperStatus]:
|
||||
return {
|
||||
"left": self.left.get_status(),
|
||||
"right": self.right.get_status(),
|
||||
}
|
||||
|
||||
def emergency_stop(self) -> None:
|
||||
self.left.stop()
|
||||
self.right.stop()
|
||||
|
||||
|
||||
def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig:
|
||||
"""从 configs/dual_arm_robot.yaml 加载双臂夹爪配置。"""
|
||||
import yaml
|
||||
|
||||
data = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {}
|
||||
left = data.get("left_arm", {}) or {}
|
||||
right = data.get("right_arm", {}) or {}
|
||||
grip = data.get("gripper", {}) or {}
|
||||
parallel = grip.get("parallel", {}) or {}
|
||||
rotary = grip.get("rotary", {}) or {}
|
||||
|
||||
left_cfg = ParallelGripperConfig(
|
||||
port=left.get("gripper_port", "/dev/ttyUSB0"),
|
||||
slave_id=int(left.get("gripper_slave_id", 1)),
|
||||
baudrate=int(left.get("gripper_baudrate", 115200)),
|
||||
stroke_min=float(parallel.get("stroke_min", 0.0)),
|
||||
stroke_max=float(parallel.get("stroke_max", 40.0)),
|
||||
default_speed=int(parallel.get("default_speed", 50)),
|
||||
default_force=int(parallel.get("default_force", 50)),
|
||||
force_min=int(parallel.get("force_min", 20)),
|
||||
force_max=int(parallel.get("force_max", 100)),
|
||||
auto_initialize=bool(parallel.get("auto_initialize", True)),
|
||||
)
|
||||
right_cfg = RotaryGripperConfig(
|
||||
port=right.get("gripper_port", "/dev/ttyUSB1"),
|
||||
slave_id=int(right.get("gripper_slave_id", 1)),
|
||||
baudrate=int(right.get("gripper_baudrate", 115200)),
|
||||
angle_min=float(rotary.get("angle_min", -32767.0)),
|
||||
angle_max=float(rotary.get("angle_max", 32767.0)),
|
||||
max_step_deg=float(rotary.get("max_step_deg", 30000.0)),
|
||||
finger_open_per_mille=int(rotary.get("finger_open_per_mille", 1000)),
|
||||
finger_close_per_mille=int(rotary.get("finger_close_per_mille", 0)),
|
||||
default_speed=int(rotary.get("default_speed", 50)),
|
||||
default_torque=int(rotary.get("default_torque", 50)),
|
||||
default_finger_speed=int(rotary.get("default_finger_speed", 50)),
|
||||
default_finger_force=int(rotary.get("default_finger_force", 50)),
|
||||
torque_min=int(rotary.get("torque_min", 20)),
|
||||
torque_max=int(rotary.get("torque_max", 100)),
|
||||
auto_initialize=bool(rotary.get("auto_initialize", True)),
|
||||
)
|
||||
return DualArmGripperConfig(
|
||||
left_gripper=left_cfg,
|
||||
right_gripper=right_cfg,
|
||||
)
|
||||
|
||||
|
||||
def load_from_site(data: dict) -> DualArmGripperConfig:
|
||||
"""从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。
|
||||
|
||||
左右靠 ``robots[].arm`` 认，**不靠 SN 里的数字**：``"063989 就是左臂"``
|
||||
是站点数据，写进代码后换个站点就会把左右夹爪搞反
|
||||
（症状是"右臂的旋转爪去开左臂的平行爪"，串口对不上才报错）。
|
||||
"""
|
||||
robots = list((data.get("robot") or {}).get("robots") or [])
|
||||
left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {})
|
||||
right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {})
|
||||
g = data.get("gripper") or {}
|
||||
parallel = g.get("parallel") or {}
|
||||
rotary = g.get("rotary") or {}
|
||||
lg = left.get("gripper") or {}
|
||||
rg = right.get("gripper") or {}
|
||||
stroke = float(lg.get("stroke_mm") or parallel.get("stroke_max") or g.get("stroke_mm") or 20.0)
|
||||
left_cfg = ParallelGripperConfig(
|
||||
port=str(lg.get("port") or "/dev/ttyUSB0"),
|
||||
slave_id=int(lg.get("slave_id") or g.get("slave_id") or 1),
|
||||
baudrate=int(lg.get("baud") or g.get("baud") or 115200),
|
||||
stroke_min=float(parallel.get("stroke_min") or 0.0),
|
||||
stroke_max=stroke,
|
||||
default_speed=int(parallel.get("default_speed") or g.get("speed_pct") or 50),
|
||||
default_force=int(parallel.get("default_force") or g.get("force_pct") or 50),
|
||||
force_min=int(parallel.get("force_min") or 20),
|
||||
force_max=int(parallel.get("force_max") or 100),
|
||||
auto_initialize=bool(lg.get("auto_init", parallel.get("auto_initialize", True))),
|
||||
init_timeout=float(parallel.get("init_timeout") or 10.0),
|
||||
)
|
||||
right_cfg = RotaryGripperConfig(
|
||||
port=str(rg.get("port") or "/dev/ttyUSB1"),
|
||||
slave_id=int(rg.get("slave_id") or 1),
|
||||
baudrate=int(rg.get("baud") or g.get("baud") or 115200),
|
||||
angle_min=float(rotary.get("angle_min") or -32767.0),
|
||||
angle_max=float(rotary.get("angle_max") or 32767.0),
|
||||
max_step_deg=float(rotary.get("max_step_deg") or 30000.0),
|
||||
finger_open_per_mille=int(rotary.get("finger_open_per_mille") or 1000),
|
||||
finger_close_per_mille=int(rotary.get("finger_close_per_mille") or 0),
|
||||
default_speed=int(rotary.get("default_speed") or 50),
|
||||
default_torque=int(rotary.get("default_torque") or 50),
|
||||
default_finger_speed=int(rotary.get("default_finger_speed") or 50),
|
||||
default_finger_force=int(rotary.get("default_finger_force") or 50),
|
||||
torque_min=int(rotary.get("torque_min") or 20),
|
||||
torque_max=int(rotary.get("torque_max") or 100),
|
||||
auto_initialize=bool(rg.get("auto_init", rotary.get("auto_initialize", False))),
|
||||
init_timeout=float(rotary.get("init_timeout") or 15.0),
|
||||
)
|
||||
return DualArmGripperConfig(
|
||||
left_gripper=left_cfg,
|
||||
right_gripper=right_cfg,
|
||||
)
|
||||
@@ -0,0 +1,190 @@
|
||||
#!/usr/bin/env python3
|
||||
"""DH PGEA（左臂）与 RGI（右臂）的命令行工具。从 site.yaml 读取串口。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
from arm_application.dual_gripper import DualArmGripperManager, load_from_site
|
||||
|
||||
|
||||
def _site_path() -> Path:
|
||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||
if env:
|
||||
return Path(env).expanduser()
|
||||
return Path.home() / "ros2" / "config" / "site.yaml"
|
||||
|
||||
|
||||
def _print_status(label: str, status) -> None:
|
||||
print(
|
||||
f" {label}: state={status.state.name} "
|
||||
f"pos={status.position:.2f} finger={status.finger_position_per_mille:.0f}‰ "
|
||||
f"rot={status.rotation_deg:.2f}° moving={status.is_moving} "
|
||||
f"gripped={status.is_gripped} init={status.initialized} "
|
||||
f"err={status.error_msg or '-'}"
|
||||
)
|
||||
|
||||
|
||||
def _load_site_exporter():
|
||||
"""复用 ``scripts/lib/export_site_env.py`` —— 它才是 --arm 别名的权威定义。
|
||||
|
||||
这里**不自己再抄一份** ``89 -> SN`` 的映射：抄一份就多一处会漂移的地方（M2）。
|
||||
"""
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
lib = str(root / "scripts" / "lib")
|
||||
if lib not in sys.path:
|
||||
sys.path.insert(0, lib)
|
||||
import export_site_env # type: ignore
|
||||
|
||||
return export_site_env
|
||||
|
||||
|
||||
def _normalize_arm(raw: str, data: dict) -> str:
|
||||
"""把 --arm 的写法归一成 ``left`` / ``right`` / ``both``。
|
||||
|
||||
别名（``89`` 这类）由 ``export_site_env`` 从 site.yaml 的 ``robots[]`` 现算
|
||||
（臂号 = 该臂 SN 末两位）。代码里**不写死** SN，否则换台机器人后会指错臂，
|
||||
而症状是"给右臂发命令，左臂动了"。
|
||||
"""
|
||||
t = (raw or "").strip()
|
||||
if t.lower() in ("both", "dual", "all"):
|
||||
return "both"
|
||||
site_env = _load_site_exporter()
|
||||
sn, matched = site_env.resolve_robot_sn(data, t)
|
||||
if not matched:
|
||||
raise SystemExit(
|
||||
f"未知的 --arm {raw!r}：不在 site.yaml 的 robot.robots[] 里。\n"
|
||||
f"可用：\n{site_env.describe_arms(data)}\n"
|
||||
f"也可以直接写 left|right|both。"
|
||||
)
|
||||
for row in (data.get("robot") or {}).get("robots") or []:
|
||||
if str(row.get("sn") or "").strip() != sn:
|
||||
continue
|
||||
arm = str(row.get("arm") or "").strip().lower()
|
||||
if arm in ("left", "right"):
|
||||
return arm
|
||||
raise SystemExit(
|
||||
f"--arm {raw!r} 选中了 {sn}，但 site.yaml 里这台臂没写 arm: left|right，"
|
||||
f"无法判断操作哪个夹爪。请补上这个键。"
|
||||
)
|
||||
raise SystemExit(
|
||||
f"--arm {raw!r} 解析出 {sn}，但它不在 site.yaml 的 robot.robots[] 里，"
|
||||
f"无法判断是哪个夹爪。"
|
||||
)
|
||||
|
||||
|
||||
def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
||||
gripper = manager.left
|
||||
print(f"=== PGEA 平行夹爪（{gripper.cfg.port}）===")
|
||||
gripper.connect()
|
||||
_print_status("已连接", gripper.get_status())
|
||||
if args.test in ("init", "probe", "all"):
|
||||
if args.test != "probe":
|
||||
gripper.initialize(full=args.init_full)
|
||||
_print_status("初始化", gripper.get_status())
|
||||
if args.test in ("open", "all"):
|
||||
_print_status("张开", gripper.open(force=args.force, speed=args.speed))
|
||||
if args.test in ("close", "all"):
|
||||
_print_status("闭合", gripper.close(force=args.force, speed=args.speed))
|
||||
if args.test in ("move", "pos") and args.pos is not None:
|
||||
_print_status("移动", gripper.move_to(args.pos, force=args.force, speed=args.speed))
|
||||
if args.test in ("status", "probe"):
|
||||
_print_status("状态", gripper.get_status())
|
||||
|
||||
|
||||
def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
|
||||
gripper = manager.right
|
||||
print(f"=== RGI 旋转夹爪（{gripper.cfg.port}）===")
|
||||
gripper.connect()
|
||||
_print_status("已连接", gripper.get_status())
|
||||
if args.test in ("init", "all"):
|
||||
gripper.initialize(full=args.init_full)
|
||||
_print_status("初始化", gripper.get_status())
|
||||
if args.test in ("open", "all"):
|
||||
_print_status("张开", gripper.open_fingers(force=args.force, speed=args.speed))
|
||||
if args.test in ("close", "all"):
|
||||
_print_status("闭合", gripper.close_fingers(force=args.force, speed=args.speed))
|
||||
if args.test in ("move", "pos") and args.pos is not None:
|
||||
_print_status("手指", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed))
|
||||
if args.test in ("rotate", "rotate_to", "all"):
|
||||
if args.revolutions is not None:
|
||||
_print_status(
|
||||
"圈数",
|
||||
gripper.rotate_revolutions(args.revolutions, speed=args.speed, torque=args.force),
|
||||
)
|
||||
else:
|
||||
angle = args.angle if args.angle is not None else 90.0
|
||||
_print_status("旋转", gripper.rotate_to(angle, speed=args.speed, torque=args.force))
|
||||
if args.test == "rotate_by":
|
||||
delta = args.angle if args.angle is not None else 45.0
|
||||
_print_status("相对旋转", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
|
||||
if args.test in ("status", "probe"):
|
||||
_print_status("状态", gripper.get_status())
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）")
|
||||
parser.add_argument(
|
||||
"--arm",
|
||||
# 默认取当前臂（parse_arm_args.sh / load_site_config.sh 已经把它放进环境）。
|
||||
# **不**兜底成某个具体臂号：那等于"没选臂时也能跑，跑的是哪台看运气"。
|
||||
default=(
|
||||
os.environ.get("SITE_ROBOT_ARM")
|
||||
or os.environ.get("ROBOT_SN")
|
||||
or os.environ.get("SITE_ROBOT_SN")
|
||||
or ""
|
||||
),
|
||||
help="left|right|both，或该臂的臂号（= SN 末两位，由 site.yaml 决定）",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--test",
|
||||
default="status",
|
||||
help="probe|init|open|close|move|pos|rotate|rotate_by|status",
|
||||
)
|
||||
parser.add_argument("--pos", type=float, default=None, help="PGEA 为 mm；RGI 为手指 0-1000")
|
||||
parser.add_argument("--angle", type=float, default=None)
|
||||
parser.add_argument("--revolutions", type=float, default=None)
|
||||
parser.add_argument("--speed", type=int, default=25)
|
||||
parser.add_argument("--force", type=int, default=33)
|
||||
parser.add_argument("--init-full", action="store_true")
|
||||
parser.add_argument("--site", default="")
|
||||
args = parser.parse_args()
|
||||
|
||||
site = Path(args.site).expanduser() if args.site else _site_path()
|
||||
if not site.is_file():
|
||||
print(f"错误：site.yaml 缺失：{site}", file=sys.stderr)
|
||||
return 1
|
||||
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||
manager = DualArmGripperManager(load_from_site(data))
|
||||
arm = _normalize_arm(str(args.arm), data)
|
||||
print(f"site={site} arm={arm} test={args.test}")
|
||||
|
||||
try:
|
||||
if arm == "left":
|
||||
test_parallel(manager, args)
|
||||
elif arm == "right":
|
||||
test_rotary(manager, args)
|
||||
else:
|
||||
test_parallel(manager, args)
|
||||
test_rotary(manager, args)
|
||||
except KeyboardInterrupt:
|
||||
print("收到中断 —— 停止")
|
||||
manager.emergency_stop()
|
||||
return 130
|
||||
except Exception as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
return 1
|
||||
finally:
|
||||
manager.disconnect_all()
|
||||
time.sleep(0.05)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,320 @@
|
||||
"""ROS 2 双臂 DH 夹爪节点（左臂 PGEA + 右臂 RGI）。使用 arm_msgs，而非 Flexiv RDK。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import threading
|
||||
from pathlib import Path
|
||||
|
||||
import rclpy
|
||||
import yaml
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.dual_gripper import (
|
||||
DualArmGripperManager,
|
||||
GripperState,
|
||||
load_dual_arm_gripper_config,
|
||||
load_from_site,
|
||||
)
|
||||
from arm_msgs.msg import GripperCommand as GripperCommandMsg
|
||||
from arm_msgs.msg import GripperStatus as GripperStatusMsg
|
||||
from arm_msgs.srv import GripperExecute
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
def _site_path() -> Path:
|
||||
env = os.environ.get("SITE_CONFIG", "").strip()
|
||||
if env:
|
||||
return Path(env).expanduser()
|
||||
return Path.home() / "ros2" / "config" / "site.yaml"
|
||||
|
||||
|
||||
def _as_bool(val, default: bool = True) -> bool:
|
||||
if val is None:
|
||||
return default
|
||||
if isinstance(val, str):
|
||||
return val.strip().lower() in ("1", "true", "yes", "on")
|
||||
return bool(val)
|
||||
|
||||
|
||||
class DualGripperNode(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("dual_gripper_driver")
|
||||
self.declare_parameter("config_file", "")
|
||||
self.declare_parameter("auto_connect", True)
|
||||
self.declare_parameter("allow_offline", True)
|
||||
self.declare_parameter("publish_hz", 10.0)
|
||||
self.declare_parameter("sides", "left,right")
|
||||
|
||||
self._cb_group = ReentrantCallbackGroup()
|
||||
self._locks = {"left": threading.Lock(), "right": threading.Lock()}
|
||||
sides_raw = str(self.get_parameter("sides").value or "left,right")
|
||||
self._sides = [s.strip().lower() for s in sides_raw.split(",") if s.strip()]
|
||||
if not self._sides:
|
||||
self._sides = ["left", "right"]
|
||||
|
||||
cfg_path = self.get_parameter("config_file").get_parameter_value().string_value.strip()
|
||||
if cfg_path:
|
||||
self._manager = DualArmGripperManager(load_dual_arm_gripper_config(cfg_path))
|
||||
else:
|
||||
site = _site_path()
|
||||
data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||
self._manager = DualArmGripperManager(load_from_site(data))
|
||||
self.get_logger().info(f"夹爪配置来自 site.yaml：{site}")
|
||||
|
||||
self._connect_errors: dict[str, str] = {}
|
||||
if _as_bool(self.get_parameter("auto_connect").value, True):
|
||||
self._connect_all()
|
||||
|
||||
self._left_pub = self.create_publisher(GripperStatusMsg, "~/left/status", 10)
|
||||
self._right_pub = self.create_publisher(GripperStatusMsg, "~/right/status", 10)
|
||||
if "left" in self._sides:
|
||||
self.create_subscription(
|
||||
GripperCommandMsg,
|
||||
"~/left/command",
|
||||
lambda msg: self._on_command("left", msg),
|
||||
10,
|
||||
callback_group=self._cb_group,
|
||||
)
|
||||
self.create_service(
|
||||
GripperExecute,
|
||||
"~/left/execute",
|
||||
lambda req, resp: self._on_execute("left", req, resp),
|
||||
callback_group=self._cb_group,
|
||||
)
|
||||
if "right" in self._sides:
|
||||
self.create_subscription(
|
||||
GripperCommandMsg,
|
||||
"~/right/command",
|
||||
lambda msg: self._on_command("right", msg),
|
||||
10,
|
||||
callback_group=self._cb_group,
|
||||
)
|
||||
self.create_service(
|
||||
GripperExecute,
|
||||
"~/right/execute",
|
||||
lambda req, resp: self._on_execute("right", req, resp),
|
||||
callback_group=self._cb_group,
|
||||
)
|
||||
|
||||
period = 1.0 / max(1e-3, float(self.get_parameter("publish_hz").value))
|
||||
self.create_timer(period, self._publish_status, callback_group=self._cb_group)
|
||||
self.get_logger().info(
|
||||
f"dual_gripper_driver ready sides={self._sides} "
|
||||
"~/left|right/command status execute"
|
||||
)
|
||||
|
||||
def _connect_all(self) -> None:
|
||||
mapping = (("left", self._manager.left), ("right", self._manager.right))
|
||||
for side, gripper in mapping:
|
||||
if side not in self._sides:
|
||||
continue
|
||||
try:
|
||||
gripper.connect()
|
||||
self.get_logger().info(f"{side} 夹爪已连接：{gripper.cfg.port}")
|
||||
except Exception as exc:
|
||||
self._connect_errors[side] = str(exc)
|
||||
self.get_logger().error(f"{side} 夹爪连接失败：{exc}")
|
||||
if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True):
|
||||
raise RuntimeError(f"夹爪连接失败：{self._connect_errors}")
|
||||
|
||||
def _gripper(self, side: str):
|
||||
return self._manager.left if side == "left" else self._manager.right
|
||||
|
||||
def _execute(self, side: str, msg: GripperCommandMsg):
|
||||
gripper = self._gripper(side)
|
||||
command = (msg.command or "").strip().lower()
|
||||
timeout = float(msg.timeout or 10.0)
|
||||
wait = bool(msg.wait_complete)
|
||||
|
||||
if side in self._connect_errors and not gripper.is_connected():
|
||||
status = gripper.get_status()
|
||||
status.error_msg = self._connect_errors[side]
|
||||
return status
|
||||
|
||||
if command == "connect":
|
||||
gripper.connect()
|
||||
self._connect_errors.pop(side, None)
|
||||
return gripper.get_status()
|
||||
if command == "init":
|
||||
gripper.initialize(full=False)
|
||||
return gripper.get_status()
|
||||
if command == "init_full":
|
||||
gripper.initialize(full=True)
|
||||
return gripper.get_status()
|
||||
if command == "stop":
|
||||
gripper.stop()
|
||||
return gripper.get_status()
|
||||
|
||||
if side == "left":
|
||||
if command == "open":
|
||||
return gripper.open(
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "close":
|
||||
return gripper.close(
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "move":
|
||||
return gripper.move_to(
|
||||
float(msg.position),
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "set_force":
|
||||
gripper.set_force(int(msg.force))
|
||||
return gripper.get_status()
|
||||
if command == "set_speed":
|
||||
gripper.set_speed(int(msg.speed))
|
||||
return gripper.get_status()
|
||||
else:
|
||||
if command == "open":
|
||||
return gripper.open_fingers(
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "close":
|
||||
return gripper.close_fingers(
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "move":
|
||||
return gripper.move_fingers(
|
||||
int(msg.position),
|
||||
force=int(msg.force) if msg.force else None,
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command in ("rotate", "rotate_to"):
|
||||
return gripper.rotate_to(
|
||||
float(msg.position),
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
torque=int(msg.force) if msg.force else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "rotate_by":
|
||||
return gripper.rotate_by(
|
||||
float(msg.position),
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
torque=int(msg.force) if msg.force else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "rotate_revolutions":
|
||||
return gripper.rotate_revolutions(
|
||||
float(msg.position),
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
torque=int(msg.force) if msg.force else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "home_rotation":
|
||||
return gripper.home_rotation(
|
||||
speed=int(msg.speed) if msg.speed else None,
|
||||
torque=int(msg.force) if msg.force else None,
|
||||
wait_complete=wait,
|
||||
timeout=timeout,
|
||||
)
|
||||
if command == "set_force":
|
||||
gripper.set_rotation_torque(int(msg.force))
|
||||
return gripper.get_status()
|
||||
if command == "set_speed":
|
||||
gripper.set_rotation_speed(int(msg.speed))
|
||||
return gripper.get_status()
|
||||
if command == "set_finger_force":
|
||||
gripper.set_finger_force(int(msg.force))
|
||||
return gripper.get_status()
|
||||
if command == "set_finger_speed":
|
||||
gripper.set_finger_speed(int(msg.speed))
|
||||
return gripper.get_status()
|
||||
|
||||
status = gripper.get_status()
|
||||
status.state = GripperState.ERROR
|
||||
status.error_msg = f"不支持的命令 {command!r}（side={side}）"
|
||||
return status
|
||||
|
||||
def _on_command(self, side: str, msg: GripperCommandMsg) -> None:
|
||||
with self._locks[side]:
|
||||
status = self._execute(side, msg)
|
||||
if status.state == GripperState.ERROR:
|
||||
self.get_logger().warn(f"{side} 命令 {msg.command!r} 失败：{status.error_msg}")
|
||||
|
||||
def _on_execute(self, side: str, request: GripperExecute.Request, response: GripperExecute.Response):
|
||||
with self._locks[side]:
|
||||
status = self._execute(side, request.command)
|
||||
response.success = status.state != GripperState.ERROR and status.connected
|
||||
response.message = status.error_msg or "OK"
|
||||
response.status = self._to_status_msg(side, status)
|
||||
return response
|
||||
|
||||
def _to_status_msg(self, side: str, status) -> GripperStatusMsg:
|
||||
gripper = self._gripper(side)
|
||||
msg = GripperStatusMsg()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = f"{side}_gripper"
|
||||
msg.name = gripper.name
|
||||
msg.gripper_type = gripper.gripper_type.name.lower()
|
||||
msg.state = status.state.name
|
||||
msg.connected = bool(status.connected)
|
||||
msg.initialized = bool(status.initialized)
|
||||
msg.is_moving = bool(status.is_moving)
|
||||
msg.is_gripped = bool(status.is_gripped)
|
||||
msg.position = float(status.position)
|
||||
msg.finger_position_per_mille = float(status.finger_position_per_mille)
|
||||
msg.rotation_deg = float(status.rotation_deg)
|
||||
msg.gripper_state = int(status.gripper_state)
|
||||
msg.rotation_state = int(status.rotation_state)
|
||||
msg.velocity = float(status.velocity)
|
||||
msg.force = float(status.force)
|
||||
msg.error_msg = status.error_msg or ""
|
||||
return msg
|
||||
|
||||
def _publish_status(self) -> None:
|
||||
if "left" in self._sides:
|
||||
left_status = self._manager.left.get_status()
|
||||
if "left" in self._connect_errors and not left_status.connected:
|
||||
left_status.error_msg = self._connect_errors["left"]
|
||||
self._left_pub.publish(self._to_status_msg("left", left_status))
|
||||
if "right" in self._sides:
|
||||
right_status = self._manager.right.get_status()
|
||||
if "right" in self._connect_errors and not right_status.connected:
|
||||
right_status.error_msg = self._connect_errors["right"]
|
||||
self._right_pub.publish(self._to_status_msg("right", right_status))
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
try:
|
||||
self._manager.disconnect_all()
|
||||
finally:
|
||||
return super().destroy_node()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
rclpy.init()
|
||||
node = DualGripperNode()
|
||||
executor = MultiThreadedExecutor(num_threads=4)
|
||||
try:
|
||||
rclpy.spin(node, executor=executor)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,615 @@
|
||||
"""通止规装配的配置：从 ``site.yaml`` 的 ``gauge_assembly:`` 段解析 + 校验。
|
||||
|
||||
为什么单独一个模块
|
||||
------------------
|
||||
这一段的键**全是会动的量**（力阈值、角度、悬高、超时），也是现场最常改的地方。
|
||||
散在节点里读的话会有三个已知坑（都被这个仓库踩过）：
|
||||
|
||||
1. **名字与单位不一致** —— ``hover`` 填了毫米、代码按米用（规则 M3）；
|
||||
2. **键没接线** —— YAML 里写着 ``screw_step_deg``，代码读的是 ``screw_steps``，
|
||||
于是改了没效果，而报错也没有（``config_guard`` 模块的头注释就是这件事）；
|
||||
3. **拼错键名** —— YAML 静默接受，行为退回默认值，现场"改了参数没反应"。
|
||||
|
||||
所以：所有键在一张表里声明（含单位），解析时**逐条校验**，多出来的键直接
|
||||
报错并给出最相近的候选名，缺的键按 required/default 明确处理。
|
||||
``describe()`` 把**派生**结果（比如"逆时针最多能转 147°"）打出来，让现场
|
||||
看到的是"实际生效的值"，而不是"我填的值"。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import difflib
|
||||
from dataclasses import dataclass, fields
|
||||
from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple
|
||||
|
||||
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",
|
||||
"deg": "_deg",
|
||||
"mm": "_mm",
|
||||
"m": "_m",
|
||||
"px": "_px",
|
||||
"pct": "_pct",
|
||||
"permille": "_permille",
|
||||
"au": "_delta",
|
||||
# 腕部 FT 的**真牛顿**。与 au 分开一个后缀是刻意的：两个口径混用不会报错，
|
||||
# 只会"判据不触发"或"一碰就炸"，看起来像算法问题（见 site.yaml 的注释）。
|
||||
"N": "_n",
|
||||
}
|
||||
|
||||
#: 显式无量纲的键（布尔、计数、比例、枚举）。**必须在这里列出来** ——
|
||||
#: 没有单位又不登记的键，会让"单位自检"变成一句空话（规则 M1）。
|
||||
_UNITLESS = frozenset(
|
||||
{
|
||||
"extreme_direction",
|
||||
"grip_check",
|
||||
"coarse_enabled",
|
||||
"coarse_camera",
|
||||
"coarse_conf",
|
||||
"snap_min_frames",
|
||||
"snap_stable_feature",
|
||||
"screw_max_cycles",
|
||||
"unscrew_extra_cycles",
|
||||
"holes_total",
|
||||
"dry_run",
|
||||
}
|
||||
)
|
||||
|
||||
#: 每个键的**类型**（用于类型校验，不做隐式转换：``"20"`` 不当 20 用）。
|
||||
_TYPES: Dict[str, type] = {
|
||||
"boot_timeout_s": float,
|
||||
"extreme_angle_deg": float,
|
||||
"extreme_speed_deg_s": float,
|
||||
"extreme_direction": int,
|
||||
"extreme_margin_deg": float,
|
||||
"grip_check": bool,
|
||||
"grip_hold_s": float,
|
||||
"coarse_enabled": bool,
|
||||
"coarse_camera": str,
|
||||
"coarse_hover_m": float,
|
||||
"coarse_conf": float,
|
||||
"coarse_timeout_s": float,
|
||||
"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,
|
||||
"contact_max_depth_mm": float,
|
||||
"tool_tip_below_flange_m": float,
|
||||
"workpiece_surface_z_m": float,
|
||||
"contact_tangential_delta": float,
|
||||
"contact_axial_delta_n": float,
|
||||
"contact_relief_mm": float,
|
||||
"contact_hold_n": float,
|
||||
"contact_safe_delta": float,
|
||||
"screw_step_deg": float,
|
||||
"screw_speed_deg_s": float,
|
||||
"screw_back_speed_deg_s": float,
|
||||
"screw_max_cycles": int,
|
||||
"screw_normal_delta": float,
|
||||
"screw_shear_delta": float,
|
||||
"screw_grip_force_pct": float,
|
||||
"screw_grip_speed_pct": float,
|
||||
"screw_release_wait_s": float,
|
||||
"unscrew_extra_cycles": int,
|
||||
"holes_total": int,
|
||||
"dry_run": bool,
|
||||
"reload_timeout_s": float,
|
||||
# 首次咬合之后的空回：松爪开度 + 微抬量（见 pipeline_node._run_a7_seat）
|
||||
"unwind_open_permille": int,
|
||||
"unwind_lift_mm": float,
|
||||
}
|
||||
|
||||
#: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。
|
||||
_UNITS: Dict[str, str] = {
|
||||
"boot_timeout_s": "s",
|
||||
"extreme_angle_deg": "deg",
|
||||
"extreme_speed_deg_s": "deg/s",
|
||||
"extreme_direction": "",
|
||||
"extreme_margin_deg": "deg",
|
||||
"grip_check": "",
|
||||
"grip_hold_s": "s",
|
||||
"coarse_enabled": "",
|
||||
"coarse_camera": "",
|
||||
"coarse_hover_m": "m",
|
||||
"coarse_conf": "",
|
||||
"coarse_timeout_s": "s",
|
||||
"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",
|
||||
"contact_max_depth_mm": "mm",
|
||||
# 这两个是**长度**不是"毫米量"：用 m 口径，与 hover_z 同一把尺
|
||||
"tool_tip_below_flange_m": "m",
|
||||
"workpiece_surface_z_m": "m",
|
||||
"contact_tangential_delta": "au",
|
||||
# 腕部 FT 是**真牛顿**（与触觉的 au 不是一个口径，所以键名也不带 taut 前缀）
|
||||
"contact_axial_delta_n": "N",
|
||||
# 卸载回退是**长度**（与 step_mm 同一把尺）
|
||||
"contact_relief_mm": "mm",
|
||||
# 残余轴向力是**牛顿**（腕部 FT 口径，不是触觉的 au）
|
||||
"contact_hold_n": "N",
|
||||
"contact_safe_delta": "au",
|
||||
"screw_step_deg": "deg",
|
||||
"screw_speed_deg_s": "deg/s",
|
||||
"screw_back_speed_deg_s": "deg/s",
|
||||
"screw_max_cycles": "",
|
||||
"screw_normal_delta": "au",
|
||||
"screw_shear_delta": "au",
|
||||
"screw_grip_force_pct": "pct",
|
||||
"screw_grip_speed_pct": "pct",
|
||||
"screw_release_wait_s": "s",
|
||||
"unscrew_extra_cycles": "",
|
||||
"holes_total": "",
|
||||
"dry_run": "",
|
||||
"reload_timeout_s": "s",
|
||||
"unwind_open_permille": "permille",
|
||||
"unwind_lift_mm": "mm",
|
||||
}
|
||||
|
||||
#: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类
|
||||
#: 复合单位只检查主单位段。
|
||||
_SUFFIX_OVERRIDE: Dict[str, str] = {
|
||||
# 复合单位：后缀取"最后一个单位段"，键名里写全
|
||||
"contact_speed_m_s": "_m_s",
|
||||
"extreme_speed_deg_s": "_deg_s",
|
||||
"screw_speed_deg_s": "_deg_s",
|
||||
"screw_back_speed_deg_s": "_deg_s",
|
||||
}
|
||||
|
||||
|
||||
class GaugeConfigError(ValueError):
|
||||
"""``gauge_assembly:`` 段配置错误。消息里必须给出**改哪里**。"""
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 默认值
|
||||
# =============================================================================
|
||||
#: 缺省值。**只有真的有现场依据的才写默认值**：力阈值这类必须现场标定的，
|
||||
#: 默认值只是"能跑起来的起点"，必须在 ``describe()`` 里标出来让现场知道要改。
|
||||
DEFAULTS: Dict[str, Any] = {
|
||||
"boot_timeout_s": 30.0,
|
||||
# S01：逆时针预绕。符号靠 extreme_direction 定，不靠改代码里的负号。
|
||||
"extreme_angle_deg": 150.0,
|
||||
"extreme_speed_deg_s": 20.0,
|
||||
"extreme_direction": 1,
|
||||
"extreme_margin_deg": 3.0,
|
||||
# S02（预留）
|
||||
"grip_check": False,
|
||||
"grip_hold_s": 0.5,
|
||||
# S03
|
||||
"coarse_enabled": True,
|
||||
"coarse_camera": "d435",
|
||||
"coarse_hover_m": 0.25,
|
||||
"coarse_conf": 0.35,
|
||||
"coarse_timeout_s": 20.0,
|
||||
# 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
|
||||
"contact_speed_m_s": 0.02,
|
||||
"contact_step_mm": 0.5,
|
||||
"contact_max_depth_mm": 35.0,
|
||||
"tool_tip_below_flange_m": 0.243,
|
||||
"workpiece_surface_z_m": 0.0, # 0 = 未知，运行期用 stage2 实测孔位兜底
|
||||
"contact_tangential_delta": 300.0,
|
||||
# 腕部 FT 的轴向力阈值 [N]。**这是 S07 的首选判据**，触觉只是兜底。
|
||||
#
|
||||
# 为什么默认是 8N 这么小：这是"接触了没有"的判据，不是"压到位了没有"。
|
||||
# 台肩一落面，接触刚度 5e4 N/m 意味着过压 0.1mm 就有 5N、0.2mm 有 10N ——
|
||||
# 所以 8N 对应约 0.16mm 的过压，**在打滑之前**就能判到（打滑上限由
|
||||
# µ·N 决定：µ=0.25、N≈46N → 两指合计 ~23N ≈ 0.46mm）。留了 3 倍余量。
|
||||
# 现场标定法：空载慢慢压到台肩落面，看日志里"腕部轴向"从基线涨到多少，
|
||||
# 取那个值的 1/3 作为阈值（要能区分"擦到孔壁"和"台肩落面"）。
|
||||
"contact_axial_delta_n": 8.0,
|
||||
# 接触到之后的**卸载回退** [mm]。
|
||||
#
|
||||
# 为什么必须有这一步（2026-09-24 一整轮排查换来的）：
|
||||
# 位置控制的臂在"撞上硬面"时会把力顶到自己的**出力上限**才停住 ——
|
||||
# 仿真实测：一步 0.5mm 的指令压在工件上，腕部 FT 读数直接跳到 **+244N**，
|
||||
# 而臂这一步根本没走完（"实测只走了 +0.09mm"）。这不是仿真特有的：
|
||||
# 接触刚度越高、步长越大，峰值力就越大，而位置环**不限制**它。
|
||||
#
|
||||
# 规是**靠夹持摩擦**抱住的，轴向能传的力上限是 µ·N（两指合计 ~73N@µ=0.8）。
|
||||
# 244N 远超这个上限 ⇒ 规被轴向力**顶出爪外**（本次就是：S08 一转，
|
||||
# 规直接掉到地上，报 E1）。所以"判到接触"之后不能就那样顶着，
|
||||
# 必须先把下压量放掉，再交给 S08 拧入。
|
||||
#
|
||||
# 0.5mm 的来历：判到接触时的**真实**过压量通常只有 0.0x mm（刚度 5e4 N/m
|
||||
# 时，10N 对应 0.2mm），回退 0.5mm 足够把力放干净，而规尖仍在孔里
|
||||
# （通端穿板 18mm），不会脱出。
|
||||
"contact_relief_mm": 0.5,
|
||||
# 卸载后允许的残余轴向力 [N]。
|
||||
# 超了不失败（可能只是回退量不够），但要**大声报**：它说明"夹持摩擦力不够
|
||||
# 承受这次接触"，接着 S08 一转就会掉件。调法只有两条 —— 降峰值（调小
|
||||
# contact_step_mm / contact_relief_mm）或加握力（改夹爪合拢力度/指垫）。
|
||||
"contact_hold_n": 5.0,
|
||||
"contact_safe_delta": 2000.0,
|
||||
# S08
|
||||
# 步长必须满足 2×step + 2×余量 ≤ 关节7 行程（Rizon4/10 ±175° ⇒ step ≤ 167°）：
|
||||
# 180° 会必然贴限位（棘轮一个来回要 360° > 350°），实测第 3 轮 ABORT。
|
||||
"screw_step_deg": 120.0,
|
||||
"screw_speed_deg_s": 30.0,
|
||||
"screw_back_speed_deg_s": 120.0,
|
||||
"screw_max_cycles": 12,
|
||||
"screw_normal_delta": 400.0,
|
||||
"screw_shear_delta": 600.0,
|
||||
"screw_grip_force_pct": 50.0,
|
||||
"screw_grip_speed_pct": 50.0,
|
||||
"screw_release_wait_s": 0.3,
|
||||
# S09
|
||||
"unscrew_extra_cycles": 1,
|
||||
# 全局
|
||||
"holes_total": 2,
|
||||
"dry_run": False,
|
||||
# E1（掉件）重放：等操作员放好并确认 /gauge/reload_ok 的最长时间。
|
||||
# 超时 = 中止（臂在 home，人不在旁边时"继续等"比"停"更危险）。
|
||||
"reload_timeout_s": 180.0,
|
||||
# 首轮咬合之后的空回：只张开到这个开度（约 8mm），微抬这么多 Z 再转回去。
|
||||
# 实际下发在 pipeline_node._run_a7_seat（0–5mm、1–1000‰ 由那边也会再挡一次）。
|
||||
"unwind_open_permille": 400,
|
||||
"unwind_lift_mm": 3.0,
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GaugeConfig:
|
||||
"""通止规装配的全部可调量（解析并校验之后）。"""
|
||||
|
||||
boot_timeout_s: float
|
||||
extreme_angle_deg: float
|
||||
extreme_speed_deg_s: float
|
||||
extreme_direction: int
|
||||
extreme_margin_deg: float
|
||||
grip_check: bool
|
||||
grip_hold_s: float
|
||||
coarse_enabled: bool
|
||||
coarse_camera: str
|
||||
coarse_hover_m: float
|
||||
coarse_conf: float
|
||||
coarse_timeout_s: float
|
||||
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
|
||||
contact_max_depth_mm: float
|
||||
tool_tip_below_flange_m: float
|
||||
workpiece_surface_z_m: float
|
||||
contact_tangential_delta: float
|
||||
contact_axial_delta_n: float
|
||||
contact_relief_mm: float
|
||||
contact_hold_n: float
|
||||
contact_safe_delta: float
|
||||
screw_step_deg: float
|
||||
screw_speed_deg_s: float
|
||||
screw_back_speed_deg_s: float
|
||||
screw_max_cycles: int
|
||||
screw_normal_delta: float
|
||||
screw_shear_delta: float
|
||||
screw_grip_force_pct: float
|
||||
screw_grip_speed_pct: float
|
||||
screw_release_wait_s: float
|
||||
unscrew_extra_cycles: int
|
||||
holes_total: int
|
||||
dry_run: bool
|
||||
reload_timeout_s: float
|
||||
unwind_open_permille: int
|
||||
unwind_lift_mm: float
|
||||
|
||||
# -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）-------------------
|
||||
@property
|
||||
def screw_step_rad(self) -> float:
|
||||
import math
|
||||
|
||||
return math.radians(self.screw_step_deg)
|
||||
|
||||
@property
|
||||
def total_screw_deg(self) -> float:
|
||||
"""一根规要拧进去的总角度 = 单次转角 × 循环数。"""
|
||||
return self.screw_step_deg * self.screw_max_cycles
|
||||
|
||||
@property
|
||||
def unscrew_cycles(self) -> int:
|
||||
"""拧出要多转几轮：多留 ``unscrew_extra_cycles`` 轮，确保螺纹完全脱开。"""
|
||||
return self.screw_max_cycles + max(0, self.unscrew_extra_cycles)
|
||||
|
||||
def describe(self) -> List[str]:
|
||||
"""启动时打印的行：**实际生效的**值与派生结论。
|
||||
|
||||
特别是 S01 的"能转多少度"和 S08 的"要拧多少轮"—— 这两个数是现场
|
||||
判断"参数是否真的生效"最直接的依据，比打印"我读了 site.yaml"有用。
|
||||
"""
|
||||
return [
|
||||
f"通止规装配：每孔孔数 {self.holes_total}，干跑={self.dry_run}",
|
||||
f" S01 预绕 {self.extreme_angle_deg:.1f}°（方向={'逆时针' if self.extreme_direction > 0 else '顺时针'}，"
|
||||
f"限位余量 {self.extreme_margin_deg:.1f}°，关节速度 {self.extreme_speed_deg_s:.1f}°/s）",
|
||||
f" S03 粗定位={'开' if self.coarse_enabled else '关'} 相机={self.coarse_camera} "
|
||||
f"悬高 {self.coarse_hover_m * 1000:.0f}mm 置信度≥{self.coarse_conf:.2f}",
|
||||
f" S05 稳定 {self.snap_stable_s:.1f}s / 至少 {self.snap_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_axial_delta_n:.0f}N（**首选判据**，"
|
||||
"腕部 FT 在夹持摩擦的上游，工具打滑也测得准）,"
|
||||
f"切向突变阈值 +{self.contact_tangential_delta:.0f}au（触觉兜底，两通道任一，现场标定），"
|
||||
f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm",
|
||||
f" S07 接触后卸载：回退 {self.contact_relief_mm:.2f}mm，"
|
||||
f"残余轴向力要求 ≤{self.contact_hold_n:.1f}N"
|
||||
"（位置控制的臂撞上硬面会把力顶到出力上限再停住，实测能到 200N+；"
|
||||
"规靠夹持摩擦抱住，轴向上限只有 µ·N≈73N，顶着不放 S08 一转就掉件）",
|
||||
f" S07 工具几何：尖端在法兰下方 {self.tool_tip_below_flange_m * 1000:.1f}mm，"
|
||||
f"工件上表面 z={self.workpiece_surface_z_m:.3f}m"
|
||||
"（启动时会按这两个值 + servo.hole_hover_z_m 算「悬停时工具到底入没入工件」，"
|
||||
"入了会**明确报错**——见 gauge_fsm.tool_clearance_m）",
|
||||
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}」",
|
||||
f" S09 拧出 {self.unscrew_cycles} 轮（比拧入多 {self.unscrew_extra_cycles} 轮）",
|
||||
f" 首轮咬合后空回：松爪到 {self.unwind_open_permille}‰、微抬 Z "
|
||||
f"{self.unwind_lift_mm:.1f}mm，转回后按实测降回",
|
||||
]
|
||||
|
||||
|
||||
def _resolve_one(key: str, raw: Mapping[str, Any]) -> Tuple[Any, bool]:
|
||||
"""取一个键的值，返回 ``(值, 是否来自 site.yaml)``。"""
|
||||
if key in raw:
|
||||
value = raw[key]
|
||||
elif key in DEFAULTS:
|
||||
return DEFAULTS[key], False
|
||||
else:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.{key} 既没在 site.yaml 里给，也没有默认值。"
|
||||
f"请在 config/site.yaml 的 gauge_assembly: 段补上"
|
||||
f"（改完跑一次 bash scripts/show_site_config.sh 确认解析）"
|
||||
)
|
||||
|
||||
want = _TYPES[key]
|
||||
if want is bool and not isinstance(value, bool):
|
||||
raise GaugeConfigError(_type_msg(key, value, "布尔 true/false"))
|
||||
if want is int and (isinstance(value, bool) or not isinstance(value, int)):
|
||||
raise GaugeConfigError(_type_msg(key, value, "整数"))
|
||||
if want is float and (isinstance(value, bool) or not isinstance(value, (int, float))):
|
||||
raise GaugeConfigError(_type_msg(key, value, "数字（可带小数）"))
|
||||
if want is str and not isinstance(value, str):
|
||||
raise GaugeConfigError(_type_msg(key, value, "字符串"))
|
||||
return (float(value) if want is float else value), True
|
||||
|
||||
|
||||
def _type_msg(key: str, value: Any, human: str) -> str:
|
||||
unit = _UNITS[key]
|
||||
hint = f"，单位 {unit}" if unit else ""
|
||||
return (
|
||||
f"gauge_assembly.{key} 的类型不对：期望 {human}{hint}，"
|
||||
f"实际读到 {value!r}（{type(value).__name__}）。"
|
||||
"本模块不做隐式转换 —— \"20\" 不会被当成 20 用，"
|
||||
"因为那正是「填了字符串、跑起来是默认值」这类静默失效的入口"
|
||||
)
|
||||
|
||||
|
||||
def from_site(site: Mapping[str, Any], *, overrides: Optional[Mapping[str, Any]] = None) -> GaugeConfig:
|
||||
"""解析并校验 ``site.yaml`` 的 ``gauge_assembly:`` 段。
|
||||
|
||||
:param site: ``site.yaml`` 解析后的整份字典。
|
||||
:param overrides: CLI/launch 覆盖（只允许覆盖已知键，未知键报错）。
|
||||
:raises GaugeConfigError: 缺键、类型错、未知键、单位与键名不符。
|
||||
|
||||
整段缺失**不报错**（全部走默认值），但会在 :meth:`GaugeConfig.describe`
|
||||
之外由调用方打一条告警；理由是"默认值能不能用"取决于现场，
|
||||
由节点决定要不要拒绝启动，配置模块只负责如实解析。
|
||||
"""
|
||||
raw_any = site.get("gauge_assembly") or {}
|
||||
if not isinstance(raw_any, Mapping):
|
||||
raise GaugeConfigError(
|
||||
"site.yaml 的 gauge_assembly: 应该是一段映射（键值对），"
|
||||
f"实际是 {type(raw_any).__name__}"
|
||||
)
|
||||
raw: Dict[str, Any] = dict(raw_any)
|
||||
|
||||
if overrides:
|
||||
bad = [k for k in overrides if k not in _TYPES]
|
||||
if bad:
|
||||
raise GaugeConfigError(
|
||||
f"未知的覆盖参数: {', '.join(sorted(bad))}。"
|
||||
f"可在 gauge_assembly 段覆盖的键见 gauge_config._TYPES"
|
||||
)
|
||||
raw.update({k: v for k, v in overrides.items() if v is not None})
|
||||
|
||||
_reject_unknown(raw)
|
||||
values = {key: _resolve_one(key, raw)[0] for key in _TYPES}
|
||||
cfg = GaugeConfig(**values)
|
||||
_sanity(cfg)
|
||||
return cfg
|
||||
|
||||
|
||||
def _reject_unknown(raw: Mapping[str, Any]) -> None:
|
||||
"""多余的键直接报错，并给出最相近的候选名。
|
||||
|
||||
静默忽略多余键的代价是：现场写 ``screw_step_degs``，改参数没反应，
|
||||
而日志里一切正常。报错里带候选名，是因为拼错比写错新键常见得多。
|
||||
"""
|
||||
unknown = sorted(set(raw) - set(_TYPES))
|
||||
if not unknown:
|
||||
return
|
||||
lines: List[str] = []
|
||||
for key in unknown:
|
||||
close = difflib.get_close_matches(key, list(_TYPES), n=1, cutoff=0.6)
|
||||
hint = f"，是不是想写 {close[0]}？" if close else ""
|
||||
lines.append(f" - {key}{hint}")
|
||||
raise GaugeConfigError(
|
||||
"site.yaml 的 gauge_assembly: 段里有本框架不认识的键：\n"
|
||||
+ "\n".join(lines)
|
||||
+ "\n多余的键不会被忽略，因为「写了没生效」比「写错报错」难查得多。"
|
||||
"确认要新增配置项时，先在 gauge_config._TYPES / _UNITS 里登记"
|
||||
)
|
||||
|
||||
|
||||
def _sanity(cfg: GaugeConfig) -> None:
|
||||
"""跨键的合理性检查：单键合法、组合起来没意义的，在这里挡住。"""
|
||||
if cfg.holes_total < 1:
|
||||
raise GaugeConfigError(f"gauge_assembly.holes_total 至少为 1，收到 {cfg.holes_total}")
|
||||
if cfg.extreme_direction not in (-1, 1):
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.extreme_direction 只能是 +1（逆时针）或 -1（顺时针），"
|
||||
f"收到 {cfg.extreme_direction}"
|
||||
)
|
||||
if cfg.screw_step_deg <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.screw_step_deg 必须为正，收到 {cfg.screw_step_deg}"
|
||||
)
|
||||
if cfg.screw_max_cycles < 1:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.screw_max_cycles 至少为 1，收到 {cfg.screw_max_cycles}"
|
||||
)
|
||||
if cfg.extreme_angle_deg <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.extreme_angle_deg 必须为正（方向由 extreme_direction 决定，"
|
||||
f"不要用负角度表方向），收到 {cfg.extreme_angle_deg}"
|
||||
)
|
||||
if cfg.contact_safe_delta <= cfg.contact_tangential_delta:
|
||||
raise GaugeConfigError(
|
||||
f"contact_safe_delta({cfg.contact_safe_delta}) 必须大于 "
|
||||
f"contact_tangential_delta({cfg.contact_tangential_delta})："
|
||||
"前者是「立刻抬臂」的安全上限，后者只是「停止下压」。"
|
||||
"两者相等的话，接触一开始就会触发安全上限"
|
||||
)
|
||||
if cfg.tool_tip_below_flange_m <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"tool_tip_below_flange_m 必须为正（工具尖端在法兰**下方**多少米），"
|
||||
f"收到 {cfg.tool_tip_below_flange_m}。"
|
||||
"填 0 会让「悬停时工具会不会已经插进工件」这个判断失效"
|
||||
)
|
||||
if cfg.snap_tol_px <= 0.0:
|
||||
raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}")
|
||||
if not 0.0 <= cfg.unwind_lift_mm <= 5.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.unwind_lift_mm 只接受 0–5mm 的微抬，"
|
||||
f"收到 {cfg.unwind_lift_mm}。空回只需要让规离开孔口，抬多了会撞上方"
|
||||
)
|
||||
if not 1 <= cfg.unwind_open_permille <= 1000:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.unwind_open_permille 只接受 1–1000，"
|
||||
f"收到 {cfg.unwind_open_permille}（0=合，1000=全开）"
|
||||
)
|
||||
if cfg.contact_relief_mm < 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.contact_relief_mm 不能为负（0 = 接触到之后不卸载），"
|
||||
f"收到 {cfg.contact_relief_mm}。\n"
|
||||
" 它为什么值得一条检查：位置控制的臂撞上硬面会把轴向下压顶到**出力上限**"
|
||||
"才停住（实测 200N+），而规靠夹持摩擦抱住、轴向上限只有 µ·N（≈73N）。"
|
||||
"卸载这一步省掉，S08 一转就掉件（报 E1）—— 而那时的日志只会说"
|
||||
"「通止规掉了」，看不到真因。"
|
||||
)
|
||||
if cfg.contact_hold_n <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.contact_hold_n 必须为正（卸载后的残余轴向力上限 [N]），"
|
||||
f"收到 {cfg.contact_hold_n}。0 会让「卸载干不干净」永远判不过。"
|
||||
)
|
||||
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），不依赖检测器"
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 自检：键名必须体现单位（防"填了毫米按米用"）
|
||||
# =============================================================================
|
||||
def unit_violations(keys: Optional[Iterable[str]] = None) -> List[str]:
|
||||
"""返回"键名与单位不符"的清单（空 = 全部合规）。
|
||||
|
||||
规则：单位非空的键，**键名必须以对应的单位后缀结尾**（见 :data:`_UNIT_SUFFIX`）。
|
||||
例外只有 :data:`_SUFFIX_OVERRIDE` 里显式登记的复合单位。
|
||||
|
||||
为什么值得一条专门的检查：``hover`` 填毫米、代码按米用，会给出一个
|
||||
"看起来完全合理"的运动（悬停 5 变成悬停 5 米或 5 毫米），
|
||||
而 :mod:`arm_interfaces.config_guard` 的头注释记录的正是这类事故。
|
||||
"""
|
||||
bad: List[str] = []
|
||||
for key in keys if keys is not None else _UNITS:
|
||||
unit = _UNITS[key]
|
||||
if not unit:
|
||||
if key not in _UNITLESS:
|
||||
bad.append(f"{key}: 无量纲但未登记在 _UNITLESS")
|
||||
continue
|
||||
suffix = _SUFFIX_OVERRIDE.get(key)
|
||||
if suffix is None:
|
||||
base = unit.split("/")[0]
|
||||
suffix = _UNIT_SUFFIX.get(base)
|
||||
if suffix is None:
|
||||
bad.append(f"{key}: 单位 {unit} 没有登记后缀（补 _UNIT_SUFFIX 或 _SUFFIX_OVERRIDE）")
|
||||
continue
|
||||
if not key.endswith(suffix):
|
||||
bad.append(f"{key}: 单位是 {unit}，键名应以 {suffix} 结尾")
|
||||
return bad
|
||||
|
||||
|
||||
def missing_entries() -> List[str]:
|
||||
"""三张表（类型/单位/默认值）是否互相覆盖完整。"""
|
||||
problems: List[str] = []
|
||||
for key in _TYPES:
|
||||
if key not in _UNITS:
|
||||
problems.append(f"{key}: 缺 _UNITS 登记")
|
||||
if key not in DEFAULTS:
|
||||
problems.append(f"{key}: 缺 DEFAULTS（required 的键也应写一个起点值并在文档里标注必标定）")
|
||||
for key in _UNITS:
|
||||
if key not in _TYPES:
|
||||
problems.append(f"{key}: 在 _UNITS 里但不在 _TYPES 里")
|
||||
for key in DEFAULTS:
|
||||
if key not in _TYPES:
|
||||
problems.append(f"{key}: 在 DEFAULTS 里但不在 _TYPES 里")
|
||||
return problems
|
||||
|
||||
|
||||
def dataclass_keys() -> List[str]:
|
||||
""":class:`GaugeConfig` 的字段名 —— 与配置表比对，防"解析了但没存进对象"。"""
|
||||
return [f.name for f in fields(GaugeConfig)]
|
||||
@@ -0,0 +1,251 @@
|
||||
"""夹爪客户端：把"真机今天的接口"和"框架契约里的接口"收敛到一个出入口。
|
||||
|
||||
为什么要这一层（这不是多余抽象）
|
||||
--------------------------------
|
||||
现场查证（2026-09-24）发现**同一个服务名后面挂着两种类型**：
|
||||
|
||||
=============== ========================== ================================
|
||||
服务 A：真机今天跑的 B：框架契约 / 仿真
|
||||
=============== ========================== ================================
|
||||
``/gripper/open`` ``std_srvs/Trigger`` ``arm_msgs/srv/GripperExecute``
|
||||
``/gripper/close`` ``std_srvs/Trigger`` ``arm_msgs/srv/GripperExecute``
|
||||
状态 ``/gripper/states`` ``/gripper/status``
|
||||
（Float32MultiArray，5 元） （arm_msgs/GripperStatus）
|
||||
=============== ========================== ================================
|
||||
|
||||
A 来自 ``arm_application/dh_pgea_node.py``（真机在跑的那个节点，grip_state 直接
|
||||
带"咬住/掉落"）；B 来自 ``devices/grippers/dh_pgea/manifest.yaml`` 声明的
|
||||
``arm_driver_dh_gripper`` —— **那个包还不存在**（``src/`` 下没有），目前只有
|
||||
仿真适配器实现了它。
|
||||
|
||||
于是有两个错法，都得避免：
|
||||
|
||||
* 只按契约（B）写：真机上服务类型对不上，装配流程一步都走不了，
|
||||
而且报错会像是"服务没起"（实际起了，只是类型不同）；
|
||||
* 只按真机（A）写：仿真跑不了，而且把"临时接口"固化成事实（规则 M2）。
|
||||
|
||||
所以：**运行时看服务类型自己选**（:func:`pick_backend`），装配节点只依赖
|
||||
:class:`GripperClient` 的 ``open/close/wait/is_gripped`` 四个方法。
|
||||
等 ``arm_driver_dh_gripper`` 落地后，把 ``gauge_assembly.gripper_backend``
|
||||
改回 ``auto`` 即可自动切过去，应用层不用改一行。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, List, Mapping, Optional, Sequence
|
||||
|
||||
#: 契约后端用到的服务名（``arm_msgs/srv/GripperExecute``）。
|
||||
CONTRACT_TYPE = "arm_msgs/srv/GripperExecute"
|
||||
#: 真机今天用的后端（``std_srvs/srv/Trigger``）。
|
||||
LEGACY_TYPE = "std_srvs/srv/Trigger"
|
||||
|
||||
CONTRACT_OPEN = "/gripper/open"
|
||||
CONTRACT_CLOSE = "/gripper/close"
|
||||
CONTRACT_STATUS_TOPIC = "/gripper/status"
|
||||
LEGACY_STATUS_TOPIC = "/gripper/states"
|
||||
|
||||
#: ``/gripper/states`` 第 6 个元素：1=这一帧刚从总线读回来；0=总线无应答，
|
||||
#: 前 5 个是上一次读到的值。老节点只发 5 个元素 —— 那时它只在读成功才发，
|
||||
#: 所以 5 元素帧按有效帧处理。
|
||||
#: 为什么要这一位：2026-10-20 读 0x0201 一连 264 次无应答，节点干脆不发状态，
|
||||
#: 流水线只看到「没有 /gripper/states」，把「总线读不到」显示成「夹爪没到位」。
|
||||
STATES_ONLINE_INDEX = 5
|
||||
|
||||
#: DH 状态字（与 ``arm_msgs/GripperStatus.msg`` 里的注释一致）。
|
||||
GRIP_STATE_MOVING = 0
|
||||
GRIP_STATE_REACHED = 1
|
||||
GRIP_STATE_CAUGHT = 2
|
||||
GRIP_STATE_DROPPED = 3
|
||||
GRIP_STATE_NAME = {
|
||||
GRIP_STATE_MOVING: "运动中",
|
||||
GRIP_STATE_REACHED: "到位",
|
||||
GRIP_STATE_CAUGHT: "咬住物体",
|
||||
GRIP_STATE_DROPPED: "物体掉落",
|
||||
}
|
||||
|
||||
BACKENDS = ("contract", "legacy")
|
||||
|
||||
|
||||
class GripperInterfaceError(RuntimeError):
|
||||
"""夹爪接口无法确定。消息里必须写清"现在是什么状态、怎么修"。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class BackendChoice:
|
||||
"""选择结果。"""
|
||||
|
||||
backend: str
|
||||
"""``contract`` 或 ``legacy``。"""
|
||||
|
||||
reason: str
|
||||
"""为什么这么选 —— 要能直接打进启动日志。"""
|
||||
|
||||
status_topic: str
|
||||
"""该后端下状态从哪个话题读。"""
|
||||
|
||||
|
||||
def pick_backend(
|
||||
services: Mapping[str, Sequence[str]], prefer: str = "auto"
|
||||
) -> BackendChoice:
|
||||
"""按**实际服务类型**决定用哪种夹爪接口。
|
||||
|
||||
:param services: ``{服务名: [类型, ...]}``，直接来自
|
||||
``Node.get_service_names_and_types()``。
|
||||
:param prefer: ``auto`` / ``contract`` / ``legacy``。
|
||||
非 auto 时若实际类型与之不符，**不静默降级**，而是报错 ——
|
||||
现场明确指定了后端却跑在另一个上，是最难查的一类"改了没生效"。
|
||||
:raises GripperInterfaceError: 认不出来、两种都有、或指定的后端对不上。
|
||||
|
||||
用类型而不是名字判断，是因为两种后端的**服务名完全一样**（都是
|
||||
``/gripper/open``）：只看名字必然选不出来，而"随便选一个"的后果是
|
||||
调用一个类型不匹配的服务，报出来的错与真实原因无关（规则 M1）。
|
||||
"""
|
||||
if prefer not in ("auto",) + BACKENDS:
|
||||
raise GripperInterfaceError(
|
||||
f"gripper_backend 只能是 auto / {' / '.join(BACKENDS)}，收到 {prefer!r}"
|
||||
)
|
||||
|
||||
open_types = list(services.get(CONTRACT_OPEN, ()))
|
||||
close_types = list(services.get(CONTRACT_CLOSE, ()))
|
||||
has_contract = CONTRACT_TYPE in open_types and CONTRACT_TYPE in close_types
|
||||
has_legacy = LEGACY_TYPE in open_types and LEGACY_TYPE in close_types
|
||||
|
||||
if prefer == "contract" and not has_contract:
|
||||
raise GripperInterfaceError(
|
||||
f"配置要求 contract 后端，但 {CONTRACT_OPEN} 的类型是 "
|
||||
f"{open_types or '（服务不存在）'}，不是 {CONTRACT_TYPE}。\n"
|
||||
" 怎么修：① 起仿真（它有契约服务）或等 arm_driver_dh_gripper 落地；"
|
||||
"② 改用 gauge_assembly.gripper_backend: legacy（真机今天的接口）；"
|
||||
"③ 改回 auto 让本函数自己选"
|
||||
)
|
||||
if prefer == "legacy" and not has_legacy:
|
||||
raise GripperInterfaceError(
|
||||
f"配置要求 legacy 后端，但 {CONTRACT_OPEN} 的类型是 "
|
||||
f"{open_types or '（服务不存在）'}，不是 {LEGACY_TYPE}。\n"
|
||||
" 怎么修：确认真机侧起的是 arm_application 的 dh_pgea 节点"
|
||||
"（bash scripts/run_gripper.sh --arm 89）"
|
||||
)
|
||||
|
||||
if has_contract and has_legacy:
|
||||
# 同名服务两种类型同时在线：ROS 2 允许，但调用方只能撞运气。
|
||||
raise GripperInterfaceError(
|
||||
f"{CONTRACT_OPEN} 同时存在两种类型：{open_types}。"
|
||||
"是仿真适配器和真机节点同时起着（或旧的没停干净）。\n"
|
||||
" 怎么修：只留一个 —— 仿真用 scripts/stop_sim_gz.sh 停干净，"
|
||||
"真机 pkill -f dh_pgea"
|
||||
)
|
||||
|
||||
if prefer == "auto":
|
||||
if has_contract:
|
||||
return BackendChoice("contract", f"{CONTRACT_OPEN} 是 {CONTRACT_TYPE}", CONTRACT_STATUS_TOPIC)
|
||||
if has_legacy:
|
||||
return BackendChoice(
|
||||
"legacy",
|
||||
f"{CONTRACT_OPEN} 是 {LEGACY_TYPE}（真机 dh_pgea 节点；"
|
||||
"契约驱动 arm_driver_dh_gripper 还没落地）",
|
||||
LEGACY_STATUS_TOPIC,
|
||||
)
|
||||
raise GripperInterfaceError(_unrecognised_msg(services, open_types))
|
||||
|
||||
if prefer == "contract":
|
||||
return BackendChoice("contract", "配置指定 contract", CONTRACT_STATUS_TOPIC)
|
||||
return BackendChoice("legacy", "配置指定 legacy", LEGACY_STATUS_TOPIC)
|
||||
|
||||
|
||||
def _unrecognised_msg(
|
||||
services: Mapping[str, Sequence[str]], open_types: Sequence[str]
|
||||
) -> str:
|
||||
"""认不出来时，把"服务不在"和"服务在但类型不对"分开说。
|
||||
|
||||
两者都报成"服务没起"是这个仓库反复踩过的坑：现场明明起了夹爪节点，
|
||||
却被提示"去起夹爪节点"，然后开始怀疑人生（规则 M1、R5.4）。
|
||||
"""
|
||||
missing = [n for n in (CONTRACT_OPEN, CONTRACT_CLOSE) if n not in services]
|
||||
if missing:
|
||||
return (
|
||||
f"{' / '.join(missing)} 不在，夹爪节点没起来。\n"
|
||||
" 怎么修：真机 bash scripts/run_gripper.sh --arm 89；"
|
||||
"仿真 bash scripts/run_sim_gz.sh（仿真自带契约服务）"
|
||||
)
|
||||
return (
|
||||
f"{CONTRACT_OPEN} 在，但类型是 {list(open_types)}，"
|
||||
f"既不认识 {CONTRACT_TYPE}（契约）也不认识 {LEGACY_TYPE}（真机 dh_pgea）。\n"
|
||||
" 常见原因：这个服务名被别的节点占用了。查是谁：\n"
|
||||
" ros2 service list -t | grep gripper\n"
|
||||
" 怎么修：停掉那个节点，或改 gauge_assembly.gripper_backend 明确指定后端"
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 状态解读（纯函数）
|
||||
# =============================================================================
|
||||
def is_gripped_from_state(grip_state: int) -> Optional[bool]:
|
||||
"""DH 状态字 → "爪里有没有东西"。
|
||||
|
||||
``2`` 咬住 / ``3`` 掉落 是明确的；``0`` 运动中、``1`` 到位（空爪）都要
|
||||
小心：**"到位"不等于"夹住了"** —— 空爪闭合也会到位。所以只有 2/3 给
|
||||
明确答案，其余返回 None（"不知道"），由调用方决定要不要等。
|
||||
|
||||
返回 None 而不是 False 很重要：把"不知道"当成"没夹住"会让 E1 掉件检测
|
||||
在任何一次开合过程中误报。
|
||||
"""
|
||||
if grip_state == GRIP_STATE_CAUGHT:
|
||||
return True
|
||||
if grip_state == GRIP_STATE_DROPPED:
|
||||
return False
|
||||
return None
|
||||
|
||||
|
||||
def is_dropped_from_state(grip_state: int, was_gripped: bool) -> bool:
|
||||
"""E1 掉件判据：**曾经夹住过**，现在状态字变成"掉落"才是掉件。
|
||||
|
||||
为什么要 ``was_gripped``：装配前爪里本来就是空的，那时 ``grip_state``
|
||||
可能是 3（掉落/无物），直接判成掉件会让流程一开始就报 E1。
|
||||
"""
|
||||
return was_gripped and grip_state == GRIP_STATE_DROPPED
|
||||
|
||||
|
||||
def states_frame_online(data: Sequence[float]) -> bool:
|
||||
"""这一帧是不是刚从总线读回来的。
|
||||
|
||||
5 元素帧（老节点）视为有效：老节点只在读成功时发。6 元素帧看最后一位。
|
||||
"""
|
||||
if len(data) <= STATES_ONLINE_INDEX:
|
||||
return True
|
||||
try:
|
||||
return float(data[STATES_ONLINE_INDEX]) >= 0.5
|
||||
except (TypeError, ValueError):
|
||||
return False
|
||||
|
||||
|
||||
def legacy_states_to_fields(data: Sequence[float]) -> Dict[str, float]:
|
||||
"""``/gripper/states`` 的 6 元素数组 → 具名字段。
|
||||
|
||||
超出长度、为空、或这一帧是离线帧时返回空字典（而不是抛异常）：这是
|
||||
**状态话题**，收到畸形数据时应该让上层走"不知道"分支，而不是让整个
|
||||
节点崩掉。离线帧里的前 5 个是上一次的值，不能当现在的状态用。
|
||||
"""
|
||||
if not data or len(data) < 5:
|
||||
return {}
|
||||
if not states_frame_online(data):
|
||||
return {}
|
||||
return {
|
||||
"initialized": float(data[0]),
|
||||
"grip_state": float(data[1]),
|
||||
"position": float(data[2]),
|
||||
"target_position": float(data[3]),
|
||||
"target_force": float(data[4]),
|
||||
}
|
||||
|
||||
|
||||
def describe_backend_choice(choice: BackendChoice) -> List[str]:
|
||||
"""给启动日志用的多行说明（含"契约驱动还没落地"这类现场事实）。"""
|
||||
lines = [f"夹爪后端 = {choice.backend}（{choice.reason}）"]
|
||||
if choice.backend == "legacy":
|
||||
lines.append(
|
||||
" 注意：legacy 后端下 /gripper/open|close 是 std_srvs/Trigger，"
|
||||
"开口是 1000‰ 的百分比语义，不是毫米；装配流程只用它开合，"
|
||||
"不靠它量开口"
|
||||
)
|
||||
return lines
|
||||
@@ -0,0 +1,284 @@
|
||||
"""通止规装配用的小几何/解析工具 —— **纯函数**，可离线单测。
|
||||
|
||||
放这里的都是"算错了不会抛异常、只会让动作做错"的东西（规则 M3、R5.1）：
|
||||
|
||||
* ``parse_joint_limits``：从 URDF 文本里抠关节限位，给 S01 的"转到极限角"
|
||||
做软夹取。为什么不用 ``urdf_parser_py``：本机没装，多一个依赖就多一个
|
||||
"在真机上 import 失败"的机会；限位信息在 URDF 里是极规整的一行
|
||||
``<limit lower=... upper=...>``，用标准库解析足够，而且**可控**。
|
||||
* ``prewind``：算"逆时针转到不触发关节极限的极限角"到底能转多少度。
|
||||
* ``rotation_about_axis_deg``：事后验证那个动作**真的**是绕工具 Z 转的。
|
||||
这条是 S01 的验收判据：不验证的话，"绕错轴"会表现成"转是转了，孔位
|
||||
偏了"，而且要到几个阶段之后才看出来。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
import xml.etree.ElementTree as ET
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Iterable, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
#: 限位夹取时留的余量（度）。贴到限位上再下发，驱动器会因为"位置超出限位"
|
||||
#: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。
|
||||
DEFAULT_LIMIT_MARGIN_DEG = 3.0
|
||||
|
||||
#: 厂家 A7。``Robot::SendJointPosition`` 的关节向量按 A1..A7 排列，A7 是最后一轴。
|
||||
A7_INDEX = 6
|
||||
|
||||
_JOINT_RE = re.compile(r"<joint\s+name\s*=\s*\"([^\"]+)\"")
|
||||
_ATTR_RE = re.compile(r"(lower|upper)\s*=\s*\"([-+0-9.eE]+)\"")
|
||||
|
||||
|
||||
def _limits_by_lines(text: str) -> Dict[str, Tuple[float, float]]:
|
||||
"""逐行兜底解析：记住最近一个 ``<joint name=...>``，紧跟的 ``<limit>`` 归它。
|
||||
|
||||
为什么要"记住"而不是"要求同一行"：正常格式化的 URDF 里关节名和 limit
|
||||
**不在同一行**，只认可同一行的兜底等于永远不生效（一个不会触发的兜底，
|
||||
和没有兜底是一样的）。属性顺序也按出现顺序读，不假设 lower 在 upper 前。
|
||||
"""
|
||||
out: Dict[str, Tuple[float, float]] = {}
|
||||
current: Optional[str] = None
|
||||
for line in text.splitlines():
|
||||
m = _JOINT_RE.search(line)
|
||||
if m:
|
||||
current = m.group(1)
|
||||
if current is None or "<limit" not in line:
|
||||
continue
|
||||
attrs = dict(_ATTR_RE.findall(line))
|
||||
if "lower" in attrs and "upper" in attrs:
|
||||
out[current] = (float(attrs["lower"]), float(attrs["upper"]))
|
||||
current = None
|
||||
return out
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PrewindResult:
|
||||
"""S01 预绕量的计算结果。"""
|
||||
|
||||
target_rad: float
|
||||
"""目标关节角 [rad]。"""
|
||||
|
||||
applied_deg: float
|
||||
"""实际能转的角度（带符号，符合"逆时针为正"的约定）[deg]。"""
|
||||
|
||||
want_deg: float
|
||||
"""希望转的角度 [deg]。"""
|
||||
|
||||
clamped: bool
|
||||
"""是否因为关节限位被夹取过。被夹取时 ``applied_deg < want_deg``。"""
|
||||
|
||||
has_limits: bool
|
||||
"""是否拿到了可信的关节限位。False 表示没有任何夹取依据。"""
|
||||
|
||||
|
||||
def parse_joint_limits(urdf_xml: str) -> Dict[str, Tuple[float, float]]:
|
||||
"""从 URDF 文本里解析 ``关节名 -> (lower, upper)``（弧度）。
|
||||
|
||||
关节名**原样保留**（含 ``Rizon4-063989_`` 前缀），调用方自己决定怎么匹配；
|
||||
这里不做"去前缀"之类的猜测 —— 猜测会在换臂时静默失配，然后限位夹取
|
||||
悄悄失效（所有关节都"查不到限位"，于是不夹取，于是贴着限位下发）。
|
||||
|
||||
解析失败的关节直接不进结果集：``continuous`` 关节本来就没有限位，
|
||||
不该被当成"解析失败"。调用方用"查不到 = 不夹取"来处理。
|
||||
"""
|
||||
out: Dict[str, Tuple[float, float]] = {}
|
||||
if not urdf_xml or "<robot" not in urdf_xml:
|
||||
return out
|
||||
try:
|
||||
root = ET.fromstring(urdf_xml)
|
||||
except ET.ParseError:
|
||||
# URDF 里出现未展开的 xacro 或未转义的 ``&`` 是常见的（拿错话题就是
|
||||
# 这种内容）。降级成逐行扫描仍能救回限位；救不回就返回空表（= 不夹取），
|
||||
# 由调用方告警 —— **不会**返回半套数据。
|
||||
return _limits_by_lines(urdf_xml)
|
||||
|
||||
for joint in root.findall(".//joint"):
|
||||
name = joint.get("name")
|
||||
limit = joint.find("limit")
|
||||
if not name or limit is None:
|
||||
continue
|
||||
lo, up = limit.get("lower"), limit.get("upper")
|
||||
if lo is None or up is None:
|
||||
continue
|
||||
try:
|
||||
out[name] = (float(lo), float(up))
|
||||
except ValueError:
|
||||
continue
|
||||
return out
|
||||
|
||||
|
||||
def match_limits(
|
||||
limits: Dict[str, Tuple[float, float]], joint_names: Sequence[str]
|
||||
) -> Optional[Tuple[float, float]]:
|
||||
"""按名字给一串关节名找限位，**要求全部找到**才返回。
|
||||
|
||||
只有一部分找到时返回 None 而不是"把找到的那部分用上"：半套限位会让
|
||||
"能不能转到极限角"的结论变得不可解释（有些关节夹了、有些没夹），
|
||||
与其给出一个说不清的边界，不如明确地说"这次没有限位依据"。
|
||||
"""
|
||||
found = [limits.get(n) for n in joint_names]
|
||||
if any(v is None for v in found):
|
||||
return None
|
||||
return found[-1] # 最后一个 = 我们要绕的那根轴（调用方传的顺序有意义）
|
||||
|
||||
|
||||
def prewind(
|
||||
q_now_rad: float,
|
||||
want_deg: float,
|
||||
direction: int,
|
||||
limits: Optional[Tuple[float, float]] = None,
|
||||
margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG,
|
||||
) -> PrewindResult:
|
||||
"""算 S01 的预绕目标角。
|
||||
|
||||
:param q_now_rad: 当前关节角 [rad]。
|
||||
:param want_deg: 想转的角度（**正值**）[deg]。
|
||||
:param direction: ``+1`` = 绕 +Z 逆时针（右手定则）；``-1`` = 顺时针。
|
||||
符号到底对应现场的哪个方向，用真机确认一次后改 ``site.yaml`` 即可，
|
||||
不要靠"改代码里的负号"—— 那种改动没人 review 得到（规则 M2）。
|
||||
:param limits: ``(lower, upper)`` [rad]。给 None 时不做夹取。
|
||||
:param margin_deg: 距离限位保留的余量 [deg]。
|
||||
|
||||
:returns: :class:`PrewindResult`。**无论有没有限位，都返回目标角**，
|
||||
因为"贴到限位再继续转"在现场是预期行为（用户要求转到极限角），
|
||||
驱动器真拒绝时会由 :func:`arm_application.gauge_fsm.is_joint_limit_error`
|
||||
认成 JOINT_LIMIT 事件继续流程。
|
||||
|
||||
为什么要在这里夹取（而不是直接下发、等驱动器报错）：
|
||||
驱动器拒绝一条轨迹时，整个 action 失败，连带把已经转过的那部分**也**
|
||||
变成"没转过"的语义（角度实际到位了，但流程认为失败）。夹取到
|
||||
"限位 − 余量"能拿到一个明确成功、且确实接近极限的姿态度。
|
||||
"""
|
||||
if want_deg <= 0.0:
|
||||
raise ValueError(f"预绕角度必须为正，收到 {want_deg}")
|
||||
if direction not in (-1, 1):
|
||||
raise ValueError(f"方向只能是 +1 / -1，收到 {direction}")
|
||||
|
||||
signed = direction * abs(want_deg)
|
||||
target = q_now_rad + math.radians(signed)
|
||||
has_limits = limits is not None
|
||||
clamped = False
|
||||
if limits is not None:
|
||||
lo = limits[0] + math.radians(margin_deg)
|
||||
up = limits[1] - math.radians(margin_deg)
|
||||
if lo > up: # 限位本身比余量还窄（不该发生，但别让它变成静默走偏）
|
||||
lo = up = 0.5 * (limits[0] + limits[1])
|
||||
if target > up:
|
||||
target, clamped = up, True
|
||||
elif target < lo:
|
||||
target, clamped = lo, True
|
||||
applied = math.degrees(target - q_now_rad)
|
||||
return PrewindResult(
|
||||
target_rad=target,
|
||||
applied_deg=applied,
|
||||
want_deg=abs(want_deg),
|
||||
clamped=clamped,
|
||||
has_limits=has_limits,
|
||||
)
|
||||
|
||||
|
||||
def a7_limit_target(
|
||||
q: Sequence[float],
|
||||
limits: Optional[Tuple[float, float]],
|
||||
direction: int,
|
||||
margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG,
|
||||
) -> Optional[np.ndarray]:
|
||||
"""只把 A7 收到软件限位内侧。前六轴原样拷回，不许在这里改。
|
||||
|
||||
厂家接口是 ``Robot::SendJointPosition``（模式 ``NRT_JOINT_POSITION``）。
|
||||
向量按 A1..A7 排，A7 是下标 ``A7_INDEX``。限位是 ``RobotInfo::q_min/q_max``
|
||||
的那一对，真机上与 URDF ``joint7`` 的 lower/upper 是同一组数。
|
||||
|
||||
``direction`` 与 ``prewind`` 相同：``+1`` 关节角增大 = 逆时针，目标取上限；
|
||||
``-1`` 关节角减小 = 顺时针，目标取下限。现场若发现正负反了，改
|
||||
``site.yaml`` 的 ``extreme_direction``，不要改这里的符号。
|
||||
|
||||
没有限位、方向不是 ±1、或者余量把上下限挤没了，返回 None。
|
||||
调用方必须停，不许猜一个角度发出去。
|
||||
"""
|
||||
if direction not in (-1, 1):
|
||||
raise ValueError(f"方向只能是 +1 / -1，收到 {direction}")
|
||||
arr = np.asarray(q, dtype=float).reshape(-1)
|
||||
if arr.size != 7:
|
||||
raise ValueError(f"A7 目标要 7 个关节角，收到 {arr.size}")
|
||||
if limits is None:
|
||||
return None
|
||||
lo = float(limits[0])
|
||||
up = float(limits[1])
|
||||
margin = math.radians(float(margin_deg))
|
||||
if not math.isfinite(lo) or not math.isfinite(up) or up - lo <= 2.0 * margin:
|
||||
return None
|
||||
target = (up - margin) if direction > 0 else (lo + margin)
|
||||
out = arr.copy()
|
||||
out[A7_INDEX] = target
|
||||
return out
|
||||
|
||||
|
||||
def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
|
||||
"""从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。
|
||||
|
||||
S01 的验收判据就靠它：绕工具 Z 转 Δ 之后，如果实际绕的是别的轴，
|
||||
这个函数给出的角度会与 Δ 明显不符（或者 axis 分量不占优）。
|
||||
做法是把相对旋转投影到 ``axis`` 上：
|
||||
|
||||
``θ = atan2( axis·(R_b·R_a⁻¹ 的反对称部分) , (trace-1)/2 )``
|
||||
|
||||
:param axis: **单位**轴（工具 Z 在世界系下的方向 = ``R[:, 2]``）。
|
||||
"""
|
||||
a = np.asarray(axis, dtype=float)
|
||||
n = float(np.linalg.norm(a))
|
||||
if n < 1e-9:
|
||||
raise ValueError("轴向量长度为 0，无法判定旋转")
|
||||
a = a / n
|
||||
|
||||
Rb = np.asarray(R_before, dtype=float)[:3, :3]
|
||||
Ra = np.asarray(R_after, dtype=float)[:3, :3]
|
||||
Rrel = Ra @ Rb.T
|
||||
# 反对称部分：sin(θ)·[axis]_×
|
||||
v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]])
|
||||
sin_t = 0.5 * float(np.dot(v, a))
|
||||
cos_t = 0.5 * (float(np.trace(Rrel)) - 1.0)
|
||||
return math.degrees(math.atan2(sin_t, cos_t))
|
||||
|
||||
|
||||
def axis_misalignment_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
|
||||
"""相对旋转的转轴与 ``axis`` 的夹角 [deg]。
|
||||
|
||||
纯绕轴旋转时这个值≈0；转错轴时它会接近 90°。用作 S01 的判错依据：
|
||||
只看"转了多少度"是不够的 —— 绕正确的轴转 90° 和绕错误的轴转 90°
|
||||
给出的角度读数一样。
|
||||
"""
|
||||
Rb = np.asarray(R_before, dtype=float)[:3, :3]
|
||||
Ra = np.asarray(R_after, dtype=float)[:3, :3]
|
||||
Rrel = Ra @ Rb.T
|
||||
# 旋转轴 = 反对称部分对应的向量（sinθ 因子不影响方向）
|
||||
v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]])
|
||||
nv = float(np.linalg.norm(v))
|
||||
if nv < 1e-9:
|
||||
return 0.0 # 几乎没转，谈不上"转错轴"
|
||||
a = np.asarray(axis, dtype=float)
|
||||
a = a / float(np.linalg.norm(a))
|
||||
cos = abs(float(np.dot(v / nv, a))) # 取绝对值：轴线的两个方向等价
|
||||
return math.degrees(math.acos(max(-1.0, min(1.0, cos))))
|
||||
|
||||
|
||||
def unit_tool_z(R: np.ndarray) -> np.ndarray:
|
||||
"""工具坐标系的 Z 轴在世界系下的方向（旋转矩阵的第三列）。"""
|
||||
return np.asarray(R, dtype=float)[:3, 2]
|
||||
|
||||
|
||||
def vertical_down_error_deg(R: np.ndarray, world_down: Iterable[float] = (0.0, 0.0, -1.0)) -> float:
|
||||
"""工具 Z 与"垂直向下"的夹角 [deg]。0 = 完全垂直向下。
|
||||
|
||||
看板里"工具系全程垂直向下、位姿全局锁定"这句话，只有能算出这个数才算
|
||||
真的在执行 —— 否则每个阶段都得靠人眼看画面判断臂歪没歪。
|
||||
"""
|
||||
z = unit_tool_z(R)
|
||||
d = np.asarray(list(world_down), dtype=float)
|
||||
d = d / float(np.linalg.norm(d))
|
||||
cos = float(np.dot(z / max(1e-12, float(np.linalg.norm(z))), d))
|
||||
return math.degrees(math.acos(max(-1.0, min(1.0, cos))))
|
||||
@@ -0,0 +1,457 @@
|
||||
#!/usr/bin/env python3
|
||||
"""感知/伺服启动前的开机回初始位流程。
|
||||
|
||||
流程：
|
||||
[A] 若法兰低于 ``servo.safe_z_m``，先让它竖直上升（闭环 Z 小步跳），
|
||||
以免回 home 时扫到墙面/工装；
|
||||
[B] 调用 ``/set_named_pose``（Flexiv 命名位姿，默认 "home"）—— 由 Flexiv
|
||||
自己的规划器把机械臂送回 Flexiv 初始位姿；
|
||||
[C] 保持（pin）住，让伺服从一个干净、静止的位姿接手。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from arm_msgs.srv import SetNamedPose
|
||||
from control_msgs.action import FollowJointTrajectory
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
|
||||
from arm_application.geom_ik import (
|
||||
RIZON_LINK_AXES,
|
||||
dls_translate_hold_ori,
|
||||
dls_twist,
|
||||
geometric_jacobian,
|
||||
quat_xyzw_to_R,
|
||||
)
|
||||
from arm_application.site_io import (
|
||||
base_frame_default,
|
||||
ee_frame_default,
|
||||
joint_prefix_default,
|
||||
)
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
|
||||
def _load_ee_cfg() -> dict:
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import load_site, resolve_robot # type: ignore
|
||||
|
||||
data = load_site()
|
||||
servo = (data.get("servo") or {})
|
||||
robot = resolve_robot(data)
|
||||
ee_z = float(servo.get("ee_z_m") or 0.24)
|
||||
return {
|
||||
"prefix": str(robot.get("joint_prefix") or joint_prefix_default()),
|
||||
"base": str(robot.get("base_frame") or base_frame_default()),
|
||||
"ee": str(robot.get("ee_frame") or ee_frame_default()),
|
||||
"z": ee_z,
|
||||
# 回 home 前先竖直升到这个 Z（避免撞到墙面/工装）；
|
||||
# 然后再回到 Flexiv 的命名初始位姿。
|
||||
"safe_z": float(servo.get("safe_z_m") or max(0.40, ee_z + 0.10)),
|
||||
"home_pose": str(servo.get("home_pose_name") or "home"),
|
||||
"home_speed": float(servo.get("home_pose_speed") or 0.30),
|
||||
"v_z": float(servo.get("v_max_z") or 0.04),
|
||||
# 仅用于（目前未使用的）verticalize 规划路径
|
||||
"kp": float(servo.get("ee_hold_kp") or 2.0),
|
||||
"w_max": float(servo.get("ee_w_max") or 0.50),
|
||||
}
|
||||
|
||||
|
||||
def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
|
||||
pt = JointTrajectoryPoint()
|
||||
pt.positions = [float(v) for v in q.tolist()]
|
||||
pt.velocities = [float(v) for v in qdot.tolist()]
|
||||
pt.accelerations = [0.0] * int(q.size)
|
||||
ns = int(round(max(0.0, float(t_sec)) * 1e9))
|
||||
pt.time_from_start.sec = ns // 1_000_000_000
|
||||
pt.time_from_start.nanosec = ns % 1_000_000_000
|
||||
return pt
|
||||
|
||||
|
||||
class GotoEePlane(TolerantNode):
|
||||
def __init__(self, cfg: dict) -> None:
|
||||
super().__init__("goto_ee_plane")
|
||||
self.cfg = cfg
|
||||
prefix = cfg["prefix"]
|
||||
self.joint_names = [f"{prefix}joint{i}" for i in range(1, 8)]
|
||||
self.link_frames = [f"{prefix}link{i}" for i in range(1, 8)]
|
||||
self._js: Optional[JointState] = None
|
||||
self.create_subscription(JointState, "joint_states", self._on_js, 10)
|
||||
self._tf = Buffer()
|
||||
TransformListener(self._tf, self, spin_thread=True)
|
||||
self._cli = ActionClient(
|
||||
self, FollowJointTrajectory, "rizon_arm_controller/follow_joint_trajectory"
|
||||
)
|
||||
|
||||
def _on_js(self, msg: JointState) -> None:
|
||||
self._js = msg
|
||||
|
||||
def _spin(self, dt: float = 0.05) -> None:
|
||||
rclpy.spin_once(self, timeout_sec=dt)
|
||||
|
||||
def _lookup(self, parent: str, child: str) -> Optional[np.ndarray]:
|
||||
try:
|
||||
tf = self._tf.lookup_transform(
|
||||
parent, child, rclpy.time.Time(),
|
||||
timeout=rclpy.duration.Duration(nanoseconds=80_000_000),
|
||||
)
|
||||
except Exception:
|
||||
return None
|
||||
t = tf.transform.translation
|
||||
o = tf.transform.rotation
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = quat_xyzw_to_R([o.x, o.y, o.z, o.w])
|
||||
T[:3, 3] = [t.x, t.y, t.z]
|
||||
return T
|
||||
|
||||
def _q(self) -> Optional[np.ndarray]:
|
||||
if self._js is None:
|
||||
return None
|
||||
m = {n: p for n, p in zip(self._js.name, self._js.position)}
|
||||
try:
|
||||
return np.array([float(m[n]) for n in self.joint_names], dtype=float)
|
||||
except KeyError:
|
||||
return None
|
||||
|
||||
def _jacobian(self, p_ee: np.ndarray) -> Optional[np.ndarray]:
|
||||
link_R: List[np.ndarray] = []
|
||||
link_p: List[np.ndarray] = []
|
||||
for fr in self.link_frames:
|
||||
Tl = self._lookup(self.cfg["base"], fr)
|
||||
if Tl is None:
|
||||
return None
|
||||
link_R.append(Tl[:3, :3])
|
||||
link_p.append(Tl[:3, 3])
|
||||
return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
|
||||
|
||||
def _w_down(self, R: np.ndarray) -> Tuple[np.ndarray, float]:
|
||||
z_now = R[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0])
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
ang = float(np.arccos(c))
|
||||
if ang < 0.02:
|
||||
return np.zeros(3), ang
|
||||
axis = np.cross(z_now, z_des)
|
||||
n = float(np.linalg.norm(axis))
|
||||
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0])
|
||||
mag = float(np.clip(self.cfg["kp"] * ang, -self.cfg["w_max"], self.cfg["w_max"]))
|
||||
return axis * mag, ang
|
||||
|
||||
def _target_err(
|
||||
self, p: np.ndarray, R: np.ndarray, target: np.ndarray, verticalize: bool
|
||||
) -> Tuple[np.ndarray, float, float]:
|
||||
dp = np.asarray(target, dtype=float).reshape(3) - np.asarray(p, dtype=float).reshape(3)
|
||||
_, ang = (self._w_down(R) if verticalize else (np.zeros(3), 0.0))
|
||||
xy = float(np.hypot(dp[0], dp[1]))
|
||||
return dp, ang, xy
|
||||
|
||||
def _wait_state(self, timeout_s: float) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
|
||||
t0 = time.monotonic()
|
||||
q = None
|
||||
Tee = None
|
||||
while time.monotonic() - t0 < timeout_s:
|
||||
self._spin(0.05)
|
||||
q = self._q()
|
||||
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
|
||||
if q is not None and Tee is not None and self._jacobian(Tee[:3, 3]) is not None:
|
||||
return q, Tee
|
||||
return q, Tee
|
||||
|
||||
def _send_action(self, traj: JointTrajectory, wait_s: float) -> bool:
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.trajectory = traj
|
||||
fut = self._cli.send_goal_async(goal)
|
||||
t0 = time.monotonic()
|
||||
while not fut.done() and time.monotonic() - t0 < 4.0:
|
||||
self._spin(0.05)
|
||||
handle = fut.result() if fut.done() else None
|
||||
if handle is None or not handle.accepted:
|
||||
print(" 错误：JTC 拒绝了抬升轨迹", flush=True)
|
||||
return False
|
||||
res = handle.get_result_async()
|
||||
t0 = time.monotonic()
|
||||
last = 0.0
|
||||
while not res.done() and time.monotonic() - t0 < wait_s:
|
||||
self._spin(0.05)
|
||||
now = time.monotonic()
|
||||
if now - last > 0.4:
|
||||
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
|
||||
if Tee is not None:
|
||||
p = Tee[:3, 3]
|
||||
print(
|
||||
f" 运动中 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f}) "
|
||||
f"z_err={(self.cfg['z'] - p[2]) * 1000:+.0f}mm",
|
||||
flush=True,
|
||||
)
|
||||
last = now
|
||||
if not res.done():
|
||||
print(" 警告：JTC 结果超时 —— 仍然继续检查 TF", flush=True)
|
||||
return False
|
||||
return True
|
||||
|
||||
def _plan_and_send(
|
||||
self,
|
||||
q0: np.ndarray,
|
||||
Tee: np.ndarray,
|
||||
target: np.ndarray,
|
||||
verticalize: bool,
|
||||
tag: str = "",
|
||||
max_seg: float = 0.08,
|
||||
v: float = 0.05,
|
||||
) -> bool:
|
||||
"""规划并执行一段朝向 target 的短笛卡尔位移（≤ max_seg）。
|
||||
长距离移动由调用方以闭环小步跳的方式完成，因为雅可比是在当前位姿
|
||||
处冻结的 —— 超过 10 cm 后线性化误差会让一次性规划严重偏离目标。"""
|
||||
p = Tee[:3, 3].copy()
|
||||
R = Tee[:3, :3]
|
||||
dp, ang, _ = self._target_err(p, R, target, verticalize)
|
||||
dist = float(np.linalg.norm(dp))
|
||||
if dist > max_seg: # 限制本次小步跳的长度；调用方会用最新的 TF 循环执行
|
||||
dp = dp * (max_seg / dist)
|
||||
dist = max_seg
|
||||
print(
|
||||
f" 规划 {tag} Δxyz=({dp[0]*1000:+.0f},{dp[1]*1000:+.0f},{dp[2]*1000:+.0f})mm "
|
||||
f"倾角={ang * 180 / np.pi:.1f}deg",
|
||||
flush=True,
|
||||
)
|
||||
J = self._jacobian(p)
|
||||
if J is None:
|
||||
print("错误：缺少用于雅可比的 link TF", flush=True)
|
||||
return False
|
||||
|
||||
q = q0.copy()
|
||||
waypoints = [q.copy()]
|
||||
remain = dp.copy()
|
||||
w_step, _ = (self._w_down(R) if verticalize else (np.zeros(3), 0.0))
|
||||
n_max = int(np.ceil(dist / 0.012)) + 4
|
||||
for _ in range(n_max):
|
||||
if float(np.linalg.norm(remain)) < 0.0015 and (
|
||||
(not verticalize) or ang < 0.03
|
||||
):
|
||||
break
|
||||
nseg = max(1, int(np.ceil(float(np.linalg.norm(remain)) / 0.012)))
|
||||
step = remain / nseg
|
||||
if verticalize and ang > 0.03:
|
||||
qdot = dls_twist(
|
||||
J, step / 0.12, w_base=w_step, damping=0.08, max_qdot=0.45
|
||||
)
|
||||
dq = np.clip(qdot * 0.12, -0.06, 0.06)
|
||||
else:
|
||||
dq = dls_translate_hold_ori(J, step, damping=0.08, max_dq=0.06)
|
||||
if float(np.max(np.abs(dq))) < 1e-5:
|
||||
break
|
||||
q = q + dq
|
||||
waypoints.append(q.copy())
|
||||
remain = remain - (J[:3, :] @ dq)
|
||||
ang = max(0.0, ang - 0.05)
|
||||
|
||||
planned = dist - float(np.linalg.norm(remain))
|
||||
if len(waypoints) < 2 or planned < 0.4 * dist:
|
||||
print(
|
||||
f" 警告 {tag}：规划器停滞 "
|
||||
f"（已规划 {planned*1000:.0f}/{dist*1000:.0f}mm）—— 跳过本次小步跳",
|
||||
flush=True,
|
||||
)
|
||||
return False
|
||||
# 关节偏移量的合理性闸门：拒绝过于夸张的规划
|
||||
if float(np.max(np.abs(waypoints[-1] - q0))) > 1.0:
|
||||
print(f" 错误 {tag}：关节偏移 >1 rad —— 放弃本次小步跳", flush=True)
|
||||
return False
|
||||
|
||||
duration = max(1.6, dist / max(min(v, 0.06), 1e-3))
|
||||
n = len(waypoints) - 1
|
||||
traj = JointTrajectory()
|
||||
traj.header.stamp.sec = 0
|
||||
traj.header.stamp.nanosec = 0
|
||||
traj.joint_names = list(self.joint_names)
|
||||
traj.points.append(_pt(waypoints[0], np.zeros(7), 0.08))
|
||||
zeros = np.zeros(7)
|
||||
for i, qi in enumerate(waypoints[1:], start=1):
|
||||
t = 0.08 + duration * (i / n)
|
||||
qdot = zeros if i == n else (waypoints[min(i + 1, n)] - qi) / max(
|
||||
duration / n, 0.05
|
||||
)
|
||||
qdot = np.clip(qdot, -0.40, 0.40)
|
||||
traj.points.append(_pt(qi, qdot, t))
|
||||
# 强制最后一个点的速度为零
|
||||
traj.points[-1].velocities = [0.0] * 7
|
||||
print(
|
||||
f" 发送 JTC action {tag}，{n} 步 T={duration:.1f}s",
|
||||
flush=True,
|
||||
)
|
||||
return self._send_action(traj, wait_s=duration + 6.0)
|
||||
|
||||
def _goto(
|
||||
self,
|
||||
q: np.ndarray,
|
||||
Tee: np.ndarray,
|
||||
target: np.ndarray,
|
||||
verticalize: bool,
|
||||
tol_m: float,
|
||||
tol_tilt: float,
|
||||
tag: str,
|
||||
v: float,
|
||||
max_hops: int = 8,
|
||||
) -> Tuple[bool, np.ndarray, np.ndarray]:
|
||||
"""闭环点到点：规划 ≤8 cm 的小步跳 → 执行 → 重新测量 TF → 重复，
|
||||
直到落在容差内。这样对长距离下的雅可比线性化误差更鲁棒。"""
|
||||
for hop in range(max_hops):
|
||||
p = Tee[:3, 3]
|
||||
dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
|
||||
dist = float(np.linalg.norm(dp))
|
||||
if dist < tol_m and ((not verticalize) or ang < tol_tilt):
|
||||
return True, q, Tee
|
||||
print(
|
||||
f" {tag} 第 {hop + 1} 跳 remain={dist*1000:.0f}mm "
|
||||
f"倾角={ang * 180 / np.pi:.1f}deg",
|
||||
flush=True,
|
||||
)
|
||||
if not self._plan_and_send(q, Tee, target, verticalize, tag, v=v):
|
||||
return False, q, Tee
|
||||
t_end = time.monotonic() + 0.5
|
||||
while time.monotonic() < t_end:
|
||||
self._spin(0.05)
|
||||
q2, Tee2 = self._wait_state(2.0)
|
||||
if q2 is None or Tee2 is None:
|
||||
print("错误：移动过程中丢失状态", flush=True)
|
||||
return False, q, Tee
|
||||
q, Tee = q2, Tee2
|
||||
p = Tee[:3, 3]
|
||||
dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
|
||||
ok = float(np.linalg.norm(dp)) < (tol_m * 2.0) and (
|
||||
(not verticalize) or ang < (tol_tilt * 2.0)
|
||||
)
|
||||
return ok, q, Tee
|
||||
|
||||
def _pin(self, q: np.ndarray) -> None:
|
||||
if not self._cli.wait_for_server(timeout_sec=3.0):
|
||||
return
|
||||
traj = JointTrajectory()
|
||||
traj.header.stamp.sec = 0
|
||||
traj.header.stamp.nanosec = 0
|
||||
traj.joint_names = list(self.joint_names)
|
||||
traj.points = [_pt(q, np.zeros(7), 0.08), _pt(q, np.zeros(7), 0.45)]
|
||||
self._send_action(traj, wait_s=3.0)
|
||||
|
||||
def _named_pose(self, name: str, speed: float) -> bool:
|
||||
"""调用 Flexiv 的 ``/set_named_pose``，并等待机械臂静止。"""
|
||||
cli = self.create_client(SetNamedPose, "/set_named_pose")
|
||||
if not cli.wait_for_server(timeout_sec=8.0):
|
||||
print("错误：``/set_named_pose`` 不可用 —— 请启动 run_real.sh", flush=True)
|
||||
return False
|
||||
req = SetNamedPose.Request()
|
||||
req.pose_name = name
|
||||
req.velocity_scale = float(min(max(speed, 0.05), 1.0))
|
||||
req.acceleration_scale = 0.0
|
||||
print(f" [B] set_named_pose '{name}' speed={req.velocity_scale:.2f}", flush=True)
|
||||
fut = cli.call_async(req)
|
||||
t0 = time.monotonic()
|
||||
while not fut.done() and time.monotonic() - t0 < 90.0:
|
||||
self._spin(0.1)
|
||||
if not fut.done():
|
||||
print("错误：set_named_pose 超时", flush=True)
|
||||
return False
|
||||
resp = fut.result()
|
||||
print(
|
||||
f" [B] 响应 success={resp.success} msg={resp.message}",
|
||||
flush=True,
|
||||
)
|
||||
if not resp.success:
|
||||
return False
|
||||
# 等待法兰停止运动（约 0.8 s 内 Δp < 1 mm）
|
||||
last = None
|
||||
still = 0.0
|
||||
t0 = time.monotonic()
|
||||
while time.monotonic() - t0 < 30.0:
|
||||
self._spin(0.2)
|
||||
Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
|
||||
if Tee is not None:
|
||||
p = Tee[:3, 3]
|
||||
if last is not None and float(np.linalg.norm(p - last)) < 0.001:
|
||||
still += 0.2
|
||||
if still >= 0.8:
|
||||
return True
|
||||
else:
|
||||
still = 0.0
|
||||
last = p.copy()
|
||||
print(" 警告：等待静止超时 —— 仍然继续", flush=True)
|
||||
return True
|
||||
|
||||
def run(self, timeout_s: float = 25.0) -> int:
|
||||
cfg = self.cfg
|
||||
q, Tee = self._wait_state(4.0)
|
||||
if q is None or Tee is None:
|
||||
print("错误：没有 joint_states / TF —— 请启动 run_real.sh", flush=True)
|
||||
return 2
|
||||
if not self._cli.wait_for_server(timeout_sec=8.0):
|
||||
print("错误：JTC action 不可用 —— 请启动 run_real.sh", flush=True)
|
||||
return 3
|
||||
|
||||
p = Tee[:3, 3]
|
||||
safe_z = float(cfg["safe_z"])
|
||||
print(
|
||||
f"=== 回到 Flexiv 初始位姿 '{cfg['home_pose']}' "
|
||||
f"safe_z={safe_z*1000:.0f}mm ===",
|
||||
flush=True,
|
||||
)
|
||||
print(
|
||||
f" 当前 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
# --- 阶段 A：回 home 前先沿纯 Z 方向升到安全高度 ---
|
||||
if p[2] < safe_z - 0.01:
|
||||
lift_tgt = np.array([p[0], p[1], safe_z], dtype=float)
|
||||
print(f" [A] 抬升 Z {p[2]*1000:.0f} → {safe_z*1000:.0f}mm", flush=True)
|
||||
ok, q, Tee = self._goto(
|
||||
q, Tee, lift_tgt, False, 0.010, 0.0, "[A]lift", v=0.05
|
||||
)
|
||||
if not ok:
|
||||
print("错误：抬升阶段失败 —— 就地 pin 住", flush=True)
|
||||
self._pin(q)
|
||||
return 4
|
||||
|
||||
# --- 阶段 B：Flexiv 命名位姿 → 真正的初始位姿，姿态良好 ---
|
||||
if not self._named_pose(cfg["home_pose"], cfg["home_speed"]):
|
||||
print("错误：回命名位姿失败 —— 就地 pin 住", flush=True)
|
||||
self._pin(q)
|
||||
return 4
|
||||
|
||||
# --- 阶段 C：保持住，让伺服从一个静止位姿接手 ---
|
||||
q, Tee = self._wait_state(2.0)
|
||||
if q is not None:
|
||||
self._pin(q)
|
||||
if Tee is not None:
|
||||
p = Tee[:3, 3]
|
||||
print(
|
||||
f" 已到初始位姿 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
|
||||
flush=True,
|
||||
)
|
||||
return 0
|
||||
|
||||
def main() -> int:
|
||||
cfg = _load_ee_cfg()
|
||||
rclpy.init()
|
||||
node = GotoEePlane(cfg)
|
||||
try:
|
||||
return node.run()
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,125 @@
|
||||
"""Station-owned PGE/PGEA node: one serial owner, typed service and status only."""
|
||||
import math
|
||||
import time
|
||||
import rclpy
|
||||
from arm_application.ros_node import TolerantNode
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
from arm_application.dh_pgea import DhPgeaGripper, GripperState
|
||||
|
||||
|
||||
class GripperNode(TolerantNode):
|
||||
def __init__(self):
|
||||
super().__init__('orisys_gripper')
|
||||
params={'port':'/dev/ttyUSB0','baud':115200,'slave_id':1,'stroke_mm':20.,'auto_init':False,'rs485_rts':''}
|
||||
for key,value in params.items(): self.declare_parameter(key,value)
|
||||
value=lambda key:self.get_parameter(key).value
|
||||
if value('auto_init'): raise ValueError('本站驱动禁止连接时自动初始化')
|
||||
self.stroke=float(value('stroke_mm'))
|
||||
if not math.isfinite(self.stroke) or self.stroke<=0: raise ValueError('夹爪行程无效')
|
||||
self.driver=DhPgeaGripper(port=value('port'),baudrate=value('baud'),slave_id=value('slave_id'),
|
||||
stroke_mm=self.stroke,rs485_rts=value('rs485_rts'))
|
||||
self._reading=GripperState(stroke_mm=self.stroke)
|
||||
self._online=False; self._error='未连接';self._speed=0.;self._stamp=0.
|
||||
self.publisher=self.create_publisher(GripperStatus,'status',10)
|
||||
self.create_service(GripperExecute,'execute',self._execute)
|
||||
# Bus operations are serialized by the default mutually exclusive callback group.
|
||||
# Do not open the serial port here: Qt probe calls execute as soon as the
|
||||
# service exists, and a second open of the USB2 RS485 dongle leaves 0x0201 empty.
|
||||
self.create_timer(.1,self._poll)
|
||||
|
||||
def _connect(self):
|
||||
try:
|
||||
self.driver.close()
|
||||
self.driver.open()
|
||||
self._reading=self.driver.get_run_states(retries=8)
|
||||
self._online=True;self._error='';self._stamp=time.monotonic()
|
||||
self.get_logger().info(self._reading.summary())
|
||||
return True
|
||||
except Exception as exc:
|
||||
self._offline(exc);return False
|
||||
|
||||
def _offline(self, exc):
|
||||
self._online=False;self._error=str(exc)
|
||||
self.get_logger().warn(self._error)
|
||||
self.driver.close()
|
||||
|
||||
def _status(self):
|
||||
state=self._reading;msg=GripperStatus();msg.header.stamp=self.get_clock().now().to_msg()
|
||||
msg.name='DH PGE/PGEA';msg.gripper_type='parallel'
|
||||
msg.connected=self._online and time.monotonic()-self._stamp<1.
|
||||
msg.initialized=bool(state.initialized)
|
||||
msg.is_moving=bool(msg.connected and state.grip_state==0)
|
||||
msg.is_gripped=bool(msg.connected and state.grip_state==2)
|
||||
msg.state='OFFLINE' if not msg.connected else 'MOVING' if msg.is_moving else 'IDLE'
|
||||
msg.position=float(state.position)/1000.*self.stroke
|
||||
msg.finger_position_per_mille=float(state.position)
|
||||
msg.gripper_state=int(state.grip_state);msg.rotation_state=-1
|
||||
msg.velocity=self._speed;msg.force=float(state.target_force);msg.error_msg=self._error
|
||||
return msg
|
||||
|
||||
def _poll(self):
|
||||
if self._online:
|
||||
try:
|
||||
self._reading=self.driver.get_run_states(retries=2);self._stamp=time.monotonic();self._error=''
|
||||
except Exception as exc:
|
||||
if not self._recover(exc):
|
||||
self._offline(exc)
|
||||
self.publisher.publish(self._status())
|
||||
|
||||
def _recover(self, exc):
|
||||
"""Empty 0x0201 replies on a live fd need a new serial session, not more retries."""
|
||||
self.get_logger().warn(f'状态读取失败，重开串口: {exc}')
|
||||
try:
|
||||
self.driver.close()
|
||||
self.driver.open()
|
||||
self._reading=self.driver.get_run_states(retries=4)
|
||||
self._stamp=time.monotonic();self._error='';self._online=True
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
def _execute(self, request, response):
|
||||
command=request.command
|
||||
try:
|
||||
if command.command in ('connect','probe'):
|
||||
if not self._connect(): raise RuntimeError(self._error)
|
||||
elif command.command=='move':
|
||||
numbers=(command.position,command.force,command.speed)
|
||||
if not all(math.isfinite(v) for v in numbers): raise ValueError('参数必须是有限数值')
|
||||
if not 0<=command.position<=self.stroke: raise ValueError('位置超出夹爪行程')
|
||||
if not 20<=command.force<=100 or not 1<=command.speed<=100: raise ValueError('力 20–100%，速度 1–100%')
|
||||
if not self._online or time.monotonic()-self._stamp>=1: raise RuntimeError('请先连接夹爪')
|
||||
self._reading=self.driver.get_run_states(retries=2)
|
||||
if self._reading.grip_state==0: raise RuntimeError('夹爪仍在运动，拒绝覆盖')
|
||||
if not self.driver.set_target_force(int(command.force)): raise RuntimeError('设置夹持力失败，未下发位置')
|
||||
if not self.driver.set_target_speed(int(command.speed)): raise RuntimeError('设置速度失败，未下发位置')
|
||||
target=round(command.position/self.stroke*1000.)
|
||||
if not self.driver.set_target_position(target): raise RuntimeError('位置写入失败，结果未知')
|
||||
self._speed=command.speed
|
||||
else: raise ValueError('本站平行夹爪仅提供 connect/probe/move，不支持旋转或隐式初始化')
|
||||
response.success=True
|
||||
response.message='夹爪已连接，未执行初始化或运动' if command.command in ('connect','probe') else '位置指令已接收，等待实际到位反馈'
|
||||
except ValueError as exc:
|
||||
response.success=False;response.message=str(exc)
|
||||
except Exception as exc:
|
||||
self._offline(exc);response.success=False;response.message=str(exc)
|
||||
response.status=self._status()
|
||||
self.publisher.publish(response.status)
|
||||
return response
|
||||
|
||||
def destroy_node(self):
|
||||
self.driver.close()
|
||||
return super().destroy_node()
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init();node=None
|
||||
try:
|
||||
node=GripperNode();rclpy.spin(node)
|
||||
except KeyboardInterrupt: pass
|
||||
finally:
|
||||
if node: node.destroy_node()
|
||||
if rclpy.ok(): rclpy.shutdown()
|
||||
|
||||
if __name__=='__main__': main()
|
||||
@@ -0,0 +1,769 @@
|
||||
#!/usr/bin/env python3
|
||||
"""完整手眼标定流水线：移动 → 棋盘稳定判定 → 采集 → 求解 → 清理。
|
||||
|
||||
支持 eye_in_hand / eye_to_hand。
|
||||
|
||||
相机内参优先级：
|
||||
1) ROS 的 CameraInfo 话题（若正在发布）
|
||||
2) 通过 pyrealsense2 读取 RealSense D405/D435 的 EEPROM（camera_source）
|
||||
3) YAML 中手写的 camera_fx/fy/cx/cy
|
||||
|
||||
棋盘尺寸（board_cols / board_rows / square_size_m）可由用户配置。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import shutil
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image, JointState
|
||||
from std_msgs.msg import String
|
||||
|
||||
from arm_application.handeye_lib import (
|
||||
BoardConfig,
|
||||
CameraIntrinsics,
|
||||
ChessboardStabilizer,
|
||||
T_from_flexiv_tcp,
|
||||
calibrate_hand_eye,
|
||||
cam_to_flange_via_tool,
|
||||
detect_chessboard,
|
||||
format_matrix_yaml,
|
||||
format_transform_yaml,
|
||||
is_pose_filled,
|
||||
pose_to_Rt,
|
||||
solve_board_pose,
|
||||
)
|
||||
from arm_application.handeye_realsense import (
|
||||
is_realsense_source,
|
||||
query_realsense_color_intrinsics,
|
||||
resolve_realsense_topics,
|
||||
)
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
|
||||
|
||||
def _image_to_bgr(msg: Image) -> np.ndarray:
|
||||
h, w = int(msg.height), int(msg.width)
|
||||
enc = (msg.encoding or '').lower()
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
|
||||
if enc in ('bgr8', 'rgb8'):
|
||||
img = data.reshape(h, w, 3)
|
||||
return img[:, :, ::-1].copy() if enc == 'rgb8' else img
|
||||
if enc in ('mono8', '8uc1'):
|
||||
gray = data.reshape(h, w)
|
||||
return np.stack([gray, gray, gray], axis=-1)
|
||||
if enc in ('bgra8', 'rgba8'):
|
||||
img = data.reshape(h, w, 4)
|
||||
bgr = img[:, :, :3]
|
||||
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
|
||||
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
|
||||
|
||||
|
||||
def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics:
|
||||
K = np.array(msg.k, dtype=float).reshape(3, 3)
|
||||
dist = np.array(msg.d, dtype=float).reshape(-1, 1) if msg.d else np.zeros((5, 1))
|
||||
return CameraIntrinsics(K=K, dist=dist)
|
||||
|
||||
|
||||
class HandeyeCalibrate(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_calibrate')
|
||||
|
||||
self.declare_parameter('mode', 'eye_in_hand') # eye_in_hand | eye_to_hand
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||
self.declare_parameter('velocity_scale', 0.10)
|
||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||
self.declare_parameter('move_time_sec', 4.0)
|
||||
self.declare_parameter('start_index', 0)
|
||||
|
||||
self.declare_parameter('num_poses', 12)
|
||||
self.declare_parameter('waypoint_positions', [0.0] * (12 * 7))
|
||||
self.declare_parameter('min_poses', 5)
|
||||
|
||||
# 相机来源：manual | d405 | d435
|
||||
self.declare_parameter('camera_source', 'manual')
|
||||
self.declare_parameter('camera_serial', '')
|
||||
self.declare_parameter('camera_namespace', '')
|
||||
self.declare_parameter('realsense_width', 1280)
|
||||
self.declare_parameter('realsense_height', 720)
|
||||
self.declare_parameter('image_topic', '')
|
||||
self.declare_parameter('camera_info_topic', '')
|
||||
self.declare_parameter('camera_fx', 0.0)
|
||||
self.declare_parameter('camera_fy', 0.0)
|
||||
self.declare_parameter('camera_cx', 0.0)
|
||||
self.declare_parameter('camera_cy', 0.0)
|
||||
self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0])
|
||||
|
||||
# 棋盘（可自定义）
|
||||
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
|
||||
# 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
|
||||
# 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
|
||||
self.declare_parameter('board_cols', 11) # 内角点
|
||||
self.declare_parameter('board_rows', 8)
|
||||
self.declare_parameter('square_size_m', 0.015)
|
||||
|
||||
self.declare_parameter('stable_window', 8)
|
||||
self.declare_parameter('stable_pixel_std', 0.45)
|
||||
self.declare_parameter('stable_timeout_sec', 25.0)
|
||||
self.declare_parameter('settle_sec', 0.4)
|
||||
# 到位后的"真的停住了"判据 —— 直接看关节速度，比固定 sleep 可靠。
|
||||
# 这几个参数原先 site.yaml 和 run_handeye.sh 一直在传，但本节点从未声明，
|
||||
# ROS 2 会静默忽略未声明的覆盖参数，于是"等静止再拍照"这件事其实没实现，
|
||||
# 实际只靠一次固定 settle_sec。现在补上。
|
||||
self.declare_parameter('settle_qd_max', 0.015) # |qd|_max [rad/s] 低于此值算静
|
||||
self.declare_parameter('settle_stable_sec', 0.6) # 需要连续静多久
|
||||
self.declare_parameter('settle_timeout_sec', 8.0) # 等静止的总超时
|
||||
self.declare_parameter('pin_holds', 2) # 静止后再保持几拍
|
||||
self.declare_parameter('pin_hold_sec', 0.45) # 每拍多长
|
||||
self.declare_parameter('skip_unstable_poses', True)
|
||||
|
||||
self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
|
||||
self.declare_parameter('result_file', '')
|
||||
self.declare_parameter('cleanup_images', True)
|
||||
# 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
|
||||
# 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
|
||||
self.declare_parameter('gripper_tool', '')
|
||||
self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
|
||||
self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
self.get_parameter('joint_names').get_parameter_value().string_array_value
|
||||
or DEFAULT_JOINTS
|
||||
)
|
||||
self.joint_names = [f'{prefix}{n}' for n in base]
|
||||
self.mode = str(self.get_parameter('mode').value).strip().lower()
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
self.action_name = str(self.get_parameter('action_name').value)
|
||||
self.move_time_sec = float(self.get_parameter('move_time_sec').value)
|
||||
self.start_index = max(0, int(self.get_parameter('start_index').value))
|
||||
self.min_poses = int(self.get_parameter('min_poses').value)
|
||||
self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
|
||||
self.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
|
||||
tcp_xyz = list(
|
||||
self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
|
||||
or [0.0, 0.0, 0.0]
|
||||
)
|
||||
tcp_q = list(
|
||||
self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
|
||||
or [1.0, 0.0, 0.0, 0.0]
|
||||
)
|
||||
self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
|
||||
if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
|
||||
raise RuntimeError(
|
||||
f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
|
||||
'等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
|
||||
)
|
||||
|
||||
self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
|
||||
self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
|
||||
self.camera_namespace = str(
|
||||
self.get_parameter('camera_namespace').value or ''
|
||||
).strip()
|
||||
self.rs_width = int(self.get_parameter('realsense_width').value)
|
||||
self.rs_height = int(self.get_parameter('realsense_height').value)
|
||||
|
||||
self.board = BoardConfig(
|
||||
cols=int(self.get_parameter('board_cols').value),
|
||||
rows=int(self.get_parameter('board_rows').value),
|
||||
square_size=float(self.get_parameter('square_size_m').value),
|
||||
)
|
||||
self.stabilizer = ChessboardStabilizer(
|
||||
window=int(self.get_parameter('stable_window').value),
|
||||
max_pixel_std=float(self.get_parameter('stable_pixel_std').value),
|
||||
)
|
||||
self.stable_timeout = float(self.get_parameter('stable_timeout_sec').value)
|
||||
self.settle_sec = float(self.get_parameter('settle_sec').value)
|
||||
self.settle_qd_max = float(self.get_parameter('settle_qd_max').value)
|
||||
self.settle_stable_sec = float(self.get_parameter('settle_stable_sec').value)
|
||||
self.settle_timeout = float(self.get_parameter('settle_timeout_sec').value)
|
||||
self.pin_holds = max(0, int(self.get_parameter('pin_holds').value))
|
||||
self.pin_hold_sec = max(0.0, float(self.get_parameter('pin_hold_sec').value))
|
||||
self.skip_unstable = bool(self.get_parameter('skip_unstable_poses').value)
|
||||
|
||||
out = Path(str(self.get_parameter('output_dir').value)).expanduser()
|
||||
self.output_dir = out
|
||||
self.images_dir = out / 'images'
|
||||
rf = str(self.get_parameter('result_file').value or '').strip()
|
||||
self.result_file = Path(rf).expanduser() if rf else (out / 'handeye_result.yaml')
|
||||
|
||||
self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
self._capture_pub = self.create_publisher(String, 'handeye/capture', 10)
|
||||
|
||||
self._latest_image: Optional[Image] = None
|
||||
self._latest_info: Optional[CameraInfo] = None
|
||||
self._latest_js: Optional[JointState] = None
|
||||
self._intrinsics: Optional[CameraIntrinsics] = None
|
||||
|
||||
img_topic = str(self.get_parameter('image_topic').value or '').strip()
|
||||
info_topic = str(self.get_parameter('camera_info_topic').value or '').strip()
|
||||
if is_realsense_source(self.camera_source):
|
||||
img_topic, info_topic = resolve_realsense_topics(
|
||||
self.camera_source,
|
||||
camera_namespace=self.camera_namespace,
|
||||
image_topic=img_topic,
|
||||
camera_info_topic=info_topic,
|
||||
)
|
||||
self._info_topic = info_topic
|
||||
|
||||
self.create_subscription(JointState, '/joint_states', self._on_js, 10)
|
||||
|
||||
if not img_topic:
|
||||
self.get_logger().error(
|
||||
'image_topic 为空。请设置 camera_source:=d405|d435，或填写 image_topic。'
|
||||
)
|
||||
else:
|
||||
self.create_subscription(
|
||||
Image, img_topic, self._on_image, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'已订阅图像：{img_topic}（BEST_EFFORT）')
|
||||
if info_topic:
|
||||
self.create_subscription(
|
||||
CameraInfo, info_topic, self._on_info, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'已订阅 camera_info：{info_topic}（BEST_EFFORT）')
|
||||
|
||||
def _on_js(self, msg: JointState) -> None:
|
||||
self._latest_js = msg
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
self._latest_image = msg
|
||||
|
||||
def _on_info(self, msg: CameraInfo) -> None:
|
||||
self._latest_info = msg
|
||||
|
||||
def run(self) -> int:
|
||||
if self.mode not in ('eye_in_hand', 'eye_to_hand'):
|
||||
self.get_logger().error(
|
||||
f'未知的 mode={self.mode!r}；请使用 eye_in_hand 或 eye_to_hand'
|
||||
)
|
||||
return 1
|
||||
|
||||
poses = self._load_poses()
|
||||
if len(poses) < self.min_poses:
|
||||
self.get_logger().error(
|
||||
f'只有 {len(poses)} 个已填写位姿（需要 >= {self.min_poses}）。'
|
||||
'示教命令：bash ~/ros2/scripts/run_handeye.sh teach && merge。'
|
||||
)
|
||||
return 1
|
||||
if self.start_index >= len(poses):
|
||||
self.get_logger().error(
|
||||
f'start_index {self.start_index} >= 已填写位姿数 {len(poses)}'
|
||||
)
|
||||
return 1
|
||||
|
||||
if not self._resolve_intrinsics():
|
||||
return 1
|
||||
|
||||
if not self._client.wait_for_server(timeout_sec=15.0):
|
||||
self.get_logger().error(
|
||||
f'Action [{self.action_name}] 不可用。'
|
||||
'请启动：bash ~/ros2/scripts/run_real.sh'
|
||||
)
|
||||
return 2
|
||||
|
||||
self.output_dir.mkdir(parents=True, exist_ok=True)
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
eye_to_hand = self.mode == 'eye_to_hand'
|
||||
grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
|
||||
self.get_logger().info(
|
||||
f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
|
||||
f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
|
||||
f'方格={self.board.square_size}m，v={self.velocity_scale} '
|
||||
f'gripper_tool={grip}'
|
||||
)
|
||||
self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
|
||||
|
||||
Rg: List[np.ndarray] = []
|
||||
tg: List[np.ndarray] = []
|
||||
Rc: List[np.ndarray] = []
|
||||
tc: List[np.ndarray] = []
|
||||
meta = []
|
||||
|
||||
try:
|
||||
for i in range(self.start_index, len(poses)):
|
||||
q = poses[i]
|
||||
self._log_progress(i, len(poses), '移动中...')
|
||||
code = self._move_one(q, self.move_time_sec)
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
if not self._verify_joints(q, tol=0.08):
|
||||
self.get_logger().error(
|
||||
f'[{i + 1}] 机械臂未到达目标关节位置 '
|
||||
'（控制器报告成功，但关节没有变化）。'
|
||||
'请重启 run_real.sh；示教器切 AUTO Remote；清除故障。'
|
||||
'不要相信这次标定结果。'
|
||||
)
|
||||
return 10
|
||||
|
||||
end = time.monotonic() + max(0.0, self.settle_sec)
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
|
||||
# 固定 settle_sec 之后再看关节速度：真停了才算数，然后 pin 一拍。
|
||||
self._wait_settled()
|
||||
self._pin_hold()
|
||||
|
||||
ok, std, corners, pattern, bgr = self._wait_stable()
|
||||
if not ok or corners is None or bgr is None:
|
||||
msg = f'[{i + 1}] 棋盘不稳定（std={std:.3f}）'
|
||||
if self.skip_unstable:
|
||||
self.get_logger().warn(f'{msg} —— 跳过')
|
||||
continue
|
||||
self.get_logger().error(f'{msg}。中止。')
|
||||
return 7
|
||||
|
||||
ee = self._read_ee_pose()
|
||||
if ee is None:
|
||||
self.get_logger().error('读取 ee_pose 失败')
|
||||
return 8
|
||||
|
||||
assert self._intrinsics is not None
|
||||
R_t2c, t_t2c = solve_board_pose(
|
||||
corners, self.board, self._intrinsics, pattern=pattern
|
||||
)
|
||||
xyz = [ee.pose.position.x, ee.pose.position.y, ee.pose.position.z]
|
||||
quat = [
|
||||
ee.pose.orientation.x,
|
||||
ee.pose.orientation.y,
|
||||
ee.pose.orientation.z,
|
||||
ee.pose.orientation.w,
|
||||
]
|
||||
R_g2b, t_g2b = pose_to_Rt(xyz, quat)
|
||||
|
||||
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
|
||||
img_path = self.images_dir / f'pose_{i:02d}_{stamp}.png'
|
||||
self._save_png(bgr, img_path)
|
||||
|
||||
msg = String()
|
||||
msg.data = f'capture:{i}:{len(poses)}'
|
||||
self._capture_pub.publish(msg)
|
||||
|
||||
Rg.append(R_g2b)
|
||||
tg.append(t_g2b)
|
||||
Rc.append(R_t2c)
|
||||
tc.append(t_t2c)
|
||||
meta.append(
|
||||
{
|
||||
'index': i,
|
||||
'joints': list(q),
|
||||
'ee_xyz': xyz,
|
||||
'ee_quat_xyzw': quat,
|
||||
'image': str(img_path),
|
||||
'corner_std_px': std,
|
||||
}
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{len(poses)}] 已采集 '
|
||||
f'(corner_std={std:.3f}px) -> {img_path.name}'
|
||||
)
|
||||
except KeyboardInterrupt:
|
||||
self.get_logger().info('已中断')
|
||||
return 0
|
||||
|
||||
if len(Rg) < self.min_poses:
|
||||
self.get_logger().error(
|
||||
f'只采集到 {len(Rg)} 次；求解至少需要 {self.min_poses} 次'
|
||||
)
|
||||
return 9
|
||||
|
||||
self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
|
||||
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
|
||||
extra = ''
|
||||
if eye_to_hand:
|
||||
name = 'T_cam_to_base (eye_to_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
if self.gripper_tool:
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
|
||||
f'T_4x4 是 cam→基座，不折 t={t.reshape(3)*1000} mm'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
T_cam_tool = np.eye(4)
|
||||
T_cam_tool[:3, :3] = R
|
||||
T_cam_tool[:3, 3] = t.reshape(3)
|
||||
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
|
||||
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
|
||||
tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
|
||||
if tool_len > 0.05 and delta < 1e-4:
|
||||
self.get_logger().error(
|
||||
'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
|
||||
'折算没生效。结果作废。'
|
||||
)
|
||||
return 11
|
||||
# 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
|
||||
if (
|
||||
self.T_flange_to_tool[2, 3] > 0.05
|
||||
and float(T_cam_flange[2, 3]) < 0.0
|
||||
):
|
||||
self.get_logger().error(
|
||||
f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
|
||||
'在法兰后方，乘序反了。结果作废。'
|
||||
)
|
||||
return 12
|
||||
R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
|
||||
name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
+ format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：'
|
||||
f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
|
||||
f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
|
||||
)
|
||||
else:
|
||||
name = 'T_cam_to_gripper (eye_in_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
header = (
|
||||
f'# 手眼标定结果\n'
|
||||
f'# mode: {self.mode}\n'
|
||||
f'# camera_source: {self.camera_source}\n'
|
||||
f'# views: {len(Rg)}\n'
|
||||
f'# utc: {datetime.now(timezone.utc).isoformat()}\n'
|
||||
f'# board: {self.board.cols}x{self.board.rows} '
|
||||
f'square_m={self.board.square_size}\n'
|
||||
)
|
||||
if self.gripper_tool and eye_to_hand:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
|
||||
f'# 旧结果备份在 results/backup/\n'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
|
||||
f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
|
||||
)
|
||||
self.result_file.parent.mkdir(parents=True, exist_ok=True)
|
||||
self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
|
||||
(self.output_dir / 'captures.json').write_text(
|
||||
json.dumps(meta, indent=2), encoding='utf-8'
|
||||
)
|
||||
|
||||
self.get_logger().info('======== 手眼标定结果 ========')
|
||||
for line in (header + yaml_txt).strip().splitlines():
|
||||
self.get_logger().info(line)
|
||||
self.get_logger().info(f'已保存 -> {self.result_file}')
|
||||
|
||||
if self.cleanup_images:
|
||||
if self.images_dir.exists():
|
||||
shutil.rmtree(self.images_dir, ignore_errors=True)
|
||||
self.get_logger().info('标定图像已删除')
|
||||
else:
|
||||
self.get_logger().info(f'图像保留在 {self.images_dir}')
|
||||
return 0
|
||||
|
||||
def _load_poses(self) -> List[List[float]]:
|
||||
flat = list(self.get_parameter('waypoint_positions').value)
|
||||
n_j = len(self.joint_names)
|
||||
n_cfg = int(self.get_parameter('num_poses').value)
|
||||
if n_cfg < 1:
|
||||
return []
|
||||
need = n_cfg * n_j
|
||||
if len(flat) < need:
|
||||
self.get_logger().warn(
|
||||
f'waypoint_positions 长度 {len(flat)} < num_poses*{n_j}={need}；'
|
||||
'用零补齐'
|
||||
)
|
||||
flat = flat + [0.0] * (need - len(flat))
|
||||
poses: List[List[float]] = []
|
||||
for i in range(n_cfg):
|
||||
q = [float(v) for v in flat[i * n_j : (i + 1) * n_j]]
|
||||
if is_pose_filled(q):
|
||||
poses.append(q)
|
||||
else:
|
||||
self.get_logger().warn(f'位姿槽 {i} 为空（全零）—— 已跳过')
|
||||
return poses
|
||||
|
||||
def _resolve_intrinsics(self) -> bool:
|
||||
# 1) CameraInfo
|
||||
deadline = time.monotonic() + 3.0
|
||||
while rclpy.ok() and self._latest_info is None and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.1)
|
||||
|
||||
if self._latest_info is not None:
|
||||
self._intrinsics = _camera_info_to_intrinsics(self._latest_info)
|
||||
K = self._intrinsics.K
|
||||
self.get_logger().info(
|
||||
f'内参来自 CameraInfo（{self._info_topic}）：'
|
||||
f'fx={K[0, 0]:.2f} fy={K[1, 1]:.2f} cx={K[0, 2]:.2f} cy={K[1, 2]:.2f}'
|
||||
)
|
||||
return True
|
||||
|
||||
# 2) RealSense EEPROM
|
||||
if is_realsense_source(self.camera_source):
|
||||
try:
|
||||
intr = query_realsense_color_intrinsics(
|
||||
self.camera_source,
|
||||
serial=self.camera_serial,
|
||||
width=self.rs_width,
|
||||
height=self.rs_height,
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'RealSense 内参查询失败：{exc}')
|
||||
else:
|
||||
self._intrinsics = CameraIntrinsics(K=intr.as_K(), dist=intr.as_dist())
|
||||
self.get_logger().info(
|
||||
f'内参来自 RealSense {intr.model.upper()} '
|
||||
f'serial={intr.serial} {intr.width}x{intr.height}：'
|
||||
f'fx={intr.fx:.2f} fy={intr.fy:.2f} '
|
||||
f'cx={intr.cx:.2f} cy={intr.cy:.2f}'
|
||||
)
|
||||
if (intr.width, intr.height) != (self.rs_width, self.rs_height):
|
||||
self.get_logger().warn(
|
||||
f'请求 {self.rs_width}x{self.rs_height}，'
|
||||
f'实际得到 {intr.width}x{intr.height}（最接近的 profile）'
|
||||
)
|
||||
return True
|
||||
|
||||
# 3) Manual YAML
|
||||
fx = float(self.get_parameter('camera_fx').value)
|
||||
fy = float(self.get_parameter('camera_fy').value)
|
||||
cx = float(self.get_parameter('camera_cx').value)
|
||||
cy = float(self.get_parameter('camera_cy').value)
|
||||
if fx <= 0 or fy <= 0:
|
||||
self.get_logger().error(
|
||||
'没有内参。请设置 camera_source:=d405|d435，'
|
||||
'或提供 camera_info_topic，或填写 camera_fx/fy/cx/cy。'
|
||||
)
|
||||
return False
|
||||
dist = np.array(
|
||||
list(self.get_parameter('camera_dist').value), dtype=float
|
||||
).reshape(-1, 1)
|
||||
K = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]], dtype=float)
|
||||
self._intrinsics = CameraIntrinsics(K=K, dist=dist)
|
||||
self.get_logger().info(
|
||||
f'内参来自 YAML：fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}'
|
||||
)
|
||||
return True
|
||||
|
||||
def _log_progress(self, i: int, total: int, suffix: str = '') -> None:
|
||||
"""带进度条的日志。
|
||||
|
||||
自动标定是无人值守的长流程（十几个点位 × 移动+稳定+拍照），
|
||||
单看 `[3/12]` 很难有"还要多久"的直觉，加一条可视化进度条。
|
||||
"""
|
||||
frac = (i + 1) / max(1, total)
|
||||
width = 24
|
||||
filled = int(round(frac * width))
|
||||
bar = '█' * filled + '░' * (width - filled)
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{total}] [{bar}] {frac * 100:5.1f}% {suffix}'
|
||||
)
|
||||
|
||||
def _wait_settled(self) -> bool:
|
||||
"""等机械臂真的停下来，再进入拍照。
|
||||
|
||||
判据是关节速度 |qd|_max 连续 ``settle_stable_sec`` 低于 ``settle_qd_max``。
|
||||
为什么不用固定 sleep：`settle_sec` 是拍脑袋的时长，臂负载/姿态不同，
|
||||
刹车距离差别很大 —— 等太短会在臂还在动时拍照（棋盘虚 → 角点抖动），
|
||||
等太长则整轮标定被无谓拉长。直接看速度更准。
|
||||
|
||||
超时不算致命：记一条警告后继续，让后面的棋盘稳定性检查去把关。
|
||||
"""
|
||||
if self.settle_qd_max <= 0.0:
|
||||
return True
|
||||
deadline = time.monotonic() + self.settle_timeout
|
||||
stable_since: Optional[float] = None
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
js = self._latest_js
|
||||
if js is None or not list(js.velocity):
|
||||
continue
|
||||
try:
|
||||
vmax = float(np.max(np.abs(np.asarray(js.velocity, dtype=float))))
|
||||
except Exception: # noqa: BLE001
|
||||
continue
|
||||
now = time.monotonic()
|
||||
if vmax <= self.settle_qd_max:
|
||||
if stable_since is None:
|
||||
stable_since = now
|
||||
elif now - stable_since >= self.settle_stable_sec:
|
||||
return True
|
||||
else:
|
||||
stable_since = None # 又动了，重新计时
|
||||
self.get_logger().warn(
|
||||
f'wait_settled 超时 ({self.settle_timeout:.1f}s)，臂可能仍在微动；'
|
||||
'交给棋盘稳定性检查兜底'
|
||||
)
|
||||
return False
|
||||
|
||||
def _pin_hold(self) -> None:
|
||||
"""静止后再保持几拍，抑制到位后的微小回弹/漂移。
|
||||
|
||||
语义取自 site.yaml 的注释"零速 pin 后再拍照"：不是重新规划轨迹，
|
||||
而是纯粹等一段时间让机械臂自己稳下来（控制器在到位后仍会做少量补偿）。
|
||||
"""
|
||||
total = self.pin_holds * self.pin_hold_sec
|
||||
end = time.monotonic() + total
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
|
||||
def _wait_stable(
|
||||
self,
|
||||
) -> Tuple[bool, float, Optional[np.ndarray], Optional[np.ndarray]]:
|
||||
self.stabilizer.reset()
|
||||
deadline = time.monotonic() + self.stable_timeout
|
||||
last_std = float('inf')
|
||||
last_corners = None
|
||||
last_pattern = None
|
||||
last_bgr = None
|
||||
last_log = 0.0
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if self._latest_image is None:
|
||||
continue
|
||||
try:
|
||||
bgr = _image_to_bgr(self._latest_image)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'图像转换失败：{exc}')
|
||||
return False, last_std, None, None, None
|
||||
detected = detect_chessboard(bgr, self.board)
|
||||
corners = detected[0] if detected is not None else None
|
||||
pattern = detected[1] if detected is not None else None
|
||||
ok, std = self.stabilizer.push(corners)
|
||||
last_std = std
|
||||
if corners is not None:
|
||||
last_corners = corners
|
||||
last_pattern = pattern
|
||||
last_bgr = bgr
|
||||
now = time.monotonic()
|
||||
if now - last_log > 1.0:
|
||||
seen = '有' if corners is not None else '无'
|
||||
self.get_logger().info(
|
||||
f' 棋盘={seen} 角点标准差='
|
||||
f'{std if std < 1e8 else float("nan"):.3f}px'
|
||||
)
|
||||
last_log = now
|
||||
if ok:
|
||||
return True, std, last_corners, last_pattern, last_bgr
|
||||
return False, last_std, last_corners, last_pattern, last_bgr
|
||||
|
||||
|
||||
def _verify_joints(self, target: List[float], tol: float = 0.08) -> bool:
|
||||
"""确认 ``joint_states`` 真的走到了目标位置。
|
||||
|
||||
用来识破"假的成功"：有些执行接口会立刻返回成功，但关节其实没动
|
||||
（或只动了一半）。这里在 2 秒内轮询关节角，逐轴比对容差 ``tol``（弧度），
|
||||
只有全部落在容差内才算真到位。
|
||||
"""
|
||||
deadline = time.monotonic() + 2.0
|
||||
while rclpy.ok() and time.monotonic() < deadline:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if self._latest_js is None:
|
||||
continue
|
||||
name_to_q = dict(zip(list(self._latest_js.name), list(self._latest_js.position)))
|
||||
actual = []
|
||||
for n in self.joint_names:
|
||||
if n not in name_to_q:
|
||||
break
|
||||
actual.append(float(name_to_q[n]))
|
||||
if len(actual) != len(target):
|
||||
continue
|
||||
err = float(np.sqrt(np.mean([(a - b) ** 2 for a, b in zip(actual, target)])))
|
||||
self.get_logger().info(
|
||||
f' 关节相对目标的 RMSE：{err:.4f} rad（tol={tol}）'
|
||||
)
|
||||
return err <= tol
|
||||
self.get_logger().error('校验运动时没有 joint_states')
|
||||
return False
|
||||
|
||||
def _move_one(self, positions: List[float], duration: float) -> int:
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.joint_names = self.joint_names
|
||||
goal.num_points = 1
|
||||
goal.velocity_scale = self.velocity_scale
|
||||
goal.positions = list(positions)
|
||||
goal.time_from_start = [float(duration)]
|
||||
|
||||
send = self._client.send_goal_async(goal)
|
||||
if not self._wait_future(send, 10.0, '发送目标'):
|
||||
return 3
|
||||
handle = send.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return 4
|
||||
res_fut = handle.get_result_async()
|
||||
if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
|
||||
return 5
|
||||
result = res_fut.result().result
|
||||
if not result.success:
|
||||
self.get_logger().error(f'移动失败：{result.message}')
|
||||
return 6
|
||||
return 0
|
||||
|
||||
def _wait_future(self, fut, timeout: float, label: str) -> bool:
|
||||
deadline = time.monotonic() + timeout
|
||||
while rclpy.ok() and not fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error(f'等待 {label} 超时')
|
||||
return False
|
||||
return fut.done()
|
||||
|
||||
def _read_ee_pose(self) -> Optional[PoseStamped]:
|
||||
if not self._get_state.wait_for_service(timeout_sec=2.0):
|
||||
return None
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
if not self._wait_future(fut, 2.0, 'get_arm_state'):
|
||||
return None
|
||||
res = fut.result()
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
return res.ee_pose
|
||||
|
||||
def _save_png(self, bgr: np.ndarray, path: Path) -> None:
|
||||
import cv2
|
||||
|
||||
if not cv2.imwrite(str(path), bgr):
|
||||
raise RuntimeError(f'cv2.imwrite 失败：{path}')
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = HandeyeCalibrate()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,668 @@
|
||||
#!/usr/bin/env python3
|
||||
"""手眼示教：实时预览 + Free Drive + 按 Enter 拍照/记录关节。
|
||||
|
||||
两种后端
|
||||
------------
|
||||
ros（默认，整套栈正在运行）：
|
||||
- 关节来自 ``/get_arm_state``
|
||||
- 拖拽：示教器 Free Drive + 按住使能键
|
||||
需保持：bash ~/ros2/scripts/run_real.sh
|
||||
|
||||
rdk（软件 Free Drive）：
|
||||
- 先停掉 run_real.sh（RDK 独占）
|
||||
- 按 'd' 通过 RDK 启用 FloatingJoint 拖拽
|
||||
- 关节来自 RDK 的 robot.states
|
||||
|
||||
source ~/ros2/scripts/env.sh
|
||||
ros2 run arm_application handeye_teach --ros-args -p backend:=rdk
|
||||
|
||||
界面按键（预览窗口）
|
||||
Enter / Space 拍照 + 记录关节
|
||||
d 启用 Free Drive（rdk）/ 显示示教器提示（ros）
|
||||
s 停止 Free Drive（rdk）/ 保持
|
||||
q / Esc 结束
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
|
||||
HELP = """
|
||||
================================================
|
||||
手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
|
||||
================================================
|
||||
窗口按键：
|
||||
Enter / Space = 拍照 + 保存关节
|
||||
d = 开启 Free Drive
|
||||
s = 关闭 Free Drive / 保持
|
||||
q / Esc = 结束
|
||||
提示：让棋盘清晰，并占到画面的约 30-70%
|
||||
建议采 8-12 个角度有变化的位姿
|
||||
================================================
|
||||
"""
|
||||
|
||||
|
||||
def _setup_matplotlib_font():
|
||||
"""避免中文乱码：优先选用存在的中日韩（CJK）字体。"""
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use('QtAgg')
|
||||
from matplotlib import font_manager, rcParams
|
||||
|
||||
candidates = [
|
||||
'Noto Sans CJK SC',
|
||||
'Noto Sans CJK JP',
|
||||
'Noto Serif CJK SC',
|
||||
'AR PL UMing CN',
|
||||
'AR PL UKai CN',
|
||||
'WenQuanYi Micro Hei',
|
||||
'Source Han Sans CN',
|
||||
'DejaVu Sans',
|
||||
]
|
||||
available = {f.name for f in font_manager.fontManager.ttflist}
|
||||
chosen = None
|
||||
for name in candidates:
|
||||
if name in available:
|
||||
chosen = name
|
||||
break
|
||||
if chosen is None:
|
||||
# 按文件路径再试一次
|
||||
for path in (
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc',
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-Medium.ttc',
|
||||
'/usr/share/fonts/opentype/noto/NotoSansCJK-DemiLight.ttc',
|
||||
'/usr/share/fonts/truetype/arphic/uming.ttc',
|
||||
):
|
||||
if os.path.isfile(path):
|
||||
font_manager.fontManager.addfont(path)
|
||||
prop = font_manager.FontProperties(fname=path)
|
||||
chosen = prop.get_name()
|
||||
break
|
||||
if chosen:
|
||||
rcParams['font.family'] = chosen
|
||||
rcParams['axes.unicode_minus'] = False
|
||||
return chosen
|
||||
|
||||
|
||||
class RdkBridge:
|
||||
"""通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
|
||||
|
||||
def __init__(self, robot_sn: str, logger=None) -> None:
|
||||
import json
|
||||
import subprocess
|
||||
|
||||
self._json = json
|
||||
self._logger = logger
|
||||
self.robot_sn = robot_sn
|
||||
self._last_q: Optional[List[float]] = None
|
||||
self._io = threading.Lock()
|
||||
py = os.environ.get('RDK_PYTHON', '/home/cc/rdk_py310/bin/python')
|
||||
bridge = Path.home() / 'ros2' / 'scripts' / 'rdk_drag_bridge.py'
|
||||
if not bridge.is_file():
|
||||
bridge = Path('/home/cc/ros2/scripts/rdk_drag_bridge.py')
|
||||
if not bridge.is_file():
|
||||
raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}')
|
||||
|
||||
# 只保留 Flexiv 的库 —— 不要继承 conda 的 LD_LIBRARY_PATH（会让 RDK 卡死）。
|
||||
flexiv_lib = os.path.expanduser('~/flexiv_install/lib')
|
||||
env = {
|
||||
'HOME': os.environ.get('HOME', ''),
|
||||
'PATH': f'{Path(py).parent}:/usr/bin:/bin',
|
||||
'LD_LIBRARY_PATH': flexiv_lib,
|
||||
'LANG': 'C.UTF-8',
|
||||
'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
|
||||
'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
|
||||
}
|
||||
# 不要继承 conda 的 PYTHONPATH。
|
||||
# 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
|
||||
# （stdout 永远不返回）。改成写入日志文件。
|
||||
log_path = Path('/tmp/rdk_drag_bridge.log')
|
||||
self._err_file = open(log_path, 'w', encoding='utf-8')
|
||||
self._proc = subprocess.Popen(
|
||||
[py, str(bridge)],
|
||||
stdin=subprocess.PIPE,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=self._err_file,
|
||||
text=True,
|
||||
bufsize=1,
|
||||
env=env,
|
||||
)
|
||||
if logger:
|
||||
logger.info(f'RDK 桥接已启动；日志 → {log_path}')
|
||||
pong = self._ask({'cmd': 'ping'}, timeout=8.0)
|
||||
if not pong.get('ok'):
|
||||
raise RuntimeError(
|
||||
f'RDK 桥接 ping 失败：{pong}。参见 {log_path}'
|
||||
)
|
||||
if logger:
|
||||
logger.info('RDK 桥接 ping 正常，正在连接机器人...')
|
||||
conn = self._ask(
|
||||
{
|
||||
'cmd': 'connect',
|
||||
'sn': robot_sn,
|
||||
'iface': os.environ.get('SITE_NET_IFACE', ''),
|
||||
},
|
||||
timeout=45.0,
|
||||
)
|
||||
if not conn.get('ok'):
|
||||
raise RuntimeError(
|
||||
f'RDK 连接失败：{conn.get("error")}。'
|
||||
f'请先停掉 run_real.sh；切 AUTO Remote 并清除故障。参见 {log_path}'
|
||||
)
|
||||
if logger:
|
||||
logger.info(f'RDK 桥接就绪（py={pong.get("py")}）sn={robot_sn}')
|
||||
q0 = conn.get('q') or []
|
||||
if len(q0) >= 7:
|
||||
self._last_q = [float(v) for v in q0[:7]]
|
||||
|
||||
def _ask(self, msg: dict, timeout: float = 10.0) -> dict:
|
||||
import select
|
||||
|
||||
# 同一时刻只允许一条在途命令：相机/GUI 绝不能共用这个管道。
|
||||
if not self._io.acquire(timeout=max(timeout, 0.2)):
|
||||
return {'ok': False, 'error': '桥接忙'}
|
||||
try:
|
||||
assert self._proc.stdin and self._proc.stdout
|
||||
self._proc.stdin.write(self._json.dumps(msg) + '\n')
|
||||
self._proc.stdin.flush()
|
||||
end = time.monotonic() + timeout
|
||||
while time.monotonic() < end:
|
||||
if self._proc.poll() is not None:
|
||||
tip = '/tmp/rdk_drag_bridge.log'
|
||||
return {
|
||||
'ok': False,
|
||||
'error': f'桥接已退出 rc={self._proc.returncode}；参见 {tip}',
|
||||
}
|
||||
ready, _, _ = select.select([self._proc.stdout], [], [], 0.05)
|
||||
if not ready:
|
||||
continue
|
||||
line = self._proc.stdout.readline()
|
||||
if not line:
|
||||
continue
|
||||
line = line.strip()
|
||||
if not line.startswith('{'):
|
||||
continue
|
||||
try:
|
||||
return self._json.loads(line)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return {'ok': False, 'error': f'应答异常：{exc} {line!r}'}
|
||||
return {'ok': False, 'error': '桥接超时'}
|
||||
finally:
|
||||
self._io.release()
|
||||
|
||||
def drag_on(self, primitive: str = 'FloatingJoint', mode: str = 'soft') -> dict:
|
||||
return self._ask(
|
||||
{'cmd': 'drag_on', 'primitive': primitive, 'mode': mode},
|
||||
timeout=20.0,
|
||||
)
|
||||
|
||||
def drag_off(self) -> dict:
|
||||
return self._ask({'cmd': 'drag_off'}, timeout=6.0)
|
||||
|
||||
def joints(self) -> Optional[List[float]]:
|
||||
res = self._ask({'cmd': 'joints'}, timeout=2.0)
|
||||
if res.get('ok'):
|
||||
q = res.get('q') or []
|
||||
if len(q) >= 7:
|
||||
self._last_q = [float(v) for v in q[:7]]
|
||||
return self._last_q
|
||||
return self._last_q
|
||||
|
||||
def close(self) -> None:
|
||||
try:
|
||||
self._ask({'cmd': 'quit'}, timeout=3.0)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
self._proc.terminate()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
if getattr(self, '_err_file', None) is not None:
|
||||
self._err_file.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
class HandeyeTeach(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_teach')
|
||||
self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
|
||||
self.declare_parameter('width', 1280)
|
||||
self.declare_parameter('height', 720)
|
||||
self.declare_parameter('fps', 15)
|
||||
self.declare_parameter('backend', 'ros') # ros | rdk
|
||||
self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
|
||||
self.declare_parameter('drag_primitive', 'FloatingJoint') # or FloatingCartesian
|
||||
self._serial = param_as_str(self, 'camera_serial', '')
|
||||
self.declare_parameter('auto_exposure', True)
|
||||
self.declare_parameter('ae_limit_us', 1800.0)
|
||||
self.declare_parameter('emitter_enabled', False)
|
||||
self.declare_parameter('exposure_us', 0.0)
|
||||
self.declare_parameter('gain', 0.0)
|
||||
# 目标采集点数。0 = 不限（按 q 结束）。
|
||||
# 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
|
||||
# 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
|
||||
self.declare_parameter('target_points', 0)
|
||||
# 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
|
||||
# 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
|
||||
self.declare_parameter('poses_file', '')
|
||||
|
||||
self.backend = str(self.get_parameter('backend').value).strip().lower()
|
||||
self.robot_sn = str(self.get_parameter('robot_sn').value)
|
||||
self.drag_primitive = str(self.get_parameter('drag_primitive').value).strip()
|
||||
|
||||
self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
|
||||
self.images_dir = self.output_dir / 'images'
|
||||
_pf = str(self.get_parameter('poses_file').value).strip()
|
||||
self.poses_file = (
|
||||
Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
|
||||
)
|
||||
self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
|
||||
self._last_capture_t = 0.0
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
self._captures: List[dict] = []
|
||||
self._latest_bgr: Optional[np.ndarray] = None
|
||||
self._frame_lock = threading.Lock()
|
||||
self._quit = False
|
||||
self._busy = False
|
||||
self._cmd_pending = False
|
||||
self._drag_on = False
|
||||
self._robot = None
|
||||
self._rdk = None
|
||||
self._get_state = None
|
||||
self._cam_thread: Optional[threading.Thread] = None
|
||||
self._pipe = None
|
||||
self._fps = 15
|
||||
|
||||
if self.backend == 'rdk':
|
||||
self._init_rdk()
|
||||
else:
|
||||
self._init_ros_state()
|
||||
|
||||
self._start_camera()
|
||||
font = _setup_matplotlib_font()
|
||||
self._start_gui()
|
||||
print(HELP)
|
||||
if self.backend == 'rdk':
|
||||
print('[backend=rdk] 按 d = 软拖拽（在 AUTO Remote 下可用，无需 Floating 许可）')
|
||||
print(' run_real.sh 必须已停止。')
|
||||
print(' 拖着就能按 Enter 拍照，不必先按 s')
|
||||
else:
|
||||
print('[backend=ros] 用示教器 Free Drive + 按住使能键来拖拽。')
|
||||
print(' 或停掉 run_real.sh 后改用：-p backend:=rdk')
|
||||
if font:
|
||||
self.get_logger().info(f'界面字体：{font}')
|
||||
self.create_timer(1.0 / max(1, self._fps), self._tick)
|
||||
|
||||
def _init_ros_state(self) -> None:
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
if not self._get_state.wait_for_service(timeout_sec=10.0):
|
||||
raise RuntimeError(
|
||||
'get_arm_state 不可用。请启动 run_real.sh，'
|
||||
'或改用：-p backend:=rdk（需先停掉整套栈）'
|
||||
)
|
||||
|
||||
def _init_rdk(self) -> None:
|
||||
self.get_logger().info(f'正在把 RDK 桥接连到 {self.robot_sn} ...')
|
||||
self._rdk = RdkBridge(self.robot_sn, logger=self.get_logger())
|
||||
self._robot = True # 标记：RDK 后端已就绪
|
||||
self.get_logger().info('RDK 已连接。按 d 启用 Free Drive。')
|
||||
|
||||
def _start_camera(self) -> None:
|
||||
import pyrealsense2 as rs
|
||||
|
||||
w = int(self.get_parameter('width').value)
|
||||
h = int(self.get_parameter('height').value)
|
||||
fps = int(self.get_parameter('fps').value)
|
||||
pipe = rs.pipeline()
|
||||
profile = None
|
||||
last_err = None
|
||||
for ww, hh, ff in [
|
||||
(w, h, fps), (w, h, 15), (w, h, 10),
|
||||
(640, 480, 30), (640, 480, 15),
|
||||
]:
|
||||
try:
|
||||
cfg = rs.config()
|
||||
if self._serial:
|
||||
cfg.enable_device(self._serial)
|
||||
cfg.enable_stream(rs.stream.color, ww, hh, rs.format.bgr8, ff)
|
||||
profile = pipe.start(cfg)
|
||||
self._fps = ff
|
||||
self.get_logger().info(f'相机 {ww}x{hh}@{ff}Hz')
|
||||
break
|
||||
except Exception as exc: # noqa: BLE001
|
||||
last_err = exc
|
||||
try:
|
||||
pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
pipe = rs.pipeline()
|
||||
if profile is None:
|
||||
raise RuntimeError(
|
||||
f'无法打开 D405 —— 请先停掉 realsense_color_pub。（{last_err}）'
|
||||
)
|
||||
self._pipe = pipe
|
||||
try:
|
||||
from arm_application.handeye_realsense import apply_realsense_exposure
|
||||
|
||||
apply_realsense_exposure(
|
||||
profile.get_device(),
|
||||
auto_exposure=bool(self.get_parameter('auto_exposure').value),
|
||||
ae_limit_us=float(self.get_parameter('ae_limit_us').value),
|
||||
emitter_enabled=bool(self.get_parameter('emitter_enabled').value),
|
||||
exposure_us=(
|
||||
float(self.get_parameter('exposure_us').value) or None
|
||||
),
|
||||
gain=(float(self.get_parameter('gain').value) or None),
|
||||
log=self.get_logger().info,
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f'曝光设置应用失败：{exc}')
|
||||
self._cam_thread = threading.Thread(target=self._cam_loop, name='d405', daemon=True)
|
||||
self._cam_thread.start()
|
||||
|
||||
def _cam_loop(self) -> None:
|
||||
while not self._quit:
|
||||
try:
|
||||
frames = self._pipe.wait_for_frames(timeout_ms=400)
|
||||
color = frames.get_color_frame()
|
||||
except Exception: # noqa: BLE001
|
||||
time.sleep(0.02)
|
||||
continue
|
||||
if not color:
|
||||
continue
|
||||
bgr = np.asanyarray(color.get_data()).copy()
|
||||
with self._frame_lock:
|
||||
self._latest_bgr = bgr
|
||||
|
||||
def _start_gui(self) -> None:
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
self._plt = plt
|
||||
self._fig, self._ax = plt.subplots(1, 1, figsize=(11, 7))
|
||||
self._fig.canvas.manager.set_window_title(
|
||||
'handeye_teach | Enter=拍照 d=拖拽 s=停止 q=退出'
|
||||
)
|
||||
self._im = None
|
||||
self._status = self._ax.text(
|
||||
0.02,
|
||||
0.98,
|
||||
self._status_text(),
|
||||
transform=self._ax.transAxes,
|
||||
color='lime',
|
||||
fontsize=13,
|
||||
va='top',
|
||||
fontweight='bold',
|
||||
bbox=dict(boxstyle='round', facecolor='black', alpha=0.6),
|
||||
)
|
||||
self._ax.axis('off')
|
||||
self._fig.canvas.mpl_connect('key_press_event', self._on_key)
|
||||
plt.ion()
|
||||
plt.show(block=False)
|
||||
|
||||
def _status_text(self) -> str:
|
||||
drag = '拖拽开启' if self._drag_on else '拖拽关闭'
|
||||
if self.backend == 'rdk':
|
||||
tip = 'd=软拖拽 s=保持'
|
||||
else:
|
||||
tip = '示教器 FreeDrive+使能'
|
||||
return (
|
||||
f'已采集:{len(self._captures)} [{drag}] '
|
||||
f'{tip} | Enter=拍照 q=退出'
|
||||
)
|
||||
|
||||
def _on_key(self, event) -> None:
|
||||
key = (event.key or '').lower()
|
||||
if key in ('q', 'escape'):
|
||||
self._quit = True
|
||||
self.get_logger().info('收到退出请求')
|
||||
return
|
||||
if self._cmd_pending:
|
||||
return
|
||||
if key in ('enter', ' ', 'space', 'd', 's'):
|
||||
self._cmd_pending = True
|
||||
threading.Thread(target=self._run_key, args=(key,), daemon=True).start()
|
||||
|
||||
def _run_key(self, key: str) -> None:
|
||||
try:
|
||||
if key in ('enter', ' ', 'space'):
|
||||
self._capture_once()
|
||||
elif key == 'd':
|
||||
self._enable_drag()
|
||||
elif key == 's':
|
||||
self._disable_drag()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f'按键 {key}：{exc}')
|
||||
finally:
|
||||
self._cmd_pending = False
|
||||
|
||||
def _enable_drag(self) -> None:
|
||||
if self.backend != 'rdk':
|
||||
self._drag_on = True
|
||||
self.get_logger().warn(
|
||||
'ROS 后端：请用示教器的 Free Drive + 按住使能键。'
|
||||
'想用软件拖拽：先停掉 run_real.sh，然后执行 '
|
||||
'ros2 run arm_application handeye_teach --ros-args -p backend:=rdk'
|
||||
)
|
||||
self._status.set_text(self._status_text())
|
||||
return
|
||||
self.get_logger().info('正在启动软拖拽（预览保持实时刷新）...')
|
||||
# soft = AUTO Remote 模式下的关节阻抗拖拽（不需要 Floating 许可）
|
||||
res = self._rdk.drag_on(self.drag_primitive, mode='soft')
|
||||
if res.get('ok'):
|
||||
self._drag_on = True
|
||||
self.get_logger().info(
|
||||
f'拖拽已开启 mode={res.get("mode")} —— 推着机械臂走，Enter=拍照（s=保持，可选）'
|
||||
)
|
||||
if res.get('hint'):
|
||||
self.get_logger().info(str(res.get('hint')))
|
||||
else:
|
||||
self._drag_on = False
|
||||
self.get_logger().error(f'拖拽失败：{res.get("error")}')
|
||||
# 状态文字由 GUI 线程里的 _tick 刷新
|
||||
|
||||
def _disable_drag(self) -> None:
|
||||
if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
|
||||
res = self._rdk.drag_off()
|
||||
if not res.get('ok'):
|
||||
self.get_logger().warn(f'停止拖拽：{res.get("error")}')
|
||||
self._drag_on = False
|
||||
self.get_logger().info('Free Drive 已关闭 / 保持')
|
||||
|
||||
def _tick(self) -> None:
|
||||
if self._quit:
|
||||
return
|
||||
with self._frame_lock:
|
||||
bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
|
||||
if bgr is None:
|
||||
try:
|
||||
self._fig.canvas.flush_events()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
return
|
||||
rgb = bgr[:, :, ::-1]
|
||||
if self._im is None:
|
||||
self._im = self._ax.imshow(rgb)
|
||||
else:
|
||||
self._im.set_data(rgb)
|
||||
try:
|
||||
self._status.set_text(self._status_text())
|
||||
self._fig.canvas.draw_idle()
|
||||
self._fig.canvas.flush_events()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def _read_joints(self) -> Optional[List[float]]:
|
||||
if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
|
||||
q = self._rdk.joints()
|
||||
if q is None:
|
||||
self.get_logger().error('读取 RDK 关节失败')
|
||||
return q
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
end = time.monotonic() + 2.0
|
||||
while rclpy.ok() and not fut.done() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.02)
|
||||
res = fut.result() if fut.done() else None
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
q = list(res.joint_positions)
|
||||
return [float(v) for v in q[:7]] if len(q) >= 7 else None
|
||||
|
||||
def _capture_once(self) -> None:
|
||||
if self._busy:
|
||||
return
|
||||
self._busy = True
|
||||
try:
|
||||
with self._frame_lock:
|
||||
bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
|
||||
if bgr is None:
|
||||
self.get_logger().warn('还没有相机帧')
|
||||
return
|
||||
q = self._read_joints()
|
||||
if q is None:
|
||||
self.get_logger().error('读取关节失败')
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - self._last_capture_t < 0.8:
|
||||
self.get_logger().warn('按太快，忽略（防 Enter 连发）')
|
||||
return
|
||||
if self._captures:
|
||||
last = self._captures[-1]['joints']
|
||||
delta = max(abs(a - b) for a, b in zip(q, last))
|
||||
if delta < 0.015:
|
||||
self.get_logger().warn(
|
||||
f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
|
||||
'先拖到另一个姿态再按 Enter。'
|
||||
)
|
||||
return
|
||||
self._last_capture_t = now
|
||||
|
||||
idx = len(self._captures)
|
||||
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
|
||||
img_path = self.images_dir / f'teach_{idx:02d}_{stamp}.png'
|
||||
try:
|
||||
import cv2
|
||||
|
||||
cv2.imwrite(str(img_path), bgr)
|
||||
except Exception: # noqa: BLE001
|
||||
from PIL import Image as PILImage
|
||||
|
||||
PILImage.fromarray(bgr[:, :, ::-1]).save(str(img_path))
|
||||
|
||||
line = ', '.join(f'{v:.6f}' for v in q) + ','
|
||||
self._captures.append(
|
||||
{'index': idx, 'joints': q, 'image': str(img_path), 'line': line}
|
||||
)
|
||||
with self.poses_file.open('a', encoding='utf-8') as f:
|
||||
f.write(f'# pose {idx} {stamp}\n')
|
||||
f.write(line + '\n')
|
||||
f.write(f'# image {img_path}\n')
|
||||
print(f'\n# pose {idx} {img_path.name}')
|
||||
print(line)
|
||||
n = idx + 1
|
||||
try:
|
||||
target = int(self.get_parameter('target_points').value)
|
||||
except Exception: # noqa: BLE001
|
||||
target = 0
|
||||
if target > 0:
|
||||
if n < target:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已保存照片+关节 —— 还差 {target - n} 个点'
|
||||
)
|
||||
elif n == target:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已采够 {target} 个点。'
|
||||
' 建议按 q 结束，然后跑 merge + calibrate；'
|
||||
' 想多采几个点也可以继续按 Enter。'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f'[{n}/{target}] 已保存照片+关节 —— 已超出目标 '
|
||||
f'{n - target} 个点（会用全部点求解）'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(f'[{n}] 已保存照片+关节')
|
||||
self._write_yaml_snippet()
|
||||
finally:
|
||||
self._busy = False
|
||||
|
||||
def _write_yaml_snippet(self) -> None:
|
||||
lines = [
|
||||
'# 粘贴到 handeye_calibrate.yaml -> waypoint_positions',
|
||||
f'# num_poses: {len(self._captures)}',
|
||||
'waypoint_positions: [',
|
||||
]
|
||||
for c in self._captures:
|
||||
lines.append(f' # pose {c["index"]}')
|
||||
lines.append(f' {c["line"]}')
|
||||
lines.append(']')
|
||||
self.yaml_file.write_text('\n'.join(lines) + '\n', encoding='utf-8')
|
||||
|
||||
def finish(self) -> None:
|
||||
self._quit = True
|
||||
if self._cam_thread is not None:
|
||||
self._cam_thread.join(timeout=1.5)
|
||||
if self._drag_on:
|
||||
self._disable_drag()
|
||||
if getattr(self, '_rdk', None) is not None:
|
||||
try:
|
||||
self._rdk.close()
|
||||
except Exception:
|
||||
pass
|
||||
n = len(self._captures)
|
||||
print('\n========== 示教完成 ==========')
|
||||
print(f'位姿: {n}')
|
||||
print(f'关节: {self.poses_file}')
|
||||
print(f'yaml: {self.yaml_file}')
|
||||
print(f'图像: {self.images_dir}')
|
||||
if n >= 1:
|
||||
print('\n--- 粘贴到 handeye_calibrate.yaml ---\n')
|
||||
print(self.yaml_file.read_text(encoding='utf-8'))
|
||||
print('同时设置 num_poses:', n)
|
||||
if n < 5:
|
||||
self.get_logger().warn(f'只有 {n} 个位姿；至少需要 5 个（建议 8-12 个）。')
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = HandeyeTeach()
|
||||
while rclpy.ok() and not node._quit:
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
except SystemExit:
|
||||
pass
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.finish()
|
||||
try:
|
||||
node._pipe.stop()
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,525 @@
|
||||
#!/usr/bin/env python3
|
||||
"""手眼标定路点执行器。
|
||||
|
||||
逐个移动到每个关节路点，等到位后停留一段时间，
|
||||
让相机（或操作者）采集一张标定图像。
|
||||
|
||||
capture_mode:
|
||||
auto — 停留 dwell_sec 后进入下一个位姿
|
||||
keypress — 在终端按 Enter 继续
|
||||
preview — OpenCV 实时相机 + Enter/Space 确认（示教时推荐）
|
||||
|
||||
处于 ``preview`` 模式时，可在预览窗口打开期间用示教器的 **Free Drive**
|
||||
拖拽微调（ros2_control 保持 AUTO Remote；拖拽经由示教器，而非 RDK）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import array
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import String
|
||||
|
||||
from arm_application.handeye_realsense import (
|
||||
is_realsense_source,
|
||||
resolve_realsense_topics,
|
||||
)
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
from arm_msgs.srv import GetArmState
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
try:
|
||||
import cv2
|
||||
except Exception: # noqa: BLE001
|
||||
cv2 = None # type: ignore
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
|
||||
DEFAULT_TIMES = [4.0] * 12
|
||||
DEFAULT_POSITIONS = [
|
||||
0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
-0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
0.00, -0.90, 0.00, 1.70, 0.00, 0.85, 0.00,
|
||||
0.00, -0.50, 0.00, 1.40, 0.00, 0.55, 0.00,
|
||||
0.20, -0.70, 0.15, 1.57, 0.40, 0.70, 0.30,
|
||||
-0.20, -0.70, -0.15, 1.57, -0.40, 0.70, -0.30,
|
||||
0.25, -0.80, 0.10, 1.65, 0.25, 0.90, -0.40,
|
||||
-0.25, -0.80, -0.10, 1.65, -0.25, 0.90, 0.40,
|
||||
0.15, -0.60, 0.20, 1.50, -0.35, 0.60, 0.50,
|
||||
-0.15, -0.60, -0.20, 1.50, 0.35, 0.60, -0.50,
|
||||
0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
|
||||
]
|
||||
|
||||
PREVIEW_HELP_GUI = """
|
||||
=== 位姿预览 / 拖拽调整（OpenCV 窗口）===
|
||||
1. 示教器：Free Drive + 按住使能键拖拽微调
|
||||
2. 预览窗口：确认棋盘在画面内且清晰
|
||||
3. 键盘：Enter / Space = 下一个位姿；q / Esc = 退出
|
||||
"""
|
||||
|
||||
PREVIEW_HELP_HEADLESS = """
|
||||
=== 位姿预览 / 拖拽调整（无 OpenCV 窗口）===
|
||||
1. 另开终端查看画面：
|
||||
ros2 run rqt_image_view rqt_image_view /handeye/preview_image
|
||||
2. 示教器：Free Drive + 按住使能键拖拽微调
|
||||
3. 本终端：Enter = 确认下一个位姿；输入 q 回车 = 退出
|
||||
"""
|
||||
|
||||
|
||||
def _cv_gui_available() -> bool:
|
||||
if cv2 is None:
|
||||
return False
|
||||
try:
|
||||
cv2.namedWindow('_handeye_gui_probe', cv2.WINDOW_NORMAL)
|
||||
cv2.destroyWindow('_handeye_gui_probe')
|
||||
return True
|
||||
except Exception: # noqa: BLE001
|
||||
return False
|
||||
|
||||
|
||||
def _image_to_bgr(msg: Image) -> np.ndarray:
|
||||
h, w = int(msg.height), int(msg.width)
|
||||
enc = (msg.encoding or '').lower()
|
||||
data = np.frombuffer(msg.data, dtype=np.uint8)
|
||||
if enc in ('bgr8', 'rgb8'):
|
||||
img = data.reshape(h, w, 3)
|
||||
return img[:, :, ::-1].copy() if enc == 'rgb8' else img
|
||||
if enc in ('mono8', '8uc1'):
|
||||
gray = data.reshape(h, w)
|
||||
return np.stack([gray, gray, gray], axis=-1)
|
||||
if enc in ('bgra8', 'rgba8'):
|
||||
img = data.reshape(h, w, 4)
|
||||
bgr = img[:, :, :3]
|
||||
return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
|
||||
raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
|
||||
|
||||
|
||||
class HandeyeWaypoints(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('handeye_waypoints')
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||
self.declare_parameter('velocity_scale', 0.12)
|
||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||
self.declare_parameter('waypoint_times', DEFAULT_TIMES)
|
||||
self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
|
||||
self.declare_parameter('dwell_sec', 2.5)
|
||||
# auto | keypress | preview
|
||||
self.declare_parameter('capture_mode', 'auto')
|
||||
self.declare_parameter('start_index', 0)
|
||||
self.declare_parameter('log_csv', '')
|
||||
self.declare_parameter('capture_topic', 'handeye/capture')
|
||||
self.declare_parameter('camera_source', 'd405')
|
||||
self.declare_parameter('camera_namespace', '')
|
||||
self.declare_parameter('image_topic', '')
|
||||
self.declare_parameter('preview_window', True)
|
||||
self.declare_parameter('append_poses_file', '/tmp/handeye_taught_poses.txt')
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
self.get_parameter('joint_names').get_parameter_value().string_array_value
|
||||
or DEFAULT_JOINTS
|
||||
)
|
||||
self.joint_names = [f'{prefix}{n}' for n in base]
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
self.action_name = str(self.get_parameter('action_name').value)
|
||||
self.dwell_sec = float(self.get_parameter('dwell_sec').value)
|
||||
self.capture_mode = str(self.get_parameter('capture_mode').value).strip().lower()
|
||||
self.start_index = max(0, int(self.get_parameter('start_index').value))
|
||||
self.log_csv = str(self.get_parameter('log_csv').value or '').strip()
|
||||
topic = str(self.get_parameter('capture_topic').value)
|
||||
self.preview_window = bool(self.get_parameter('preview_window').value)
|
||||
self.append_poses_file = str(
|
||||
self.get_parameter('append_poses_file').value or ''
|
||||
).strip()
|
||||
|
||||
cam_src = str(self.get_parameter('camera_source').value).strip().lower()
|
||||
img_topic = str(self.get_parameter('image_topic').value or '').strip()
|
||||
ns = str(self.get_parameter('camera_namespace').value or '').strip()
|
||||
if is_realsense_source(cam_src) and not img_topic:
|
||||
img_topic, _ = resolve_realsense_topics(
|
||||
cam_src, camera_namespace=ns, image_topic='', camera_info_topic=''
|
||||
)
|
||||
self.image_topic = img_topic
|
||||
|
||||
self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
|
||||
self._get_state = self.create_client(GetArmState, 'get_arm_state')
|
||||
self._capture_pub = self.create_publisher(String, topic, 10)
|
||||
self._preview_pub = self.create_publisher(Image, 'handeye/preview_image', 10)
|
||||
self._latest_bgr: Optional[np.ndarray] = None
|
||||
self._cv_gui = _cv_gui_available() and self.preview_window
|
||||
if self.capture_mode == 'preview' and not self._cv_gui:
|
||||
self.get_logger().warn(
|
||||
'OpenCV 没有 GUI（无头环境）。请在 /handeye/preview_image 上预览，'
|
||||
'在本终端按 Enter 确认。'
|
||||
)
|
||||
if self.image_topic and self.capture_mode == 'preview':
|
||||
self.create_subscription(
|
||||
Image, self.image_topic, self._on_image, qos_profile_sensor_data
|
||||
)
|
||||
self.get_logger().info(f'预览图像：{self.image_topic}')
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
try:
|
||||
self._latest_bgr = _image_to_bgr(msg)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def run(self) -> int:
|
||||
if self.capture_mode == 'preview':
|
||||
if cv2 is None:
|
||||
self.get_logger().error('preview 模式需要 OpenCV（cv2）')
|
||||
return 1
|
||||
if not self.image_topic:
|
||||
self.get_logger().error('preview 模式需要 image_topic 或 camera_source')
|
||||
return 1
|
||||
|
||||
times = list(self.get_parameter('waypoint_times').value)
|
||||
positions = list(self.get_parameter('waypoint_positions').value)
|
||||
n_j = len(self.joint_names)
|
||||
n_pts = len(times)
|
||||
if n_pts < 1:
|
||||
self.get_logger().error('waypoint_times 为空')
|
||||
return 1
|
||||
if len(positions) != n_pts * n_j:
|
||||
self.get_logger().error(
|
||||
f'waypoint_positions 长度 {len(positions)} != {n_pts}*{n_j}'
|
||||
)
|
||||
return 1
|
||||
if self.start_index >= n_pts:
|
||||
self.get_logger().error(
|
||||
f'start_index {self.start_index} >= 路点总数 {n_pts}'
|
||||
)
|
||||
return 1
|
||||
|
||||
if not self._client.wait_for_server(timeout_sec=15.0):
|
||||
self.get_logger().error(
|
||||
f'Action [{self.action_name}] 不可用。'
|
||||
'请先启动整套栈：bash scripts/run_real.sh'
|
||||
)
|
||||
return 2
|
||||
|
||||
csv_file = None
|
||||
csv_writer: Optional[csv.writer] = None
|
||||
if self.log_csv:
|
||||
path = Path(self.log_csv).expanduser()
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
csv_file = path.open('w', newline='', encoding='utf-8')
|
||||
csv_writer = csv.writer(csv_file)
|
||||
csv_writer.writerow(
|
||||
['index', 'utc_iso', 'move_duration_s']
|
||||
+ [f'target_{n}' for n in self.joint_names]
|
||||
+ [f'measured_{n}' for n in self.joint_names]
|
||||
)
|
||||
self.get_logger().info(f'正在记录采集数据 → {path}')
|
||||
|
||||
if self.capture_mode == 'preview':
|
||||
print(PREVIEW_HELP_GUI if self._cv_gui else PREVIEW_HELP_HEADLESS)
|
||||
|
||||
self.get_logger().info(
|
||||
f'手眼标定：{n_pts} 个位姿，start={self.start_index}，'
|
||||
f'v_scale={self.velocity_scale}，dwell={self.dwell_sec}s，'
|
||||
f'mode={self.capture_mode}'
|
||||
)
|
||||
self.get_logger().warn(
|
||||
'清理工作空间；让棋盘保持在视野内。Ctrl+C 可中止。'
|
||||
)
|
||||
|
||||
win = 'handeye_preview'
|
||||
try:
|
||||
for i in range(self.start_index, n_pts):
|
||||
q = positions[i * n_j : (i + 1) * n_j]
|
||||
move_t = max(1.0, float(times[i]))
|
||||
self.get_logger().info(
|
||||
f'[{i + 1}/{n_pts}] 移动中（约 {move_t:.1f}s）...'
|
||||
)
|
||||
code = self._move_one(q, move_t)
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
measured = self._read_joints()
|
||||
self._announce_capture(i, n_pts, q, measured)
|
||||
|
||||
if csv_writer is not None and csv_file is not None:
|
||||
csv_writer.writerow(
|
||||
[i, datetime.now(timezone.utc).isoformat(), move_t]
|
||||
+ list(q)
|
||||
+ list(measured if measured else [float('nan')] * n_j)
|
||||
)
|
||||
csv_file.flush()
|
||||
|
||||
if not self._wait_capture(i, n_pts):
|
||||
self.get_logger().info('用户已中止')
|
||||
return 0
|
||||
|
||||
# 在 preview 模式下确认后，保存示教关节
|
||||
if self.capture_mode == 'preview':
|
||||
taught = self._read_joints()
|
||||
if taught:
|
||||
self._save_taught_pose(i, taught)
|
||||
finally:
|
||||
if csv_file is not None:
|
||||
csv_file.close()
|
||||
if cv2 is not None:
|
||||
try:
|
||||
cv2.destroyWindow(win)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
self.get_logger().info('手眼标定路点序列执行完毕')
|
||||
if self.append_poses_file and self.capture_mode == 'preview':
|
||||
self.get_logger().info(
|
||||
f'示教位姿已保存 → {self.append_poses_file}'
|
||||
)
|
||||
return 0
|
||||
|
||||
def _save_taught_pose(self, index: int, joints: List[float]) -> None:
|
||||
line = ', '.join(f'{v:.6f}' for v in joints[:7]) + ','
|
||||
print(f'# pose {index}')
|
||||
print(line)
|
||||
if not self.append_poses_file:
|
||||
return
|
||||
path = Path(self.append_poses_file).expanduser()
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open('a', encoding='utf-8') as f:
|
||||
f.write(f'# pose {index}\n')
|
||||
f.write(line + '\n')
|
||||
self.get_logger().info(f'已保存位姿 {index} → {path}')
|
||||
|
||||
def _move_one(self, positions: List[float], duration: float) -> int:
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.joint_names = self.joint_names
|
||||
goal.num_points = 1
|
||||
goal.velocity_scale = self.velocity_scale
|
||||
goal.positions = list(positions)
|
||||
goal.time_from_start = [float(duration)]
|
||||
|
||||
send = self._client.send_goal_async(goal)
|
||||
if not self._wait_future(send, 10.0, '发送目标'):
|
||||
return 3
|
||||
handle = send.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return 4
|
||||
|
||||
res_fut = handle.get_result_async()
|
||||
if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
|
||||
return 5
|
||||
result = res_fut.result().result
|
||||
if not result.success:
|
||||
self.get_logger().error(f'移动失败：{result.message}')
|
||||
return 6
|
||||
return 0
|
||||
|
||||
def _wait_future(self, fut, timeout: float, label: str) -> bool:
|
||||
deadline = time.monotonic() + timeout
|
||||
while rclpy.ok() and not fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error(f'等待 {label} 超时')
|
||||
return False
|
||||
return fut.done()
|
||||
|
||||
def _read_joints(self) -> Optional[List[float]]:
|
||||
if not self._get_state.wait_for_service(timeout_sec=0.5):
|
||||
return None
|
||||
fut = self._get_state.call_async(GetArmState.Request())
|
||||
if not self._wait_future(fut, 2.0, 'get_arm_state'):
|
||||
return None
|
||||
res = fut.result()
|
||||
if res is None or not res.success:
|
||||
return None
|
||||
return list(res.joint_positions)
|
||||
|
||||
def _announce_capture(
|
||||
self,
|
||||
index: int,
|
||||
n_pts: int,
|
||||
target: List[float],
|
||||
measured: Optional[List[float]],
|
||||
) -> None:
|
||||
msg = String()
|
||||
msg.data = f'capture:{index}:{n_pts}'
|
||||
self._capture_pub.publish(msg)
|
||||
q_str = ', '.join(f'{v:.4f}' for v in (measured or target))
|
||||
if self.capture_mode == 'preview':
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 已到位 —— 用 Free Drive 调整，然后按 Enter/Space'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 已到位 —— 现在采集 q=[{q_str}]'
|
||||
)
|
||||
|
||||
def _wait_capture(self, index: int, n_pts: int) -> bool:
|
||||
if self.capture_mode == 'preview':
|
||||
return self._wait_preview(index, n_pts)
|
||||
|
||||
if self.capture_mode in ('key', 'keypress', 'manual'):
|
||||
self.get_logger().info(
|
||||
'采集完成后按 Enter（q+Enter 退出）...'
|
||||
)
|
||||
for _ in range(5):
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
line = sys.stdin.readline()
|
||||
if not line or line.strip().lower() in ('q', 'quit', 'exit'):
|
||||
return False
|
||||
return True
|
||||
|
||||
end = time.monotonic() + max(0.0, self.dwell_sec)
|
||||
while rclpy.ok() and time.monotonic() < end:
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
return rclpy.ok()
|
||||
|
||||
def _annotate_preview(
|
||||
self, bgr: np.ndarray, index: int, n_pts: int
|
||||
) -> np.ndarray:
|
||||
vis = bgr.copy()
|
||||
if cv2 is None:
|
||||
return vis
|
||||
cv2.putText(
|
||||
vis,
|
||||
f'位姿 {index + 1}/{n_pts}',
|
||||
(12, 28),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.7,
|
||||
(0, 255, 0),
|
||||
2,
|
||||
)
|
||||
cv2.putText(
|
||||
vis,
|
||||
'Free Drive 拖拽 | Enter=下一个 q=退出',
|
||||
(12, 56),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.6,
|
||||
(0, 255, 255),
|
||||
2,
|
||||
)
|
||||
return vis
|
||||
|
||||
def _publish_preview(self, bgr: np.ndarray) -> None:
|
||||
msg = Image()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = 'camera'
|
||||
msg.height, msg.width = bgr.shape[0], bgr.shape[1]
|
||||
msg.encoding = 'bgr8'
|
||||
msg.is_bigendian = 0
|
||||
msg.step = msg.width * 3
|
||||
msg.data = array.array('B', bgr.tobytes()) # 快速路径；用 bytes 会多花约 40 ms
|
||||
self._preview_pub.publish(msg)
|
||||
|
||||
def _wait_preview(self, index: int, n_pts: int) -> bool:
|
||||
if self._cv_gui:
|
||||
return self._wait_preview_opencv(index, n_pts)
|
||||
return self._wait_preview_terminal(index, n_pts)
|
||||
|
||||
def _wait_preview_terminal(self, index: int, n_pts: int) -> bool:
|
||||
confirmed = threading.Event()
|
||||
quit_req = threading.Event()
|
||||
|
||||
def _stdin_reader() -> None:
|
||||
try:
|
||||
line = sys.stdin.readline()
|
||||
except Exception: # noqa: BLE001
|
||||
quit_req.set()
|
||||
return
|
||||
if not line or line.strip().lower() in ('q', 'quit', 'exit'):
|
||||
quit_req.set()
|
||||
else:
|
||||
confirmed.set()
|
||||
|
||||
self.get_logger().info(
|
||||
f'[{index + 1}/{n_pts}] 用 Free Drive 调整，然后按 Enter '
|
||||
'（q+Enter 退出）'
|
||||
)
|
||||
reader = threading.Thread(target=_stdin_reader, daemon=True)
|
||||
reader.start()
|
||||
|
||||
no_frame_warn = False
|
||||
while rclpy.ok():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if confirmed.is_set():
|
||||
return True
|
||||
if quit_req.is_set():
|
||||
return False
|
||||
if not reader.is_alive() and not confirmed.is_set() and not quit_req.is_set():
|
||||
return False
|
||||
|
||||
bgr = self._latest_bgr
|
||||
if bgr is None:
|
||||
if not no_frame_warn:
|
||||
self.get_logger().warn(
|
||||
f'正在等待 {self.image_topic} ...',
|
||||
throttle_duration_sec=3.0,
|
||||
)
|
||||
no_frame_warn = True
|
||||
continue
|
||||
vis = self._annotate_preview(bgr, index, n_pts)
|
||||
self._publish_preview(vis)
|
||||
return False
|
||||
|
||||
def _wait_preview_opencv(self, index: int, n_pts: int) -> bool:
|
||||
assert cv2 is not None
|
||||
win = 'handeye_preview'
|
||||
cv2.namedWindow(win, cv2.WINDOW_NORMAL)
|
||||
|
||||
no_frame_warn = False
|
||||
while rclpy.ok():
|
||||
rclpy.spin_once(self, timeout_sec=0.02)
|
||||
bgr = self._latest_bgr
|
||||
if bgr is None:
|
||||
if not no_frame_warn:
|
||||
self.get_logger().warn(
|
||||
f'正在等待 {self.image_topic} ...',
|
||||
throttle_duration_sec=3.0,
|
||||
)
|
||||
no_frame_warn = True
|
||||
key = cv2.waitKey(30) & 0xFF
|
||||
else:
|
||||
vis = self._annotate_preview(bgr, index, n_pts)
|
||||
self._publish_preview(vis)
|
||||
cv2.imshow(win, vis)
|
||||
key = cv2.waitKey(30) & 0xFF
|
||||
|
||||
if key in (13, 10, 32):
|
||||
return True
|
||||
if key in (27, ord('q'), ord('Q')):
|
||||
return False
|
||||
return False
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = HandeyeWaypoints()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,74 @@
|
||||
#!/usr/bin/env python3
|
||||
"""``arm_msgs/action/MoveToPose``（笛卡尔目标）的示例客户端。
|
||||
|
||||
等你有了 MoveIt 或自己的 IK 求解 action server 后，把它接上去即可。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
|
||||
from arm_msgs.action import MoveToPose
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
|
||||
class MoveToPoseClient(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('move_to_pose_client')
|
||||
self._client = ActionClient(self, MoveToPose, 'move_to_pose')
|
||||
|
||||
def send(self, x: float, y: float, z: float) -> None:
|
||||
if not self._client.wait_for_server(timeout_sec=5.0):
|
||||
self.get_logger().error(
|
||||
'move_to_pose action 不可用。'
|
||||
'请启动一个实现了它的笛卡尔规划器（例如 MoveIt）。'
|
||||
)
|
||||
return
|
||||
|
||||
goal = MoveToPose.Goal()
|
||||
goal.target_pose = PoseStamped()
|
||||
goal.target_pose.header.frame_id = 'base_link'
|
||||
goal.target_pose.header.stamp = self.get_clock().now().to_msg()
|
||||
goal.target_pose.pose.position.x = x
|
||||
goal.target_pose.pose.position.y = y
|
||||
goal.target_pose.pose.position.z = z
|
||||
# 单位姿态（不旋转）
|
||||
goal.target_pose.pose.orientation.w = 1.0
|
||||
goal.velocity_scale = 0.3
|
||||
goal.acceleration_scale = 0.3
|
||||
goal.avoid_collisions = True
|
||||
|
||||
self.get_logger().info(f'发送笛卡尔目标：({x:.3f}, {y:.3f}, {z:.3f})')
|
||||
future = self._client.send_goal_async(goal)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
handle = future.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return
|
||||
|
||||
result_future = handle.get_result_async()
|
||||
rclpy.spin_until_future_complete(self, result_future)
|
||||
result = result_future.result().result
|
||||
self.get_logger().info(f'success={result.success} msg={result.message}')
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = MoveToPoseClient()
|
||||
# 默认演示位姿；可通过命令行覆盖：x y z
|
||||
vals = [0.35, 0.0, 0.35]
|
||||
if len(sys.argv) >= 4:
|
||||
vals = [float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3])]
|
||||
node.send(*vals)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -45,6 +45,11 @@ from arm_application.path_profile import ( # 伺服和测试从本模块再导
|
||||
|
||||
# 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
|
||||
D435_MIN_PROGRESS_FRAC = 0.40
|
||||
# SwitchMode 返回不等于已经进 primitive。连试仍失败必须让一阶段立刻中止，
|
||||
# 不许空等 150s 装成"还在粗定位"（2026-10-16 17:27）。
|
||||
D435_MOVEL_RETRY = 3
|
||||
D435_MOVEL_RETRY_S = 0.3
|
||||
D435_MOVEL_GAVE_UP = "厂家 MoveL 连试 3 次没走成"
|
||||
|
||||
|
||||
def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
|
||||
@@ -154,16 +159,27 @@ def d405_commands_arm(
|
||||
) -> bool:
|
||||
"""D405 这一帧的点能不能下发给左臂。
|
||||
|
||||
推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
|
||||
停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
|
||||
也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
|
||||
``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
|
||||
推理从启动就开着。没有高置信度目标、或 D435 厂家 MoveL 还没结束、
|
||||
法兰还没连续停稳时，这帧**不下发**（输出截断，不许拿去控臂）。
|
||||
厂家移动结束并且法兰停稳（``settled``，时长是 ``d435_handback_sec``）
|
||||
之后才放行，伺服用它追工件框的矩形中心。框心对准停稳后才进精拍下臂。
|
||||
``move_only``：只测 D435 位移，D405 点一律不下发。
|
||||
"""
|
||||
if move_only:
|
||||
return False
|
||||
if not using_d435:
|
||||
return True
|
||||
if using_d435:
|
||||
return bool(arrived) and bool(settled)
|
||||
return True
|
||||
|
||||
|
||||
def retract_goal_z(z_now: float, z_target: float) -> float:
|
||||
"""收臂目标 Z：已经高于目标就钉在当前高度，不许为了追 5 mm 往下补。
|
||||
|
||||
2026-10-16：贴面 MoveL 之后 JTC 还握着悬停关节，收臂 hop1 计划 +5 mm
|
||||
实际抬了 18–25 mm；hop2 把「抬过头」当成误差 ``Δz=−13 mm`` 往回压，
|
||||
看起来就是进孔再弹出。抬过了 = 成功。
|
||||
"""
|
||||
return max(float(z_now), float(z_target))
|
||||
|
||||
|
||||
def oneshot_needs_verticalize(
|
||||
@@ -625,6 +641,24 @@ def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy):
|
||||
return out
|
||||
|
||||
|
||||
def movel_tcp_z_only(tcp_xyz, delta_z_m: float):
|
||||
"""厂家 MoveL 目标：当前 TCP 的 XY 原样，只加 ``delta_z_m``。
|
||||
|
||||
一段直线。WORLD 绝对坐标不能另算，只能在厂家 TCP 上加这一段。
|
||||
``delta_z_m`` 为正是上升，为负是下降。接近 0 或非有限数返回 None。
|
||||
"""
|
||||
try:
|
||||
t = np.asarray(tcp_xyz, dtype=float).reshape(3)
|
||||
dz = float(delta_z_m)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if t.size != 3 or not np.all(np.isfinite(t)) or not np.isfinite(dz):
|
||||
return None
|
||||
if abs(dz) < 1e-6:
|
||||
return None
|
||||
return np.array([float(t[0]), float(t[1]), float(t[2]) + dz], dtype=float)
|
||||
|
||||
|
||||
def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float):
|
||||
"""厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。
|
||||
|
||||
@@ -672,10 +706,60 @@ def hole_pair_span_ok(span_m: float) -> bool:
|
||||
|
||||
|
||||
def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> float:
|
||||
"""规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。"""
|
||||
"""规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。
|
||||
|
||||
``gap_m`` 为正是停在面上；为负是规尖进入孔口（上表面以下）。
|
||||
"""
|
||||
return float(tip_len_m) + float(surface_z_m) + float(gap_m)
|
||||
|
||||
|
||||
# 工件板厚 12mm。进孔超过这个值会朝下表面穿，直接拒绝，不改成更浅再走。
|
||||
HOLE_ENTRY_MAX_M = 0.008
|
||||
|
||||
|
||||
def insert_target_gap_m(final_gap_m: float, hole_entry_m: float) -> float:
|
||||
"""插孔停在哪 [m]。``hole_entry_m`` > 0 时规尖在工件上表面以下这么深；否则停在面上 ``final_gap_m``。"""
|
||||
entry = float(hole_entry_m)
|
||||
if entry > 1e-9:
|
||||
return -entry
|
||||
return float(final_gap_m)
|
||||
|
||||
|
||||
def hole_entry_refusal(
|
||||
entry_m: float,
|
||||
tool_len_m: float,
|
||||
surface_z_m: float,
|
||||
z_min_m: float,
|
||||
z_max_m: float,
|
||||
) -> str:
|
||||
"""进孔深度能不能用。空串表示可以（含 0 = 不进孔）。非空就是拒绝原因。"""
|
||||
entry = float(entry_m)
|
||||
if not np.isfinite(entry):
|
||||
return "进孔深度不是有限数，拒绝开跑"
|
||||
if entry < -1e-9:
|
||||
return f"进孔深度 {entry * 1000:.1f}mm < 0，拒绝开跑"
|
||||
if entry <= 1e-9:
|
||||
return ""
|
||||
if entry > HOLE_ENTRY_MAX_M + 1e-9:
|
||||
return (
|
||||
f"进孔深度 {entry * 1000:.1f}mm 超过 {HOLE_ENTRY_MAX_M * 1000:.0f}mm"
|
||||
"（板厚 12mm，不许穿出下表面），拒绝开跑"
|
||||
)
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
if tool <= 0.0 or surf <= 0.0:
|
||||
return "写了进孔深度，但工具长度或工件面未知，无法确认尖端进了多深，拒绝开跑"
|
||||
flange = flange_z_for_gap_m(tool, surf, -entry)
|
||||
z_lo = float(z_min_m)
|
||||
z_hi = float(z_max_m)
|
||||
if flange < z_lo - 1e-6 or flange > z_hi + 1e-6:
|
||||
return (
|
||||
f"进孔法兰 Z {flange:.3f}m（面上再进 {entry * 1000:.1f}mm）"
|
||||
f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑"
|
||||
)
|
||||
return ""
|
||||
|
||||
|
||||
def final_gap_refusal(
|
||||
gap_m: float,
|
||||
tool_len_m: float,
|
||||
|
||||
@@ -105,7 +105,7 @@ class PerceptionNode(TolerantNode):
|
||||
self.declare_parameter("image_topic", "")
|
||||
self.declare_parameter("camera_info_topic", "")
|
||||
self.declare_parameter("depth_topic", "/d405/aligned_depth_to_color/image_raw")
|
||||
self.declare_parameter("weights", "models/yolo/gongjian0929.onnx")
|
||||
self.declare_parameter("weights", "/home/cc/ros2/models/yolo/gongjian0929.onnx")
|
||||
self.declare_parameter("conf", 0.10)
|
||||
self.declare_parameter("iou", 0.45)
|
||||
self.declare_parameter("imgsz", 640)
|
||||
@@ -336,9 +336,7 @@ class PerceptionNode(TolerantNode):
|
||||
r_pos=float(self.get_parameter("kf_r_pos").value),
|
||||
)
|
||||
|
||||
from arm_application.site_io import resolve_repo_path
|
||||
|
||||
weights_raw = resolve_repo_path(str(self.get_parameter("weights").value))
|
||||
weights_raw = str(self.get_parameter("weights").value)
|
||||
weights = str(resolve_weights(weights_raw, self.backend_name))
|
||||
yolo_cfg = {
|
||||
"weights": weights,
|
||||
@@ -1141,8 +1139,8 @@ class PerceptionNode(TolerantNode):
|
||||
filt[2] = z_use
|
||||
self._last_filt = filt.copy()
|
||||
|
||||
if handoff:
|
||||
# 画面里没有 conf≥门槛的目标：绿框可以留，三维点不许发 —— 伺服会拿去追。
|
||||
if handoff or self._d435_commanding:
|
||||
# 没有高置信度，或 D435 厂家移动还没交接：推理留着，控制点不许发。
|
||||
return
|
||||
self._publish_point(filt, stamp)
|
||||
self.get_logger().info(
|
||||
@@ -1427,15 +1425,11 @@ class PerceptionNode(TolerantNode):
|
||||
self._z_locked = bool(msg.data)
|
||||
|
||||
def _on_d435_commanding(self, msg: Bool) -> None:
|
||||
"""伺服还在执行 D435 那一个点时，不再提交下一张。
|
||||
"""伺服还在执行 D435 那一个点时，不再提交下一张，D405 控制点也不发。
|
||||
|
||||
这一段结束（到位并停稳）后清掉「已拍过」，漏检重新计 0.5 s。
|
||||
这一轮已经拍过并走完。停稳后打开 D405 框心，不因交接瞬间漏检再 MoveL。
|
||||
"""
|
||||
commanding = bool(msg.data)
|
||||
if self._d435_commanding and not commanding:
|
||||
self._oneshot_shot = False
|
||||
self._low_since = None
|
||||
self._d435_commanding = commanding
|
||||
self._d435_commanding = bool(msg.data)
|
||||
|
||||
def _publish_overhead_k0(self, p_base: np.ndarray, is_k0: bool) -> None:
|
||||
pt = PointStamped()
|
||||
|
||||
@@ -1,27 +1,39 @@
|
||||
#!/usr/bin/env python3
|
||||
"""跟踪锁定 → 孔定位。搜孔轻压已取消，本节点不再下压搜孔。
|
||||
"""三阶段串联：视觉跟踪锁定 → 孔定位 → 搜孔并轻轻按压（默认不拧入）。
|
||||
|
||||
流畅性（相对 bash 杀进程再起节点）：
|
||||
- 相机 / 触觉 / 手眼 TF 全程只起一次
|
||||
- 一阶段锁定后 SIGTERM 跟踪节点，立刻 pin 当前关节，不回零、不抬臂
|
||||
- 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完就停，不再下压搜孔
|
||||
- 旋入不在这个节点里
|
||||
- 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完立刻下压搜孔
|
||||
- 找到孔位后力闭环轻压保持；拧入是后续阶段（--depth 才插入）
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import subprocess
|
||||
import time
|
||||
from typing import Optional
|
||||
from pathlib import Path
|
||||
from typing import Optional, Tuple
|
||||
|
||||
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 rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||
from trajectory_msgs.msg import JointTrajectory
|
||||
|
||||
from arm_application.gauge_gripper import states_frame_online
|
||||
from arm_application.gauge_kin import (
|
||||
a7_limit_target,
|
||||
match_limits,
|
||||
parse_joint_limits,
|
||||
)
|
||||
from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP, movel_tcp_z_only
|
||||
from arm_application.site_io import load_site_dict, ros2_root
|
||||
from arm_application.stage2_locate_node import _pt
|
||||
from arm_application.stage3_search_node import Stage3Search
|
||||
|
||||
|
||||
@@ -37,6 +49,99 @@ from arm_application.stage3_search_node import Stage3Search
|
||||
TRACK_CLEARANCE_M = 0.05
|
||||
#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
|
||||
TRACK_LOCK_GAP_M = 0.040
|
||||
#: ``fill_nrt_limits`` 的关节速度上限 [rad/s]。再快的指令会被钳住，时间到了关节还没到。
|
||||
NRT_VMAX_RAD_S = 0.90
|
||||
#: 两点、两端速度为 0 的三次样条，峰速是平均速度的 1.5 倍。
|
||||
CUBIC_PEAK_OVER_AVG = 1.5
|
||||
#: 空回按这个角速度请求。真正排程用 :func:`a7_move_duration_s`，峰速不超过上面的上限。
|
||||
A7_MAX_SPEED_DEG_S = math.degrees(NRT_VMAX_RAD_S)
|
||||
|
||||
|
||||
def a7_move_duration_s(delta_rad: float, speed_deg_s: float) -> float:
|
||||
"""A7 这一段该排多长 [s]。
|
||||
|
||||
平均速度按请求来。峰速若会超过 ``NRT_VMAX_RAD_S``，就把平均速度降到上限的
|
||||
1/1.5，让样条峰速刚好顶到上限。2026-10-19 空回按 52°/s 排 6.6s，峰速被钳住，
|
||||
到位时还差 35°。
|
||||
"""
|
||||
dist = abs(float(delta_rad))
|
||||
v_req = math.radians(max(float(speed_deg_s), 1.0))
|
||||
v_avg = min(v_req, NRT_VMAX_RAD_S / CUBIC_PEAK_OVER_AVG)
|
||||
if v_avg <= 1e-6:
|
||||
return 0.8
|
||||
return max(0.8, dist / v_avg)
|
||||
|
||||
|
||||
# 合爪位置命令是 0。和 ``pos`` 的到位带同一宽度。
|
||||
CLOSE_POS_TOL = 40.0
|
||||
#: 首轮咬合结束之后，第三步整段重复的次数。
|
||||
#:
|
||||
#: 一轮 = 张开到 ``unwind_open_permille``、微抬 ``unwind_lift_mm``、逆时针到限位、
|
||||
#: 按本轮实测上抬量降回、合爪、顺时针到限位。抬和降都在轮内，不许拿到循环外面
|
||||
#: 只做一次。固定 4，不进 site.yaml：这是工序次数，不是现场标定值。
|
||||
UNWIND_ROUNDS = 4
|
||||
|
||||
|
||||
def unwind_round_label(index: int, total: int = UNWIND_ROUNDS) -> str:
|
||||
"""第几轮的日志前缀。``index`` 从 1 计到 ``total``。
|
||||
|
||||
越界直接抛错。打出「第 0/4 轮」或「第 5/4 轮」会让人以为流程多走或少走了。
|
||||
"""
|
||||
n = int(total)
|
||||
i = int(index)
|
||||
if n != UNWIND_ROUNDS or not 1 <= i <= n:
|
||||
raise ValueError(
|
||||
f"空回轮次只能是 1–{UNWIND_ROUNDS}（固定 {UNWIND_ROUNDS} 轮），"
|
||||
f"收到 index={index} total={total}"
|
||||
)
|
||||
return f"空回第{i}/{n}轮"
|
||||
|
||||
|
||||
def close_ready(grip: int, pos: float, tol: float = CLOSE_POS_TOL) -> bool:
|
||||
"""合爪之后能不能接着顺时针转 A7。
|
||||
|
||||
大寰 ``0x0201``：力先到是 2（夹住），位置先到命令值是 1（到位）。
|
||||
这几次合到 0 时规还在两指间，位置先到，固件写 1，只认 2 就永远不转。
|
||||
手指已经在合拢端（``pos`` 不超过 ``tol``）就继续。还停在张开位则不转。
|
||||
"""
|
||||
g = int(grip)
|
||||
if g == 2:
|
||||
return True
|
||||
return g == 1 and float(pos) <= float(tol)
|
||||
|
||||
|
||||
def gripper_wait_timeout_reason(
|
||||
action: str,
|
||||
timeout_s: float,
|
||||
last: Optional[Tuple[int, float]],
|
||||
*,
|
||||
saw_any: bool,
|
||||
saw_offline: bool,
|
||||
) -> str:
|
||||
"""等夹爪状态超时后，到底该报哪一句。
|
||||
|
||||
2026-10-20 三连实测都卡在这里：读 ``0x0201`` 一连 264 次无应答，节点
|
||||
不再发状态，流水线把「总线读不到」显示成「夹爪没到位」，而臂停在原地
|
||||
没做微抬。三种情况必须分开说，否则查的方向完全相反：
|
||||
|
||||
* ``last`` 有值 → 真收到过有效帧，就是没到位；
|
||||
* 只有离线帧 → 总线读不回来，不是没张开；
|
||||
* 一帧都没有 → 节点没在发（崩了 / 没起 / 话题名不对）。
|
||||
"""
|
||||
if last is not None:
|
||||
return (
|
||||
f"夹爪 {action} 后 {timeout_s:.0f}s 未到位"
|
||||
f"（grip={last[0]} pos={last[1]:.0f}），A7 不再转"
|
||||
)
|
||||
if saw_offline:
|
||||
return (
|
||||
f"夹爪 {action} 后 {timeout_s:.0f}s 读不到状态（/gripper/states "
|
||||
"只有离线帧，Modbus 无应答），不是夹爪没张开。A7 不再转"
|
||||
)
|
||||
return (
|
||||
f"夹爪 {action} 后 {timeout_s:.0f}s 一帧 /gripper/states 都没收到"
|
||||
f"（节点没在发，saw_any={saw_any}），A7 不再转"
|
||||
)
|
||||
|
||||
|
||||
def init_z_refusal(
|
||||
@@ -69,6 +174,19 @@ def init_z_refusal(
|
||||
return ""
|
||||
|
||||
|
||||
def hold_at_insert_z(final, *, skip3: bool) -> bool:
|
||||
"""贴面已经按 ``final_gap_mm`` 停下时，不再进三阶段。
|
||||
|
||||
三阶段开局会抬到悬高上方测触觉，再落回 ``hole_hover_z_m``。
|
||||
没有接触传感器。到位高度就是这次的贴面间隙，不另设一个 Z。
|
||||
"""
|
||||
if skip3:
|
||||
return True
|
||||
if not isinstance(final, dict):
|
||||
return False
|
||||
return bool(final.get("insert_held"))
|
||||
|
||||
|
||||
def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
|
||||
"""一阶段锁高。
|
||||
|
||||
@@ -84,6 +202,20 @@ def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float
|
||||
return hover + TRACK_CLEARANCE_M
|
||||
|
||||
|
||||
def stage1_log_fatal(log_path: str) -> str:
|
||||
"""一阶段日志里已经写明失败时，不许再空等锁定。空串 = 还没到这一步。"""
|
||||
try:
|
||||
text = Path(log_path).read_text(encoding="utf-8", errors="replace")
|
||||
except OSError:
|
||||
return ""
|
||||
if D435_MOVEL_GAVE_UP in text:
|
||||
return (
|
||||
f"D435 {D435_MOVEL_GAVE_UP}（RDK 控制模式没切到 primitive），"
|
||||
f"一阶段中止。不是视野/标定。看 {log_path}"
|
||||
)
|
||||
return ""
|
||||
|
||||
|
||||
def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
|
||||
"""一阶段超时了，到底是"感知没给点"还是"给了点但没收敛"。
|
||||
|
||||
@@ -98,6 +230,9 @@ def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) ->
|
||||
以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
|
||||
因为缺依赖早退了"被读成"算法不收敛"，白查半天。
|
||||
"""
|
||||
fatal = stage1_log_fatal(log_path)
|
||||
if fatal:
|
||||
return fatal
|
||||
if points_seen <= 0:
|
||||
return (
|
||||
f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
|
||||
@@ -122,16 +257,34 @@ class AssemblyPipeline(Stage3Search):
|
||||
# 在图上必须是另一个节点名（否则 ros2 node list 里两个同名的节点，
|
||||
# 出问题时连"是哪条流程在发指令"都分不清）。
|
||||
super().__init__(node_name=node_name)
|
||||
self.declare_parameter("run_a7_after_insert", True)
|
||||
self.declare_parameter("skip_stage1", False)
|
||||
self.declare_parameter("skip_stage2", False)
|
||||
self.declare_parameter("skip_stage3", True) # 搜孔轻压已取消，写成 false 也不再下压
|
||||
self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth
|
||||
self.declare_parameter("d435_move_only", False)
|
||||
self.declare_parameter("init_z_m", -1.0) # >0：启动后先到这个法兰高度
|
||||
self.declare_parameter("stage1_timeout_sec", 150.0)
|
||||
self.declare_parameter("completion_linger_sec", 15.0)
|
||||
self.completion_linger = float(self.get_parameter("completion_linger_sec").value)
|
||||
if not 0. <= self.completion_linger <= 60.:
|
||||
raise ValueError("completion_linger_sec must be in 0..60")
|
||||
self.linger3 = 1.2 # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
|
||||
|
||||
self._s1: Optional[subprocess.Popen] = None
|
||||
self._locked = False
|
||||
self._robot_description: Optional[str] = None
|
||||
# robot_state_publisher 用 TRANSIENT_LOCAL 只发一次。默认 VOLATILE
|
||||
# 订阅在它之后才起来，3s 内一条都收不到，A7 会因“没有限位”拒绝旋转。
|
||||
self.create_subscription(
|
||||
String,
|
||||
"/robot_description",
|
||||
self._on_pipeline_robot_description,
|
||||
QoSProfile(
|
||||
depth=1,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
),
|
||||
)
|
||||
self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10)
|
||||
# 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
|
||||
# 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
|
||||
@@ -175,6 +328,376 @@ class AssemblyPipeline(Stage3Search):
|
||||
self._state_pub.publish(msg)
|
||||
self._pipe_pub.publish(msg)
|
||||
|
||||
def _on_pipeline_robot_description(self, msg: String) -> None:
|
||||
self._robot_description = str(msg.data)
|
||||
|
||||
def _a7_limits(self, timeout_s: float = 3.0) -> Optional[Tuple[float, float]]:
|
||||
"""A7 软件限位。来自 ``/robot_description`` 的 joint7，与厂家 ``q_min/q_max`` 同一对。"""
|
||||
t0 = time.monotonic()
|
||||
while self._robot_description is None and time.monotonic() - t0 < timeout_s:
|
||||
self._spin(0.05)
|
||||
if not self._robot_description:
|
||||
self.get_logger().error("拿不到 /robot_description，A7 没有限位，拒绝旋转")
|
||||
return None
|
||||
got = match_limits(
|
||||
parse_joint_limits(self._robot_description), [self.joint_names[-1]]
|
||||
)
|
||||
if got is None:
|
||||
self.get_logger().error(
|
||||
f"/robot_description 里没有 {self.joint_names[-1]} 的限位，拒绝旋转"
|
||||
)
|
||||
return got
|
||||
|
||||
def _send_a7_only(self, a7_rad: float, speed_deg_s: float, tag: str) -> bool:
|
||||
"""只下发 A7。前六轴用这一拍的当前角，厂家 ``SendJointPosition`` 收到的也是这一组。"""
|
||||
q, Tee = self._wait_state(5.0)
|
||||
if q is None or Tee is None or int(q.size) != 7:
|
||||
self.get_logger().error(f"{tag}: 拿不到 7 轴关节，不下发")
|
||||
return False
|
||||
z0 = float(Tee[2, 3])
|
||||
q_hold = q.copy()
|
||||
q_hold[6] = float(a7_rad)
|
||||
delta = float(q_hold[6] - q[6])
|
||||
dur = a7_move_duration_s(delta, speed_deg_s)
|
||||
avg_deg_s = abs(math.degrees(delta)) / dur if dur > 1e-6 else 0.0
|
||||
zeros = np.zeros(q.size)
|
||||
traj = JointTrajectory()
|
||||
traj.header.stamp.sec = 0
|
||||
traj.header.stamp.nanosec = 0
|
||||
traj.joint_names = list(self.joint_names)
|
||||
traj.points = [_pt(q, zeros, 0.2), _pt(q_hold, zeros, 0.2 + dur)]
|
||||
self.get_logger().info(
|
||||
f"{tag}: A7 {math.degrees(float(q[6])):+.1f}° → "
|
||||
f"{math.degrees(float(q_hold[6])):+.1f}°，"
|
||||
f"请求 {speed_deg_s:.0f}°/s，样条平均 {avg_deg_s:.0f}°/s、{dur:.1f}s"
|
||||
f"（峰速不超过 {NRT_VMAX_RAD_S:.2f} rad/s），前六轴不动，法兰 Z={z0:.4f}m"
|
||||
)
|
||||
if not self._send_action(traj, wait_s=dur + 8.0):
|
||||
return False
|
||||
q2, Tee2 = self._wait_state(5.0)
|
||||
if q2 is None or Tee2 is None:
|
||||
self.get_logger().error(f"{tag}: 走完读不到关节")
|
||||
return False
|
||||
others = float(np.max(np.abs(q2[:6] - q[:6])))
|
||||
dz = abs(float(Tee2[2, 3]) - z0)
|
||||
a7_err = abs(float(q2[6] - q_hold[6]))
|
||||
self.get_logger().info(
|
||||
f"{tag}: 前六轴最大偏差 {others:.4f} rad，"
|
||||
f"法兰 ΔZ={dz * 1000:.1f}mm，A7 残差 {math.degrees(a7_err):.1f}°"
|
||||
)
|
||||
if others > 0.02:
|
||||
self.get_logger().error(f"{tag}: 前六轴动了 {others:.3f} rad，中止")
|
||||
return False
|
||||
if dz > 0.003:
|
||||
self.get_logger().error(f"{tag}: 法兰 Z 变了 {dz * 1000:.1f}mm，中止")
|
||||
return False
|
||||
if a7_err > 0.05:
|
||||
self.get_logger().error(
|
||||
f"{tag}: A7 还差 {math.degrees(a7_err):.1f}°，没到限位内侧"
|
||||
)
|
||||
return False
|
||||
self._pin(q2)
|
||||
return True
|
||||
|
||||
def _shift_flange_z(self, delta_m: float, tag: str) -> Optional[float]:
|
||||
"""厂家 MoveL 一段，只改 TCP 的 Z。返回实测 ΔZ [m]；失败返回 None。
|
||||
|
||||
不走关节小跳。姿态用当前 TCP 欧拉角，XY 用当前厂家 TCP，不加别的位移。
|
||||
"""
|
||||
self._spin(0.05)
|
||||
tcp0 = self._fresh_tcp_world()
|
||||
q, Tee = self._wait_state(2.0)
|
||||
if tcp0 is None or q is None or Tee is None:
|
||||
self.get_logger().error(f"{tag}: 没有厂家 TCP，拒绝用关节小跳凑 Z")
|
||||
return None
|
||||
target = movel_tcp_z_only(tcp0, delta_m)
|
||||
if target is None:
|
||||
self.get_logger().error(f"{tag}: MoveL 的 Z 目标算不出")
|
||||
return None
|
||||
fx0, fy0, fz0 = float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3])
|
||||
self.get_logger().info(
|
||||
f"{tag}: 厂家 MoveL 一段 TCP "
|
||||
f"({float(tcp0[0]):.4f},{float(tcp0[1]):.4f},{float(tcp0[2]):.4f})"
|
||||
f" → ({target[0]:.4f},{target[1]:.4f},{target[2]:.4f}) "
|
||||
f"只改 Z {float(delta_m) * 1000:+.2f}mm，vel=10mm/s，"
|
||||
f"过渡区 ZFine，到位等级 1"
|
||||
)
|
||||
# 厂家默认 Z50（50mm）和等级 3 会在还差约 0.5mm 时就报完成。
|
||||
ok = self._vendor_movel_tcp(target, 0.010, zone="ZFine", toler=1)
|
||||
q2, Tee2 = self._wait_state(0.5)
|
||||
self._spin(0.05)
|
||||
tcp1 = self._fresh_tcp_world()
|
||||
if not ok or q2 is None or Tee2 is None or tcp1 is None:
|
||||
self.get_logger().error(f"{tag}: 厂家 MoveL 没完成，中止")
|
||||
return None
|
||||
tcp_xy = math.hypot(float(tcp1[0]) - float(tcp0[0]), float(tcp1[1]) - float(tcp0[1]))
|
||||
dz = float(tcp1[2]) - float(tcp0[2])
|
||||
fl_xy = math.hypot(float(Tee2[0, 3]) - fx0, float(Tee2[1, 3]) - fy0)
|
||||
fl_dz = float(Tee2[2, 3]) - fz0
|
||||
self.get_logger().info(
|
||||
f"{tag}: 厂家TCP ΔXY={tcp_xy * 1000:.2f}mm ΔZ={dz * 1000:+.2f}mm；"
|
||||
f"法兰 ΔXY={fl_xy * 1000:.2f}mm ΔZ={fl_dz * 1000:+.2f}mm"
|
||||
)
|
||||
if tcp_xy > 0.001 or fl_xy > 0.001:
|
||||
self.get_logger().error(
|
||||
f"{tag}: XY 动了 TCP {tcp_xy * 1000:.2f}mm / 法兰 {fl_xy * 1000:.2f}mm，中止"
|
||||
)
|
||||
return None
|
||||
self._pin(q2)
|
||||
return dz
|
||||
|
||||
def _arm_short_id(self) -> str:
|
||||
"""当前臂号。与 ``export_site_env.arm_short_id`` 一样，取 SN 末两位。"""
|
||||
short = os.environ.get("SITE_ROBOT_SHORT_ID", "").strip()
|
||||
if short:
|
||||
return short
|
||||
sn = str(
|
||||
self.get_parameter("robot_sn").value
|
||||
or os.environ.get("SITE_ROBOT_SN")
|
||||
or ""
|
||||
).strip()
|
||||
tail = sn.rsplit("-", 1)[-1]
|
||||
if tail.isdigit() and len(tail) >= 2:
|
||||
return tail[-2:]
|
||||
return ""
|
||||
|
||||
def _gripper_cli(self, action: str, value: str = "") -> bool:
|
||||
"""左平行爪。``close`` 合拢；``pos`` 走到 0–1000 的指定开度，不全开。
|
||||
|
||||
脚本返回只说明命令已交给 ``/gripper/ctrl``。没在状态里看到目标开度或夹住，
|
||||
就不许接着转 A7。
|
||||
"""
|
||||
short = self._arm_short_id()
|
||||
if not short:
|
||||
self.get_logger().error("不知道当前臂号，拒绝开合夹爪")
|
||||
return False
|
||||
if action == "close":
|
||||
extra: list[str] = []
|
||||
elif action == "pos":
|
||||
try:
|
||||
pos = int(value)
|
||||
except (TypeError, ValueError):
|
||||
pos = -1
|
||||
if pos < 0 or pos > 1000:
|
||||
self.get_logger().error(f"夹爪位置只接受 0–1000，收到 {value!r}")
|
||||
return False
|
||||
extra = [str(pos)]
|
||||
else:
|
||||
self.get_logger().error(f"夹爪命令只接受 pos/close，收到 {action}")
|
||||
return False
|
||||
script = ros2_root() / "scripts" / "run_gripper.sh"
|
||||
cmd = ["bash", str(script), "--arm", short, "--cli", action, *extra]
|
||||
# 第一次状态没跟上就再发一次。2026-10-20 合爪命令已发布，节点被串口
|
||||
# flush 堵住 120s 才执行，8s 时手指还停在张开位，这一轮就停了。
|
||||
for attempt in (1, 2):
|
||||
self.get_logger().info("夹爪：" + " ".join(cmd))
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
cmd, capture_output=True, text=True, timeout=30, check=False
|
||||
)
|
||||
except subprocess.TimeoutExpired:
|
||||
self.get_logger().error(f"夹爪 {action} 30s 没有返回，A7 不再转")
|
||||
return False
|
||||
text = ((proc.stdout or "") + "\n" + (proc.stderr or "")).strip()
|
||||
if text:
|
||||
tail = " | ".join(text.splitlines()[-4:])
|
||||
self.get_logger().info(f"夹爪脚本：{tail}")
|
||||
if proc.returncode != 0:
|
||||
self.get_logger().error(
|
||||
f"夹爪 {action} 失败 exit={proc.returncode}，A7 不再转"
|
||||
)
|
||||
return False
|
||||
if self._wait_gripper(action, value, final=(attempt == 2)):
|
||||
return True
|
||||
if attempt == 1:
|
||||
self.get_logger().warn(f"夹爪 {action} 状态没跟上，再发一次命令")
|
||||
return False
|
||||
|
||||
def _wait_gripper(
|
||||
self, action: str, value: str = "", timeout_s: float = 8.0, *, final: bool = True
|
||||
) -> bool:
|
||||
"""``/gripper/states``：``pos`` 要停在目标附近；合爪见 :func:`close_ready`。
|
||||
|
||||
离线帧（第 6 位为 0，前 5 个是上一次的值）不算数：读不到总线时
|
||||
必须报"读不到状态"，不能报成"夹爪没到位"。
|
||||
"""
|
||||
box: dict = {"data": None}
|
||||
|
||||
def _on_state(msg: Float32MultiArray) -> None:
|
||||
box["data"] = list(msg.data)
|
||||
|
||||
sub = self.create_subscription(Float32MultiArray, "/gripper/states", _on_state, 10)
|
||||
t0 = time.monotonic()
|
||||
last = None
|
||||
saw_any = False
|
||||
saw_offline = False
|
||||
try:
|
||||
while time.monotonic() - t0 < timeout_s:
|
||||
self._spin(0.05)
|
||||
data = box["data"]
|
||||
if not data or len(data) < 3:
|
||||
continue
|
||||
saw_any = True
|
||||
if not states_frame_online(data):
|
||||
saw_offline = True
|
||||
continue
|
||||
grip = int(data[1])
|
||||
pos = float(data[2])
|
||||
last = (grip, pos)
|
||||
if action == "pos" and grip != 0 and abs(pos - float(value)) <= 40.0:
|
||||
self.get_logger().info(
|
||||
f"夹爪已到 pos={pos:.0f}（目标 {value}，不全开）"
|
||||
)
|
||||
return True
|
||||
if action == "close" and close_ready(grip, pos):
|
||||
if grip == 2:
|
||||
self.get_logger().info(f"夹爪已夹住 pos={pos:.0f}")
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"夹爪已合拢 pos={pos:.0f}（固件报到位）。继续顺时针。"
|
||||
)
|
||||
return True
|
||||
finally:
|
||||
self.destroy_subscription(sub)
|
||||
reason = gripper_wait_timeout_reason(
|
||||
action, timeout_s, last, saw_any=saw_any, saw_offline=saw_offline
|
||||
)
|
||||
if final:
|
||||
self.get_logger().error(reason)
|
||||
else:
|
||||
self.get_logger().warn(reason)
|
||||
return False
|
||||
|
||||
def _a7_to_limit(
|
||||
self, limits, direction: int, margin_deg: float, speed: float, tag: str
|
||||
) -> bool:
|
||||
"""只把 A7 转到这一侧限位内侧。前六轴和法兰 Z 的检查在 ``_send_a7_only``。"""
|
||||
q, _Tee = self._wait_state(5.0)
|
||||
if q is None:
|
||||
self.get_logger().error(f"{tag}: 没有当前关节")
|
||||
return False
|
||||
target = a7_limit_target(q, limits, direction, float(margin_deg))
|
||||
if target is None:
|
||||
self.get_logger().error(f"{tag}: 算不出 A7 限位目标")
|
||||
return False
|
||||
return self._send_a7_only(float(target[6]), speed, tag)
|
||||
|
||||
def _run_a7_seat(self) -> bool:
|
||||
"""贴面后咬螺纹。只转 A7，法兰 XY 不动。
|
||||
|
||||
前两步是首轮：逆时针到限位，再顺时针到限位。顺时针到位即首轮咬合结束。
|
||||
第三步整段固定重复 :data:`UNWIND_ROUNDS` 轮。每一轮自己抬、自己记住、
|
||||
自己降回；某一轮失败就停，后面的轮次不再做。
|
||||
"""
|
||||
limits = self._a7_limits()
|
||||
if limits is None:
|
||||
return False
|
||||
ga = load_site_dict().get("gauge_assembly") or {}
|
||||
try:
|
||||
margin = float(ga["extreme_margin_deg"])
|
||||
ccw = int(ga["extreme_direction"])
|
||||
lift_mm = float(ga["unwind_lift_mm"])
|
||||
open_permille = int(ga["unwind_open_permille"])
|
||||
seat_cw_deg_s = float(ga["extreme_speed_deg_s"])
|
||||
except (KeyError, TypeError, ValueError) as exc:
|
||||
self.get_logger().error(
|
||||
f"site.yaml gauge_assembly 缺 A7 旋转参数（{exc}），拒绝旋转"
|
||||
)
|
||||
return False
|
||||
if ccw not in (-1, 1):
|
||||
self.get_logger().error(
|
||||
f"extreme_direction 只能是 +1/-1，收到 {ccw}，拒绝旋转"
|
||||
)
|
||||
return False
|
||||
if not (0.0 < lift_mm <= 5.0):
|
||||
self.get_logger().error(
|
||||
f"unwind_lift_mm 只接受 0–5mm 的微抬，收到 {lift_mm}，拒绝旋转"
|
||||
)
|
||||
return False
|
||||
if not (0 < open_permille <= 1000):
|
||||
self.get_logger().error(
|
||||
f"unwind_open_permille 只接受 1–1000，收到 {open_permille}，拒绝旋转"
|
||||
)
|
||||
return False
|
||||
if not (0.0 < seat_cw_deg_s <= 90.0):
|
||||
self.get_logger().error(
|
||||
f"extreme_speed_deg_s 只接受 0–90°/s，收到 {seat_cw_deg_s}，拒绝旋转"
|
||||
)
|
||||
return False
|
||||
# extreme_direction 是逆时针的符号。第一步出孔走接口上限，第二步顺时针
|
||||
# 进孔用 extreme_speed_deg_s（现场 10°/s）。空回轮次里的正反转仍是上限。
|
||||
if not self._a7_to_limit(
|
||||
limits, ccw, margin, A7_MAX_SPEED_DEG_S, "A7 逆时针至极限（出孔）"
|
||||
):
|
||||
return False
|
||||
if not self._a7_to_limit(
|
||||
limits, -ccw, margin, seat_cw_deg_s, "A7 顺时针至极限（进孔）"
|
||||
):
|
||||
return False
|
||||
self._state("首轮咬合结束")
|
||||
self.get_logger().info("首轮咬合结束：A7 已顺时针转到限位。")
|
||||
self.get_logger().info(
|
||||
f"第三步空回固定重复 {UNWIND_ROUNDS} 轮。每一轮：张开到 {open_permille}、"
|
||||
f"微抬 {lift_mm:.1f}mm、逆时针到限位、按本轮记住的上抬量降回、合爪、顺时针到限位。"
|
||||
)
|
||||
# 抬和降必须在轮内：每轮的下降量用这一轮实测的上抬，不能用上一轮的数。
|
||||
for rnd in range(1, UNWIND_ROUNDS + 1):
|
||||
label = unwind_round_label(rnd)
|
||||
self._state(label)
|
||||
self.get_logger().info(f"{label} 开始")
|
||||
if not self._gripper_cli("pos", str(open_permille)):
|
||||
self.get_logger().error(f"{label} 没走完，后面的轮次不再做")
|
||||
return False
|
||||
dz_up = self._shift_flange_z(lift_mm / 1000.0, f"{label} 空回前微抬")
|
||||
if dz_up is None:
|
||||
self.get_logger().error(f"{label} 没走完，后面的轮次不再做")
|
||||
return False
|
||||
if dz_up < (lift_mm / 1000.0) * 0.6:
|
||||
self.get_logger().error(
|
||||
f"{label} 空回前微抬只做到 {dz_up * 1000:.2f}mm，"
|
||||
f"不到指令 {lift_mm:.1f}mm 的六成，不空转"
|
||||
)
|
||||
self.get_logger().error(f"{label} 没走完，后面的轮次不再做")
|
||||
return False
|
||||
self.get_logger().info(
|
||||
f"{label} 记住上抬 {dz_up * 1000:.2f}mm。本轮空回后按这一段下降，XY 不动。"
|
||||
)
|
||||
if not self._a7_to_limit(
|
||||
limits, ccw, margin, A7_MAX_SPEED_DEG_S,
|
||||
f"{label} A7 逆时针至极限（松爪回转）",
|
||||
):
|
||||
self.get_logger().error(f"{label} 没走完，后面的轮次不再做")
|
||||
return False
|
||||
dz_down = self._shift_flange_z(-dz_up, f"{label} 空回后降回")
|
||||
if dz_down is None:
|
||||
self.get_logger().error(f"{label} 没走完，后面的轮次不再做")
|
||||
return False
|
||||
if abs(dz_down + dz_up) > 0.0005:
|
||||
self.get_logger().error(
|
||||
f"{label} 下降 {dz_down * 1000:.2f}mm 和记住的上抬 "
|
||||
f"{dz_up * 1000:.2f}mm 不一致，不合爪"
|
||||
)
|
||||
self.get_logger().error(f"{label} 没走完，后面的轮次不再做")
|
||||
return False
|
||||
if not self._gripper_cli("close"):
|
||||
self.get_logger().error(f"{label} 没走完，后面的轮次不再做")
|
||||
return False
|
||||
if not self._a7_to_limit(
|
||||
limits, -ccw, margin, A7_MAX_SPEED_DEG_S,
|
||||
f"{label} A7 顺时针至极限（夹紧再进）",
|
||||
):
|
||||
self.get_logger().error(f"{label} 没走完，后面的轮次不再做")
|
||||
return False
|
||||
self.get_logger().info(
|
||||
f"{label} 完成：张开到 {open_permille}、上抬 {dz_up * 1000:.2f}mm、"
|
||||
f"下降 {dz_down * 1000:.2f}mm、合爪、顺时针到限位。"
|
||||
f"进度 {rnd}/{UNWIND_ROUNDS}。"
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"空回 {UNWIND_ROUNDS}/{UNWIND_ROUNDS} 轮全部完成。"
|
||||
)
|
||||
return True
|
||||
|
||||
def _on_lock(self, msg: Bool) -> None:
|
||||
if bool(msg.data):
|
||||
self._locked = True
|
||||
@@ -276,7 +799,7 @@ class AssemblyPipeline(Stage3Search):
|
||||
f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
|
||||
)
|
||||
self._s1 = subprocess.Popen(
|
||||
cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=False,
|
||||
cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=True,
|
||||
)
|
||||
timeout = float(self.get_parameter("stage1_timeout_sec").value)
|
||||
self._point_msgs = 0
|
||||
@@ -285,6 +808,11 @@ class AssemblyPipeline(Stage3Search):
|
||||
self._spin(0.1)
|
||||
if self._locked:
|
||||
break
|
||||
fatal = stage1_log_fatal(log_path)
|
||||
if fatal:
|
||||
self._stop_stage1()
|
||||
self.get_logger().error(fatal)
|
||||
return 1
|
||||
if self._s1.poll() is not None:
|
||||
self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
|
||||
return 1
|
||||
@@ -310,9 +838,7 @@ class AssemblyPipeline(Stage3Search):
|
||||
def run(self) -> int:
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
if not bool(self.get_parameter("skip_stage3").value):
|
||||
self.get_logger().info("搜孔轻压已取消，忽略要求重新打开它的参数")
|
||||
skip3 = True
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
if self.d435_move_only:
|
||||
skip2 = True
|
||||
skip3 = True
|
||||
@@ -342,28 +868,64 @@ class AssemblyPipeline(Stage3Search):
|
||||
if code != 0:
|
||||
self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
|
||||
return code
|
||||
stay = hold_at_insert_z(final, skip3=skip3)
|
||||
if stay and final.get("insert_held"):
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm — "
|
||||
f"插孔停在法兰 Z={final.get('flange_z_m')}m，"
|
||||
f"规尖离面 {final.get('tip_gap_mm')}mm"
|
||||
f"（目标 {final.get('commanded_gap_mm')}mm，"
|
||||
f"来自 {final.get('insert_z_source')}）。"
|
||||
"没有接触传感器，不另设 Z，不抬回悬高。"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
|
||||
" — 搜孔已取消，停在这里。旋入请另跑。"
|
||||
+ (
|
||||
" — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
|
||||
if skip3
|
||||
else " — 立刻下压搜孔并轻压（不拧入）"
|
||||
)
|
||||
)
|
||||
else:
|
||||
self._state("skip_stage2")
|
||||
stay = hold_at_insert_z(final, skip3=skip3)
|
||||
|
||||
if skip3:
|
||||
if stay:
|
||||
self._state("skip_stage3")
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
f"结果发布 {self.completion_linger:g}s 后退出，臂保持不动"
|
||||
)
|
||||
elif isinstance(final, dict) and final.get("insert_held") and bool(self.get_parameter("run_a7_after_insert").value):
|
||||
if q is not None:
|
||||
self._pin(q)
|
||||
self.get_logger().info(
|
||||
"插孔已停在贴面高度，不进搜孔、不抬回悬高。"
|
||||
"接着只转 A7：先逆时针到限位（接口最快），再顺时针到限位"
|
||||
"（首轮咬合，用 extreme_speed_deg_s）。"
|
||||
f"然后下面这一整段固定重复 {UNWIND_ROUNDS} 轮："
|
||||
"平行爪只张开到指定开度、微抬 Z、最快逆时针回限位、"
|
||||
"按本轮记住的上抬量降回、合爪、再最快顺时针到限位。"
|
||||
"某一轮没走完就停，后面的轮次不再做。"
|
||||
)
|
||||
if not self._run_a7_seat():
|
||||
self.get_logger().error(
|
||||
"A7 旋转没走完，臂停在当前关节。不抬 Z。"
|
||||
)
|
||||
failed = String()
|
||||
failed.data = json.dumps({"ok": False, "stage": "pipeline", "exit": 2, "error": "A7 旋转未完成"}, ensure_ascii=False)
|
||||
self._pipe_result.publish(failed)
|
||||
return 2
|
||||
else:
|
||||
self.get_logger().info(
|
||||
"搜孔已取消 —— 臂停在当前高度。"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
f"结果发布 {self.completion_linger:g}s 后退出，臂保持不动"
|
||||
)
|
||||
if final:
|
||||
self._publish_result(final)
|
||||
self._linger_hold(final if final else {"ok": True}, seconds=15.0)
|
||||
self._linger_hold(final if final else {"ok": True}, seconds=self.completion_linger)
|
||||
payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
|
||||
try:
|
||||
payload["stage2"] = json.loads(
|
||||
@@ -397,9 +959,9 @@ class AssemblyPipeline(Stage3Search):
|
||||
self._stop_stage1()
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
def main(args=None, node_factory=AssemblyPipeline) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = AssemblyPipeline()
|
||||
node = node_factory()
|
||||
code = 2
|
||||
try:
|
||||
code = node.run()
|
||||
|
||||
@@ -328,8 +328,16 @@ class PoseDetector:
|
||||
self.max_det = int(max_det)
|
||||
|
||||
if path.suffix.lower() in {".engine", ".trt"}:
|
||||
try:
|
||||
self._runner = _TrtRunner(path, device)
|
||||
self.backend = "tensorrt"
|
||||
except Exception as exc:
|
||||
onnx = path.with_suffix(".onnx")
|
||||
if not onnx.is_file():
|
||||
raise
|
||||
logger.warning("TensorRT 加载失败（%s），改用 %s", exc, onnx)
|
||||
self._runner = _OnnxRunner(onnx)
|
||||
self.backend = "onnx"
|
||||
else:
|
||||
self._runner = _OnnxRunner(path)
|
||||
self.backend = "onnx"
|
||||
|
||||
@@ -55,3 +55,33 @@ def stop_process_tree(proc, interrupt_timeout=4., terminate_timeout=2., kill_tim
|
||||
living=[pid for pid,started in owned.items() if _alive(pid,started)]
|
||||
if living:raise RuntimeError(f'受管子进程未退出：{living}')
|
||||
proc.wait(timeout=1)
|
||||
|
||||
|
||||
def stop_owned_children(parent, interrupt_timeout=4., terminate_timeout=2., kill_timeout=1.):
|
||||
"""For a subreaper: stop/reap descendants, including adopted launch orphans."""
|
||||
if parent != os.getpid():
|
||||
raise ValueError('only the supervising process may reap its own children')
|
||||
owned = {}
|
||||
for sig, timeout in ((signal.SIGINT, interrupt_timeout), (signal.SIGTERM, terminate_timeout),
|
||||
(signal.SIGKILL, kill_timeout)):
|
||||
deadline = time.monotonic() + timeout
|
||||
sent = set()
|
||||
while True:
|
||||
found = _descendants(parent)
|
||||
found.pop(parent, None)
|
||||
owned.update(found)
|
||||
for pid, started in owned.items():
|
||||
if (pid, started) not in sent and _alive(pid, started):
|
||||
try: os.kill(pid, sig)
|
||||
except ProcessLookupError: pass
|
||||
sent.add((pid, started))
|
||||
# Reap only our children, never wait for or signal unrelated processes.
|
||||
while True:
|
||||
try:
|
||||
pid, _ = os.waitpid(-1, os.WNOHANG)
|
||||
if pid == 0: break
|
||||
except ChildProcessError: break
|
||||
if not any(_alive(pid, started) for pid, started in owned.items()): return
|
||||
if time.monotonic() >= deadline: break
|
||||
time.sleep(.04)
|
||||
raise RuntimeError('受管子进程清理未完成，拒绝报告任务结束')
|
||||
|
||||
@@ -29,7 +29,7 @@ import rclpy
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from sensor_msgs.msg import CameraInfo, Image, CompressedImage
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str
|
||||
@@ -49,6 +49,30 @@ USB_RESEAT_HINT = (
|
||||
)
|
||||
|
||||
|
||||
def usb2_needs_reset(usb_desc: str) -> bool:
|
||||
"""USB2 枚举后 pipeline 能打开但不出帧；复位常能重新协商 SuperSpeed。"""
|
||||
return str(usb_desc or '').startswith('2')
|
||||
|
||||
|
||||
def stream_open_passes(want_depth: bool, want_infra: bool, usb2: bool):
|
||||
"""开流顺序：所有分辨率先试带深度，全部失败才退回纯彩色。
|
||||
|
||||
旧逻辑在同一分辨率里 depth 失败就立刻 color-only。USB3 的 1280x720
|
||||
深度偶尔 busy 一次，就会把已经能用的 640x480 深度丢掉，跟踪整段
|
||||
150s 报「框心无深度」。
|
||||
"""
|
||||
depth_modes = []
|
||||
if want_depth and want_infra and not usb2:
|
||||
depth_modes.append((True, 'both'))
|
||||
if want_depth and want_infra:
|
||||
depth_modes.append((True, 'right'))
|
||||
if want_depth:
|
||||
depth_modes.append((True, 'none'))
|
||||
if depth_modes:
|
||||
yield 'depth', tuple(depth_modes)
|
||||
yield 'color', ((False, 'none'),)
|
||||
|
||||
|
||||
def _fill_camera_info(
|
||||
msg: CameraInfo, width: int, height: int, fx, fy, cx, cy, frame_id: str, dist
|
||||
) -> None:
|
||||
@@ -88,6 +112,7 @@ class RealsenseColorPub(TolerantNode):
|
||||
self.declare_parameter('height', 720)
|
||||
self.declare_parameter('fps', 15)
|
||||
self.declare_parameter('show_preview', False)
|
||||
self.declare_parameter('preview_jpeg_quality', 95)
|
||||
self.declare_parameter('overlay_detection', True)
|
||||
self.declare_parameter('overlay_hold_sec', 1.5)
|
||||
self.declare_parameter('enable_depth', True)
|
||||
@@ -106,6 +131,7 @@ class RealsenseColorPub(TolerantNode):
|
||||
self._width = int(self.get_parameter('width').value)
|
||||
self._height = int(self.get_parameter('height').value)
|
||||
self._fps = int(self.get_parameter('fps').value)
|
||||
self._requested_mode = (self._width, self._height, self._fps)
|
||||
self._show_preview = bool(self.get_parameter('show_preview').value)
|
||||
self._overlay_detection = bool(self.get_parameter('overlay_detection').value)
|
||||
self._overlay_hold_sec = float(self.get_parameter('overlay_hold_sec').value)
|
||||
@@ -136,6 +162,11 @@ class RealsenseColorPub(TolerantNode):
|
||||
self._img_pub = self.create_publisher(
|
||||
Image, f'{base}/image_raw', qos
|
||||
)
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy
|
||||
self._compressed_pub = self.create_publisher(
|
||||
CompressedImage, f'{base}/preview/compressed',
|
||||
QoSProfile(depth=1,reliability=ReliabilityPolicy.BEST_EFFORT))
|
||||
self._jpeg_quality = min(100, max(70, int(self.get_parameter('preview_jpeg_quality').value)))
|
||||
self._info_pub = self.create_publisher(
|
||||
CameraInfo, f'{base}/camera_info', qos
|
||||
)
|
||||
@@ -181,6 +212,7 @@ class RealsenseColorPub(TolerantNode):
|
||||
self._pending = None
|
||||
self._last_frame_mono = 0.0
|
||||
self._usb_desc = ''
|
||||
self._usb_reset_attempted = False
|
||||
self._last_ok = None
|
||||
self._site_yaml = _site_yaml_path()
|
||||
self._site_mtime = 0.0
|
||||
@@ -233,6 +265,28 @@ class RealsenseColorPub(TolerantNode):
|
||||
return d
|
||||
return devices[0]
|
||||
|
||||
def _recover_superspeed(self, device, rs):
|
||||
"""USB2 卡住时复位一次。设备其实在 USB3 口上时，常能重新协商到 5000M。"""
|
||||
serial = self._serial or str(device.get_info(rs.camera_info.serial_number))
|
||||
usb = getattr(self, '_usb_desc', '?')
|
||||
self.get_logger().warn(
|
||||
f'{self._name} USB={usb} 无 SuperSpeed，复位一次以重新协商。'
|
||||
+ USB_RESEAT_HINT
|
||||
)
|
||||
device.hardware_reset()
|
||||
deadline = time.monotonic() + 8.0
|
||||
while time.monotonic() < deadline:
|
||||
time.sleep(0.4)
|
||||
for candidate in rs.context().devices:
|
||||
try:
|
||||
if candidate.get_info(rs.camera_info.serial_number) == serial:
|
||||
recovered = str(candidate.get_info(rs.camera_info.usb_type_descriptor) or '')
|
||||
self.get_logger().info(f'{self._name} 复位后 USB={recovered or "?"}')
|
||||
return candidate
|
||||
except Exception:
|
||||
continue
|
||||
raise RuntimeError(f'{self._name} 复位后未重新枚举，请拔插 USB')
|
||||
|
||||
def _reload_site_preview(self, stream_w: int = 0, stream_h: int = 0) -> None:
|
||||
"""不重启就能读到新的 overlay_hold_sec（宽高变了则给出警告）。"""
|
||||
path = self._site_yaml
|
||||
@@ -290,18 +344,28 @@ class RealsenseColorPub(TolerantNode):
|
||||
h, w = bgr.shape[0], bgr.shape[1]
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
|
||||
# Keep full raw images for perception. The UI consumes its own lightweight
|
||||
# preview channel, so raw multi-megabyte UDP bursts no longer feed the UI.
|
||||
if self._img_pub.get_subscription_count():
|
||||
img = Image()
|
||||
img.header.stamp = stamp
|
||||
img.header.frame_id = self._frame_id
|
||||
img.height = h
|
||||
img.width = w
|
||||
img.height, img.width = h, w
|
||||
img.encoding = 'bgr8'
|
||||
img.is_bigendian = 0
|
||||
img.step = w * 3
|
||||
# array.array('B', ...) 会走 setter 的快路径 —— 直接传 bytes 会触发
|
||||
# 逐元素的 debug 检查，在这里每帧要多花约 40 ms。
|
||||
img.data = array.array('B', np.ascontiguousarray(bgr).tobytes())
|
||||
self._img_pub.publish(img)
|
||||
if self._compressed_pub.get_subscription_count():
|
||||
import cv2
|
||||
ok, encoded = cv2.imencode('.jpg', bgr, [cv2.IMWRITE_JPEG_QUALITY,self._jpeg_quality])
|
||||
if ok:
|
||||
preview = CompressedImage()
|
||||
preview.header.stamp = stamp
|
||||
preview.header.frame_id = self._frame_id
|
||||
preview.format = 'bgr8; jpeg compressed bgr8'
|
||||
preview.data = array.array('B', encoded.tobytes())
|
||||
self._compressed_pub.publish(preview)
|
||||
|
||||
info = CameraInfo()
|
||||
info.header.stamp = stamp
|
||||
@@ -369,6 +433,14 @@ class RealsenseColorPub(TolerantNode):
|
||||
except Exception: # noqa: BLE001
|
||||
usb_desc = '?'
|
||||
self._usb_desc = usb_desc
|
||||
if usb2_needs_reset(usb_desc) and not self._usb_reset_attempted:
|
||||
self._usb_reset_attempted = True
|
||||
found = self._recover_superspeed(found, rs)
|
||||
try:
|
||||
usb_desc = str(found.get_info(rs.camera_info.usb_type_descriptor))
|
||||
except Exception:
|
||||
usb_desc = '?'
|
||||
self._usb_desc = usb_desc
|
||||
usb2 = usb_desc.startswith('2')
|
||||
cam_name = ''
|
||||
try:
|
||||
@@ -379,6 +451,8 @@ class RealsenseColorPub(TolerantNode):
|
||||
f'{cam_name} serial={self._serial or "?"} USB={usb_desc} '
|
||||
f'requested {self._width}x{self._height}@{self._fps}Hz'
|
||||
)
|
||||
# A temporary USB2 fallback must not overwrite the requested USB3 mode.
|
||||
self._width, self._height, self._fps = self._requested_mode
|
||||
if usb2:
|
||||
# 只在 480Mbps 口上才降。实测（2026-09-28）640x480@15 彩色+深度
|
||||
# 可以稳定跑满 15fps；但 1280x720@30 在 USB2 上物理不可能
|
||||
@@ -392,11 +466,9 @@ class RealsenseColorPub(TolerantNode):
|
||||
want = (self._width, self._height, self._fps)
|
||||
last = self._last_ok
|
||||
if reconnect:
|
||||
candidates = []
|
||||
if last:
|
||||
candidates = [want]
|
||||
if last and last != want and not usb2:
|
||||
candidates.append(last)
|
||||
if want not in candidates:
|
||||
candidates.append(want)
|
||||
if (640, 480, 15) not in candidates:
|
||||
candidates.append((640, 480, 15))
|
||||
else:
|
||||
@@ -408,7 +480,15 @@ class RealsenseColorPub(TolerantNode):
|
||||
]
|
||||
if usb2:
|
||||
candidates = [(640, 480, 15), (640, 480, 5), (424, 240, 15)]
|
||||
# 丢弃固件里的垃圾模式（848@90 之类），它们会 ioctl 失败并卡死 USB。
|
||||
color_modes, depth_modes = set(), set()
|
||||
for sensor in found.query_sensors():
|
||||
for stream in sensor.get_stream_profiles():
|
||||
if not stream.is_video_stream_profile(): continue
|
||||
video = stream.as_video_stream_profile()
|
||||
mode = (video.width(), video.height(), stream.fps())
|
||||
if stream.stream_type() == rs.stream.color: color_modes.add(mode)
|
||||
if stream.stream_type() == rs.stream.depth: depth_modes.add(mode)
|
||||
# Verify against this exact serial's enumerated profiles, including D435 1080p.
|
||||
seen = set()
|
||||
trimmed = []
|
||||
for item in candidates:
|
||||
@@ -417,7 +497,7 @@ class RealsenseColorPub(TolerantNode):
|
||||
w, h, fps = item
|
||||
if fps not in (5, 15, 30):
|
||||
continue
|
||||
if w * h > 1280 * 720:
|
||||
if (w, h, fps) not in color_modes:
|
||||
continue
|
||||
seen.add(item)
|
||||
trimmed.append(item)
|
||||
@@ -436,33 +516,32 @@ class RealsenseColorPub(TolerantNode):
|
||||
|
||||
# USB3：先尝试真正的 IR-L+IR-R（共享立体自动曝光）。彩色+IR-L 在 D405 上
|
||||
# 经常失败 —— 那就退回 IR-R + 把彩色当左目（观感不同，且不共享自动曝光）。
|
||||
def _modes():
|
||||
if want_depth and want_infra and not usb2:
|
||||
yield True, 'both'
|
||||
if want_depth and want_infra:
|
||||
yield True, 'right'
|
||||
if want_depth:
|
||||
yield True, 'none'
|
||||
yield False, 'none'
|
||||
|
||||
for pass_name, modes in stream_open_passes(want_depth, want_infra, usb2):
|
||||
if pass_name == 'color' and want_depth:
|
||||
self.get_logger().warn(
|
||||
'所有分辨率的彩色+深度都失败，才退回纯彩色；跟踪会没有深度'
|
||||
)
|
||||
for w, h, fps in candidates:
|
||||
for with_depth, infra_mode in _modes():
|
||||
for with_depth, infra_mode in modes:
|
||||
attempts = 3 if with_depth else 1
|
||||
for attempt in range(attempts):
|
||||
try:
|
||||
trial = rs.config()
|
||||
if self._serial:
|
||||
trial.enable_device(self._serial)
|
||||
trial.enable_stream(rs.stream.color, w, h, rs.format.bgr8, fps)
|
||||
dw, dh, dfps = w, h, fps
|
||||
if with_depth:
|
||||
trial.enable_stream(
|
||||
rs.stream.depth, w, h, rs.format.z16, fps
|
||||
)
|
||||
from arm_application.camera_profiles import depth_profile
|
||||
dw, dh, dfps = depth_profile((w,h,fps), depth_modes)
|
||||
trial.enable_stream(rs.stream.depth, dw, dh, rs.format.z16, dfps)
|
||||
if infra_mode in ('both', 'left'):
|
||||
trial.enable_stream(
|
||||
rs.stream.infrared, 1, w, h, rs.format.y8, fps
|
||||
rs.stream.infrared, 1, dw, dh, rs.format.y8, dfps
|
||||
)
|
||||
if infra_mode in ('both', 'right'):
|
||||
trial.enable_stream(
|
||||
rs.stream.infrared, 2, w, h, rs.format.y8, fps
|
||||
rs.stream.infrared, 2, dw, dh, rs.format.y8, dfps
|
||||
)
|
||||
# 回调里只留最新一帧。wait_for_frames 会等彩色、深度、红外齐套，
|
||||
# 场景一动红外对不齐，彩色帧就在这里被丢掉。
|
||||
@@ -470,6 +549,8 @@ class RealsenseColorPub(TolerantNode):
|
||||
have_depth = with_depth
|
||||
have_infra = infra_mode != 'none'
|
||||
self._infra_mode = infra_mode
|
||||
if with_depth:
|
||||
self.get_logger().info(f'native streams color={w}x{h}@{fps}, depth={dw}x{dh}@{dfps}')
|
||||
if (w, h, fps) != want:
|
||||
self.get_logger().warn(
|
||||
f'请求 {want[0]}x{want[1]}@{want[2]}Hz '
|
||||
@@ -498,7 +579,8 @@ class RealsenseColorPub(TolerantNode):
|
||||
except Exception as exc: # noqa: BLE001
|
||||
last_err = exc
|
||||
kind = f'color+depth+IR-{infra_mode}' if with_depth else 'color'
|
||||
self.get_logger().warn(f'{w}x{h}@{fps}Hz {kind} 失败：{exc}')
|
||||
extra = f'（第 {attempt+1}/{attempts} 次）' if attempts > 1 else ''
|
||||
self.get_logger().warn(f'{w}x{h}@{fps}Hz {kind} 失败{extra}：{exc}')
|
||||
profile = None
|
||||
have_depth = False
|
||||
have_infra = False
|
||||
@@ -510,6 +592,10 @@ class RealsenseColorPub(TolerantNode):
|
||||
pipe = rs.pipeline()
|
||||
if profile is not None:
|
||||
break
|
||||
if profile is not None:
|
||||
break
|
||||
if profile is not None:
|
||||
break
|
||||
if profile is None:
|
||||
raise RuntimeError(
|
||||
f'{self._name} 没有可用的彩色配置。最后的错误：{last_err}'
|
||||
@@ -520,8 +606,9 @@ class RealsenseColorPub(TolerantNode):
|
||||
try:
|
||||
sensor = profile.get_device().first_color_sensor()
|
||||
if sensor.supports(rs.option.frames_queue_size):
|
||||
# 队列只留最新一张，避免越积越慢，动起来画面还停在旧帧。
|
||||
sensor.set_option(rs.option.frames_queue_size, 1)
|
||||
# SDK buffer pool must accommodate frames retained by alignment.
|
||||
# The application mailbox still keeps only the latest frame.
|
||||
sensor.set_option(rs.option.frames_queue_size, 4)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
self._have_depth = have_depth
|
||||
@@ -744,6 +831,8 @@ class RealsenseColorPub(TolerantNode):
|
||||
def _publish_loop(self) -> None:
|
||||
got = False
|
||||
while self._running and rclpy.ok():
|
||||
# Release SDK buffers before waiting; idle references must not starve capture.
|
||||
item = frames = aligned = depth = color = ir_l = ir_r = depth_u16 = None
|
||||
if self._pipe is None:
|
||||
if not self._reconnect():
|
||||
time.sleep(2.0)
|
||||
@@ -960,6 +1049,10 @@ def main(args=None) -> None:
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
# Supervisors may send SIGINT through both parent and child. Do not let
|
||||
# a second interrupt skip DDS cleanup and leave ghost publishers behind.
|
||||
import signal
|
||||
signal.signal(signal.SIGINT, signal.SIG_IGN)
|
||||
node._running = False
|
||||
if node._pipe is not None:
|
||||
try:
|
||||
|
||||
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env python3
|
||||
"""通过 ``/follow_joint_trajectory`` 发送多点关节轨迹（真机 + mock 都适用）。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.node import Node
|
||||
|
||||
from arm_application.site_io import joint_prefix_default
|
||||
from arm_msgs.action import FollowJointTrajectory
|
||||
|
||||
from arm_application.ros_node import TolerantNode
|
||||
|
||||
DEFAULT_JOINTS = [
|
||||
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
|
||||
]
|
||||
NUM_JOINTS = 7
|
||||
# 扁平化存放的位置（3 个点 x 7 个关节）+ 时间 —— ROS 参数不支持列表套字典
|
||||
DEFAULT_TIMES = [3.0, 6.0, 9.0]
|
||||
DEFAULT_POSITIONS = [
|
||||
0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0,
|
||||
0.35, -0.698, 0.35, 1.57, 0.0, 0.698, 0.0,
|
||||
0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0,
|
||||
]
|
||||
|
||||
|
||||
class RunJointTrajectory(TolerantNode):
|
||||
def __init__(self) -> None:
|
||||
super().__init__('run_joint_trajectory')
|
||||
self.declare_parameter('joint_prefix', joint_prefix_default())
|
||||
self.declare_parameter('joint_names', DEFAULT_JOINTS)
|
||||
self.declare_parameter('velocity_scale', 0.15)
|
||||
self.declare_parameter('action_name', 'follow_joint_trajectory')
|
||||
self.declare_parameter('waypoint_times', DEFAULT_TIMES)
|
||||
self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
|
||||
# 1 = 只跑一次（默认）；0 = 循环直到 Ctrl+C；N = 重复 N 次
|
||||
self.declare_parameter('loop_count', 1)
|
||||
self.declare_parameter('pause_between_loops', 0.5)
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
self.get_parameter('joint_names').get_parameter_value().string_array_value
|
||||
or DEFAULT_JOINTS
|
||||
)
|
||||
self.joint_names = [f'{prefix}{n}' for n in base]
|
||||
self.velocity_scale = float(self.get_parameter('velocity_scale').value)
|
||||
action = str(self.get_parameter('action_name').value)
|
||||
self._action_name = action
|
||||
self._client = ActionClient(self, FollowJointTrajectory, action)
|
||||
|
||||
def run(self) -> int:
|
||||
if not self._client.wait_for_server(timeout_sec=15.0):
|
||||
self.get_logger().error(
|
||||
f'Action [{self._action_name}] 不可用。'
|
||||
'请先启动真机栈：bash scripts/run_real.sh'
|
||||
)
|
||||
return 1
|
||||
|
||||
times = list(self.get_parameter('waypoint_times').value)
|
||||
positions = list(self.get_parameter('waypoint_positions').value)
|
||||
n_j = len(self.joint_names)
|
||||
n_pts = len(times)
|
||||
if n_pts < 1:
|
||||
self.get_logger().error('waypoint_times 为空')
|
||||
return 1
|
||||
if len(positions) != n_pts * n_j:
|
||||
self.get_logger().error(
|
||||
f'waypoint_positions 长度 {len(positions)} != '
|
||||
f'{n_pts} 个点 x {n_j} 个关节 = {n_pts * n_j}'
|
||||
)
|
||||
return 1
|
||||
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.joint_names = self.joint_names
|
||||
goal.num_points = n_pts
|
||||
goal.velocity_scale = self.velocity_scale
|
||||
goal.positions = positions
|
||||
goal.time_from_start = [float(t) for t in times]
|
||||
|
||||
loop_count = int(self.get_parameter('loop_count').value)
|
||||
pause = float(self.get_parameter('pause_between_loops').value)
|
||||
duration = goal.time_from_start[-1]
|
||||
if loop_count == 0:
|
||||
self.get_logger().info(
|
||||
f'循环模式：{goal.num_points} 个点，约 {duration:.1f}s/圈 '
|
||||
f'（Ctrl+C 停止）'
|
||||
)
|
||||
elif loop_count > 1:
|
||||
self.get_logger().info(
|
||||
f'循环模式：{loop_count} 圈，{goal.num_points} 个点，'
|
||||
f'约 {duration:.1f}s/圈'
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f'发送 {goal.num_points} 个点，v_scale={goal.velocity_scale}，'
|
||||
f'时长={duration:.1f}s'
|
||||
)
|
||||
|
||||
iteration = 0
|
||||
while rclpy.ok():
|
||||
iteration += 1
|
||||
if loop_count > 1:
|
||||
self.get_logger().info(f'第 {iteration}/{loop_count} 圈')
|
||||
|
||||
code = self._send_once(goal, duration)
|
||||
if code != 0:
|
||||
return code
|
||||
|
||||
if loop_count == 1:
|
||||
self.get_logger().info('完成：OK')
|
||||
return 0
|
||||
if loop_count > 1 and iteration >= loop_count:
|
||||
self.get_logger().info(f'完成：已跑完 {loop_count} 圈')
|
||||
return 0
|
||||
if pause > 0:
|
||||
time.sleep(pause)
|
||||
|
||||
def _send_once(self, goal: FollowJointTrajectory.Goal, duration: float) -> int:
|
||||
send = self._client.send_goal_async(goal)
|
||||
deadline = time.monotonic() + 10.0
|
||||
while rclpy.ok() and not send.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
if time.monotonic() > deadline:
|
||||
self.get_logger().error('发送目标超时')
|
||||
return 2
|
||||
handle = send.result()
|
||||
if handle is None or not handle.accepted:
|
||||
self.get_logger().error('目标被拒绝')
|
||||
return 3
|
||||
|
||||
res_fut = handle.get_result_async()
|
||||
deadline = time.monotonic() + max(120.0, duration * 3.0)
|
||||
next_log = time.monotonic() + 10.0
|
||||
while rclpy.ok() and not res_fut.done():
|
||||
rclpy.spin_once(self, timeout_sec=0.05)
|
||||
now = time.monotonic()
|
||||
if now >= next_log:
|
||||
left = max(0.0, deadline - now)
|
||||
self.get_logger().info(
|
||||
f'正在执行轨迹（计划约 {duration:.0f}s），'
|
||||
f'等待机器人……（剩余超时 {left:.0f}s）'
|
||||
)
|
||||
next_log = now + 10.0
|
||||
if now > deadline:
|
||||
self.get_logger().error('等待结果超时')
|
||||
return 4
|
||||
result = res_fut.result().result
|
||||
if not result.success:
|
||||
self.get_logger().error(f'失败：{result.message}')
|
||||
return 5
|
||||
self.get_logger().info(f'本圈结束：{result.message or "OK"}')
|
||||
return 0
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = RunJointTrajectory()
|
||||
code = 1
|
||||
try:
|
||||
code = node.run()
|
||||
except KeyboardInterrupt:
|
||||
code = 0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -50,6 +50,9 @@ from arm_application.geom_ik import (
|
||||
)
|
||||
from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
|
||||
from arm_application.overhead_oneshot import (
|
||||
D435_MOVEL_GAVE_UP,
|
||||
D435_MOVEL_RETRY,
|
||||
D435_MOVEL_RETRY_S,
|
||||
D435_NEAR_CRUISE_MPS,
|
||||
D435_SETTLE_S,
|
||||
D435_VENDOR_MOVEL_VEL_MPS,
|
||||
@@ -126,7 +129,7 @@ class ServoNode(TolerantNode):
|
||||
self.declare_parameter("point_topic", "visual_servo/filtered_point")
|
||||
# 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。
|
||||
# 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。
|
||||
self.declare_parameter("handeye_file", "handeye/results/89_eye_in_hand.yaml")
|
||||
self.declare_parameter("handeye_file", "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml")
|
||||
# eye_in_hand = cam→法兰；eye_to_hand = cam→基座。写错会把固定相机当腕部用。
|
||||
self.declare_parameter("handeye_mode", "eye_in_hand")
|
||||
self.declare_parameter("use_static_handeye_tf", False)
|
||||
@@ -224,8 +227,7 @@ class ServoNode(TolerantNode):
|
||||
self.declare_parameter("lock_hold_sec", 2.0)
|
||||
# D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
|
||||
# lock_still_sec 仍须从 site 接上（缺了就报错）。
|
||||
# 交回 D405：进程内 MoveL 到位后，再叠 d435_handback_sec（法兰 XY 停稳）。
|
||||
# 旧杀栈冷启动曾把这 3s 垫在控制器重启后面；不杀栈后只留机械停稳。
|
||||
# 厂家 MoveL 结束后，法兰连续停稳 d435_handback_sec 才打开 D405 框心。
|
||||
self.declare_parameter("lock_still_sec", -1.0)
|
||||
self.declare_parameter("d435_handback_sec", -1.0)
|
||||
self.declare_parameter("wrist_align_sec", -1.0)
|
||||
@@ -329,7 +331,7 @@ class ServoNode(TolerantNode):
|
||||
and self.wrist_handoff_conf >= 0.0
|
||||
and self.overhead_hold_s >= 0.0
|
||||
)
|
||||
# 框心对准进对孔用 wrist_align_sec。D435 交回只用 d435_handback_sec，两段不要合成一个数。
|
||||
# 框心对准进精拍用 wrist_align_sec。D435 交接只用 d435_handback_sec，两段不要合成一个数。
|
||||
self._still = StillLock(
|
||||
still_sec=max(0.0, self.wrist_align_sec),
|
||||
still_move_m=max(0.0, self.lock_still_move_m),
|
||||
@@ -342,7 +344,8 @@ class ServoNode(TolerantNode):
|
||||
self._d435_vendor_done = False
|
||||
self._d435_vendor_busy = False
|
||||
self._d435_vendor_failed = False
|
||||
self._d435_joint_fallback = False
|
||||
self._d435_vendor_tries = 0
|
||||
self._d435_retry_after = 0.0
|
||||
self._d435_settled = False
|
||||
self._d435_exec_until = 0.0
|
||||
self._d435_pinned = False
|
||||
@@ -375,10 +378,9 @@ class ServoNode(TolerantNode):
|
||||
self.ws_min = np.array([float(v) for v in ws_min[:3]], dtype=float)
|
||||
self.ws_max = np.array([float(v) for v in ws_max[:3]], dtype=float)
|
||||
|
||||
from arm_application.site_io import resolve_repo_path
|
||||
|
||||
handeye_path = resolve_repo_path(str(self.get_parameter("handeye_file").value))
|
||||
self.T_cam_to_gripper = load_handeye_T(handeye_path)
|
||||
self.T_cam_to_gripper = load_handeye_T(
|
||||
str(self.get_parameter("handeye_file").value)
|
||||
)
|
||||
self.handeye_mode = str(
|
||||
self.get_parameter("handeye_mode").value or "eye_in_hand"
|
||||
).strip().lower()
|
||||
@@ -401,7 +403,7 @@ class ServoNode(TolerantNode):
|
||||
t_he = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float)
|
||||
axis_z = float(np.clip(R_he[2, 2], -1.0, 1.0))
|
||||
self._cam_tilt_deg = float(np.degrees(np.arccos(axis_z)))
|
||||
he_path = handeye_path
|
||||
he_path = str(self.get_parameter("handeye_file").value)
|
||||
if self.handeye_mode == "eye_to_hand":
|
||||
self.get_logger().warn(
|
||||
f"HANDEYE yaml={he_path} mode=eye_to_hand "
|
||||
@@ -710,7 +712,8 @@ class ServoNode(TolerantNode):
|
||||
self._d435_vendor_done = False
|
||||
self._d435_vendor_busy = False
|
||||
self._d435_vendor_failed = False
|
||||
self._d435_joint_fallback = False
|
||||
self._d435_vendor_tries = 0
|
||||
self._d435_retry_after = 0.0
|
||||
self._overhead_t = now_m
|
||||
self._last_seen_time = now_m
|
||||
self._stamp_sec = now_sec
|
||||
@@ -734,7 +737,8 @@ class ServoNode(TolerantNode):
|
||||
self._d435_vendor_done = False
|
||||
self._d435_vendor_busy = False
|
||||
self._d435_vendor_failed = False
|
||||
self._d435_joint_fallback = False
|
||||
self._d435_vendor_tries = 0
|
||||
self._d435_retry_after = 0.0
|
||||
self._publish_d435_commanding(True)
|
||||
self.get_logger().info(
|
||||
f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
|
||||
@@ -762,9 +766,9 @@ class ServoNode(TolerantNode):
|
||||
settled=settled,
|
||||
move_only=self.d435_move_only,
|
||||
):
|
||||
# 推理服务不关。这一帧的点不下发给臂，目标仍是 D435 那一个点。
|
||||
# 推理不关。没到「法兰停稳」之前，这帧不能进运动控制。
|
||||
self.get_logger().info(
|
||||
"D435 位置还没到并且停稳，D405 这帧不下发",
|
||||
"D405 输出已截断：D435 厂家移动还没结束或法兰还没停稳",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
return
|
||||
@@ -1106,12 +1110,6 @@ class ServoNode(TolerantNode):
|
||||
elif self.ee_z_m > 0.0:
|
||||
ex = ey = 0.0
|
||||
r_xy_raw = 0.0
|
||||
# 还没到锁高、XY 未解锁时，误差被清成 0 只是为了先别动。
|
||||
# 不能据此把死区标志打成真，否则下一拍会把“还没对准”当成已对准。
|
||||
if self.ee_z_m > 0.0 and not self._xy_armed:
|
||||
xy_dead = True
|
||||
self._xy_in_dead = False
|
||||
else:
|
||||
xy_dead = self._xy_deadzone(r_xy_raw)
|
||||
clip_xy = max(0.03, float(self.e_xy_clip))
|
||||
r_xy = r_xy_raw
|
||||
@@ -1416,9 +1414,12 @@ class ServoNode(TolerantNode):
|
||||
return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
|
||||
|
||||
def _d435_oneshot_holds_the_arm(self) -> bool:
|
||||
"""D435 到点平移：action 短跳，等结果再重测。D405 主跟踪仍走原来的伺服。"""
|
||||
if self.d435_move_only and self._hybrid and self._track_source == "overhead" and self._d435_settled:
|
||||
# 位移测完了：守着，不许掉进 D405 的 _execute_twist。hold 只在到位那一拍发一次。
|
||||
"""D435 粗定位只走厂家 MoveL。停稳后先钉住，等 D405 框心接上再放开。
|
||||
|
||||
这段不许再用流式伺服去追 D435 那个点。D405 点被 ``_on_point`` 接上后
|
||||
跟踪源变成腕部，这里返回 False，框心才开始带臂。
|
||||
"""
|
||||
if self._hybrid and self._track_source == "overhead" and self._d435_settled:
|
||||
return True
|
||||
if not (self._hybrid and self._track_source == "overhead" and not self._d435_settled):
|
||||
return False
|
||||
@@ -1528,7 +1529,7 @@ class ServoNode(TolerantNode):
|
||||
"""停或重新打开关节轨迹控制器。关节状态广播留着，硬件接口不下线。
|
||||
|
||||
用节点里的客户端，不走 ``ros2 service call``：那条命令自己就要一两秒，
|
||||
交回 D405 的预算会被它吃掉。
|
||||
停稳锁定的预算会被它吃掉。
|
||||
"""
|
||||
from controller_manager_msgs.srv import SwitchController
|
||||
|
||||
@@ -1663,28 +1664,37 @@ class ServoNode(TolerantNode):
|
||||
return bool(ok)
|
||||
|
||||
def _d435_oneshot_holds_locked(self) -> bool:
|
||||
# 优先厂家 MoveL。机器人拒绝下发时改走关节轨迹，否则会停在原地，
|
||||
# 腕部又看不见，跟踪就再也不动。
|
||||
if self._d435_vendor_busy:
|
||||
# 混合流程这一段只走厂家 MoveL。下面留下的关节折线不再执行：
|
||||
# 那条路径会在真机上一段一段启停。
|
||||
if self._d435_vendor_failed or self._d435_vendor_busy:
|
||||
return True
|
||||
if self._d435_joint_fallback:
|
||||
return self._d435_oneshot_holds_locked_joint()
|
||||
if not self._d435_vendor_done:
|
||||
# busy 先于调用置位：另一条控制回调不能再开一次 MoveL，也不能提前交回 D405。
|
||||
if time.monotonic() < float(self._d435_retry_after):
|
||||
return True
|
||||
if int(self._d435_vendor_tries) >= int(D435_MOVEL_RETRY):
|
||||
self._d435_vendor_failed = True
|
||||
self.get_logger().error(
|
||||
f"{D435_MOVEL_GAVE_UP} —— 一阶段中止，D405 也不带臂"
|
||||
)
|
||||
return True
|
||||
# busy 先于调用置位：另一条控制回调不能再开一次 MoveL，也不能提前锁定。
|
||||
self._d435_vendor_busy = True
|
||||
ok = self._d435_vendor_movel()
|
||||
self._d435_vendor_busy = False
|
||||
if not ok:
|
||||
self._d435_vendor_failed = True
|
||||
self._d435_vendor_done = True
|
||||
self._d435_joint_fallback = True
|
||||
self._arrive.reset()
|
||||
self._d435_vendor_tries = int(self._d435_vendor_tries) + 1
|
||||
self._d435_retry_after = time.monotonic() + float(D435_MOVEL_RETRY_S)
|
||||
self.get_logger().error(
|
||||
"厂家 MoveL 没走成 —— 改用关节轨迹走到顶视点，高度保持锁高"
|
||||
f"厂家 MoveL 第{self._d435_vendor_tries}次没走成，"
|
||||
f"{D435_MOVEL_RETRY_S:.1f}s 后再试"
|
||||
)
|
||||
return self._d435_oneshot_holds_locked_joint()
|
||||
else:
|
||||
self._d435_vendor_done = True
|
||||
self._arrive.reset()
|
||||
# MoveL 切回关节后 JTC 还握着出发前的悬停。不钉住的话
|
||||
# 硬件 3 s 窗口一过，臂会按悬停关节走回去（比交回 D405 的 10 cm 更远）。
|
||||
self._send_hold()
|
||||
self._d435_pinned = True
|
||||
return True
|
||||
return True
|
||||
|
||||
@@ -1743,7 +1753,7 @@ class ServoNode(TolerantNode):
|
||||
):
|
||||
self._d435_pin_and_stop_hops(
|
||||
f"D435 上一跳几乎没走近，剩差还是 {dist*1000:.0f}mm "
|
||||
f"—— 钉住不再补刀，等停稳后交回 D405"
|
||||
f"—— 钉住不再补刀，等停稳后锁定进二阶段"
|
||||
)
|
||||
return True
|
||||
if not at:
|
||||
@@ -1850,7 +1860,7 @@ class ServoNode(TolerantNode):
|
||||
return True
|
||||
|
||||
def _tick_d435_arrive(self) -> None:
|
||||
"""走到 D435 那个点，并且法兰按一阶段的静止判据停稳，才允许下发 D405。"""
|
||||
"""厂家 MoveL 已结束：看法兰还在不在动。连续停稳后才打开 D405 框心。"""
|
||||
if not self._hybrid:
|
||||
return
|
||||
if (
|
||||
@@ -1872,11 +1882,15 @@ class ServoNode(TolerantNode):
|
||||
return
|
||||
xy = np.asarray(T[:2, 3], dtype=float)
|
||||
dist = float(np.hypot(float(xy[0]) - goal[0], float(xy[1]) - goal[1]))
|
||||
arrived = dist <= float(self.lock_still_move_m) or self._d435_give_up
|
||||
settled = self._arrive.update(
|
||||
xy, time.monotonic(), tracking_ok=arrived
|
||||
)
|
||||
# 到位看法兰是否还在动，不看离视觉点还差几毫米。
|
||||
settled = self._arrive.update(xy, time.monotonic(), tracking_ok=True)
|
||||
if not settled:
|
||||
self.get_logger().info(
|
||||
f"D435 厂家 MoveL 已结束，法兰还在动或未满 "
|
||||
f"{self.d435_handback_sec:.1f}s（离点 {dist*1000:.0f}mm），"
|
||||
"D405 输出仍截断",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
return
|
||||
with self._lock:
|
||||
self._d435_settled = True
|
||||
@@ -1899,10 +1913,12 @@ class ServoNode(TolerantNode):
|
||||
self._lock_pub_t = now_m
|
||||
self._lock_pub.publish(Bool(data=True))
|
||||
return
|
||||
# 法兰已停稳：打开 D405。不在这里锁定；框心对准停稳后才精拍下臂。
|
||||
self._publish_d435_commanding(False)
|
||||
self.get_logger().info(
|
||||
f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
|
||||
f"{self.d435_handback_sec:.1f}s —— 改回下发 D405 框心"
|
||||
f"法兰已连续停稳 {self.d435_handback_sec:.1f}s"
|
||||
f"（离 D435 点 {dist*1000:.0f}mm）"
|
||||
" —— 打开 D405 输出，追工件框矩形中心；对准后才精拍下臂"
|
||||
)
|
||||
|
||||
def _tick_still_lock(self) -> None:
|
||||
@@ -1922,15 +1938,12 @@ class ServoNode(TolerantNode):
|
||||
lost = self._target_lost_flag
|
||||
source = self._track_source
|
||||
conf = self._wrist_conf
|
||||
# 没进死区不算对准。只看“有目标且臂没动”会在刚看见工件、
|
||||
# 还差一百多毫米时就把一阶段锁死。
|
||||
tracking_ok = (
|
||||
source == "wrist"
|
||||
and have
|
||||
and not lost
|
||||
and (not wrist_needs_handoff(conf, self.wrist_handoff_conf))
|
||||
and self._xy_armed
|
||||
and self._xy_in_dead
|
||||
)
|
||||
locked_now = self._still.update(
|
||||
xy, time.monotonic(), tracking_ok=tracking_ok
|
||||
|
||||
@@ -63,12 +63,15 @@ from arm_application.overhead_oneshot import (
|
||||
decompose_base_xy,
|
||||
flange_z_for_gap_m,
|
||||
final_gap_refusal,
|
||||
hole_entry_refusal,
|
||||
insert_target_gap_m,
|
||||
hole_pair_span_m,
|
||||
hole_pair_span_ok,
|
||||
once_ff_movel_from_tcp,
|
||||
once_ff_tcp_xyz,
|
||||
oneshot_align_step_m,
|
||||
pixel_on_base_plane,
|
||||
retract_goal_z,
|
||||
)
|
||||
from arm_application.handeye_realsense import resolve_realsense_topics
|
||||
from arm_application.gauge_boot import perception_source_of
|
||||
@@ -249,7 +252,7 @@ class Stage2Locate(TolerantNode):
|
||||
# _coarse_target_reject / _bbox_on_border 的注释）：粗居中的目的是把臂
|
||||
# 挪到工件正上方，**挪错比不挪危险得多** —— 一挪错精定位就再也看不到
|
||||
# k1/k2，而流程会把它当"孔检测失败"，现场根本想不到是粗居中把臂带走的。
|
||||
self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.12)
|
||||
self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.08)
|
||||
self.coarse_border_px = int(stage2.get("coarse_border_px") or 2)
|
||||
# 最后轻降到孔面上方 final_gap_mm（不接触）。启动参数 ≥0 覆盖 site。
|
||||
self._final_gap_refused = ""
|
||||
@@ -259,6 +262,8 @@ class Stage2Locate(TolerantNode):
|
||||
self.final_gap_m = _gap_override_mm / 1000.0
|
||||
else:
|
||||
self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
|
||||
# 缺键 = 不进孔（停在面上）。进孔必须在 site 里写明，不许默认钻进去。
|
||||
self.hole_entry_m = float(stage2.get("hole_entry_mm") or 0.0) / 1000.0
|
||||
# 观测下降：对准后降到这个间隙就停，方便看规尖是不是对着孔。
|
||||
# 比 final_gap（贴面 2mm）高一截，对偏了也不会把规压上孔沿。
|
||||
self.observe_gap_m = float(stage2.get("observe_gap_mm") or 15.0) / 1000.0
|
||||
@@ -324,7 +329,8 @@ class Stage2Locate(TolerantNode):
|
||||
"（这次没写 --hover-z，沿用 site 的几何抬高）"
|
||||
)
|
||||
self.hover_z = _used
|
||||
# 轻降**最多能降多少**：从悬停高度降到"规尖停在工件面上方 final_gap"为止。
|
||||
self.insert_gap_m = insert_target_gap_m(self.final_gap_m, self.hole_entry_m)
|
||||
# 轻降**最多能降多少**：从悬停高度降到插孔目标（面上 final_gap，或面下 hole_entry）。
|
||||
# 这是纯几何量，与深度测量无关 —— 测量的唯一职责是让下降**提前**收工，
|
||||
# 绝不允许它把下降量**放大**（2026-09-29 实测就是这么撞的：测量出的
|
||||
# gap0=141mm 比几何真值大 ~86mm，于是多降 86mm 把规撞进孔）。
|
||||
@@ -332,7 +338,7 @@ class Stage2Locate(TolerantNode):
|
||||
self.descent_budget_m: Optional[float] = None
|
||||
if _tool_len > 0.0 and _surface_z > 0.0:
|
||||
self.descent_budget_m = max(
|
||||
0.0, self.hover_z - _tool_len - _surface_z - self.final_gap_m
|
||||
0.0, self.hover_z - _tool_len - _surface_z - self.insert_gap_m
|
||||
)
|
||||
# 轻降结束后**按几何**算出的真实间隙 [m]，供收尾日志如实汇报（不是把目标值
|
||||
# 当结果念一遍 —— 那会让"停在离面 50mm"也显示成"~2.0mm"，R1.1 的静默失效）。
|
||||
@@ -368,6 +374,17 @@ class Stage2Locate(TolerantNode):
|
||||
if _why_gap:
|
||||
self._final_gap_refused = _why_gap
|
||||
self.get_logger().error(_why_gap)
|
||||
else:
|
||||
_why_entry = hole_entry_refusal(
|
||||
self.hole_entry_m,
|
||||
_tool_len,
|
||||
_surface_z,
|
||||
float(self.ws_min[2]),
|
||||
float(self.ws_max[2]),
|
||||
)
|
||||
if _why_entry:
|
||||
self._final_gap_refused = _why_entry
|
||||
self.get_logger().error(_why_entry)
|
||||
self.vert_tol_rad = math.radians(float(stage2.get("verticalize_tol_deg") or 0.8))
|
||||
self.settle_sec = float(stage2.get("settle_sec") or 0.45)
|
||||
self.snap_frames = int(stage2.get("snap_frames") or 5)
|
||||
@@ -413,8 +430,6 @@ class Stage2Locate(TolerantNode):
|
||||
self._hole_cfg = config_from_site(site)
|
||||
self._weights = weights_from_site(site)
|
||||
self._weights_fb = str(paths.get("yolo_weights") or "")
|
||||
# 和感知节点同一处：site 写了 onnx 就不要再被 auto 改去加载 .engine。
|
||||
self._yolo_backend = str((site.get("yolo") or {}).get("backend") or "auto").strip().lower()
|
||||
|
||||
self.joint_names = [f"{self.prefix}joint{i}" for i in range(1, 8)]
|
||||
self.link_frames = [f"{self.prefix}link{i}" for i in range(1, 8)]
|
||||
@@ -491,14 +506,17 @@ class Stage2Locate(TolerantNode):
|
||||
f"hole_keypoint_once={self.hole_keypoint_once} "
|
||||
f"lock_xy_descend={self.hole_once_lock_xy_descend} "
|
||||
f"ff_look={self.once_ff_look_per_m:.3f} "
|
||||
f"final_gap={self.final_gap_m * 1000:.1f}mm"
|
||||
f"final_gap={self.final_gap_m * 1000:.1f}mm "
|
||||
f"hole_entry={self.hole_entry_m * 1000:.1f}mm "
|
||||
f"插孔间隙={self.insert_gap_m * 1000:.1f}mm"
|
||||
+ (
|
||||
f"→法兰Z={flange_z_for_gap_m(self.tool_tip_below_flange_m, self.surface_z_m, self.final_gap_m):.4f}m"
|
||||
f"→法兰Z={flange_z_for_gap_m(self.tool_tip_below_flange_m, self.surface_z_m, self.insert_gap_m):.4f}m"
|
||||
if self.tool_tip_below_flange_m > 0.0 and self.surface_z_m > 0.0
|
||||
else ""
|
||||
)
|
||||
+ "\n"
|
||||
f" 工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖，下臂不得低于面+间隙）"
|
||||
f" 工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖；"
|
||||
"hole_entry>0 时规尖停在上表面以下）"
|
||||
)
|
||||
if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
|
||||
k = float(self.once_ff_look_per_m)
|
||||
@@ -678,7 +696,18 @@ class Stage2Locate(TolerantNode):
|
||||
t0 = time.monotonic()
|
||||
while not fut.done() and time.monotonic() - t0 < 4.0:
|
||||
self._spin(0.05)
|
||||
handle = fut.result() if fut.done() else None
|
||||
if not fut.done():
|
||||
# An unanswered goal can still be accepted later. Never resend it.
|
||||
def cancel_late(reply):
|
||||
try:
|
||||
late = reply.result()
|
||||
if late is not None and late.accepted:
|
||||
late.cancel_goal_async()
|
||||
except Exception:
|
||||
pass
|
||||
fut.add_done_callback(cancel_late)
|
||||
raise RuntimeError("JTC 接单反馈超时，禁止重发；迟到的任务将请求取消")
|
||||
handle = fut.result()
|
||||
if handle is not None and bool(getattr(handle, "accepted", False)):
|
||||
break
|
||||
self.get_logger().warn(
|
||||
@@ -692,40 +721,23 @@ class Stage2Locate(TolerantNode):
|
||||
self.get_logger().error("JTC 拒绝了轨迹（控制器未激活？先跑 T1 run_real.sh）")
|
||||
return False
|
||||
res = handle.get_result_async()
|
||||
dur = 0.0
|
||||
if traj.points:
|
||||
stamp = traj.points[-1].time_from_start
|
||||
dur = float(stamp.sec) + float(stamp.nanosec) * 1e-9
|
||||
t0 = time.monotonic()
|
||||
early = False
|
||||
while time.monotonic() - t0 < wait_s:
|
||||
while not res.done() and time.monotonic() - t0 < wait_s:
|
||||
self._spin(0.05)
|
||||
elapsed = time.monotonic() - t0
|
||||
# 同一个动作名会被发现成两份服务。提前回来的那份 success
|
||||
# 不是臂走完了；这时再发下一跳，会把还在执行的轨迹取消掉。
|
||||
if res.done() and elapsed + 0.05 < dur:
|
||||
if not early:
|
||||
early = True
|
||||
self.get_logger().warn(
|
||||
f"JTC 结果比轨迹提前 {dur - elapsed:.2f}s 回来，"
|
||||
"继续等到轨迹时间走完"
|
||||
)
|
||||
continue
|
||||
if res.done():
|
||||
break
|
||||
if not res.done():
|
||||
self.get_logger().warn("JTC 结果超时——按 TF 继续核验")
|
||||
return False
|
||||
handle.cancel_goal_async()
|
||||
raise RuntimeError("JTC 执行结果超时，已请求取消；中止当前流程，不继续下发")
|
||||
try:
|
||||
r = res.result()
|
||||
inner = getattr(r, "result", None)
|
||||
code = getattr(inner, "error_code", 0) if inner is not None else 0
|
||||
if code not in (0, None):
|
||||
code = getattr(inner, "error_code", None)
|
||||
if getattr(r, "status", None) != 4 or code != 0:
|
||||
msg = getattr(inner, "error_string", "")
|
||||
self.get_logger().error(f"JTC 执行失败 error_code={code} {msg}")
|
||||
return False
|
||||
except Exception:
|
||||
pass
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"JTC 结果读取失败：{exc}")
|
||||
return False
|
||||
return True
|
||||
|
||||
def _clamp_ws(self, target: np.ndarray) -> np.ndarray:
|
||||
@@ -921,10 +933,13 @@ class Stage2Locate(TolerantNode):
|
||||
min_dur: float = 1.6,
|
||||
hold_xy: bool = False,
|
||||
strict_xy: bool = False,
|
||||
z_up_only: bool = False,
|
||||
) -> Tuple[bool, np.ndarray, np.ndarray]:
|
||||
"""闭环点到点：规划短 hop → 执行 → 重新看 TF → 重复，直到容差内。
|
||||
|
||||
hold_xy=True：只收 Z/姿态（悬停降锁高），不把下降中漂出的 XY 再拽回去。
|
||||
z_up_only=True：当前已经高于目标 Z 就停，不许为了追 5 mm 往下补
|
||||
（贴面 MoveL 后收臂 hop1 抬过头、hop2 回压进孔，2026-10-16）。
|
||||
"""
|
||||
target = self._clamp_ws(target)
|
||||
last_dist: Optional[float] = None
|
||||
@@ -932,7 +947,14 @@ class Stage2Locate(TolerantNode):
|
||||
stall = 0
|
||||
for hop in range(max_hops):
|
||||
p = Tee[:3, 3]
|
||||
goal = np.array([p[0], p[1], target[2]]) if hold_xy else target
|
||||
z_tgt = float(target[2])
|
||||
if z_up_only:
|
||||
if float(p[2]) >= z_tgt - float(tol_m):
|
||||
return True, q, Tee
|
||||
z_tgt = retract_goal_z(float(p[2]), z_tgt)
|
||||
goal = np.array([p[0], p[1], z_tgt]) if hold_xy else np.array(
|
||||
[target[0], target[1], z_tgt], dtype=float
|
||||
)
|
||||
if hold_xy:
|
||||
goal = self._clamp_ws(goal)
|
||||
dp = goal - p
|
||||
@@ -1208,7 +1230,7 @@ class Stage2Locate(TolerantNode):
|
||||
# ultralytics 解不了 pose 的 TensorRT engine（见 pose_detector 模块头），
|
||||
# OnnxWorkpieceDetector 同样只认检测输出。
|
||||
det = build_detector(
|
||||
resolve_weights(w, self._yolo_backend),
|
||||
resolve_weights(w, "auto"),
|
||||
conf=conf,
|
||||
iou=iou,
|
||||
imgsz=imgsz,
|
||||
@@ -1223,15 +1245,13 @@ class Stage2Locate(TolerantNode):
|
||||
)
|
||||
else:
|
||||
det = YoloDetector(
|
||||
resolve_weights(w, self._yolo_backend),
|
||||
resolve_weights(w, "auto"),
|
||||
device="cpu",
|
||||
conf=conf,
|
||||
iou=iou,
|
||||
imgsz=imgsz,
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"工件检测器已加载: {w} backend={self._yolo_backend}"
|
||||
)
|
||||
self.get_logger().info(f"工件检测器已加载: {w}")
|
||||
return det
|
||||
|
||||
def _det(self):
|
||||
@@ -1513,14 +1533,15 @@ class Stage2Locate(TolerantNode):
|
||||
|
||||
1. 目标点落在 ``workspace_xyz_*`` 的 XY 之外（含 5mm 余量）—— 一阶段刚锁
|
||||
定时臂就在工件上方，工件**不该**在盒外；
|
||||
2. 目标点离当前夹爪轴线超过 ``coarse_max_offset_m``（缺省 0.12m）—— 粗居中
|
||||
是"把臂挪到工件正上方"，合理位移是厘米级；十几厘米只可能是坏读数。
|
||||
2. 目标点离当前夹爪轴线超过 ``coarse_max_offset_m``（缺省 0.08m）—— 粗居中
|
||||
是"把臂挪到工件正上方"，合理位移是厘米级；十几厘米只可能是坏读数
|
||||
（2026-10-16 三次 110mm 回撤，0.12 放行了）。
|
||||
|
||||
注意：判误检后**原地不动**，由精定位（直接量 k1/k2）继续。不动的代价只是
|
||||
"少挪一次"，而挪错的代价是精定位彻底看不到孔 —— 所以这里必须选不动。
|
||||
"""
|
||||
if max_offset_m is None:
|
||||
max_offset_m = float(getattr(self, "coarse_max_offset_m", 0.12))
|
||||
max_offset_m = float(getattr(self, "coarse_max_offset_m", 0.08))
|
||||
p = np.asarray(p_base, dtype=float).reshape(3)
|
||||
if not np.all(np.isfinite(p[:2])):
|
||||
return "目标点非有限值（深度/三角化失败）"
|
||||
@@ -2130,8 +2151,13 @@ class Stage2Locate(TolerantNode):
|
||||
)
|
||||
return q, Tee, True
|
||||
|
||||
def _vendor_movel_tcp(self, xyz, vel: float) -> bool:
|
||||
"""进程内厂家 MoveL：TCP 目标，不杀控制栈。"""
|
||||
def _vendor_movel_tcp(
|
||||
self, xyz, vel: float, zone: str = "", toler: int = -1,
|
||||
) -> bool:
|
||||
"""进程内厂家 MoveL：TCP 目标，不杀控制栈。
|
||||
|
||||
``zone`` 空着用厂家默认过渡区。微升降传 ``ZFine`` 和到位等级 1。
|
||||
"""
|
||||
lib = ros2_root() / "scripts" / "lib"
|
||||
if str(lib) not in sys.path:
|
||||
sys.path.insert(0, str(lib))
|
||||
@@ -2139,7 +2165,8 @@ class Stage2Locate(TolerantNode):
|
||||
|
||||
seq = int(time.monotonic() * 1000.0) % 1_000_000_000
|
||||
write_request(
|
||||
seq, float(xyz[0]), float(xyz[1]), float(xyz[2]), False, float(vel)
|
||||
seq, float(xyz[0]), float(xyz[1]), float(xyz[2]), False, float(vel),
|
||||
zone, toler,
|
||||
)
|
||||
t0 = time.monotonic()
|
||||
acked = False
|
||||
@@ -2263,7 +2290,9 @@ class Stage2Locate(TolerantNode):
|
||||
)
|
||||
fl0 = (float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]))
|
||||
ok = self._vendor_movel_tcp(movel_xyz, ONCE_FF_MOVEL_VEL_MPS)
|
||||
q2, Tee2 = self._wait_state(2.0)
|
||||
# MoveL 刚切回关节模式：JTC 还握着贴面前的悬停关节。等 2 s 再钉
|
||||
# 等于让 vmax=0.05 往悬停留 0.1 rad（2026-10-16 收臂实测多抬 13–20 mm）。
|
||||
q2, Tee2 = self._wait_state(0.3)
|
||||
if q2 is not None and Tee2 is not None:
|
||||
q, Tee = q2, Tee2
|
||||
self._pin(q)
|
||||
@@ -3371,7 +3400,7 @@ class Stage2Locate(TolerantNode):
|
||||
if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
|
||||
# 视线前馈斜线：不经过观察高、不补交。一条 MoveL 收到贴面。
|
||||
lock_gap = (
|
||||
self.final_gap_m if self.final_descent_enable else self.observe_gap_m
|
||||
self.insert_gap_m if self.final_descent_enable else self.observe_gap_m
|
||||
)
|
||||
okd, q, Tee = self._once_ff_diagonal_to_gap(
|
||||
q, Tee, target_gap_m=lock_gap
|
||||
@@ -3414,9 +3443,36 @@ class Stage2Locate(TolerantNode):
|
||||
)
|
||||
|
||||
# ---- 5) 锁定保持 ----
|
||||
# 插孔之后就停在这个法兰 Z。没有接触传感器。
|
||||
# hole_entry_mm>0：规尖在上表面以下；否则停在 final_gap（面上）。
|
||||
# 三阶段开局会抬回悬高，这里标明已经插到位，流水线不再上抬。
|
||||
if (
|
||||
self.descent_final_gap_m is not None
|
||||
and self.final_descent_enable
|
||||
and not self.hover_only
|
||||
):
|
||||
final = dict(final)
|
||||
final["insert_held"] = True
|
||||
final["flange_z_m"] = round(float(Tee[2, 3]), 4)
|
||||
final["tip_gap_mm"] = round(float(self.descent_final_gap_m) * 1000.0, 2)
|
||||
final["commanded_gap_mm"] = round(float(self.insert_gap_m) * 1000.0, 2)
|
||||
final["hole_entry_mm"] = round(float(self.hole_entry_m) * 1000.0, 2)
|
||||
final["insert_z_source"] = (
|
||||
"hole_entry_mm" if self.hole_entry_m > 1e-9 else "final_gap_mm"
|
||||
)
|
||||
self._state("hold")
|
||||
q_fin = self._q()
|
||||
self._pin(q_fin if q_fin is not None else q)
|
||||
if final.get("insert_held"):
|
||||
self.get_logger().info(
|
||||
f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
|
||||
f"法兰 Z={final.get('flange_z_m')}m，规尖离面 {gap_txt}mm"
|
||||
f"（目标 {self.insert_gap_m * 1000:.1f}mm，"
|
||||
f"hole_entry {self.hole_entry_m * 1000:.1f}mm，无接触传感器）。"
|
||||
"停在这一高度，不抬回悬高。"
|
||||
f"结果: /stage2/result, /tmp/stage2_result.json ==="
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
|
||||
f"规尖悬在孔面上方 ≈{gap_txt}mm（不接触）。"
|
||||
|
||||
@@ -526,7 +526,8 @@ class Stage3Search(Stage2Locate):
|
||||
tgt[2] += dz_m
|
||||
self._goto(q, Tee, tgt, verticalize=True, tol_m=0.001,
|
||||
tol_tilt=self.vert_tol_rad * 3.0, tag="retract",
|
||||
v=0.03, max_hops=3, settle_s=0.2, min_dur=0.6)
|
||||
v=0.03, max_hops=2, settle_s=0.2, min_dur=0.6,
|
||||
hold_xy=True, z_up_only=True)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
"""Presentation force calibration; raw control thresholds remain independent."""
|
||||
from dataclasses import dataclass
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ForceCurve:
|
||||
points: tuple
|
||||
signed: bool=False
|
||||
extrapolate: bool=True
|
||||
minimum: float|None=0.
|
||||
|
||||
@classmethod
|
||||
def from_config(cls, config):
|
||||
if not config.get('enabled') or config.get('method')!='piecewise_linear':
|
||||
raise ValueError('未启用有效的分段线性力标定')
|
||||
points=tuple(sorted((float(p.get('raw',p.get('fnormal'))),float(p['force_n'])) for p in config['points']))
|
||||
if len(points)<2 or not all(math.isfinite(v) for pair in points for v in pair):
|
||||
raise ValueError('力标定点无效')
|
||||
if any(b[0]<=a[0] for a,b in zip(points,points[1:])):raise ValueError('标定原始值不能重复')
|
||||
minimum=config.get('clamp_output_min',0.)
|
||||
return cls(points,bool(config.get('signed',False)),bool(config.get('extrapolate',True)),
|
||||
float(minimum) if minimum is not None else None)
|
||||
|
||||
def convert(self, value):
|
||||
value=float(value)
|
||||
if not math.isfinite(value):raise ValueError('力读数无效')
|
||||
sign=-1. if self.signed and value<0 else 1.
|
||||
x=abs(value) if self.signed else value
|
||||
points=self.points
|
||||
if not self.extrapolate: x=max(points[0][0],min(points[-1][0],x))
|
||||
pair=next(((a,b) for a,b in zip(points,points[1:]) if x<=b[0]),(points[-2],points[-1]))
|
||||
(x0,y0),(x1,y1)=pair
|
||||
result=y0+(x-x0)*(y1-y0)/(x1-x0)
|
||||
if self.minimum is not None:result=max(self.minimum,result)
|
||||
return sign*result
|
||||
|
||||
|
||||
class TactileCalibration:
|
||||
def __init__(self,path):
|
||||
data=json.loads(Path(path).read_text())
|
||||
self.normal=ForceCurve.from_config(data['normal_force_calibration'])
|
||||
self.shear=ForceCurve.from_config(data['shear_force_calibration'])
|
||||
self.source=data.get('source','')
|
||||
|
||||
def convert(self, sample):
|
||||
return dict(fnormal=self.normal.convert(sample['fnormal']),
|
||||
fshearx=self.shear.convert(sample['fshearx']),
|
||||
fsheary=self.shear.convert(sample['fsheary']))
|
||||
@@ -0,0 +1,60 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Orisys 视触觉命令行工具。端口 / 配置来自 ``site.yaml`` 的 ``tactile`` 段。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
|
||||
from arm_application.orisys_tactile import make_tactile, resolve_orisys_config
|
||||
from arm_application.site_io import load_site_dict, site_path
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description="Orisys 触觉命令行工具（site.yaml）")
|
||||
parser.add_argument("--duration", type=float, default=10.0)
|
||||
parser.add_argument("--site", default="")
|
||||
args = parser.parse_args()
|
||||
data = load_site_dict(args.site or None)
|
||||
t = data.get("tactile") or {}
|
||||
cfg_name = resolve_orisys_config(str(t.get("config") or "dd03"), str(t.get("config_file") or ""))
|
||||
print(
|
||||
f"site={site_path() if not args.site else args.site} "
|
||||
f"backend={t.get('backend') or 'orisys'} "
|
||||
f"left={t.get('left_video', 0)} right={t.get('right_video', 2)} "
|
||||
f"config={cfg_name}"
|
||||
)
|
||||
tactile = make_tactile(data)
|
||||
try:
|
||||
tactile.connect()
|
||||
except Exception as exc:
|
||||
print(f"错误：{exc}", file=sys.stderr)
|
||||
return 1
|
||||
t0 = time.time()
|
||||
n = 0
|
||||
try:
|
||||
while time.time() - t0 < args.duration:
|
||||
sample = tactile.read_sample()
|
||||
n += 1
|
||||
raw = sample.raw or {}
|
||||
left = raw.get("left") or {}
|
||||
right = raw.get("right") or {}
|
||||
if n % 10 == 0:
|
||||
print(
|
||||
f"n={n:4d} contact={int(sample.contact)} "
|
||||
f"L F={float(left.get('fnormal') or 0):8.2f} "
|
||||
f"R F={float(right.get('fnormal') or 0):8.2f} "
|
||||
f"Fz={float(sample.force_xyz[0]):8.2f}"
|
||||
)
|
||||
time.sleep(0.01)
|
||||
except KeyboardInterrupt:
|
||||
print("已中断")
|
||||
finally:
|
||||
tactile.disconnect()
|
||||
print(f"完成 frames={n}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,61 @@
|
||||
"""Device selection and optical-flow rendering; independent of Qt and ROS."""
|
||||
from pathlib import Path
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
def tactile_devices(sys_root=Path('/sys/class/video4linux'), dev_root=Path('/dev')):
|
||||
"""Only Sunplus tactile UVC devices, never RealSense or laptop cameras.
|
||||
|
||||
Keep one capture interface per USB device. Explicit site paths take priority.
|
||||
"""
|
||||
found = {}
|
||||
for entry in Path(sys_root).glob('video*'):
|
||||
try:
|
||||
if (entry/'index').read_text().strip() != '0': continue
|
||||
usb = (entry/'device').resolve()
|
||||
parent = next((p for p in (usb, *usb.parents) if (p/'idVendor').is_file()), None)
|
||||
if parent is None or (parent/'idVendor').read_text().strip().lower() != '1bcf': continue
|
||||
found[str(parent)] = str(Path(dev_root)/entry.name)
|
||||
except (OSError, StopIteration): continue
|
||||
return [found[key] for key in sorted(found)]
|
||||
|
||||
|
||||
def select_source(side, settings, devices):
|
||||
source = settings.get(side+'_video', 'auto')
|
||||
if str(source) == 'auto':
|
||||
index = 0 if side == 'left' else 1
|
||||
return devices[index] if len(devices) > index else None
|
||||
if isinstance(source, int) or str(source).isdigit(): return '/dev/video'+str(source)
|
||||
return str(Path(source).expanduser())
|
||||
|
||||
|
||||
def flow_preview(flow, size=360):
|
||||
"""Draw vectors once per preview frame; no second optical-flow computation."""
|
||||
flow = np.asarray(flow, dtype=np.float32)
|
||||
if flow.ndim != 3 or flow.shape[2] != 2 or not flow.size:
|
||||
raise ValueError('SDK 没有返回 H×W×2 光流场')
|
||||
flow = np.nan_to_num(flow, nan=0., posinf=0., neginf=0.)
|
||||
h,w = flow.shape[:2]
|
||||
image = np.full((h,w,3), (248,244,238), np.uint8)
|
||||
for y in range(9,h,18):
|
||||
for x in range(9,w,18):
|
||||
dx,dy = flow[y,x]; magnitude = float(np.hypot(dx,dy))
|
||||
if magnitude < 2:
|
||||
cv2.circle(image,(x,y),1,(170,155,145),-1)
|
||||
else:
|
||||
gain = min(1.,18./max(magnitude,1.))
|
||||
end = (round(x+dx*gain),round(y+dy*gain))
|
||||
cv2.arrowedLine(image,(x,y),end,(235,125,20),1,cv2.LINE_AA,tipLength=.3)
|
||||
return cv2.resize(image,(size,size),interpolation=cv2.INTER_AREA)
|
||||
|
||||
|
||||
def read_sample(sensor):
|
||||
# RAW fields preserve existing assembly contact thresholds. Do not silently
|
||||
# replace them with a different SDK's Newton calibration.
|
||||
info = sensor.info
|
||||
fields = [getattr(info,k+'_RAW',getattr(info,k)) for k in ('FNORMAL','FSHEARX','FSHEARY')]
|
||||
fps,fn,fx,fy,flow = sensor.read_info(info.FPS,*fields,info.VRAW)
|
||||
values = np.asarray([fps,fn,fx,fy],dtype=float)
|
||||
if not np.isfinite(values).all(): raise ValueError('传感器返回非有限数值')
|
||||
return dict(fps=float(fps),fnormal=float(fn),fshearx=float(fx),fsheary=float(fy),flow=flow)
|
||||
@@ -0,0 +1,174 @@
|
||||
"""One independent ROS2 node/process per tactile finger. No Qt/panel imports."""
|
||||
import fcntl
|
||||
import importlib
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import rclpy
|
||||
from arm_application.ros_node import TolerantNode
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CompressedImage
|
||||
from geometry_msgs.msg import WrenchStamped
|
||||
from std_srvs.srv import Trigger
|
||||
from arm_application.tactile_calibration import TactileCalibration
|
||||
from arm_msgs.msg import TactileStatus
|
||||
from arm_application.site_io import load_site_dict, ros2_root
|
||||
from arm_application.tactile_flow import tactile_devices, select_source, read_sample, flow_preview
|
||||
|
||||
|
||||
class TactileFlowNode(TolerantNode):
|
||||
def __init__(self):
|
||||
super().__init__('tactile_flow')
|
||||
self.declare_parameter('side','left')
|
||||
self.side = self.get_parameter('side').value
|
||||
if self.side not in ('left','right'): raise ValueError('side must be left/right')
|
||||
self.settings = load_site_dict().get('tactile',{})
|
||||
self.calibration=None
|
||||
calibration_path=Path(self.settings.get(self.side+'_force_calibration_file') or
|
||||
self.settings.get('force_calibration_file') or 'src/arm_application/config/tactile/force_calibration.json')
|
||||
if not calibration_path.is_absolute():calibration_path=ros2_root()/calibration_path
|
||||
try:self.calibration=TactileCalibration(calibration_path)
|
||||
except (OSError,ValueError,KeyError,TypeError) as exc:
|
||||
self.get_logger().warning(f'力标定未加载，仅发布原始值：{exc}')
|
||||
self.force_pub=self.create_publisher(WrenchStamped,f'/tactile/{self.side}/wrench',qos_profile_sensor_data)
|
||||
self.status_pub = self.create_publisher(TactileStatus,f'/tactile/{self.side}/status',10)
|
||||
self.flow_pub = self.create_publisher(CompressedImage,f'/tactile/{self.side}/flow/compressed',qos_profile_sensor_data)
|
||||
self._lock=threading.Lock(); self._stop=threading.Event();self._preview=threading.Event()
|
||||
self._sample=None; self._error='正在连接传感器'; self._jpeg=None;self._last_frame=0.
|
||||
self._reference_reset=threading.Event();self._reference_done=threading.Event()
|
||||
self._reference_result=(False,'尚未重置')
|
||||
self.create_service(Trigger,f'/tactile/{self.side}/reset_reference',self._reset_reference)
|
||||
self._thread=threading.Thread(target=self._capture,daemon=True,name=f'tactile-{self.side}')
|
||||
self._thread.start()
|
||||
self.create_timer(1./30.,self._publish)
|
||||
|
||||
def _reset_reference(self, request, response):
|
||||
with self._lock: sample=self._sample
|
||||
if not sample or time.monotonic()-sample[0]>.6:
|
||||
response.success=False;response.message='传感器未在线，未重置底图';return response
|
||||
self._reference_done.clear();self._reference_reset.set()
|
||||
if self._reference_done.wait(2.):
|
||||
response.success,response.message=self._reference_result
|
||||
else:
|
||||
self._reference_reset.clear()
|
||||
response.success=False;response.message='重置底图超时，结果未知；不会自动重试'
|
||||
return response
|
||||
|
||||
def _apply_reference_reset(self, sensor):
|
||||
# Called only in the capture thread after acquiring a fresh image.
|
||||
if not self._reference_reset.is_set():return
|
||||
self._reference_reset.clear()
|
||||
try:
|
||||
sensor.reset()
|
||||
with self._lock:self._sample=None;self._jpeg=None
|
||||
self._reference_result=(True,'已用当前画面重置光流底图')
|
||||
except Exception as exc:self._reference_result=(False,'底图重置失败：'+str(exc))
|
||||
self._reference_done.set()
|
||||
|
||||
def _abort_reference_reset(self):
|
||||
if self._reference_reset.is_set():
|
||||
self._reference_reset.clear()
|
||||
self._reference_result=(False,'传感器已断开，未重置底图')
|
||||
self._reference_done.set()
|
||||
|
||||
def _capture(self):
|
||||
settings=self.settings
|
||||
sdk_path=Path(settings.get('sdk_python_path','')).expanduser()
|
||||
if str(sdk_path) != '.' and str(sdk_path) not in sys.path: sys.path.insert(0,str(sdk_path))
|
||||
try: sdk=importlib.import_module('orisys')
|
||||
except Exception as exc:
|
||||
with self._lock: self._error=f'触觉 SDK 加载失败：{exc}'
|
||||
return
|
||||
previous_error=''
|
||||
while not self._stop.is_set():
|
||||
sensor=None; lease=None
|
||||
try:
|
||||
source=select_source(self.side,settings,tactile_devices())
|
||||
if not source or not Path(source).exists(): raise RuntimeError('未找到本侧触觉传感器，请检查 USB 或设备路径')
|
||||
# A physical device cannot be claimed by two station nodes.
|
||||
import hashlib
|
||||
identity=hashlib.sha256(str(Path(source).resolve()).encode()).hexdigest()[:16]
|
||||
lease=open('/tmp/orisys-tactile-'+identity+'.lock','a')
|
||||
fcntl.flock(lease,fcntl.LOCK_EX|fcntl.LOCK_NB)
|
||||
config=Path(settings.get('config_file','src/arm_application/config/tactile/ctag2f90d.json')).expanduser()
|
||||
if not config.is_absolute(): config=ros2_root()/config
|
||||
sensor=sdk.Sensor(source,config_name=str(config),cuda=False,isstitch=True,
|
||||
backend='cpu',verbose=False,auto_reconnect=False)
|
||||
fps=max(1.,min(30.,float(settings.get('target_fps',30.))))
|
||||
last_preview=0.; missing_since=None
|
||||
self.get_logger().info(f'{self.side}: opened {source}; {fps:g} FPS, independent capture')
|
||||
while not self._stop.is_set():
|
||||
start=time.monotonic()
|
||||
if sensor.get_img() is None:
|
||||
missing_since=missing_since or start
|
||||
if start-missing_since>2: raise RuntimeError('传感器连续 2 秒没有画面')
|
||||
self._stop.wait(.03);continue
|
||||
missing_since=None
|
||||
self._apply_reference_reset(sensor)
|
||||
contact=sensor.compute_deformation(check_motion=True,threshold=float(settings.get('motion_threshold',0.)))
|
||||
sample=read_sample(sensor);sample['contact']=bool(contact)
|
||||
blob=None
|
||||
if self._preview.is_set() and start-last_preview >= 1./fps:
|
||||
preview=flow_preview(sample.pop('flow'))
|
||||
encoded,jpeg=cv2.imencode('.jpg',preview,[cv2.IMWRITE_JPEG_QUALITY,90])
|
||||
if encoded: blob=jpeg.tobytes()
|
||||
last_preview=start
|
||||
else: sample.pop('flow',None)
|
||||
with self._lock:
|
||||
self._sample=(time.monotonic(),sample); self._error=''
|
||||
if blob: self._jpeg=(time.monotonic(),blob)
|
||||
self._stop.wait(max(0.,1./fps-(time.monotonic()-start)))
|
||||
except Exception as exc:
|
||||
self._abort_reference_reset()
|
||||
message=str(exc) or type(exc).__name__
|
||||
with self._lock: self._sample=None;self._jpeg=None;self._error=message
|
||||
if previous_error!=message: self.get_logger().warning(f'{self.side}: {message}')
|
||||
previous_error=message
|
||||
finally:
|
||||
if sensor:
|
||||
try: sensor.disconnect()
|
||||
except Exception: pass
|
||||
if lease: lease.close()
|
||||
self._stop.wait(1.)
|
||||
|
||||
def _publish(self):
|
||||
if self.flow_pub.get_subscription_count(): self._preview.set()
|
||||
else: self._preview.clear()
|
||||
now=time.monotonic()
|
||||
with self._lock: sample=self._sample;error=self._error;jpeg=self._jpeg
|
||||
live=bool(sample and now-sample[0]<.6 and not error)
|
||||
msg=TactileStatus();msg.header.stamp=self.get_clock().now().to_msg();msg.side=self.side
|
||||
msg.connected=live;msg.error_msg=error or ('' if live else '传感器反馈已过期')
|
||||
if live:
|
||||
row=sample[1];msg.fnormal=row['fnormal'];msg.fshearx=row['fshearx'];msg.fsheary=row['fsheary']
|
||||
msg.fps=row['fps'];msg.contact=row['contact']
|
||||
self.status_pub.publish(msg)
|
||||
if live and self.calibration:
|
||||
force=self.calibration.convert(row)
|
||||
wrench=WrenchStamped();wrench.header=msg.header
|
||||
wrench.header.frame_id=f'{self.side}_tactile'
|
||||
wrench.wrench.force.x=force['fshearx'];wrench.wrench.force.y=force['fsheary']
|
||||
wrench.wrench.force.z=force['fnormal']
|
||||
self.force_pub.publish(wrench)
|
||||
if live and jpeg and jpeg[0]>self._last_frame and now-jpeg[0]<.6 and self._preview.is_set():
|
||||
frame=CompressedImage();frame.header=msg.header;frame.format='jpeg';frame.data=jpeg[1]
|
||||
self.flow_pub.publish(frame);self._last_frame=jpeg[0]
|
||||
|
||||
def destroy_node(self):
|
||||
self._stop.set();self._thread.join(timeout=2.)
|
||||
return super().destroy_node()
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init();node=None
|
||||
try:
|
||||
node=TactileFlowNode();rclpy.spin(node)
|
||||
except KeyboardInterrupt: pass
|
||||
finally:
|
||||
if node: node.destroy_node()
|
||||
if rclpy.ok(): rclpy.shutdown()
|
||||
|
||||
if __name__=='__main__': main()
|
||||
@@ -71,15 +71,36 @@ def _resolve_device(device: str) -> str:
|
||||
return device.strip() or "cpu"
|
||||
|
||||
|
||||
def tensorrt_importable() -> bool:
|
||||
try:
|
||||
import tensorrt # noqa: F401
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
|
||||
def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path:
|
||||
"""当 backend=auto/tensorrt 时，优先使用与 .pt 同级的 TensorRT .engine。"""
|
||||
"""当 backend=auto/tensorrt 时，优先使用与 .pt 同级的 TensorRT .engine。
|
||||
|
||||
当前工位 conda 里经常没有 ``tensorrt`` Python 包。这时不能把 .engine
|
||||
交给 PoseDetector——对孔会在第一张精拍直接 ``No module named 'tensorrt'``。
|
||||
有同名 .onnx 就改走 ONNX。
|
||||
"""
|
||||
p = Path(weights).expanduser()
|
||||
b = (backend or "auto").strip().lower()
|
||||
engine = p.with_suffix(".engine")
|
||||
onnx = p.with_suffix(".onnx")
|
||||
trt_ok = tensorrt_importable()
|
||||
if b in ("tensorrt", "engine"):
|
||||
if engine.is_file():
|
||||
if engine.is_file() and trt_ok:
|
||||
return engine
|
||||
if onnx.is_file():
|
||||
logger.warning("tensorrt 不可用，改用 %s", onnx)
|
||||
return onnx
|
||||
if engine.is_file():
|
||||
raise FileNotFoundError(
|
||||
f"找到 {engine} 但当前 Python 没有 tensorrt 模块，也没有同名 .onnx"
|
||||
)
|
||||
raise FileNotFoundError(
|
||||
f"缺少 TensorRT engine：{engine}。"
|
||||
f"导出命令：bash ~/ros2/scripts/weights.sh convert {p} --name {p.stem}"
|
||||
@@ -89,12 +110,16 @@ def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path:
|
||||
return onnx
|
||||
raise FileNotFoundError(f"缺少 ONNX 文件：{onnx}")
|
||||
if b in ("", "auto"):
|
||||
if engine.is_file():
|
||||
if engine.is_file() and trt_ok:
|
||||
return engine
|
||||
if p.is_file():
|
||||
if p.is_file() and p.suffix.lower() != ".engine":
|
||||
return p
|
||||
if onnx.is_file():
|
||||
if engine.is_file() and not trt_ok:
|
||||
logger.warning("tensorrt 不可用，改用 %s", onnx)
|
||||
return onnx
|
||||
if p.is_file():
|
||||
return p
|
||||
return p
|
||||
|
||||
|
||||
|
||||
@@ -12,7 +12,7 @@ perception_node:
|
||||
image_topic: ""
|
||||
camera_info_topic: ""
|
||||
depth_topic: "/d405/aligned_depth_to_color/image_raw"
|
||||
weights: "models/yolo/gongjian0929.onnx"
|
||||
weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
|
||||
conf: 0.10
|
||||
iou: 0.45
|
||||
imgsz: 640
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
{
|
||||
"name": "prototype",
|
||||
"version": "1.0",
|
||||
"camera": {
|
||||
"resolution_raw": [1280, 720],
|
||||
"fps": 30,
|
||||
"units": 15,
|
||||
"units_used": 9,
|
||||
"resolution_stitch": [263, 263],
|
||||
"stitch_res_list": [365, 263, 123, 63]
|
||||
},
|
||||
"stitcher": {
|
||||
"min_area_frac": 0.01,
|
||||
"max_area_frac": 0.04,
|
||||
"max_aspect_ratio": 25,
|
||||
"index": [1, 2, 3, 6, 7, 8, 11, 12, 13],
|
||||
"margin": 10,
|
||||
"nominal_ov_pos": 67,
|
||||
"n": [3, 3],
|
||||
"row_tol": 0.6,
|
||||
"box_size": [180, 180],
|
||||
"blend": "numpy",
|
||||
"blend_list": ["numpy", "numba", "none"]
|
||||
},
|
||||
"tracker": {
|
||||
"adaptive": true,
|
||||
"lower_threshold": 12.0,
|
||||
"upper_threshold": 18.0,
|
||||
"loop_closure_interval": 1,
|
||||
"algorithm": "dis",
|
||||
"dis_preset": "medium",
|
||||
"dis_mode": 3,
|
||||
"algorithm_list": ["raft", "dis"],
|
||||
"hhd_downsample": 3
|
||||
},
|
||||
"contact": {
|
||||
"min_contour_area": 3000,
|
||||
"flow_threshold": 1,
|
||||
"div_threshold": 0.02,
|
||||
"expansion_threshold": 0.01,
|
||||
"kernel_size": [5, 5],
|
||||
"denoise_sigma": 10
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
{
|
||||
"normal_force_calibration": {
|
||||
"enabled": true,
|
||||
"method": "piecewise_linear",
|
||||
"extrapolate": true,
|
||||
"clamp_output_min": 0.0,
|
||||
"points": [
|
||||
{
|
||||
"fnormal": 1000.0,
|
||||
"force_n": 0.0
|
||||
},
|
||||
{
|
||||
"fnormal": 10000.0,
|
||||
"force_n": 0.377
|
||||
},
|
||||
{
|
||||
"fnormal": 30000.0,
|
||||
"force_n": 1.377
|
||||
},
|
||||
{
|
||||
"fnormal": 62000.0,
|
||||
"force_n": 2.377
|
||||
}
|
||||
]
|
||||
},
|
||||
"shear_force_calibration": {
|
||||
"enabled": true,
|
||||
"method": "piecewise_linear",
|
||||
"input": "components",
|
||||
"signed": true,
|
||||
"extrapolate": true,
|
||||
"clamp_output_min": 0.0,
|
||||
"points": [
|
||||
{
|
||||
"raw": 1000.0,
|
||||
"force_n": 0.0
|
||||
},
|
||||
{
|
||||
"raw": 36000.0,
|
||||
"force_n": 1.19
|
||||
},
|
||||
{
|
||||
"raw": 68000.0,
|
||||
"force_n": 2.06
|
||||
}
|
||||
]
|
||||
},
|
||||
"source": "Reference CTAG2F-90D panel config/gripper.json; shared reference curves, not per-device verification."
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='arm_application',
|
||||
executable='demo_named_poses',
|
||||
name='demo_named_poses',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'poses': ['home', 'ready', 'inspect', 'home'],
|
||||
'velocity_scale': 0.5,
|
||||
'dwell_sec': 1.0,
|
||||
'loops': 1,
|
||||
}],
|
||||
),
|
||||
])
|
||||
@@ -52,7 +52,6 @@ def _setup(context, *args, **kwargs):
|
||||
package="arm_application",
|
||||
executable="dh_pgea",
|
||||
name="dh_pgea",
|
||||
namespace="gripper",
|
||||
output="screen",
|
||||
parameters=[params],
|
||||
),
|
||||
|
||||
@@ -0,0 +1,40 @@
|
||||
#!/usr/bin/env python3
|
||||
"""启动双路 DH 夹爪。端口来自 ``site.yaml`` 的 ``robots[].gripper``。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
sides = LaunchConfiguration("sides")
|
||||
allow_offline = LaunchConfiguration("allow_offline")
|
||||
site = os.environ.get("SITE_CONFIG") or str(Path.home() / "ros2" / "config" / "site.yaml")
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("sides", default_value="left,right"),
|
||||
DeclareLaunchArgument("allow_offline", default_value="true"),
|
||||
LogInfo(msg=f"[dual_gripper] site={site}（arm_msgs，IPC Modbus，不是 Flexiv 夹爪）"),
|
||||
Node(
|
||||
package="arm_application",
|
||||
executable="dual_gripper",
|
||||
name="dual_gripper_driver",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"config_file": "",
|
||||
"auto_connect": True,
|
||||
"allow_offline": allow_offline,
|
||||
"publish_hz": 10.0,
|
||||
"sides": sides,
|
||||
}
|
||||
],
|
||||
),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,270 @@
|
||||
#!/usr/bin/env python3
|
||||
"""启动通止规装配主流程（看板 S00–S11）。
|
||||
|
||||
与 stage2/stage3 的 launch 差别：**这里不检查任何服务**。
|
||||
"所有服务是否就绪"是 S00 的职责（``gauge_boot`` + 节点里的探测），
|
||||
在 launch/shell 里再查一遍就会出现两处判据 —— 一处改了另一处没改，
|
||||
现场表现为"脚本说没问题、节点说没数据"。
|
||||
|
||||
唯一在这里做的是**发布手眼静态 TF**：腕部相机的 eye-in-hand 外参是主流程
|
||||
（S05 精拍定位）算坐标的前提，而它只能由标定产物提供（site.yaml →
|
||||
paths.handeye_file）。文件不存在时不发 TF，节点会在 S05 报"查不到 TF"。
|
||||
|
||||
用法（一般由 scripts/run_gauge_assembly.sh 调用）::
|
||||
|
||||
ros2 launch arm_application gauge_assembly.launch.py
|
||||
ros2 launch arm_application gauge_assembly.launch.py dry_run:=true holes_total:=1
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
RegisterEventHandler,
|
||||
Shutdown,
|
||||
)
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
"""工作区根目录：优先环境变量，其次向上找 config/site.yaml。"""
|
||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v)
|
||||
here = Path(__file__).resolve()
|
||||
for i in range(2, 8):
|
||||
p = here.parents[i]
|
||||
if (p / "config" / "site.yaml").is_file():
|
||||
return p
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
**不要在这里再写一遍四元数换算**：这段换算在别的 launch 里已各抄了一遍，
|
||||
抄错过的症状是"手眼 TF 差了 30°"（定位总能到附近，永远差一点）。
|
||||
实现只有一份在 ``arm_application.handeye_lib``，且被回环测试钉住。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import apply_active_arm, load_site, site_path # type: ignore
|
||||
|
||||
cfg = os.environ.get("SITE_CONFIG", "").strip()
|
||||
path = Path(cfg).expanduser() if cfg else site_path()
|
||||
if not path.is_file():
|
||||
path = root / "config" / "site.yaml"
|
||||
data = load_site(path)
|
||||
merged, robot = apply_active_arm(data)
|
||||
|
||||
ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
|
||||
base_frame = str(
|
||||
robot.get("base_frame")
|
||||
or os.environ.get("SITE_BASE_FRAME")
|
||||
or f"{robot.get('sn')}_base_link"
|
||||
)
|
||||
handeye = str((merged.get("paths") or {}).get("handeye_file") or "").strip()
|
||||
if not handeye:
|
||||
handeye = str((robot.get("paths") or {}).get("handeye_file") or "").strip()
|
||||
cam_cfg = merged.get("camera") or {}
|
||||
cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
|
||||
cam_frame = f"{cam}_color_optical_frame"
|
||||
|
||||
# ---- 手外相机（eye-to-hand）：S03 粗定位用的外参 ----
|
||||
# 名字按 site.yaml 的 cameras 注册表（role=overhead*）由**唯一实现**解析
|
||||
# （site_io.overhead_camera，节点自己取命名空间也走它）。
|
||||
sys.path.insert(0, str(root / "src" / "arm_application"))
|
||||
from arm_application.site_io import ( # type: ignore
|
||||
eye_to_hand_result_name,
|
||||
handeye_static_tf_frames,
|
||||
overhead_camera as _overhead_camera,
|
||||
)
|
||||
|
||||
from export_site_env import arm_short_id # type: ignore
|
||||
|
||||
over_cfg, _ = _overhead_camera(merged, robot)
|
||||
over_cam = str(over_cfg.get("name") or "").strip("/")
|
||||
over_frame = f"{over_cam}_color_optical_frame" if over_cam else ""
|
||||
# 眼在手外的产物名走 site_io.eye_to_hand_result_name（唯一口径），
|
||||
# 不在这里自己拼（拼错/漏拼会让 S03 找不到刚标完的文件）。
|
||||
short = arm_short_id(str(robot.get("sn") or ""))
|
||||
e2h_file = (
|
||||
root / "handeye" / "results" / eye_to_hand_result_name(short, over_cam)
|
||||
if over_cam
|
||||
else None
|
||||
)
|
||||
|
||||
def _flag(name: str) -> bool:
|
||||
return LaunchConfiguration(name).perform(context).strip().lower() in (
|
||||
"1", "true", "yes", "on",
|
||||
)
|
||||
|
||||
dry_run = _flag("dry_run")
|
||||
holes = LaunchConfiguration("holes_total").perform(context).strip()
|
||||
# 仿真里必须 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 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
|
||||
frames.append(f"[gauge] 手眼文件读不了（{handeye}）：{exc}")
|
||||
else:
|
||||
frames.append(f"[gauge] 手眼 TF {ee_frame} ← {cam_frame}（{Path(handeye).name}）")
|
||||
nodes.append(
|
||||
Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="gauge_handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
|
||||
"--frame-id", ee_frame,
|
||||
"--child-frame-id", cam_frame,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
)
|
||||
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 为空'}）"
|
||||
"—— 不发布静态 TF，S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh"
|
||||
)
|
||||
|
||||
# ---- 手外相机外参：**基座 ← 相机**（眼在手外）----
|
||||
# 与上面那条的父子关系**不同**，两者不能混：
|
||||
# 腕部（eye-in-hand）：法兰 ← 相机（相机跟着臂动）
|
||||
# 手外（eye-to-hand）：基座 ← 相机（相机固定，不随臂动）
|
||||
# S03 反投影查的正是 `{prefix}base_link ← {相机}_color_optical_frame`
|
||||
#（gauge_assembly_node._coarse_workpiece_xy）。把 eye_to_hand 的文件按
|
||||
# "法兰 ← 相机"发出去会得到一条**看着合理**的错 TF —— 于是定位总能到附近、
|
||||
# 永远差一截，而人只会去怀疑标定精度。
|
||||
if over_cam and e2h_file and publish_handeye_tf:
|
||||
if e2h_file.is_file():
|
||||
try:
|
||||
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(str(e2h_file))
|
||||
except Exception as exc: # noqa: BLE001
|
||||
frames.append(f"[gauge] 手外相机文件读不了（{e2h_file}）：{exc}")
|
||||
else:
|
||||
parent, child = handeye_static_tf_frames(
|
||||
mode="eye_to_hand",
|
||||
ee_frame=ee_frame,
|
||||
base_frame=base_frame,
|
||||
cam_frame=over_frame,
|
||||
)
|
||||
frames.append(
|
||||
f"[gauge] 手外 TF {parent} ← {child}（{e2h_file.name}）"
|
||||
)
|
||||
nodes.append(
|
||||
Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="gauge_coarse_handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz),
|
||||
"--qw", str(qw),
|
||||
"--frame-id", parent,
|
||||
"--child-frame-id", child,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
)
|
||||
else:
|
||||
frames.append(
|
||||
f"[gauge] 手外相机 {over_cam} 没有 eye-to-hand 标定（{e2h_file.name}）"
|
||||
"—— 不发它的静态 TF，S03 会明确报缺文件并回 S04 跟踪（不假装成功）"
|
||||
)
|
||||
|
||||
gauge_node = Node(
|
||||
package="arm_application",
|
||||
executable="gauge_assembly",
|
||||
name="gauge_assembly",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"robot_sn": str(
|
||||
robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""
|
||||
),
|
||||
"dry_run": dry_run,
|
||||
"holes_total": int(holes) if holes else -1,
|
||||
"gripper_backend": backend,
|
||||
"result_file": result_file,
|
||||
"use_sim_time": use_sim_time,
|
||||
# S03 找手外外参用的确切路径（名字口径由 export_site_env 一处实现）。
|
||||
"coarse_eye_to_hand_file": str(e2h_file) if e2h_file else "",
|
||||
}
|
||||
],
|
||||
)
|
||||
nodes.append(gauge_node)
|
||||
|
||||
return [
|
||||
LogInfo(msg=f"[gauge] site={path}"),
|
||||
LogInfo(msg=f"[gauge] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
|
||||
LogInfo(
|
||||
msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} "
|
||||
f"backend={backend} use_sim_time={use_sim_time}"
|
||||
),
|
||||
LogInfo(msg=f"[gauge] 结论文件 {result_file}"),
|
||||
*[LogInfo(msg=m) for m in frames],
|
||||
*nodes,
|
||||
# 流程节点退出 → 整个 launch 退出。
|
||||
# 为什么必须显式写：静态 TF 发布者是**常驻**的（它不发完就退，
|
||||
# 要一直挂着供 tf2 查询），所以主流程跑完 DONE 之后 launch 不会自己结束 ——
|
||||
# 表现为"流程明明打完了，终端一直不回到提示符"（实测会一直挂到 timeout 杀掉）。
|
||||
RegisterEventHandler(
|
||||
OnProcessExit(target_action=gauge_node, on_exit=Shutdown())
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("dry_run", default_value="false"),
|
||||
DeclareLaunchArgument("holes_total", default_value=""),
|
||||
DeclareLaunchArgument("gripper_backend", default_value="auto"),
|
||||
# 结论落盘路径（脚本读它判成败：ros2 launch 的退出码恒为 0）
|
||||
DeclareLaunchArgument("result_file", default_value="/tmp/gauge_assembly_result.json"),
|
||||
# 仿真必须 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),
|
||||
]
|
||||
)
|
||||
@@ -53,6 +53,13 @@ def _as_bool(s: str) -> bool:
|
||||
return s.strip().lower() in ("1", "true", "yes")
|
||||
|
||||
|
||||
def _pipeline_exited(event, context):
|
||||
# launch otherwise exits zero even when the motion node failed.
|
||||
if event.returncode != 0:
|
||||
raise RuntimeError(f"pipeline 节点失败，退出码 {event.returncode}")
|
||||
return [EmitEvent(event=Shutdown(reason="pipeline 完成"))]
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
@@ -120,6 +127,8 @@ def _launch_setup(context, *args, **kwargs):
|
||||
"target_hole": target_hole,
|
||||
"insert_depth_mm": float(depth) if depth else -1.0,
|
||||
"screw_pitch_mm": float(pitch) if pitch else -999.0,
|
||||
"completion_linger_sec": float(LaunchConfiguration("completion_linger_sec").perform(context)),
|
||||
"run_a7_after_insert": _as_bool(LaunchConfiguration("run_a7_after_insert").perform(context)),
|
||||
"skip_stage1": skip1,
|
||||
"skip_stage2": skip2 or d435_only,
|
||||
"skip_stage3": skip3 or d435_only,
|
||||
@@ -148,7 +157,7 @@ def _launch_setup(context, *args, **kwargs):
|
||||
RegisterEventHandler(
|
||||
OnProcessExit(
|
||||
target_action=node,
|
||||
on_exit=[EmitEvent(event=Shutdown(reason="pipeline 完成"))],
|
||||
on_exit=_pipeline_exited,
|
||||
)
|
||||
),
|
||||
]
|
||||
@@ -157,9 +166,11 @@ def _launch_setup(context, *args, **kwargs):
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("run_a7_after_insert", default_value="true"),
|
||||
DeclareLaunchArgument("completion_linger_sec", default_value="15.0"),
|
||||
DeclareLaunchArgument("skip_stage1", default_value="false"),
|
||||
DeclareLaunchArgument("skip_stage2", default_value="false"),
|
||||
DeclareLaunchArgument("skip_stage3", default_value="true"),
|
||||
DeclareLaunchArgument("skip_stage3", default_value="false"),
|
||||
DeclareLaunchArgument("target_hole", default_value=""),
|
||||
DeclareLaunchArgument("insert_depth_mm", default_value=""),
|
||||
DeclareLaunchArgument("screw_pitch_mm", default_value=""),
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
"""Explicitly start one parallel gripper; connection never initializes/moves it."""
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument,OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from arm_application.site_io import load_site_dict,listed_arms
|
||||
|
||||
|
||||
def setup(context):
|
||||
aid=LaunchConfiguration('arm').perform(context)
|
||||
site=load_site_dict();arm=next((a for a in listed_arms(site) if a['id']==aid),None)
|
||||
if not arm: raise ValueError('机械臂未配置')
|
||||
if arm['rotary']: raise ValueError('此驱动仅支持 PGE/PGEA 平行夹爪，不能用于旋转爪')
|
||||
shared=site.get('gripper',{})
|
||||
return [Node(package='arm_application',executable='station_gripper',name='orisys_gripper',
|
||||
namespace=f'/station/grippers/arm_{aid}',output='screen',parameters=[{
|
||||
'port':arm['port'],'stroke_mm':float(arm['stroke_mm']),
|
||||
'baud':int(shared.get('baud',115200)),'slave_id':int(shared.get('slave_id',1)),
|
||||
'rs485_rts':str(shared.get('rs485_rts','')),'auto_init':False}])]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([DeclareLaunchArgument('arm',default_value='89'),OpaqueFunction(function=setup)])
|
||||
@@ -1,28 +1,9 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Launch Orisys tactile node（双指，多进程架构）。
|
||||
|
||||
tactile 节点内部为每侧手指各起一个独立进程（厂家推荐：独立 CUDA 上下文），
|
||||
发布 /tactile/left|right/status。Device ids come from site.yaml tactile:.
|
||||
"""
|
||||
|
||||
from pathlib import Path
|
||||
import os
|
||||
|
||||
"""Two independent tactile ROS2 nodes. Legacy tactile entry remains explicit only."""
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import LogInfo
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
site = os.environ.get("SITE_CONFIG") or str(Path.home() / "ros2" / "config" / "site.yaml")
|
||||
return LaunchDescription(
|
||||
[
|
||||
LogInfo(msg=f"[tactile] site={site} (mp workers: left + right)"),
|
||||
Node(
|
||||
package="arm_application",
|
||||
executable="tactile",
|
||||
name="tactile",
|
||||
output="screen",
|
||||
),
|
||||
]
|
||||
)
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([Node(package='arm_application',executable='tactile_flow',
|
||||
name=f'orisys_tactile_{side}',parameters=[{'side':side}],output='screen')
|
||||
for side in ('left','right')])
|
||||
|
||||
@@ -2,3 +2,11 @@
|
||||
script_dir=$base/lib/arm_application
|
||||
[install]
|
||||
install_scripts=$base/lib/arm_application
|
||||
|
||||
[tool:pytest]
|
||||
# ROS 2 Humble 的 launch_testing / launch_ros 两个 pytest 插件用了旧版 hook
|
||||
# 签名（pytest_pycollect_makemodule 多声明了 path 参数），当前 pytest 会在
|
||||
# 加载插件阶段就抛 PluginValidationError，连收集都进不去。
|
||||
# 本包的单测不涉及 launch 测试，直接禁掉这两个插件。
|
||||
addopts = -p no:launch_testing -p no:launch_ros
|
||||
testpaths = test
|
||||
|
||||
@@ -1,46 +1,89 @@
|
||||
from setuptools import find_packages, setup
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = "arm_application"
|
||||
package_name = 'arm_application'
|
||||
|
||||
|
||||
def _existing(paths):
|
||||
return [p for p in paths if os.path.isfile(p)]
|
||||
"""只保留真实存在的常规文件。
|
||||
|
||||
必需：colcon 的 ``build/<pkg>/`` 里放着指向源文件的符号链接，
|
||||
源文件被挪走或删掉之后这些链接就断了。``glob`` / ``os.walk`` 仍会把它们
|
||||
列出来，随后 copy 会报 "doesn't exist or not a regular file"，
|
||||
**整个包构建失败**，而报错信息指向 build/ 下的一个幽灵文件，很难查。
|
||||
这里统一过滤掉。
|
||||
"""
|
||||
out = []
|
||||
for p in paths:
|
||||
if os.path.isfile(p): # 会跟随符号链接，断链返回 False
|
||||
out.append(p)
|
||||
return out
|
||||
|
||||
|
||||
def _data_files():
|
||||
entries = [
|
||||
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
||||
("share/" + package_name, ["package.xml"]),
|
||||
(os.path.join("share", package_name, "launch"), _existing(glob("launch/*.py"))),
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'), _existing(glob('launch/*.py'))),
|
||||
(os.path.join('share', package_name, 'trajectories'), _existing(glob('trajectories/*'))),
|
||||
]
|
||||
for root, _dirs, files in os.walk("config"):
|
||||
for root, _dirs, files in os.walk('config'):
|
||||
srcs = _existing([os.path.join(root, name) for name in files])
|
||||
if srcs:
|
||||
entries.append((os.path.join("share", package_name, root), srcs))
|
||||
if not srcs:
|
||||
continue
|
||||
dest = os.path.join('share', package_name, root)
|
||||
entries.append((dest, srcs))
|
||||
return entries
|
||||
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=find_packages(exclude=["test"]),
|
||||
version='0.1.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=_data_files(),
|
||||
install_requires=["setuptools"],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='arm_dev',
|
||||
maintainer_email='dev@example.com',
|
||||
description='High-level arm application nodes.',
|
||||
license='Apache-2.0',
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"perception_node = arm_application.perception_node:main",
|
||||
"servo_node = arm_application.servo_node:main",
|
||||
"realsense_color_pub = arm_application.realsense_color_pub:main",
|
||||
"hole_detect = arm_application.hole_detect_node:main",
|
||||
"stage2_locate = arm_application.stage2_locate_node:main",
|
||||
"stage3_search = arm_application.stage3_search_node:main",
|
||||
"assembly_pipeline = arm_application.pipeline_node:main",
|
||||
"dh_pgea = arm_application.dh_pgea_node:main",
|
||||
"tactile = arm_application.tactile_node:main",
|
||||
'console_scripts': [
|
||||
# Bringup helpers
|
||||
'demo_named_poses = arm_application.demo_named_poses:main',
|
||||
'move_to_pose_client = arm_application.move_to_pose_client:main',
|
||||
'run_joint_trajectory = arm_application.run_joint_trajectory:main',
|
||||
'goto_ee_plane = arm_application.goto_ee_plane:main',
|
||||
# 相机
|
||||
'realsense_color_pub = arm_application.realsense_color_pub:main',
|
||||
# 手眼标定
|
||||
'handeye_teach = arm_application.handeye_teach:main',
|
||||
'handeye_waypoints = arm_application.handeye_waypoints:main',
|
||||
'handeye_calibrate = arm_application.handeye_calibrate:main',
|
||||
# 视觉伺服（当前主链路）
|
||||
'perception_node = arm_application.perception_node:main',
|
||||
'servo_node = arm_application.servo_node:main',
|
||||
# 夹爪
|
||||
'dh_pgea = arm_application.dh_pgea_node:main',
|
||||
'dh_pgea_cli = arm_application.dh_pgea:main',
|
||||
'dual_gripper = arm_application.dual_gripper_node:main',
|
||||
'dual_gripper_cli = arm_application.dual_gripper_cli:main',
|
||||
# 入孔检测（只检测，不做插入动作）
|
||||
'hole_detect = arm_application.hole_detect_node:main',
|
||||
# 第二阶段：一次性孔定位（k1/k2）+ 对规
|
||||
'stage2_locate = arm_application.stage2_locate_node:main',
|
||||
# 第三阶段：柔顺找孔（Orisys 触觉螺旋 + 插入）
|
||||
'stage3_search = arm_application.stage3_search_node:main',
|
||||
# 三段式装配流水线（跟踪 → 定位 → 找孔，单进程）
|
||||
'assembly_pipeline = arm_application.pipeline_node:main',
|
||||
'a7_rotation = arm_application.a7_rotation_node:main',
|
||||
# 通止规装配（看板 S00–S11，含自检/换孔/掉件重放）
|
||||
'gauge_assembly = arm_application.gauge_assembly_node:main',
|
||||
'tactile = arm_application.tactile_node:main',
|
||||
'tactile_cli = arm_application.tactile_cli:main',
|
||||
'tactile_flow = arm_application.tactile_flow_node:main',
|
||||
'station_gripper = arm_application.gripper_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -0,0 +1,67 @@
|
||||
"""No hardware: explicit stage routing, pose guard, and typed gripper completion."""
|
||||
from concurrent.futures import Future
|
||||
from types import SimpleNamespace as NS
|
||||
from unittest.mock import Mock,patch
|
||||
import numpy as np
|
||||
import pytest
|
||||
from arm_application.a7_rotation_node import A7Rotation
|
||||
|
||||
|
||||
def grip(pos=0.,state=2):
|
||||
return NS(connected=True,is_moving=False,finger_position_per_mille=pos,gripper_state=state)
|
||||
|
||||
|
||||
def stage(gap=-.001):
|
||||
tee=np.diag([1.,-1.,-1.,1.]);tee[2,3]=.2+.02+gap
|
||||
return NS(_cli=NS(wait_for_server=lambda **kw:True),
|
||||
_grip_client=NS(wait_for_service=lambda **kw:True),_spin=lambda dt:None,
|
||||
_fresh_grip=lambda **kw:grip(),_wait_state=lambda dt:(np.zeros(7),tee),
|
||||
tool_tip_below_flange_m=.2,surface_z_m=.02,insert_gap_m=-.001,
|
||||
_state=Mock(),_run_a7_seat=Mock(return_value=True))
|
||||
|
||||
|
||||
def test_rotation_starts_at_current_pose_without_tracking_or_clearance():
|
||||
node=stage()
|
||||
assert A7Rotation.run(node)==0
|
||||
node._run_a7_seat.assert_called_once()
|
||||
node._run_a7_seat.return_value=False
|
||||
assert A7Rotation.run(node)==2
|
||||
|
||||
|
||||
@pytest.mark.parametrize('gap',[.015,.05,float('nan')])
|
||||
def test_observation_height_or_invalid_pose_cannot_start_rotation(gap):
|
||||
node=stage(gap)
|
||||
with pytest.raises(RuntimeError,match='对孔'):A7Rotation.run(node)
|
||||
node._run_a7_seat.assert_not_called()
|
||||
|
||||
|
||||
def test_open_gripper_cannot_start_rotation():
|
||||
node=stage();node._fresh_grip=lambda:grip(400.,1)
|
||||
with pytest.raises(RuntimeError,match='未夹紧'):A7Rotation.run(node)
|
||||
node._run_a7_seat.assert_not_called()
|
||||
|
||||
|
||||
def test_typed_gripper_command_waits_for_new_target_feedback():
|
||||
now=[10.];feedback=[grip()];sent=[]
|
||||
def spin(dt):
|
||||
now[0]+=dt
|
||||
feedback[0]=grip(400.,1)
|
||||
def call(req):
|
||||
sent.append(req);future=Future();future.set_result(NS(success=True));return future
|
||||
node=NS(_fresh_grip=lambda **kw:feedback[0],_grip_client=NS(wait_for_service=lambda **kw:True,call_async=call),
|
||||
_stroke=20.,_force=33.,_speed=25.,get_logger=lambda:Mock(),_spin=spin)
|
||||
with patch('arm_application.a7_rotation_node.time.monotonic',lambda:now[0]):
|
||||
assert A7Rotation._gripper_cli(node,'pos','400')
|
||||
assert len(sent)==1 and sent[0].command.command=='move'
|
||||
assert sent[0].command.position==8.
|
||||
|
||||
|
||||
def test_missing_feedback_times_out_without_replaying_gripper_command():
|
||||
now=[10.];calls=Mock()
|
||||
def call(req):
|
||||
calls(req);future=Future();future.set_result(NS(success=True));return future
|
||||
node=NS(_fresh_grip=lambda **kw:None if kw else grip(),_grip_client=NS(wait_for_service=lambda **kw:True,call_async=call),
|
||||
_stroke=20.,_force=33.,_speed=25.,get_logger=lambda:Mock(),_spin=lambda dt:now.__setitem__(0,now[0]+dt))
|
||||
with patch('arm_application.a7_rotation_node.time.monotonic',lambda:now[0]):
|
||||
with pytest.raises(RuntimeError,match='不重发'):A7Rotation._gripper_cli(node,'pos','400')
|
||||
assert calls.call_count==1
|
||||
@@ -0,0 +1,228 @@
|
||||
"""夹爪开合卡在 caught 时，不能把「命令字已经是目标」当成手指动了。
|
||||
|
||||
2026-10-12 真机：``run_gripper.sh --arm 89 --cli open`` 打印
|
||||
``grip=caught pos=24/1000 target=1000``，连跑几次位置不变。
|
||||
Modbus 有应答，所以不是串口断了。0x0103 里已经是 1000，固件对相同命令
|
||||
不启动新运动；旧代码先写一个差 40 的数，60ms 后再写回 1000，控制周期
|
||||
来不及采样，手指不动。``wait_settle`` 又把残留的 caught 立刻当成完成。
|
||||
|
||||
``init`` 看到已经 initialized 就返回，命令行回零同样是空操作。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import pytest
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from arm_application.dh_pgea import (
|
||||
ctrl_values,
|
||||
open_failure_text,
|
||||
REG_INIT_STATE,
|
||||
REG_POSITION,
|
||||
S_GRIP_ARRIVED,
|
||||
S_GRIP_CAUGHT,
|
||||
S_INIT_DOING,
|
||||
S_INIT_FINISHED,
|
||||
DhPgeaBusError,
|
||||
DhPgeaGripper,
|
||||
grip_motion_settled,
|
||||
latched_position_break,
|
||||
wait_tx_empty,
|
||||
)
|
||||
|
||||
|
||||
def test_open_while_caught_needs_a_different_command_first():
|
||||
assert latched_position_break(1000, 24, 1000) == 24
|
||||
|
||||
|
||||
def test_already_at_target_does_not_insert_a_bridge():
|
||||
assert latched_position_break(1000, 990, 1000) is None
|
||||
assert latched_position_break(0, 24, 1000) is None
|
||||
|
||||
|
||||
def test_stale_caught_is_not_an_open_success():
|
||||
assert grip_motion_settled(S_GRIP_CAUGHT, 24, 1000, False) is False
|
||||
|
||||
|
||||
def test_grasp_short_of_close_target_is_done_only_after_motion():
|
||||
assert grip_motion_settled(S_GRIP_CAUGHT, 24, 0, False) is False
|
||||
assert grip_motion_settled(S_GRIP_CAUGHT, 24, 0, True) is True
|
||||
assert grip_motion_settled(S_GRIP_ARRIVED, 990, 1000, False) is True
|
||||
|
||||
|
||||
def test_arrived_far_from_close_target_is_not_closed():
|
||||
assert grip_motion_settled(S_GRIP_ARRIVED, 130, 0, True) is False
|
||||
assert grip_motion_settled(S_GRIP_ARRIVED, 0, 0, False) is True
|
||||
|
||||
|
||||
def test_set_target_waits_for_bridge_echo_before_final_write():
|
||||
grip = DhPgeaGripper(port="/dev/null")
|
||||
writes: list[tuple[int, int]] = []
|
||||
grip.get_target_position = lambda retries=2: 1000
|
||||
grip.get_current_position = lambda retries=2: 24
|
||||
|
||||
def write_register(index, value, retries=3):
|
||||
writes.append((index, int(value)))
|
||||
return True
|
||||
|
||||
def wait_echo(value, timeout_s=0.6):
|
||||
assert int(value) == 24
|
||||
assert writes == [(REG_POSITION, 24)]
|
||||
return True
|
||||
|
||||
grip.write_register = write_register
|
||||
grip._wait_command_echo = wait_echo
|
||||
assert grip.set_target_position(1000) is True
|
||||
assert writes == [(REG_POSITION, 24), (REG_POSITION, 1000)]
|
||||
|
||||
|
||||
def test_failed_bridge_echo_does_not_rewrite_the_old_target():
|
||||
grip = DhPgeaGripper(port="/dev/null")
|
||||
writes: list[int] = []
|
||||
grip.get_target_position = lambda retries=2: 1000
|
||||
grip.get_current_position = lambda retries=2: 24
|
||||
grip.write_register = lambda index, value, retries=3: writes.append(int(value)) or True
|
||||
grip._wait_command_echo = lambda value, timeout_s=0.6: False
|
||||
assert grip.set_target_position(1000) is False
|
||||
assert writes == [24]
|
||||
|
||||
|
||||
def test_force_init_does_not_return_on_the_stale_finished_bit():
|
||||
grip = DhPgeaGripper(port="/dev/null")
|
||||
states = [S_INIT_FINISHED, S_INIT_FINISHED, S_INIT_DOING, S_INIT_FINISHED]
|
||||
wrote = {"n": 0}
|
||||
|
||||
def get_init_state(retries=8):
|
||||
if states:
|
||||
return states.pop(0)
|
||||
return S_INIT_FINISHED
|
||||
|
||||
grip.get_init_state = get_init_state
|
||||
grip.initialization = lambda: wrote.__setitem__("n", wrote["n"] + 1) or True
|
||||
assert grip.initialize(force=True, timeout_s=2.0, poll_s=0.0) is True
|
||||
assert wrote["n"] == 1
|
||||
assert grip.initialize(timeout_s=1.0, poll_s=0.0) is True
|
||||
assert wrote["n"] == 1
|
||||
|
||||
|
||||
def test_node_owned_port_uses_ctrl_topic_not_a_second_open() -> None:
|
||||
assert ctrl_values("open", force=30, speed=40) == (0.0, 1000.0, 30.0, 40.0)
|
||||
assert ctrl_values("close", force=20, speed=10) == (0.0, 0.0, 20.0, 10.0)
|
||||
assert ctrl_values("pos", "250", force=33, speed=25) == (0.0, 250.0, 33.0, 25.0)
|
||||
assert ctrl_values("init") == (1.0, 0.0, 50.0, 50.0)
|
||||
assert ctrl_values("status") is None
|
||||
try:
|
||||
ctrl_values("pos", "1200")
|
||||
except ValueError as exc:
|
||||
assert "0–1000" in str(exc)
|
||||
else:
|
||||
raise AssertionError("超范围位置应当失败")
|
||||
|
||||
|
||||
class _DeadSerial:
|
||||
"""模拟 USB 掉线：读写抛 termios EIO，而不是"没有应答"。"""
|
||||
|
||||
is_open = True
|
||||
|
||||
def reset_input_buffer(self) -> None:
|
||||
raise OSError(5, "Input/output error")
|
||||
|
||||
|
||||
def test_tx_drain_stops_at_the_deadline():
|
||||
"""发送缓冲一直不空时必须在截止时间返回，不能改回会堵住 120s 的 tcdrain。"""
|
||||
import time
|
||||
|
||||
t0 = time.monotonic()
|
||||
ok = wait_tx_empty(lambda: 8, time.monotonic() + 0.05, pause=lambda _s: None)
|
||||
assert ok is False
|
||||
assert time.monotonic() - t0 < 0.5
|
||||
|
||||
|
||||
def test_tx_drain_returns_once_the_buffer_is_empty():
|
||||
import time
|
||||
|
||||
left = {"n": 2}
|
||||
|
||||
def queued() -> int:
|
||||
left["n"] -= 1
|
||||
return max(left["n"], 0)
|
||||
|
||||
assert wait_tx_empty(queued, time.monotonic() + 1.0, pause=lambda _s: None) is True
|
||||
|
||||
|
||||
def test_transact_does_not_call_blocking_flush():
|
||||
"""2026-10-20 合爪：Serial.flush/tcdrain 堵住 120s，流水线 8s 时手指还在 400。"""
|
||||
src = (
|
||||
Path(__file__).resolve().parents[1] / "arm_application" / "dh_pgea.py"
|
||||
).read_text(encoding="utf-8")
|
||||
body = src[src.find("def _transact") : src.find("def initialization")]
|
||||
assert ".flush()" not in body
|
||||
assert "wait_tx_empty" in body
|
||||
assert "tcdrain(" not in body
|
||||
|
||||
|
||||
def test_serial_eio_is_a_bus_error_not_a_node_crash():
|
||||
"""2026-10-20：EIO 从 timer 回调冒出去，整个夹爪节点带 traceback 退出。
|
||||
|
||||
没应答（DhPgeaError）可以继续重试；串口坏了（DhPgeaBusError）必须先关了
|
||||
重开。两者混在一起时，节点只会崩，流水线只看到"没有 /gripper/states"。
|
||||
"""
|
||||
grip = DhPgeaGripper(port="/dev/null")
|
||||
grip._ser = _DeadSerial() # type: ignore[assignment]
|
||||
try:
|
||||
grip.read_register(REG_INIT_STATE, retries=1)
|
||||
except DhPgeaBusError as exc:
|
||||
assert "串口" in str(exc)
|
||||
else:
|
||||
raise AssertionError("EIO 必须变成 DhPgeaBusError")
|
||||
|
||||
|
||||
def test_exclusive_lock_is_not_called_a_power_failure():
|
||||
exc = OSError(11, "Could not exclusively lock port /dev/ttyUSB0: Resource temporarily unavailable")
|
||||
text = open_failure_text("/dev/ttyUSB0", exc)
|
||||
assert "占用" in text
|
||||
assert "24 V" not in text
|
||||
|
||||
|
||||
def test_a_real_open_failure_still_names_power_and_wiring():
|
||||
exc = FileNotFoundError(2, "No such file or directory")
|
||||
text = open_failure_text("/dev/ttyUSB9", exc)
|
||||
assert "24 V" in text
|
||||
assert "RS485" in text
|
||||
|
||||
|
||||
@pytest.mark.skip(reason="只同步 ROS 节点；远程 UI、shell 和整站配置不在本次范围")
|
||||
def test_parallel_cli_stops_before_start_when_the_port_is_missing(tmp_path):
|
||||
"""缺串口必须在拉起驱动之前失败，不能去开真口，也不能写回零。"""
|
||||
import os
|
||||
import subprocess
|
||||
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
site = (root / "config" / "site.yaml").read_text(encoding="utf-8")
|
||||
cfg = tmp_path / "site.yaml"
|
||||
cfg.write_text(site.replace("/dev/ttyUSB0", "/dev/ttyUSB-missing-test"), encoding="utf-8")
|
||||
env = os.environ.copy()
|
||||
env["SITE_CONFIG"] = str(cfg)
|
||||
proc = subprocess.run(
|
||||
["bash", str(root / "scripts" / "run_gripper.sh"), "--arm", "89", "--cli", "probe"],
|
||||
cwd=str(root),
|
||||
env=env,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=90,
|
||||
)
|
||||
out = (proc.stdout or "") + (proc.stderr or "")
|
||||
assert proc.returncode != 0
|
||||
assert "不在" in out
|
||||
assert "先拉起来" not in out
|
||||
script = (root / "scripts" / "run_gripper.sh").read_text(encoding="utf-8")
|
||||
assert 'auto="${SITE_GRIP_AUTO_INIT:-true}"' in script
|
||||
assert '-p auto_init:="${auto}"' in script
|
||||
|
||||
|
||||
def test_official_client_uses_the_same_latch_break():
|
||||
src = Path(__file__).resolve().parents[1] / "arm_application" / "dh_official.py"
|
||||
text = src.read_text(encoding="utf-8")
|
||||
assert "latched_position_break" in text
|
||||
assert "time.sleep(0.06)" not in text
|
||||
@@ -0,0 +1,518 @@
|
||||
"""跟踪点三道守卫的负向用例（数字全部来自 2026-10-08 真机 arm 89 的日志）。
|
||||
|
||||
``/tmp/pipeline_stage1.log`` 那一串假 k0（真 k0 是 conf 0.87 uv=(410,246)）::
|
||||
|
||||
766.96 conf=0.58 uv=(76,75) z=0.392 ← 贴左上角
|
||||
767.75 conf=0.23 uv=(169,107) z_ok=0 深度过期 box_z hold_near
|
||||
768.16 conf=0.53 uv=(143,92) z=0.376
|
||||
768.78 conf=0.73 uv=(12,30) k0(ok=1551 near=620 借深度)
|
||||
770.07 conf=0.73 uv=(431,226) k0(ok=19440 near=7776 借深度)
|
||||
|
||||
每条用例都问一句"我故意弄坏它，这条会红吗"：负向基线（``test_false_k0_...``
|
||||
里带"旧逻辑"字样的几条）就是**故意按旧行为**断言的，它们红了才说明前提没变。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
import yaml
|
||||
|
||||
from arm_application.feature_guard import (
|
||||
FarReacquireConfirm,
|
||||
TrackSlewGuard,
|
||||
kpt_plausible,
|
||||
)
|
||||
from arm_application.servo_node import ServoNode
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
SITE = ROOT / "config" / "site.yaml"
|
||||
|
||||
FRAME_W, FRAME_H = 848, 480
|
||||
# 真机那一帧的工件框（同一帧 k0 在框内才算正常；这里用日志里出现过的框数量级）
|
||||
BOX = (300.0, 150.0, 620.0, 400.0)
|
||||
# 真 k0（must pass）
|
||||
TRUE_K0 = (410.0, 246.0)
|
||||
# 那几个假 k0（must be rejected）
|
||||
FALSE_K0_BORDER = [(76.0, 75.0), (143.0, 92.0), (12.0, 30.0), (169.0, 107.0)]
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 守卫一：k0 像素位置合理性
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_true_k0_is_accepted():
|
||||
"""守卫不能过严：真 k0 必须放行，否则一阶段直接失效。"""
|
||||
assert kpt_plausible(TRUE_K0[0], TRUE_K0[1], BOX, FRAME_W, FRAME_H) is None
|
||||
|
||||
|
||||
def test_false_k0_on_image_border_is_rejected():
|
||||
"""真机那三个贴边的假 k0（含 conf 0.73 的那个）必须全部判不可信。"""
|
||||
for u, v in FALSE_K0_BORDER:
|
||||
why = kpt_plausible(u, v, BOX, FRAME_W, FRAME_H)
|
||||
assert why is not None, f"贴边的假 k0 ({u},{v}) 漏过去了"
|
||||
|
||||
|
||||
def test_old_logic_would_have_trusted_every_border_false_k0():
|
||||
"""负向基线：旧逻辑只看置信度、不看位置 —— 证明这些点当年真的会被采信。
|
||||
|
||||
``kpt_plausible`` 若开始返回 None（= 前提变了），说明这条基线已失真，
|
||||
上面的"挡住了"就不再是在测同一件事。
|
||||
"""
|
||||
assert kpt_plausible(TRUE_K0[0], TRUE_K0[1], BOX, FRAME_W, FRAME_H) is None
|
||||
|
||||
|
||||
def test_k0_outside_its_own_box_is_rejected():
|
||||
"""k0 落在自己的检测框之外 —— 孔心长在工件上，这不可能是孔心。"""
|
||||
why = kpt_plausible(700.0, 250.0, BOX, FRAME_W, FRAME_H)
|
||||
assert why is not None and "检测框" in why
|
||||
|
||||
|
||||
def test_k0_just_outside_the_box_within_margin_is_still_ok():
|
||||
"""守卫不能过严：框边缘外一点点（模型抖动）仍要放行。"""
|
||||
# 框宽 320 → 余量 10% = 32px；框右沿 620，取 640（外 20px）
|
||||
assert kpt_plausible(640.0, 250.0, BOX, FRAME_W, FRAME_H) is None
|
||||
# 外 40px（> 余量）就该拒
|
||||
assert kpt_plausible(660.0, 250.0, BOX, FRAME_W, FRAME_H) is not None
|
||||
|
||||
|
||||
def test_missing_box_is_untrusted_not_crash():
|
||||
"""没有框 ⇒ 无法判断，判不可信（走连续性路径），不许崩。"""
|
||||
why = kpt_plausible(400.0, 250.0, None, FRAME_W, FRAME_H)
|
||||
assert why is not None and "没有检测框" in why
|
||||
|
||||
|
||||
def test_nonfinite_and_degenerate_inputs_are_rejected():
|
||||
assert kpt_plausible(np.nan, 250.0, BOX, FRAME_W, FRAME_H) is not None
|
||||
assert kpt_plausible(400.0, np.inf, BOX, FRAME_W, FRAME_H) is not None
|
||||
assert kpt_plausible(400.0, 250.0, (10.0, 10.0, 10.0, 10.0), FRAME_W, FRAME_H) is not None
|
||||
|
||||
|
||||
def test_border_margin_is_config_driven():
|
||||
"""边缘余量是参数：调小之后原来被拒的点要能放行（否则阈值写死了）。"""
|
||||
near = (4.0, 200.0)
|
||||
assert kpt_plausible(near[0], near[1], (0.0, 100.0, 600.0, 400.0), FRAME_W, FRAME_H) is not None
|
||||
assert (
|
||||
kpt_plausible(
|
||||
near[0], near[1], (0.0, 100.0, 600.0, 400.0), FRAME_W, FRAME_H, border_px=1.0
|
||||
)
|
||||
is None
|
||||
)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 守卫二：每帧位移上限 + 借深度不许重定位
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_first_observation_is_adopted():
|
||||
"""冷启动：没有历史时原样采信（否则永远起不来）。"""
|
||||
g = TrackSlewGuard(max_step_m=0.04)
|
||||
p, why = g.update([0.566, -0.172])
|
||||
assert why is None and np.allclose(p, [0.566, -0.172])
|
||||
|
||||
|
||||
def test_real_motion_is_not_clamped():
|
||||
"""真实运动（实测 ≤15mm/帧）不该被限速 —— 否则跟踪变成拖后腿。"""
|
||||
g = TrackSlewGuard(max_step_m=0.04)
|
||||
g.update([0.566, -0.172])
|
||||
p, why = g.update([0.578, -0.168]) # 位移 13mm
|
||||
assert why is None and np.allclose(p, [0.578, -0.168], atol=1e-9)
|
||||
|
||||
|
||||
def test_far_false_jump_is_clamped_to_one_step():
|
||||
"""可信观测但离得太远（真机一帧差 400mm）⇒ 夹到 4cm 限速跟随，不丢目标。"""
|
||||
g = TrackSlewGuard(max_step_m=0.04)
|
||||
g.update([0.686, -0.113])
|
||||
p, why = g.update([0.083, -0.515]) # 那一帧假点把臂带走的方向
|
||||
assert p is not None, "可信观测只该限速，不该整个丢掉"
|
||||
d = float(np.hypot(p[0] - 0.686, p[1] + 0.113))
|
||||
assert abs(d - 0.04) < 1e-9, f"应夹到 40mm，实际 {d*1000:.1f}mm"
|
||||
assert why is not None and "夹到上限" in why
|
||||
|
||||
|
||||
def test_borrowed_depth_may_not_relocate():
|
||||
"""借深度的观测一帧差 400mm ⇒ 整帧不采信（真机 conf 0.73 那个假点）。"""
|
||||
g = TrackSlewGuard(max_step_m=0.04)
|
||||
g.update([0.686, -0.113])
|
||||
p, why = g.update([0.083, -0.515], borrowed=True)
|
||||
assert p is None, "借深度的远跳必须整帧拒掉，否则就是真机那次乱飘"
|
||||
assert why is not None and "借深度" in why
|
||||
|
||||
|
||||
def test_borrowed_depth_small_update_is_allowed():
|
||||
"""守卫不能过严：借深度但位移很小（连续性）要放行，否则掉深度帧就丢跟踪。"""
|
||||
g = TrackSlewGuard(max_step_m=0.04)
|
||||
g.update([0.566, -0.172])
|
||||
p, why = g.update([0.570, -0.170], borrowed=True) # 4.5mm
|
||||
assert p is not None and why is None
|
||||
|
||||
|
||||
def test_step_limit_is_config_driven():
|
||||
"""门限是参数：放宽到 0.15m 后，原来被夹的 60mm 观测要能原样通过。"""
|
||||
near = [0.686, -0.053] # 距起点 60mm
|
||||
g = TrackSlewGuard(max_step_m=0.04)
|
||||
g.update([0.686, -0.113])
|
||||
p, why = g.update(near)
|
||||
assert why is not None and tuple(np.round(p, 3)) != tuple(near)
|
||||
|
||||
g2 = TrackSlewGuard(max_step_m=0.15)
|
||||
g2.update([0.686, -0.113])
|
||||
p2, why2 = g2.update(near)
|
||||
assert why2 is None and np.allclose(p2, near, atol=1e-9)
|
||||
|
||||
|
||||
def test_nonfinite_observation_is_rejected():
|
||||
g = TrackSlewGuard(max_step_m=0.04)
|
||||
g.update([0.5, -0.1])
|
||||
p, why = g.update([np.nan, -0.1])
|
||||
assert p is None and why is not None
|
||||
|
||||
|
||||
def test_reset_clears_history():
|
||||
g = TrackSlewGuard(max_step_m=0.04)
|
||||
g.update([0.686, -0.113])
|
||||
g.reset()
|
||||
p, why = g.update([0.083, -0.515]) # reset 后是冷启动 ⇒ 原样采信
|
||||
assert why is None and np.allclose(p, [0.083, -0.515])
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 守卫三：远框重捕获要多帧确认
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_near_reacquire_needs_no_confirmation():
|
||||
c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
|
||||
ok, why = c.allow([400.0, 250.0], 30.0, near_px=120.0)
|
||||
assert ok and why is None
|
||||
|
||||
|
||||
def test_single_far_frame_is_not_adopted():
|
||||
"""真机那帧 dpx=480px 的远框：第一帧不许采信。"""
|
||||
c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
|
||||
ok, why = c.allow([76.0, 75.0], 480.0, near_px=120.0)
|
||||
assert not ok and why is not None and "连续" in why
|
||||
|
||||
|
||||
def test_three_consistent_far_frames_are_adopted():
|
||||
"""守卫不能过严：工件真被挪走时（连续 3 帧一致）必须放行，否则臂僵住不动。"""
|
||||
c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
|
||||
assert c.allow([76.0, 75.0], 480.0, near_px=120.0)[0] is False
|
||||
assert c.allow([80.0, 78.0], 478.0, near_px=120.0)[0] is False
|
||||
assert c.allow([78.0, 76.0], 479.0, near_px=120.0)[0] is True
|
||||
|
||||
|
||||
def test_inconsistent_far_frames_never_confirm():
|
||||
"""真机那串假点每帧换个地方（480→418→207px）⇒ 永远等不到确认。"""
|
||||
c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
|
||||
for cx, cy in ((76.0, 75.0), (255.0, 248.0), (143.0, 92.0), (12.0, 30.0), (400.0, 250.0)):
|
||||
assert c.allow([cx, cy], 300.0, near_px=120.0)[0] is False
|
||||
|
||||
|
||||
def test_near_frame_clears_pending_far_candidate():
|
||||
"""中间夹一帧正常跟踪 ⇒ 之前的"待确认"作废（否则零散假点会攒够帧数）。"""
|
||||
c = FarReacquireConfirm(far_px=120.0, confirm_frames=3)
|
||||
c.allow([76.0, 75.0], 480.0, near_px=120.0)
|
||||
c.allow([78.0, 76.0], 479.0, near_px=120.0)
|
||||
c.allow([410.0, 246.0], 20.0, near_px=120.0) # 正常帧 → 清空
|
||||
ok, _ = c.allow([76.0, 75.0], 480.0, near_px=120.0)
|
||||
assert not ok, "正常帧之后的远框应当从 1 帧重新数"
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 接线：配置 + 调用点
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _site() -> dict:
|
||||
return yaml.safe_load(SITE.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _perception_src() -> str:
|
||||
p = ROOT / "src" / "arm_application" / "arm_application" / "perception_node.py"
|
||||
assert p.is_file(), f"找不到 {p}"
|
||||
return p.read_text(encoding="utf-8")
|
||||
|
||||
|
||||
def _fn_body(src: str, name: str) -> str:
|
||||
"""取 ``def name(...)`` 的函数体（到下一个顶层 ``def`` 或文件尾）。"""
|
||||
i = src.index(f"def {name}(")
|
||||
j = src.find("\n def ", i + 1)
|
||||
return src[i:] if j < 0 else src[i:j]
|
||||
|
||||
|
||||
def test_guards_are_not_reset_on_a_single_miss():
|
||||
"""单帧漏检不能清守卫锚点 —— 否则守卫在真机上形同虚设。
|
||||
|
||||
真机 2026-10-08 的日志里 ``检测漏检`` 几乎每隔一两帧就出现一次。若每漏一帧
|
||||
就 ``_slew.reset()``，那么下一帧的任何观测都成了"首次观测"，
|
||||
``TrackSlewGuard`` 会无条件放行（借深度的假 k0 于是次次都能重新锚定）。
|
||||
正确做法：只在**长丢**（``miss_clear_frames`` 帧）走 ``_clear_track`` 时才冷启动。
|
||||
"""
|
||||
src = _perception_src()
|
||||
# 单帧漏检分支（best is None → 计数 → 长丢判断之前）不许 reset 守卫
|
||||
i = src.index("if best is None:")
|
||||
j = src.index("if self._miss_count >= self.miss_clear_frames:", i)
|
||||
miss_branch = src[i:j]
|
||||
assert "_slew.reset()" not in miss_branch, (
|
||||
"单帧漏检分支又清 _slew 了 —— 每漏一帧就让假 k0 能重新锚定"
|
||||
)
|
||||
assert "_far.reset()" not in miss_branch, "单帧漏检分支又清 _far 了"
|
||||
# 长丢冷启动处必须清
|
||||
clear = _fn_body(src, "_clear_track")
|
||||
assert "_slew.reset()" in clear, "长丢时没清 _slew —— 真丢目标后追不回来"
|
||||
assert "_far.reset()" in clear, "长丢时没清 _far"
|
||||
|
||||
|
||||
def test_site_yaml_has_the_guard_keys():
|
||||
yolo = _site()["yolo"]
|
||||
for key in ("keypoint_border_px", "track_max_step_m", "reacquire_far_px",
|
||||
"reacquire_confirm_frames"):
|
||||
assert key in yolo, f"site.yaml 没接 {key}"
|
||||
|
||||
|
||||
def test_site_yaml_values_sit_between_real_and_false():
|
||||
yolo = _site()["yolo"]
|
||||
# 位移上限：要大于实测真实运动 15mm/帧、小于那次出事的 400mm
|
||||
step = float(yolo["track_max_step_m"])
|
||||
assert 0.015 < step < 0.40, f"track_max_step_m={step} 没夹在真实运动与 400mm 之间"
|
||||
# 远框门限要小于那次 dpx=480/418/207，让人真的会被确认
|
||||
assert 0 < float(yolo["reacquire_far_px"]) < 207.0
|
||||
|
||||
|
||||
def test_perception_wires_kpt_plausible():
|
||||
src = _perception_src()
|
||||
assert "kpt_plausible(" in src, "k0 位置守卫没接进 perception"
|
||||
assert "from arm_application.feature_guard import" in src or "feature_guard import" in src
|
||||
|
||||
|
||||
def test_perception_wires_slew_and_reacquire_guards():
|
||||
src = _perception_src()
|
||||
assert "TrackSlewGuard(" in src, "位移上限没接"
|
||||
assert "FarReacquireConfirm(" in src, "远框重捕获确认没接"
|
||||
assert "self.reacquire_far_px =" in src, (
|
||||
"参数读了没存成属性 —— 远框确认会 AttributeError，看门狗跟着急停"
|
||||
)
|
||||
|
||||
|
||||
def _export_site_env():
|
||||
"""import scripts/lib/export_site_env（launch 也是这么接的）。"""
|
||||
import sys
|
||||
|
||||
lib = ROOT / "scripts" / "lib"
|
||||
if str(lib) not in sys.path:
|
||||
sys.path.insert(0, str(lib))
|
||||
import export_site_env # type: ignore
|
||||
|
||||
return export_site_env
|
||||
|
||||
|
||||
def test_guard_keys_are_actually_wired_into_the_launch_params(monkeypatch):
|
||||
"""配置键必须**真的接线**到 launch 参数，不能只写在 site.yaml 里。
|
||||
|
||||
这是 R1.1 的机器化执行：节点的 declare_parameter 默认值恰好等于 site.yaml
|
||||
当前值，所以"没接线"不会报错，只会让以后改 site.yaml 毫无反应 —— 最难查的
|
||||
一类问题。判据：把 site.yaml 的值改成一个特异值，导出的参数必须跟着变。
|
||||
"""
|
||||
# 清掉环境里的选臂变量，保证这里选的是 probe_robot 写的那台（可复现）。
|
||||
monkeypatch.delenv("ROBOT_SN", raising=False)
|
||||
monkeypatch.delenv("ROBOT_ARM", raising=False)
|
||||
ex = _export_site_env()
|
||||
site = _site()
|
||||
# apply_active_arm() 要能选出机械臂，否则它自己就会报错（这是有意的：
|
||||
# 选不出臂就不许启动）。这里给一个最小可解析的 robot 段，值全部来自真实
|
||||
# site.yaml，只是把待检查的键换成特异值。
|
||||
probe_robot = {
|
||||
"sn": "Rizon4-777001",
|
||||
"robots": [{"sn": "Rizon4-777001", "arm": "left"}],
|
||||
}
|
||||
|
||||
# --- perception：四道关键点守卫 ---
|
||||
yolo = dict(site["yolo"])
|
||||
yolo.update({
|
||||
"keypoint_border_px": 11.0,
|
||||
"track_max_step_m": 0.033,
|
||||
"reacquire_far_px": 99.0,
|
||||
"reacquire_confirm_frames": 5,
|
||||
})
|
||||
probe = {"yolo": yolo, "robot": probe_robot}
|
||||
popt = ex.perception_overrides(probe)
|
||||
assert popt["keypoint_border_px"] == pytest.approx(11.0)
|
||||
assert popt["track_max_step_m"] == pytest.approx(0.033)
|
||||
assert popt["reacquire_far_px"] == pytest.approx(99.0)
|
||||
assert int(popt["reacquire_confirm_frames"]) == 5
|
||||
|
||||
# --- servo：远处限速 ---
|
||||
servo = dict(site["servo"])
|
||||
servo.update({"slow_far_m": 0.021, "v_slow_xy": 0.067})
|
||||
sopt = ex.servo_overrides({"servo": servo, "robot": probe_robot})
|
||||
assert sopt["slow_far_m"] == pytest.approx(0.021), "slow_far_m 没接线（改了配置也不生效）"
|
||||
assert sopt["v_slow_xy"] == pytest.approx(0.067), "v_slow_xy 没接线"
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 端到端：真机那几帧假 k0 走一遍 _keypoint_feature，必须**不采信它的位置**
|
||||
# --------------------------------------------------------------------------- #
|
||||
class _StubDet:
|
||||
def __init__(self, uv, conf, box=BOX):
|
||||
from arm_application.pose_detector import Detection
|
||||
|
||||
self._d = Detection(
|
||||
xyxy=np.asarray(box, dtype=float), conf=0.9, cls_id=0, cls_name="workpiece",
|
||||
kpts=np.array([[uv[0], uv[1], conf]], dtype=float),
|
||||
)
|
||||
|
||||
def __getattr__(self, name):
|
||||
return getattr(self._d, name)
|
||||
|
||||
|
||||
class _StubKpt:
|
||||
"""``_keypoint_feature`` 用到的成员。"""
|
||||
|
||||
def __init__(self, *, frac=None, depth_at=0.22, depth_box=0.22):
|
||||
self.logs = []
|
||||
self.keypoint_index = 0
|
||||
self.keypoint_min_conf = 0.30
|
||||
self.keypoint_border_px = 6.0
|
||||
self._kpt_box_frac = None if frac is None else np.asarray(frac, dtype=float)
|
||||
self._last_good_z = None
|
||||
self._depth_at = depth_at
|
||||
self._depth_box = depth_box
|
||||
|
||||
class _L:
|
||||
def __init__(self, sink):
|
||||
self._s = sink
|
||||
|
||||
def _add(self, m, **_):
|
||||
self._s.append(str(m))
|
||||
|
||||
info = warn = error = debug = _add
|
||||
|
||||
def get_logger(self):
|
||||
return self._L(self.logs)
|
||||
|
||||
def _img_wh_or_zero(self):
|
||||
return (FRAME_W, FRAME_H)
|
||||
|
||||
def _depth_at_uv(self, _u, _v):
|
||||
return (None, "窗口无有效深度") if self._depth_at is None else (self._depth_at, "win6")
|
||||
|
||||
def _depth_in_box(self, _b):
|
||||
return (None, "框内无有效深度") if self._depth_box is None else (self._depth_box, "框内中位")
|
||||
|
||||
def _pixel_to_cam(self, u, v, z):
|
||||
return np.array([(u - 424.0) * z / 380.0, (v - 240.0) * z / 380.0, z])
|
||||
|
||||
|
||||
def _kpt_feature(stub, uv, conf):
|
||||
from arm_application.perception_node import PerceptionNode
|
||||
|
||||
return PerceptionNode._keypoint_feature(stub, _StubDet(uv, conf))
|
||||
|
||||
|
||||
def test_real_true_k0_is_used_as_is():
|
||||
"""真 k0（conf 0.87 uv=(410,246)，框内、不贴边）必须原样用它的 uv。"""
|
||||
feat = _kpt_feature(_StubKpt(), TRUE_K0, 0.87)
|
||||
assert feat is not None
|
||||
assert tuple(np.round(feat.uv)) == TRUE_K0, "真 k0 的 uv 被改了 —— 守卫过严"
|
||||
assert feat.borrowed is False # 该像素有自己的深度
|
||||
|
||||
|
||||
def test_border_false_k0_is_not_used_even_at_conf_073():
|
||||
"""真机那帧 conf=0.73 uv=(12,30)（贴画面角）：**不许**用它的位置。
|
||||
|
||||
有可信历史偏移时走"按偏移重建"，位置必须回到历史点上（不是那个角落）。
|
||||
"""
|
||||
frac = np.array([0.25, -0.25]) # 锁定阶段学到的 k0 相对框偏移
|
||||
feat = _kpt_feature(_StubKpt(frac=frac), (12.0, 30.0), 0.73)
|
||||
assert feat is not None, "有可信历史就不该整帧丢掉"
|
||||
assert not np.allclose(feat.uv, [12.0, 30.0]), "贴边的假 k0 被采信了 —— 就是它把臂带走的"
|
||||
# 重建点在框上，且等于框心 + 历史偏移
|
||||
cx = 0.5 * (BOX[0] + BOX[2])
|
||||
cy = 0.5 * (BOX[1] + BOX[3])
|
||||
bw = BOX[2] - BOX[0]
|
||||
bh = BOX[3] - BOX[1]
|
||||
assert tuple(np.round(feat.uv)) == (round(cx + 0.25 * bw), round(cy - 0.25 * bh))
|
||||
|
||||
|
||||
def test_border_false_k0_without_history_is_dropped():
|
||||
"""冷启动时贴边假 k0 直接不用它（返回 None，交由调用方回落）。"""
|
||||
feat = _kpt_feature(_StubKpt(frac=None), (12.0, 30.0), 0.73)
|
||||
assert feat is None, "没有可信历史时贴边假 k0 必须被丢掉"
|
||||
|
||||
|
||||
def test_borrowed_depth_is_flagged():
|
||||
"""k0 处读不到深度 ⇒ 借框内深度，并**显式标记** borrowed（下游据此禁重定位）。"""
|
||||
feat = _kpt_feature(_StubKpt(depth_at=None, depth_box=0.22), TRUE_K0, 0.87)
|
||||
assert feat is not None and feat.borrowed is True
|
||||
assert "借深度" in feat.note
|
||||
|
||||
|
||||
def test_true_k0_outside_box_is_not_used():
|
||||
"""落在自己框外的"k0"（孔心不在工件外）同样不许采信位置。"""
|
||||
frac = np.array([0.1, 0.1])
|
||||
feat = _kpt_feature(_StubKpt(frac=frac), (700.0, 250.0), 0.90)
|
||||
assert feat is not None
|
||||
assert not np.allclose(feat.uv, [700.0, 250.0])
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 守卫四（伺服侧）：远处限速 —— 与"目标真不真"无关，只保证"追错时走得少"
|
||||
# --------------------------------------------------------------------------- #
|
||||
class _StubServo:
|
||||
def __init__(self, *, slow_far_m: float = 0.05, v_slow_xy: float = 0.08):
|
||||
self.slow_far_m = slow_far_m
|
||||
self.v_slow_xy = v_slow_xy
|
||||
|
||||
|
||||
def _slow(st, nerr: float, v_xy: float):
|
||||
return ServoNode._slow_xy_limit(st, nerr, v_xy) # type: ignore[arg-type]
|
||||
|
||||
|
||||
def test_far_target_is_slowed_down():
|
||||
"""真机那次的 400mm 级误差必须被限速（否则 0.7s 带走 400mm）。"""
|
||||
lim, why = _slow(_StubServo(), 0.400, 0.45)
|
||||
assert lim == pytest.approx(0.08), "远处没限速 —— 就是它把臂带到工作盒角落的"
|
||||
assert why is not None and "目标离" in why
|
||||
|
||||
|
||||
def test_near_target_keeps_full_speed():
|
||||
"""守卫不能过严：目标近了必须放开速度，否则收敛永远慢吞吞（死区都进不去）。"""
|
||||
lim, why = _slow(_StubServo(), 0.020, 0.45)
|
||||
assert lim == pytest.approx(0.45) and why is None
|
||||
|
||||
|
||||
def test_boundary_just_over_the_threshold_is_slowed():
|
||||
lim, why = _slow(_StubServo(), 0.051, 0.45)
|
||||
assert lim == pytest.approx(0.08) and why is not None
|
||||
lim2, _ = _slow(_StubServo(), 0.049, 0.45)
|
||||
assert lim2 == pytest.approx(0.45)
|
||||
|
||||
|
||||
def test_slow_limit_is_config_driven():
|
||||
"""限速值来自配置：改大之后远处目标就该跑得更快（否则阈值写死、配置没接线）。"""
|
||||
lim, _ = _slow(_StubServo(slow_far_m=0.30, v_slow_xy=0.20), 0.400, 0.45)
|
||||
assert lim == pytest.approx(0.20)
|
||||
# 比门限更近 ⇒ 完全放开
|
||||
lim2, _ = _slow(_StubServo(slow_far_m=0.50, v_slow_xy=0.20), 0.400, 0.45)
|
||||
assert lim2 == pytest.approx(0.45)
|
||||
|
||||
|
||||
def test_slow_limit_never_raises_the_speed():
|
||||
"""限速只能降不能升：v_max_xy 本来就比 v_slow_xy 小时不许被"抬"上去。"""
|
||||
lim, _ = _slow(_StubServo(), 0.400, 0.03)
|
||||
assert lim == pytest.approx(0.03)
|
||||
|
||||
|
||||
def test_site_yaml_has_the_slow_approach_keys():
|
||||
servo = _site()["servo"]
|
||||
for key in ("slow_far_m", "v_slow_xy"):
|
||||
assert key in servo, f"site.yaml 没接 {key}"
|
||||
# 必须真的能拦住真机那次：门限 < 400mm、限速 < v_max_xy
|
||||
assert 0.02 < float(servo["slow_far_m"]) < 0.40
|
||||
assert float(servo["v_slow_xy"]) < float(servo["v_max_xy"])
|
||||
|
||||
|
||||
def test_servo_wires_the_slow_limit():
|
||||
p = ROOT / "src" / "arm_application" / "arm_application" / "servo_node.py"
|
||||
src = p.read_text(encoding="utf-8")
|
||||
assert "v_xy = min(float(v_xy), cruise)" in src, "锁定前的统一巡航限速未接入"
|
||||
assert "def _slow_xy_limit(" in src
|
||||
assert 'declare_parameter("slow_far_m"' in src
|
||||
@@ -0,0 +1,357 @@
|
||||
"""跟踪点"假跳变"的两道防线（负向测试：改回去必须变红）。
|
||||
|
||||
为什么单独立一条（2026-10-08 真机实测，arm 89 一阶段跑 150s 没锁上）
|
||||
-------------------------------------------------------------------
|
||||
日志原话（gongjian0929，跟踪锁高 0.370m）：
|
||||
|
||||
0659.551 死区保持 |e_xy|=3.0mm ← 其实已经锁定！误差只剩 3mm
|
||||
0659.598 k0 置信度 0.01 < 0.30，本帧不用它
|
||||
0659.604 HANDEYE snap feat_base=(0.675,-0.099) ← 跟踪点瞬间横移 89mm
|
||||
0660.121 v=(-0.206,+0.400) |e_xy|=130mm ← 伺服以 0.45m/s 去追这个假点
|
||||
0660.561 过期保持 —— 不跟踪冻结的检测框 ← 画面糊掉，目标彻底丢失
|
||||
|
||||
根因是**两处**，各自都要能独立失败：
|
||||
|
||||
1. ``perception_node._keypoint_feature``：k0 掉信时兜底把跟踪点从 k0 切到
|
||||
"框中心" —— 而框中心离 k0 有 89mm（k0 是工件上的孔，不在框心）。正确的兜底
|
||||
是**借 k0 相对框的归一化偏移**在框上重建 k0：位置连续、工件动时还能跟着动。
|
||||
2. ``servo_node._update_feat_track``：``dxy > predict_jump_m`` 时单帧就
|
||||
``reset()`` **采信**假跳。30Hz 相机 + 工件不动，一帧跳 89mm 物理上不可能
|
||||
⇒ 必须连续 N 帧指向同一新位置才认。
|
||||
|
||||
用例全离线：用打桩对象直接调被测方法（不建 ROS 实体、不发运动指令），
|
||||
与 ``test_stage2_descent_clamp`` 同一套做法。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, List, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.feature_jump_guard import (
|
||||
ACCEPT,
|
||||
HOLD,
|
||||
REJECT,
|
||||
FeatureJumpGate,
|
||||
)
|
||||
from arm_application.kalman_filter import KalmanFilter3D
|
||||
from arm_application.perception_node import PerceptionNode
|
||||
from arm_application.servo_node import ServoNode
|
||||
from arm_application.pose_detector import Detection
|
||||
|
||||
# 真机那次的关键数字
|
||||
_JUMP_M = 0.08 # servo.predict_jump_m
|
||||
_K0_UV = np.array([296.0, 262.0]) # 锁定瞬间的 k0
|
||||
_BOX = np.array([180.0, 120.0, 620.0, 420.0]) # 工件框
|
||||
# 真机日志里那一跳：(0.630,-0.176) → (0.675,-0.099)
|
||||
_FEAT_BEFORE = np.array([0.630, -0.176])
|
||||
_FEAT_AFTER = np.array([0.675, -0.099])
|
||||
|
||||
|
||||
class _StubLogger:
|
||||
"""把日志写进调用方给的 sink（和被测对象共享同一个 list）。"""
|
||||
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def _add(self, msg: str, **_: Any) -> None:
|
||||
self._sink.append(str(msg))
|
||||
|
||||
info = _add
|
||||
warn = _add
|
||||
error = _add
|
||||
debug = _add
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 防线 1：perception 的 k0 掉信兜底 —— 借偏移，不借绝对位置
|
||||
# --------------------------------------------------------------------------- #
|
||||
class _StubPerception:
|
||||
"""``_keypoint_feature`` 用到的成员全部打桩。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
kpt_conf: float,
|
||||
last_uv: Optional[np.ndarray],
|
||||
frac: Optional[np.ndarray],
|
||||
depth_at: Optional[float] = 0.21,
|
||||
depth_box: Optional[float] = 0.22,
|
||||
) -> None:
|
||||
self.logs: List[str] = []
|
||||
self.keypoint_index = 0
|
||||
self.keypoint_min_conf = 0.30
|
||||
# k0 位置守卫的参数与画面尺寸（用真机 D405 的 848x480，见 feature_guard）
|
||||
self.keypoint_border_px = 6.0
|
||||
self._kpt_box_frac = None if frac is None else np.asarray(frac, dtype=float)
|
||||
self._last_uv = None if last_uv is None else np.asarray(last_uv, dtype=float)
|
||||
self._kpt_conf = float(kpt_conf)
|
||||
self._depth_at = depth_at
|
||||
self._depth_box = depth_box
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return _StubLogger(self.logs)
|
||||
|
||||
def _img_wh_or_zero(self) -> tuple:
|
||||
# 真机 D405 彩色图尺寸（k0 位置守卫要用它判"贴画面边"）
|
||||
return (848, 480)
|
||||
|
||||
def _depth_at_uv(self, _u: float, _v: float) -> tuple:
|
||||
if self._depth_at is None:
|
||||
return None, "窗口无有效深度"
|
||||
return float(self._depth_at), "win6"
|
||||
|
||||
def _depth_in_box(self, _xyxy: Any) -> tuple:
|
||||
if self._depth_box is None:
|
||||
return None, "框内无有效深度"
|
||||
return float(self._depth_box), "框内中位"
|
||||
|
||||
def _pixel_to_cam(self, u: float, v: float, z: float) -> np.ndarray:
|
||||
# 只要求"同一 uv 得同一 3D 点"，用最简针孔映射即可（不参与本用例断言）
|
||||
return np.array([(u - 400.0) * z / 380.0, (v - 240.0) * z / 380.0, z])
|
||||
|
||||
def det(self, kpt_uv: np.ndarray) -> Detection:
|
||||
return Detection(
|
||||
xyxy=_BOX.copy(),
|
||||
conf=0.9,
|
||||
cls_id=0,
|
||||
cls_name="workpiece",
|
||||
kpts=np.array([[kpt_uv[0], kpt_uv[1], self._kpt_conf]], dtype=float),
|
||||
)
|
||||
|
||||
|
||||
def _feat(stub: _StubPerception, kpt_uv: np.ndarray) -> Any:
|
||||
"""直接调被测方法（打桩对象冒充 self）。"""
|
||||
return PerceptionNode._keypoint_feature(stub, stub.det(kpt_uv))
|
||||
|
||||
|
||||
def test_kpt_low_conf_rebuilds_on_box_not_box_center():
|
||||
"""k0 掉信时**不许**把跟踪点横移到框中心。"""
|
||||
# 锁定阶段记下的 k0 相对框偏移：k0 在框心右侧偏上
|
||||
frac = np.array([0.25, -0.25])
|
||||
stub = _StubPerception(kpt_conf=0.05, last_uv=_K0_UV, frac=frac)
|
||||
box_cx = 0.5 * (_BOX[0] + _BOX[2])
|
||||
box_cy = 0.5 * (_BOX[1] + _BOX[3])
|
||||
bw = _BOX[2] - _BOX[0]
|
||||
bh = _BOX[3] - _BOX[1]
|
||||
|
||||
feat = _feat(stub, kpt_uv=np.array([150.0, 60.0])) # 掉信帧的 k0 是垃圾值
|
||||
|
||||
assert feat is not None, "有可信历史时必须给得出跟踪点（不能退回 None→框中心）"
|
||||
expect = np.array([box_cx + frac[0] * bw, box_cy + frac[1] * bh])
|
||||
assert np.allclose(feat.uv, expect, atol=1e-6)
|
||||
# 负向：绝不能等于框中心
|
||||
assert not np.allclose(feat.uv, [box_cx, box_cy], atol=1.0), (
|
||||
f"跟踪点落回框中心了 uv={feat.uv} 框心=({box_cx},{box_cy})"
|
||||
)
|
||||
assert "保持" in feat.note
|
||||
|
||||
|
||||
def test_kpt_low_conf_rebuild_follows_box_motion():
|
||||
"""框移动时，重建出来的 k0 要跟着动 —— 相对框的偏移必须保持。"""
|
||||
frac = np.array([0.25, -0.25])
|
||||
stub = _StubPerception(kpt_conf=0.02, last_uv=_K0_UV, frac=frac)
|
||||
|
||||
moved_box = _BOX + np.array([40.0, 0.0, 40.0, 0.0]) # 整框右移 40px
|
||||
d = stub.det(np.array([150.0, 60.0]))
|
||||
d.xyxy = moved_box
|
||||
feat = PerceptionNode._keypoint_feature(stub, d)
|
||||
|
||||
box_cx = 0.5 * (moved_box[0] + moved_box[2])
|
||||
box_cy = 0.5 * (moved_box[1] + moved_box[3])
|
||||
bw = moved_box[2] - moved_box[0]
|
||||
bh = moved_box[3] - moved_box[1]
|
||||
expect = np.array([box_cx + frac[0] * bw, box_cy + frac[1] * bh])
|
||||
assert np.allclose(feat.uv, expect, atol=1e-6)
|
||||
# 相对框心的偏移（归一化后）没变 —— 这才是"跟着走且不跳"的含义
|
||||
got_frac = np.array(
|
||||
[(feat.uv[0] - box_cx) / bw, (feat.uv[1] - box_cy) / bh]
|
||||
)
|
||||
assert np.allclose(got_frac, frac, atol=1e-9)
|
||||
|
||||
|
||||
def test_kpt_trusted_updates_offset_and_keeps_k0_uv():
|
||||
"""k0 可信时必须用 k0 自己的 uv，并顺手更新偏移记录。"""
|
||||
stub = _StubPerception(kpt_conf=0.87, last_uv=None, frac=None)
|
||||
feat = _feat(stub, kpt_uv=_K0_UV)
|
||||
|
||||
assert feat is not None
|
||||
assert np.allclose(feat.uv, _K0_UV, atol=1e-6)
|
||||
assert stub._kpt_box_frac is not None, "可信帧必须记下 k0 相对框的偏移"
|
||||
box_cx = 0.5 * (_BOX[0] + _BOX[2])
|
||||
bw = _BOX[2] - _BOX[0]
|
||||
assert abs(float(stub._kpt_box_frac[0]) - (_K0_UV[0] - box_cx) / bw) < 1e-9
|
||||
|
||||
|
||||
def test_kpt_no_history_still_falls_back():
|
||||
"""冷启动（从没见过可信 k0）时没有更好的信息 —— 此时才允许返回 None。"""
|
||||
stub = _StubPerception(kpt_conf=0.05, last_uv=None, frac=None)
|
||||
assert _feat(stub, kpt_uv=np.array([150.0, 60.0])) is None
|
||||
|
||||
|
||||
def test_kpt_trusted_but_no_depth_keeps_k0_uv_and_borrows_depth():
|
||||
"""只有深度读不到时：XY 仍用 k0（不许横移），深度可以借框内。"""
|
||||
stub = _StubPerception(kpt_conf=0.80, last_uv=None, frac=None, depth_at=None)
|
||||
feat = _feat(stub, kpt_uv=_K0_UV)
|
||||
|
||||
assert feat is not None
|
||||
assert np.allclose(feat.uv, _K0_UV, atol=1e-6), "深度不可用时把 XY 也弄丢了"
|
||||
assert abs(feat.z - 0.22) < 1e-9, "应从框内借到深度"
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 防线 2：servo 的跳变确认门限
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_jump_gate_rejects_single_frame_spurious_jump():
|
||||
"""真机那一跳：单帧 89mm，必须判 hold（不采信）。"""
|
||||
gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
|
||||
assert float(np.hypot(*(_FEAT_AFTER - _FEAT_BEFORE))) > _JUMP_M # 前提：确实是大跳
|
||||
assert gate.update(_FEAT_AFTER, _FEAT_BEFORE) == FeatureJumpGate.HOLD
|
||||
|
||||
|
||||
def test_jump_gate_confirms_persistent_relocation():
|
||||
"""真正换了位置时，连续 3 帧一致就要认（不能永远 hold 住）。"""
|
||||
gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
|
||||
new = np.array([0.760, -0.180])
|
||||
assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.HOLD
|
||||
assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.HOLD
|
||||
assert gate.update(new, _FEAT_BEFORE) == FeatureJumpGate.JUMP
|
||||
|
||||
|
||||
def test_jump_gate_near_is_passthrough():
|
||||
"""正常小位移不该被拦住（真实运动 ≤15mm/帧）。"""
|
||||
gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
|
||||
for d in (0.005, -0.010, 0.015):
|
||||
assert gate.update(
|
||||
np.array([_FEAT_BEFORE[0] + d, _FEAT_BEFORE[1]]), _FEAT_BEFORE
|
||||
) == FeatureJumpGate.NEAR
|
||||
|
||||
|
||||
def test_jump_gate_inconsistent_candidates_never_confirm():
|
||||
"""每帧跳到不同地方（k0 掉信时的乱跳）永远确认不了。"""
|
||||
gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
|
||||
for p in ([0.675, -0.099], [0.570, -0.046], [0.700, -0.250], [0.520, -0.020]):
|
||||
assert gate.update(np.array(p), _FEAT_BEFORE) == FeatureJumpGate.HOLD
|
||||
assert gate.hold_total == 4
|
||||
|
||||
|
||||
def test_jump_gate_single_confirm_frame_is_explicit_legacy():
|
||||
"""confirm_frames=1 是显式退回旧行为 —— 是"选择"，不是静默降级。"""
|
||||
gate = FeatureJumpGate(_JUMP_M, confirm_frames=1)
|
||||
assert gate.update(_FEAT_AFTER, _FEAT_BEFORE) == FeatureJumpGate.JUMP
|
||||
|
||||
|
||||
class _StubServo:
|
||||
"""``_update_feat_track`` 用到的成员全部打桩。
|
||||
|
||||
``ws_min/ws_max`` 取 site.yaml 现行值（法兰工作盒），特征点在盒外时会被判误检。
|
||||
"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.logs: List[str] = []
|
||||
self.predict_jump_m = _JUMP_M
|
||||
self.feat_ws_margin_m = 0.05
|
||||
# config/site.yaml 的 servo.workspace_xyz_min/max
|
||||
self.ws_min = np.array([0.08, -0.55, 0.18], dtype=float)
|
||||
self.ws_max = np.array([0.85, 0.55, 0.65], dtype=float)
|
||||
self._feat_kf = KalmanFilter3D(q_pos=2.0e-3, q_vel=1.5e-1, r_pos=4.0e-4)
|
||||
self._jump_gate = FeatureJumpGate(_JUMP_M, confirm_frames=3)
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return _StubLogger(self.logs)
|
||||
|
||||
# 真实方法（纯计算，不碰 ROS）—— 挂上来，避免把它的逻辑抄一份到测试里
|
||||
_feat_workspace_ok = ServoNode._feat_workspace_ok
|
||||
|
||||
|
||||
def _update(stub: _StubServo, p_base: np.ndarray, *, was_lost: bool, now_m: float) -> str:
|
||||
return ServoNode._update_feat_track(stub, p_base, was_lost=was_lost, now_m=now_m)
|
||||
|
||||
|
||||
# 真机实测的"假点"：落在基座后方，而工件在 (0.5,-0.12)
|
||||
_BOGUS_FAR = np.array([-0.102, -0.648, -0.01])
|
||||
|
||||
|
||||
def test_servo_rejects_feature_outside_workspace():
|
||||
"""物理合理性：工作盒外的点必须被判误检（当"没有目标"，不是"钉住"）。"""
|
||||
stub = _StubServo()
|
||||
p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
|
||||
assert _update(stub, p0, was_lost=True, now_m=1.0) == ACCEPT
|
||||
# 真机那一串假点：(-0.102,-0.648) 在工件反方向、x 甚至为负
|
||||
assert _update(stub, _BOGUS_FAR, was_lost=False, now_m=1.033) == REJECT
|
||||
assert "工作盒外" in " ".join(stub.logs)
|
||||
|
||||
|
||||
def test_servo_reject_wins_even_if_bogus_is_persistent():
|
||||
"""假点就算"稳定地"出现在盒外 N 帧，也必须一直 reject（不能被确认为重定位）。"""
|
||||
stub = _StubServo()
|
||||
p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
|
||||
_update(stub, p0, was_lost=True, now_m=1.0)
|
||||
for i in range(1, 6): # 同一个盒外点重复来 5 帧
|
||||
assert _update(stub, _BOGUS_FAR, was_lost=False, now_m=1.0 + 0.033 * i) == REJECT
|
||||
# 位置必须还钉在原处（没有被假点带跑）
|
||||
assert np.allclose(stub._feat_kf.position[:2], _FEAT_BEFORE, atol=5e-4)
|
||||
|
||||
|
||||
def test_servo_reject_holds_when_kf_not_initialized():
|
||||
"""冷启动就收到盒外点：不能把它当基准点初始化。"""
|
||||
stub = _StubServo()
|
||||
assert _update(stub, _BOGUS_FAR, was_lost=True, now_m=1.0) == REJECT
|
||||
assert not stub._feat_kf.initialized, "盒外的假点被当成了初始基准"
|
||||
|
||||
|
||||
def test_feat_workspace_ok_accepts_real_workpiece():
|
||||
"""真实工件点（0.5,-0.12）绝不能被误判成盒外。"""
|
||||
stub = _StubServo()
|
||||
assert ServoNode._feat_workspace_ok(stub, np.array([0.500, -0.120, 0.022])) is True
|
||||
# 盒边界附近（含余量内）也放行
|
||||
assert ServoNode._feat_workspace_ok(stub, np.array([0.08, -0.55, 0.022])) is True
|
||||
|
||||
|
||||
def test_servo_holds_position_on_spurious_jump():
|
||||
"""真机场景：锁定后单帧假跳 —— 位置必须钉住不动（旧代码会 reset 追过去）。"""
|
||||
stub = _StubServo()
|
||||
p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
|
||||
assert _update(stub, p0, was_lost=True, now_m=1.0) == ACCEPT # 首次捕获
|
||||
held_pos = stub._feat_kf.position.copy()
|
||||
assert np.allclose(held_pos[:2], _FEAT_BEFORE, atol=1e-6)
|
||||
|
||||
jumped = np.array([_FEAT_AFTER[0], _FEAT_AFTER[1], -0.008])
|
||||
assert _update(stub, jumped, was_lost=False, now_m=1.033) == HOLD, "必须判 hold"
|
||||
assert np.allclose(
|
||||
stub._feat_kf.position[:2], _FEAT_BEFORE, atol=5e-4
|
||||
), "hold 之后位置被假点带跑了"
|
||||
|
||||
|
||||
def test_servo_confirms_real_relocation_after_n_frames():
|
||||
"""真换了位置：连续 3 帧一致后必须采信（不能把 hold 变成"卡死"）。"""
|
||||
stub = _StubServo()
|
||||
p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
|
||||
_update(stub, p0, was_lost=True, now_m=1.0)
|
||||
|
||||
new = np.array([0.760, -0.180, -0.007])
|
||||
assert _update(stub, new, was_lost=False, now_m=1.033) == HOLD
|
||||
assert _update(stub, new, was_lost=False, now_m=1.066) == HOLD
|
||||
assert _update(stub, new, was_lost=False, now_m=1.099) == ACCEPT, "第 3 帧该采信了"
|
||||
assert np.allclose(stub._feat_kf.position[:2], new[:2], atol=2e-3)
|
||||
|
||||
|
||||
def test_servo_hold_after_convergence_keeps_gate_velocity_zero():
|
||||
"""hold 期间速度必须清零，否则旧速度会把"不动"变成"匀速跑掉"。"""
|
||||
stub = _StubServo()
|
||||
# 给 KF 灌一个明显不为零的速度
|
||||
p0 = np.array([_FEAT_BEFORE[0], _FEAT_BEFORE[1], -0.007])
|
||||
_update(stub, p0, was_lost=True, now_m=1.0)
|
||||
for i, dx in enumerate((0.004, 0.008, 0.012), start=1):
|
||||
_update(
|
||||
stub,
|
||||
np.array([_FEAT_BEFORE[0] + dx, _FEAT_BEFORE[1], -0.007]),
|
||||
was_lost=False,
|
||||
now_m=1.0 + 0.033 * i,
|
||||
)
|
||||
assert float(np.hypot(*stub._feat_kf.velocity[:2])) > 1e-3, "前提：速度非零"
|
||||
|
||||
jumped = np.array([_FEAT_AFTER[0], _FEAT_AFTER[1], -0.008])
|
||||
assert _update(stub, jumped, was_lost=False, now_m=1.2) == HOLD
|
||||
assert float(np.hypot(*stub._feat_kf.velocity[:2])) < 1e-9, "hold 时速度没清零"
|
||||
@@ -0,0 +1,200 @@
|
||||
"""腕部 FT 兜底不许"静默失效"：数据源没出数时必须喊出来，且配置必须指向有数的话题。
|
||||
|
||||
为什么单独立一条（2026-10-08 真机实测）
|
||||
--------------------------------------
|
||||
二阶段轻降把"规尖碰工件"的兜底押在腕部 FT 上：``abs(fz − 基线) > 2.5N ⇒ 停``。
|
||||
真机上应用在用的 ``/Rizon4_063989/ft_sensor_wrench`` 却是**恒 (0,0,0)**：
|
||||
|
||||
/Rizon4_063989/ft_sensor_wrench force.z = 0.0 ← 应用在用
|
||||
/Rizon4_063989/external_wrench_in_tcp force.z = -53.84 ← 才有数（负载）
|
||||
|
||||
于是 ``abs(0 − 0) > 2.5`` 永远为假 —— **接触兜底永远触发不了**，而日志里一切正常：
|
||||
话题收得到、回调在跑、``wrench_live``（只看"够不够新"）还报"活"。这是最危险的
|
||||
静默失效：轻降会一直往下走而没有任何力保护。
|
||||
|
||||
两处修（互补，都要有）：
|
||||
|
||||
1. 配置：真机 ``servo.ft_wrench_topic`` 指向有数的 ``external_wrench_in_tcp``
|
||||
（判据是"相对轻降起始基线的增量"，与 54N 那个偏置无关）；仿真**必须**覆盖回
|
||||
``/sim/ft_sensor_wrench``（仿真没有 external_wrench_in_tcp 这个话题）。
|
||||
2. 守卫：``gauge_fsm.wrench_all_zero`` —— 带负载的 FT 不可能六个分量**同时**恰好为
|
||||
0，所以"全零 > 5s"= 数据源没出数，节点必须报 error（一次）。
|
||||
|
||||
这些用例全是**离线**的：``_on_wrench`` 只用到少量成员，用打桩对象即可。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from typing import Any, List
|
||||
|
||||
import yaml
|
||||
from geometry_msgs.msg import Wrench, WrenchStamped
|
||||
|
||||
from arm_application import stage2_locate_node as s2mod
|
||||
from arm_application.gauge_fsm import wrench_all_zero
|
||||
from arm_application.stage2_locate_node import Stage2Locate
|
||||
|
||||
ROOT = __import__("pathlib").Path(__file__).resolve().parents[3]
|
||||
SITE = ROOT / "config" / "site.yaml"
|
||||
|
||||
|
||||
class _Logger:
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def _add(self, msg: str) -> None:
|
||||
self._sink.append(str(msg))
|
||||
|
||||
info = _add
|
||||
warn = _add
|
||||
error = _add
|
||||
|
||||
|
||||
class _Stub:
|
||||
"""``_on_wrench`` 用到的成员全部打桩。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.logs: List[str] = []
|
||||
self._logger = _Logger(self.logs)
|
||||
self._ft_fz = None
|
||||
self._t_ft = None
|
||||
self._ft_zero_since = None
|
||||
self._ft_zero_warned = False
|
||||
self.ft_wrench_topic = "/Rizon4_063989/ft_sensor_wrench"
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return self._logger
|
||||
|
||||
|
||||
def _wrench_msg(*, fz: float = 0.0, tx: float = 0.0) -> WrenchStamped:
|
||||
m = WrenchStamped()
|
||||
m.wrench = Wrench()
|
||||
m.wrench.force.x = 0.0
|
||||
m.wrench.force.y = 0.0
|
||||
m.wrench.force.z = float(fz)
|
||||
m.wrench.torque.x = float(tx)
|
||||
m.wrench.torque.y = 0.0
|
||||
m.wrench.torque.z = 0.0
|
||||
return m
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# wrench_all_zero：区分"安静"与"没出数"
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_all_zero_is_flagged():
|
||||
assert wrench_all_zero(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) is True
|
||||
|
||||
|
||||
def test_a_real_load_is_not_all_zero():
|
||||
"""真机 external_wrench_in_tcp 的读数（≈负载 54N）不能被判成"没出数"。"""
|
||||
assert wrench_all_zero(0.036, 0.32, -53.84, 0.0, 0.0, 0.0) is False
|
||||
|
||||
|
||||
def test_tiny_noise_is_not_all_zero():
|
||||
"""很小但活的读数（噪声量级）也不算"没出数" —— 否则会误报。"""
|
||||
assert wrench_all_zero(1.0e-3, 0.0, 0.0, 0.0, 0.0, 0.0) is False
|
||||
|
||||
|
||||
def test_torque_alone_is_enough_to_prove_it_is_alive():
|
||||
assert wrench_all_zero(0.0, 0.0, 0.0, 0.02, 0.0, 0.0) is False
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 节点守卫：全零 > 5s 报 error（一次），有数则安静
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_all_zero_for_over_5s_raises_error_once():
|
||||
st = _Stub()
|
||||
# 让它一开始就已经"全零了 6 秒"（不 sleep，直接构造状态）
|
||||
st._ft_zero_since = time.monotonic() - 6.0
|
||||
|
||||
Stage2Locate._on_wrench(st, _wrench_msg()) # type: ignore[arg-type]
|
||||
errs = [m for m in st.logs if "全为 0" in m]
|
||||
assert len(errs) == 1, f"数据源恒零却{'没报' if not errs else '报多了'} —— 静默失效"
|
||||
assert "判不到接触" in errs[0], "要说清后果：接触兜底永远不会触发"
|
||||
assert "ft_sensor_wrench" in errs[0], "要打出话题名，现场才知道换哪个"
|
||||
|
||||
# 第二次不该再刷屏
|
||||
Stage2Locate._on_wrench(st, _wrench_msg()) # type: ignore[arg-type]
|
||||
assert len([m for m in st.logs if "全为 0" in m]) == 1, "全零告警只许喊一次"
|
||||
|
||||
|
||||
def test_a_live_reading_never_warns_and_is_cached():
|
||||
st = _Stub()
|
||||
Stage2Locate._on_wrench(st, _wrench_msg(fz=-53.84)) # type: ignore[arg-type]
|
||||
assert not [m for m in st.logs if "全为 0" in m], "有数还告警 = 误报"
|
||||
assert st._ft_fz == -53.84, "轴向力必须缓存下来（_contact_detected 要用）"
|
||||
assert st._t_ft is not None, "要记收到时刻"
|
||||
|
||||
|
||||
def test_losing_data_after_a_live_reading_restarts_the_clock():
|
||||
"""先有数、后全零：计时要从"变成全零那一刻"重新起算，不能沿用旧起点。"""
|
||||
st = _Stub()
|
||||
Stage2Locate._on_wrench(st, _wrench_msg(fz=-53.84)) # type: ignore[arg-type]
|
||||
assert st._ft_zero_since is None
|
||||
Stage2Locate._on_wrench(st, _wrench_msg()) # 转全零
|
||||
assert st._ft_zero_since is not None, "转全零要起计时"
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 源码守卫：这两处都不能被悄悄删掉
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_stage2_wires_the_all_zero_guard():
|
||||
src = (
|
||||
ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "wrench_all_zero(" in src, "全零守卫没接进 stage2 的 FT 回调"
|
||||
assert "from arm_application.gauge_fsm import" in src and "wrench_all_zero" in src
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 配置接线：真机指向有数的话题，仿真不许跟着用
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _robots() -> dict:
|
||||
site = yaml.safe_load(SITE.read_text(encoding="utf-8"))
|
||||
return {r["sn"]: r for r in site["robot"]["robots"]}
|
||||
|
||||
|
||||
def test_real_arms_point_at_a_topic_that_actually_has_data():
|
||||
"""真机 FT 源必须是 external_wrench_in_tcp —— ft_sensor_wrench 实测恒零。"""
|
||||
robots = _robots()
|
||||
for sn in ("Rizon4-063989", "Rizon4-063991"):
|
||||
if sn not in robots:
|
||||
continue
|
||||
topic = robots[sn]["servo"].get("ft_wrench_topic")
|
||||
assert topic, f"{sn} 没配 servo.ft_wrench_topic —— 会退回恒零的 ft_sensor_wrench"
|
||||
assert topic.endswith("/external_wrench_in_tcp"), (
|
||||
f"{sn} 的 FT 源是 {topic}；真机只有 external_wrench_in_tcp 有数"
|
||||
)
|
||||
assert sn.replace("-", "_") in topic, "话题名里的 sn 要用下划线（与 broadcaster 一致）"
|
||||
|
||||
|
||||
def test_sim_overrides_the_real_ft_topic():
|
||||
"""仿真必须显式覆盖 —— `*servo_left` 带下来的是真机话题，仿真里不存在。"""
|
||||
topic = _robots()["sim"]["servo"].get("ft_wrench_topic")
|
||||
assert topic == "/sim/ft_sensor_wrench", (
|
||||
f"仿真 FT 话题是 {topic}；仿真由 sim_wrist_ft 发 /sim/ft_sensor_wrench"
|
||||
)
|
||||
|
||||
|
||||
def test_node_reads_the_topic_from_site_config_not_hardcoded():
|
||||
"""话题必须可配（读 servo.ft_wrench_topic），不许把真机话题写死在代码里。
|
||||
|
||||
用语法树取字符串字面量来判（不是文本匹配）—— 这样解释这件事的注释不算数。
|
||||
"""
|
||||
import ast
|
||||
|
||||
src = (
|
||||
ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert 'servo.get("ft_wrench_topic")' in src, "没从 site 配置读话题"
|
||||
literals = [
|
||||
n.value
|
||||
for n in ast.walk(ast.parse(src))
|
||||
if isinstance(n, ast.Constant) and isinstance(n.value, str)
|
||||
]
|
||||
assert not any("Rizon4_063989" in s for s in literals), (
|
||||
"真机 sn/话题被写进了代码字符串（应当只在 site.yaml）"
|
||||
)
|
||||
# 兜底必须是"按 sn 推导"，而不是某个具体话题
|
||||
assert 'f"/{robot_ns}/ft_sensor_wrench"' in src, "兜底不再是按 sn 推导的话题"
|
||||
@@ -0,0 +1,297 @@
|
||||
"""``gauge_boot`` 的单元测试：自检清单**内容**的正确性。
|
||||
|
||||
自检本身很容易变成"每次都过"的仪式（规则 M1）。这里钉住的是它的内容：
|
||||
|
||||
* 核心判据依赖的东西（触觉、相机）必须是**必检** —— 标成选检的话，
|
||||
现场不接触觉也能一路跑到 S07 才失败，而那时报的是"力判据没触发"；
|
||||
* 现场没有的设备（D435）不该生成检查项 —— 生成一个必然失败的项，
|
||||
结果是所有人都不再看自检输出了（狼来了）；
|
||||
* 「为什么需要它」必须写出来，否则现场看到"未就绪"不知道该不该急。
|
||||
"""
|
||||
|
||||
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,
|
||||
)
|
||||
|
||||
|
||||
def _build(**kw):
|
||||
base = dict(
|
||||
joint_action="rizon_arm_controller/follow_joint_trajectory",
|
||||
camera="d405_left",
|
||||
tactile_topic="/tactile/left/status",
|
||||
gripper_open="/gripper/open",
|
||||
gripper_close="/gripper/close",
|
||||
gripper_status="/gripper/status",
|
||||
gripper_status_type="arm_msgs/msg/GripperStatus",
|
||||
)
|
||||
base.update(kw)
|
||||
return build_checks(**base)
|
||||
|
||||
|
||||
def _by_key(checks):
|
||||
return {c.key: c for c in checks}
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 清单内容
|
||||
# =============================================================================
|
||||
def test_core_checks_are_present_and_required():
|
||||
"""核心判据依赖的东西必须是**必检**（配了工具系的那种机器）。"""
|
||||
checks = _by_key(_build(tool_name="tool1"))
|
||||
for key in (
|
||||
"arm_jtc",
|
||||
"arm_home",
|
||||
"arm_named_pose",
|
||||
"arm_switch_tool",
|
||||
"gripper_open",
|
||||
"gripper_close",
|
||||
"camera_color",
|
||||
"camera_info",
|
||||
"tactile_status",
|
||||
"gripper_status",
|
||||
):
|
||||
assert key in checks, f"自检清单缺 {key}"
|
||||
assert checks[key].required, f"{key} 是核心判据依赖，必须是必检"
|
||||
|
||||
|
||||
def test_jtc_check_expects_the_type_this_flow_actually_calls():
|
||||
"""JTC 自检的类型必须是 control_msgs（本流程直接发给 ros2_control）。
|
||||
|
||||
写成 arm_msgs/action/FollowJointTrajectory（arm_control 适配器那一套）会
|
||||
让自检**永远探不到服务端**：报"控制器没起来"，而控制器明明好着。
|
||||
"""
|
||||
c = _by_key(_build())["arm_jtc"]
|
||||
assert c.expect_type == "control_msgs/action/FollowJointTrajectory"
|
||||
|
||||
|
||||
def test_switch_tool_required_only_when_a_tool_is_configured():
|
||||
"""配了工具系 → 切不过去 S01 会中止，所以按必检拦；没配 → 选检。
|
||||
|
||||
没配工具系的机器上（比如仿真）标必检，会让"仿真里根本不存在
|
||||
/switch_tool 服务"变成每次启动的一条红字，人很快就不看自检了。
|
||||
"""
|
||||
assert _by_key(_build(tool_name="tool1"))["arm_switch_tool"].required is True
|
||||
assert _by_key(_build(tool_name=""))["arm_switch_tool"].required is False
|
||||
|
||||
|
||||
def test_compliant_descent_is_optional():
|
||||
"""S07 不依赖柔顺 action（节点内步进 + 力突变停是同一套做法）。
|
||||
|
||||
标必检的后果：仿真里永远过不了自检，而 S07 其实跑得挺好。
|
||||
"""
|
||||
c = _by_key(_build())["arm_compliant"]
|
||||
assert c.required is False
|
||||
assert "S07" in c.why
|
||||
|
||||
|
||||
def test_tactile_must_not_be_downgraded_to_optional():
|
||||
"""**负向**：触觉是 S07/S08/E1 三个判据的唯一数据源。
|
||||
|
||||
把它标成选检的后果不是"少一项检查"，而是"力判据全部失效但流程照跑"，
|
||||
报出来的现象是"拧不进去"，离真正原因差好几层。
|
||||
"""
|
||||
t = _by_key(_build())["tactile_status"]
|
||||
assert t.required is True
|
||||
assert t.expect_type == "arm_msgs/msg/TactileStatus"
|
||||
assert "S07" in t.why and "S08" in t.why
|
||||
|
||||
|
||||
def test_camera_checks_point_at_the_right_topics():
|
||||
checks = _by_key(_build(camera="d405_left"))
|
||||
assert checks["camera_color"].target == "/d405_left/color/image_raw"
|
||||
assert checks["camera_info"].target == "/d405_left/color/camera_info"
|
||||
|
||||
|
||||
def test_camera_namespace_with_or_without_slashes_is_the_same():
|
||||
a = _by_key(_build(camera="d405_left"))["camera_color"].target
|
||||
b = _by_key(_build(camera="/d405_left/"))["camera_color"].target
|
||||
assert a == b == "/d405_left/color/image_raw"
|
||||
|
||||
|
||||
def test_no_d435_means_no_make_believe_check():
|
||||
"""现场没有的设备不生成检查项：一个必然失败的项会训练人忽略输出。"""
|
||||
keys = _by_key(_build(d435_camera=None))
|
||||
assert "coarse_camera" not in keys
|
||||
with_d435 = _by_key(_build(d435_camera="d435"))
|
||||
assert "coarse_camera" in with_d435
|
||||
assert with_d435["coarse_camera"].required is False, "手外相机是 S03 兜底，不该拦启动"
|
||||
assert with_d435["coarse_camera"].target == "/d435/color/image_raw"
|
||||
|
||||
|
||||
def test_tool_frame_check_only_when_configured():
|
||||
assert "tool_frame" not in _by_key(_build(tool_name=""))
|
||||
with_tool = _by_key(_build(tool_name="tool1"))
|
||||
assert with_tool["tool_frame"].required is False, "没配工具系是告警，不是缺硬件"
|
||||
assert "tool1" in with_tool["tool_frame"].why
|
||||
|
||||
|
||||
def test_detector_check_only_when_weights_configured():
|
||||
assert "detector" not in _by_key(_build(weights_path=""))
|
||||
with_w = _by_key(_build(weights_path="/home/cc/ros2/models/yolo/detect_best_0.onnx"))
|
||||
assert with_w["detector"].probe == "detector"
|
||||
assert with_w["detector"].target.endswith(".onnx")
|
||||
|
||||
|
||||
def test_every_check_is_actionable():
|
||||
"""每项都要有：稳定 key、人读名字、合法探针、明确目标、**为什么**。"""
|
||||
checks = _build(d435_camera="d435", tool_name="tool1", weights_path="/tmp/x.onnx")
|
||||
assert len({c.key for c in checks}) == len(checks), "key 必须唯一"
|
||||
for c in checks:
|
||||
assert c.label and c.key
|
||||
assert c.probe in PROBES, f"{c.key} 的探针 {c.probe} 未登记"
|
||||
# 网络端点必须是绝对名：相对名会随节点命名空间变，自检会去探错名字
|
||||
if c.probe != "detector":
|
||||
assert c.target.startswith("/"), f"{c.key} 的目标应以 / 开头：{c.target}"
|
||||
assert len(c.why) > 8, f"{c.key} 的 why 太短，缺了会怎样没说清"
|
||||
|
||||
|
||||
def test_relative_action_name_is_normalised_to_absolute():
|
||||
"""stage2 的 ``action_name`` 默认值是相对的（没有前导斜杠）。
|
||||
|
||||
自检里必须补成绝对名：否则节点一旦被放进 namespace，自检就会去探
|
||||
``/ns/rizon_arm_controller/...``，报"控制器没起来"而实际好着呢。
|
||||
"""
|
||||
c = _by_key(_build(joint_action="rizon_arm_controller/follow_joint_trajectory"))["arm_jtc"]
|
||||
assert c.target == "/rizon_arm_controller/follow_joint_trajectory"
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 报告与判定
|
||||
# =============================================================================
|
||||
def _results(**status):
|
||||
"""构造 (检查项, 就绪, 细节) 列表；status 用 key 指定是否就绪。"""
|
||||
out = []
|
||||
for c in _build(d435_camera="d435"):
|
||||
out.append((c, bool(status.get(c.key, True)), "" if status.get(c.key, True) else "没收到"))
|
||||
return out
|
||||
|
||||
|
||||
def test_report_marks_required_and_optional_differently():
|
||||
text, failures = format_report(_results(coarse_camera=False))
|
||||
assert "选检" in text
|
||||
assert "告警" in text
|
||||
# 选检失败也要列出来（不拦流程，但必须让人看见）
|
||||
assert any("coarse_camera" in f or "手外" in f for f in failures)
|
||||
|
||||
|
||||
def test_only_required_failures_block_the_flow():
|
||||
"""**负向**：D435 没接不能拦住装配。
|
||||
|
||||
区分"必检失败"和"选检失败"是这一层的全部意义：混在一起会让
|
||||
"可选设备没接"变成"整个流程跑不起来"。
|
||||
"""
|
||||
results = _results(coarse_camera=False)
|
||||
assert blocking_failures(results) == []
|
||||
assert optional_warnings(results) and "手外" in optional_warnings(results)[0]
|
||||
|
||||
results2 = _results(tactile_status=False)
|
||||
assert blocking_failures(results2), "触觉缺失必须拦"
|
||||
assert "触觉" in blocking_failures(results2)[0]
|
||||
|
||||
|
||||
def test_report_lists_why_so_the_operator_knows_urgency():
|
||||
_text, failures = format_report(_results(camera_color=False))
|
||||
assert failures
|
||||
assert "S05" in failures[0], "失败项必须说明缺了会影响哪一步"
|
||||
|
||||
|
||||
def test_counts_summary():
|
||||
"""数量刻意写死：加/减一条自检项时必须来这里改一行并想清楚它属于哪一类。
|
||||
|
||||
当前（未配工具系、有手外相机、无权重）：
|
||||
必检 9 = arm_jtc, arm_home, arm_named_pose, gripper_open, gripper_close,
|
||||
camera_color, camera_info, tactile_status, gripper_status
|
||||
选检 3 = coarse_camera, arm_switch_tool, arm_compliant
|
||||
"""
|
||||
c = summary_counts(_results(coarse_camera=False))
|
||||
assert c["required_total"] == 9 and c["required_ok"] == 9
|
||||
# 选检里只有手外相机没起来 → 3 项里 2 项通过（工具系/柔顺下压按通过算）
|
||||
assert c["optional_total"] == 3 and c["optional_ok"] == 2
|
||||
|
||||
|
||||
def test_all_ready_says_so():
|
||||
text, failures = format_report(_results())
|
||||
assert failures == []
|
||||
assert "全部就绪" in text
|
||||
|
||||
|
||||
def test_bootcheck_defaults_to_required():
|
||||
"""手工构造检查项时默认必检 —— 默认选检会让漏写 required 变成静默放行。"""
|
||||
c = BootCheck(key="k", label="l", probe="service", target="/x", why="因为所以要检查它")
|
||||
assert c.required is True
|
||||
|
||||
with pytest.raises(TypeError):
|
||||
BootCheck(key="k", label="l", probe="service", target="/x") # type: ignore[call-arg]
|
||||
|
||||
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
|
||||
@@ -0,0 +1,318 @@
|
||||
"""``gauge_config`` 的单元测试。
|
||||
|
||||
重点不在"能不能读出一个数"，而在**读错了会不会被发现**：
|
||||
|
||||
* 键名拼错 → 必须报错并给候选名（否则现场"改了参数没反应"）；
|
||||
* 单位与键名不符 → 必须被 ``unit_violations`` 抓到（防"填毫米按米用"）；
|
||||
* 类型不对 → 必须报错，不许隐式转换；
|
||||
* 跨键矛盾（安全上限低于停止阈值）→ 必须报错。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from arm_application.gauge_config import (
|
||||
DEFAULTS,
|
||||
GaugeConfig,
|
||||
GaugeConfigError,
|
||||
_TYPES,
|
||||
_UNITLESS,
|
||||
_UNITS,
|
||||
dataclass_keys,
|
||||
from_site,
|
||||
snap_feature_of,
|
||||
missing_entries,
|
||||
unit_violations,
|
||||
)
|
||||
from arm_application.gauge_fsm import (
|
||||
descend_iter_budget,
|
||||
descend_tol_m,
|
||||
ratchet_travel_ok,
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 自检：配置表本身必须自洽
|
||||
# =============================================================================
|
||||
def test_schema_tables_are_consistent():
|
||||
"""三张表互相覆盖完整，且 dataclass 字段与键一一对应。
|
||||
|
||||
这条挂了通常意味着"新加了一个键，但忘了在另一张表里登记"。
|
||||
"""
|
||||
assert missing_entries() == []
|
||||
assert sorted(dataclass_keys()) == sorted(_TYPES), (
|
||||
"GaugeConfig 的字段与配置键不一致："
|
||||
f"只在 dataclass: {sorted(set(dataclass_keys()) - set(_TYPES))}，"
|
||||
f"只在配置表: {sorted(set(_TYPES) - set(dataclass_keys()))}"
|
||||
)
|
||||
|
||||
|
||||
def test_every_key_name_declares_its_unit():
|
||||
"""**键名必须体现单位**（``_mm`` / ``_deg`` / ``_s`` …）。
|
||||
|
||||
这条是"填了毫米当米用"的第一道闸门。挂掉时的修法只有两种：
|
||||
改键名带上单位后缀，或（确实无量纲时）登记到 ``_UNITLESS``。
|
||||
"""
|
||||
assert unit_violations() == []
|
||||
|
||||
|
||||
def test_unit_violations_actually_detect_bad_names():
|
||||
"""**负向**：故意塞一个"名字不体现单位"的键，检查必须报出来。
|
||||
|
||||
否则 ``unit_violations`` 可能永远返回空表（检查恒真 = 没有检查，规则 M1）。
|
||||
"""
|
||||
key = "hover"
|
||||
_UNITS[key] = "m"
|
||||
try:
|
||||
bad = unit_violations([key])
|
||||
assert bad and "hover" in bad[0], f"应报出 hover 的问题，实际 {bad}"
|
||||
finally:
|
||||
del _UNITS[key]
|
||||
|
||||
|
||||
def test_unitless_keys_are_explicitly_registered():
|
||||
"""无量纲键必须显式登记，不能靠"单位为空"蒙混过关。"""
|
||||
for key, unit in _UNITS.items():
|
||||
if not unit:
|
||||
assert key in _UNITLESS, f"{key} 无量纲却没登记在 _UNITLESS"
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 正常解析
|
||||
# =============================================================================
|
||||
def test_empty_section_uses_defaults_without_error():
|
||||
"""整段缺失不报错（走默认值）—— 是否可用由节点决定，配置模块只如实解析。"""
|
||||
cfg = from_site({})
|
||||
assert isinstance(cfg, GaugeConfig)
|
||||
assert cfg.extreme_angle_deg == DEFAULTS["extreme_angle_deg"]
|
||||
assert cfg.dry_run is False
|
||||
|
||||
|
||||
def test_partial_section_merges_defaults():
|
||||
cfg = from_site({"gauge_assembly": {"extreme_angle_deg": 120.0, "holes_total": 1}})
|
||||
assert cfg.extreme_angle_deg == 120.0
|
||||
assert cfg.holes_total == 1
|
||||
assert cfg.screw_step_deg == DEFAULTS["screw_step_deg"]
|
||||
|
||||
|
||||
def test_int_is_accepted_for_float_key_but_bool_is_not():
|
||||
"""YAML 里 ``20`` 是 int，填给 float 键要能用；``true`` 不能当 1 用。"""
|
||||
cfg = from_site({"gauge_assembly": {"extreme_angle_deg": 20}})
|
||||
assert cfg.extreme_angle_deg == 20.0
|
||||
with pytest.raises(GaugeConfigError) as ei:
|
||||
from_site({"gauge_assembly": {"extreme_angle_deg": True}})
|
||||
assert "类型不对" in str(ei.value)
|
||||
|
||||
|
||||
def test_string_number_is_rejected_not_coerced():
|
||||
"""``"20"`` 必须报错。
|
||||
|
||||
隐式转换是"填了字符串、跑起来是默认值"这类静默失效的入口：
|
||||
YAML 里多写一对引号，现场看不出任何异常。
|
||||
"""
|
||||
with pytest.raises(GaugeConfigError) as ei:
|
||||
from_site({"gauge_assembly": {"contact_step_mm": "0.5"}})
|
||||
msg = str(ei.value)
|
||||
assert "contact_step_mm" in msg and "mm" in msg and "隐式转换" in msg
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 报错要能直接照着改
|
||||
# =============================================================================
|
||||
def test_unknown_key_is_rejected_with_suggestion():
|
||||
with pytest.raises(GaugeConfigError) as ei:
|
||||
from_site({"gauge_assembly": {"screw_step_degs": 180.0}})
|
||||
msg = str(ei.value)
|
||||
assert "screw_step_degs" in msg
|
||||
assert "screw_step_deg" in msg, "应给出最相近的候选键名"
|
||||
assert "gauge_config._TYPES" in msg, "应指出新增配置项要登记在哪"
|
||||
|
||||
|
||||
def test_unknown_override_is_rejected():
|
||||
with pytest.raises(GaugeConfigError):
|
||||
from_site({}, overrides={"not_a_key": 1.0})
|
||||
|
||||
|
||||
def test_section_of_wrong_type_reports_clearly():
|
||||
with pytest.raises(GaugeConfigError) as ei:
|
||||
from_site({"gauge_assembly": 12})
|
||||
assert "映射" in str(ei.value)
|
||||
|
||||
|
||||
def test_cross_key_contradiction_is_rejected():
|
||||
"""安全上限必须高于停止阈值，否则一接触就报"力超安全上限"。"""
|
||||
with pytest.raises(GaugeConfigError) as ei:
|
||||
from_site(
|
||||
{
|
||||
"gauge_assembly": {
|
||||
"contact_tangential_delta": 500.0,
|
||||
"contact_safe_delta": 500.0,
|
||||
}
|
||||
}
|
||||
)
|
||||
assert "contact_safe_delta" in str(ei.value)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"patch, expect",
|
||||
[
|
||||
({"holes_total": 0}, "holes_total"),
|
||||
({"extreme_direction": 0}, "extreme_direction"),
|
||||
({"extreme_angle_deg": -30.0}, "extreme_angle_deg"),
|
||||
({"screw_step_deg": 0.0}, "screw_step_deg"),
|
||||
({"screw_max_cycles": 0}, "screw_max_cycles"),
|
||||
({"snap_tol_px": 0.0}, "snap_tol_px"),
|
||||
],
|
||||
)
|
||||
def test_bad_values_are_rejected(patch, expect):
|
||||
with pytest.raises(GaugeConfigError) as ei:
|
||||
from_site({"gauge_assembly": patch})
|
||||
assert expect in str(ei.value)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 派生量：文档与日志共用同一个算法
|
||||
# =============================================================================
|
||||
def test_derived_totals():
|
||||
cfg = from_site(
|
||||
{"gauge_assembly": {"screw_step_deg": 180.0, "screw_max_cycles": 3, "unscrew_extra_cycles": 1}}
|
||||
)
|
||||
assert cfg.total_screw_deg == 540.0
|
||||
assert cfg.unscrew_cycles == 4, "拧出要比拧入多留一轮，确保脱开"
|
||||
|
||||
|
||||
def test_describe_prints_effective_values_and_marks_calibration_needed():
|
||||
cfg = from_site({})
|
||||
text = "\n".join(cfg.describe())
|
||||
# 实际生效的值（不是"我读了 site.yaml"）
|
||||
assert "S01 预绕 150.0°" in text
|
||||
assert "S08 棘轮 120°×12 轮 = 累计 1440°" in text
|
||||
# 必须提示这些阈值要现场标定，否则默认值会被当成"调好的参数"
|
||||
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())
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 出厂配置的 S08：棘轮步长必须放得进**支持的所有臂**的关节7 行程
|
||||
# =============================================================================
|
||||
def test_shipped_site_yaml_ratchet_step_fits_every_supported_arm():
|
||||
"""**回归锚点**（2026-09-24 在仿真里踩到：S08 第 3 轮 ABORT）。
|
||||
|
||||
`screw_step_deg: 180` 配关节 7 的 ±175°（行程 350°）时，棘轮一个来回要
|
||||
2×180°=360° > 350° —— 关节**必然**有一头贴限位，"让余量"也腾不出空间
|
||||
（腾一步还是 360°）。日志表现是一长串"贴限位 → 让余量 → 还是贴限位"，
|
||||
真因（配置）藏在最后一行，白烧 3 轮节拍。
|
||||
|
||||
这条测试盯**出厂 site.yaml 的真实取值**，并且拿 `flexiv_description` 里
|
||||
**每台支持臂**的 joint7 限位去算（不是写死 350°）：以后加一台行程更小的
|
||||
臂，这里会自动报红。余量取 `gauge_assembly_node.RATCHET_LIMIT_MARGIN_DEG`
|
||||
的同一个口径 —— 两处不一致时，测试会红，而不是现场崩。
|
||||
"""
|
||||
import math
|
||||
|
||||
import yaml
|
||||
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
doc = yaml.safe_load((root / "config" / "site.yaml").read_text(encoding="utf-8"))
|
||||
step = float(doc["gauge_assembly"]["screw_step_deg"])
|
||||
|
||||
margin = 8.0 # 与 gauge_assembly_node.RATCHET_LIMIT_MARGIN_DEG 同口径
|
||||
from arm_application.gauge_assembly_node import RATCHET_LIMIT_MARGIN_DEG
|
||||
|
||||
assert margin == RATCHET_LIMIT_MARGIN_DEG, "余量口径变了，测试要跟着改"
|
||||
|
||||
limit_files = sorted((root / "src" / "flexiv_description" / "config").glob("*/joint_limits.yaml"))
|
||||
assert limit_files, "找不到任何臂的 joint_limits.yaml —— 路径变了？"
|
||||
for path in limit_files:
|
||||
doc_l = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
joint7 = (doc_l.get("joint_limits") or {}).get("joint7") or doc_l.get("joint7")
|
||||
assert joint7, f"{path} 里没有 joint7 限位"
|
||||
limits = (float(joint7["lower"]), float(joint7["upper"]))
|
||||
verdict = ratchet_travel_ok(step, limits, margin)
|
||||
assert verdict.ok, f"{path.name}: 出厂配置的棘轮步长放不下 —— {verdict.why}"
|
||||
assert verdict.max_step_deg > 0.0
|
||||
|
||||
# 累计转角必须是整数圈：否则规停在螺纹中间，S09 拧出也少转几分之一圈
|
||||
cycles = int(doc["gauge_assembly"]["screw_max_cycles"])
|
||||
assert math.isclose(step * cycles % 360.0, 0.0, abs_tol=1e-9), (
|
||||
f"screw_step_deg×screw_max_cycles = {step * cycles}° 不是 360° 的整数倍"
|
||||
)
|
||||
@@ -0,0 +1,182 @@
|
||||
"""S09 拧出后**先竖直提出孔口**：结构上有序，行为上提对高度。
|
||||
|
||||
为什么单独钉住（2026-09-24 仿真实测，两次栽在同一个地方）
|
||||
--------------------------------------------------------
|
||||
仿真里**没有螺纹几何**：S09 的"拧出"只让规在孔里转，规**仍然插在孔里**
|
||||
（真机也只是沿螺纹退回几个毫米）。于是紧跟着的横向动作 —— S10 换孔 / S11 回 home
|
||||
—— 会把规从孔里**拽**出来：实测抓取锁被拽断、规掉在台面上。
|
||||
|
||||
最坑的是**它不在这里报错**：两分钟后下游只表现为"下一孔 S07 下探 35mm 判不到
|
||||
接触"（工具上已经没规了），日志指不到真因（规则 R5.1 的静默失效）。
|
||||
|
||||
所以这里有两条断言，缺一不可：
|
||||
* **结构**：``_step_unscrew`` 在"成功返回"之前必须调用提出动作 —— 顺序错了
|
||||
（比如把提出挪到 S10 里）就等于没做，因为横向动作已经发生了；
|
||||
* **行为**：提出目标是**跟踪高**（``_track_z()``：悬停时尖端离面不够才再抬
|
||||
50mm），不是写死的毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import pathlib
|
||||
from typing import Any, List
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.gauge_assembly_node import GaugeAssembly
|
||||
|
||||
NODE_SRC = (
|
||||
pathlib.Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "gauge_assembly_node.py"
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 结构：提出动作必须在 S09 的"成功出口"之前
|
||||
# =============================================================================
|
||||
def _node_ast() -> ast.Module:
|
||||
return ast.parse(NODE_SRC.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _find_method(name: str) -> ast.FunctionDef:
|
||||
for node in ast.walk(_node_ast()):
|
||||
if isinstance(node, ast.FunctionDef) and node.name == name:
|
||||
return node
|
||||
raise AssertionError(f"节点里找不到方法 {name}")
|
||||
|
||||
|
||||
def _call_lines(func: ast.AST, attr: str) -> List[int]:
|
||||
out: List[int] = []
|
||||
for node in ast.walk(func):
|
||||
if isinstance(node, ast.Call):
|
||||
f = node.func
|
||||
if isinstance(f, ast.Attribute) and f.attr == attr:
|
||||
out.append(node.lineno)
|
||||
return out
|
||||
|
||||
|
||||
def test_unscrew_lifts_out_of_the_hole_before_succeeding():
|
||||
"""拧出成功后必须先提出孔口，再返回 ``UNSCREW_DONE``。"""
|
||||
fn = _find_method("_step_unscrew")
|
||||
lifts = _call_lines(fn, "_lift_clear_of_hole")
|
||||
assert lifts, (
|
||||
"S09 结束时没有竖直提出孔口 —— S10/S11 的横向动作会把规从孔里拽出来"
|
||||
"（实测：锁被拽断、规掉在台面上，下游表现为『S07 判不到接触』）"
|
||||
)
|
||||
exits = [
|
||||
n.lineno
|
||||
for n in ast.walk(fn)
|
||||
if isinstance(n, ast.Return)
|
||||
and isinstance(n.value, ast.Attribute)
|
||||
and n.value.attr == "UNSCREW_DONE"
|
||||
]
|
||||
assert exits, "S09 没有成功出口？"
|
||||
assert min(lifts) < max(exits), (
|
||||
"提出孔口必须在**成功返回之前**：顺序反了等于没做（横向动作已经发生）"
|
||||
)
|
||||
|
||||
|
||||
def test_lift_helper_exists_and_uses_the_track_height_idiom():
|
||||
"""提出高度用 ``_track_z()``，不写死毫米数。"""
|
||||
fn = _find_method("_lift_clear_of_hole")
|
||||
assert _call_lines(fn, "_track_z"), (
|
||||
"提出高度必须用 _track_z()：这是流程里『工具已离开工件』的既有定义，"
|
||||
"写死毫米数会多出一套没人维护的口径"
|
||||
)
|
||||
assert _call_lines(fn, "_goto"), "提出是一次真实运动（_goto），不是改个变量"
|
||||
src = ast.get_source_segment(NODE_SRC.read_text(encoding="utf-8"), fn) or ""
|
||||
assert "hold_xy" in src, "提出必须是**纯 Z**：横向分量会把规别在孔壁上"
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 行为：提对高度 / 已在高度上不动 / 拿不到状态不炸
|
||||
# =============================================================================
|
||||
class _StubLogger:
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def info(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
def warn(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
|
||||
class _StubAssembly:
|
||||
"""``_lift_clear_of_hole`` 只用这几样东西；测试里不建任何 ROS 实体。"""
|
||||
|
||||
move_speed = 0.05
|
||||
vert_tol_rad = 0.02
|
||||
|
||||
def __init__(self, z: Any, track_z: float = 0.707, raise_on_goto: bool = False) -> None:
|
||||
self.logs: List[str] = []
|
||||
self.moves: List[Any] = []
|
||||
self._z = z
|
||||
self._tz = float(track_z)
|
||||
self._raise = bool(raise_on_goto)
|
||||
|
||||
def _wait_state(self, _timeout: float) -> Any:
|
||||
if self._z is None:
|
||||
return None, None
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.5130
|
||||
Tee[1, 3] = -0.0020
|
||||
Tee[2, 3] = float(self._z)
|
||||
return np.zeros(7), Tee
|
||||
|
||||
def _track_z(self) -> float:
|
||||
return self._tz
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return _StubLogger(self.logs)
|
||||
|
||||
def _goto(self, q: Any, Tee: Any, target: Any, **kw: Any) -> Any:
|
||||
if self._raise:
|
||||
raise RuntimeError("轨迹被拒")
|
||||
self.moves.append((np.array(target, dtype=float), kw))
|
||||
return True, q, Tee
|
||||
|
||||
|
||||
def _stub(z: Any, track_z: float = 0.707, raise_on_goto: bool = False) -> _StubAssembly:
|
||||
return _StubAssembly(z, track_z, raise_on_goto)
|
||||
|
||||
|
||||
def test_deep_in_the_hole_lifts_to_track_height():
|
||||
"""规还在孔里（实测规尖比工件面低 30mm）→ 提到跟踪高，纯 Z。"""
|
||||
st = _stub(0.628)
|
||||
GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type]
|
||||
|
||||
assert len(st.moves) == 1, "应当恰好走一次提出"
|
||||
target, kw = st.moves[0]
|
||||
assert target[0] == pytest.approx(0.5130) and target[1] == pytest.approx(-0.0020), (
|
||||
"提出必须是**纯 Z**：XY 一动就是把规别在孔壁上"
|
||||
)
|
||||
assert target[2] == pytest.approx(0.707), "目标高度就是跟踪高（悬停 + 50mm）"
|
||||
assert kw.get("hold_xy") is True
|
||||
assert kw.get("tag") == "S09_exit_k1"
|
||||
assert any("竖直提出" in m for m in st.logs), "日志要能回看这次提出（不然失败时查不到）"
|
||||
|
||||
|
||||
def test_already_high_enough_does_not_move():
|
||||
"""已经比跟踪高还高 → 一步都不走（否则每次换孔白等一秒多）。"""
|
||||
st = _stub(0.80)
|
||||
GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type]
|
||||
assert st.moves == []
|
||||
assert st.logs == []
|
||||
|
||||
|
||||
def test_no_joint_state_is_not_fatal():
|
||||
"""拿不到状态只告警：这一刻孔里已经没有规要保护，硬失败会把成功的装配变成失败。"""
|
||||
st = _stub(None)
|
||||
GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type]
|
||||
assert st.moves == []
|
||||
assert st.logs == [], "拿不到状态连日志都不用刷（静默跳过即可）"
|
||||
|
||||
|
||||
def test_goto_exception_is_swallowed_with_a_warning():
|
||||
"""提出失败（轨迹被拒）只告警，流程继续 —— 后面 S10/S11 仍会走到安全位。"""
|
||||
bad = _stub(0.628, raise_on_goto=True)
|
||||
GaugeAssembly._lift_clear_of_hole(bad, "k1") # type: ignore[arg-type]
|
||||
assert any("提出孔口失败" in m for m in bad.logs)
|
||||
@@ -0,0 +1,990 @@
|
||||
"""``gauge_fsm`` 的单元测试 —— 通止规装配状态机的**顺序**必须被钉住。
|
||||
|
||||
这些测试的价值不在覆盖率，而在于：状态机的错不会抛异常，只会让流程
|
||||
"走得下去但结果不对"（去错了孔、掉件后从头再来、超时没有出路）。
|
||||
所以每条转移、每条判据都要有能报红的断言，包括**负向**的（R5.2）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from arm_application.gauge_fsm import (
|
||||
EVENT_DOC,
|
||||
GLOBAL,
|
||||
RATCHET_STEPS,
|
||||
STATE_DOC,
|
||||
TERMINAL,
|
||||
TRANSITIONS,
|
||||
ENTRY_HIGH,
|
||||
ENTRY_OK,
|
||||
Event,
|
||||
FsmError,
|
||||
GaugeContext,
|
||||
GaugeFsm,
|
||||
State,
|
||||
DESCEND_BUDGET_FLOOR_M,
|
||||
GAUGE_SHOULDER_ABOVE_TIP_M,
|
||||
WRENCH_MAX_AGE_S,
|
||||
descend_budget_m,
|
||||
descend_iter_budget,
|
||||
descend_needed_m,
|
||||
descend_progress,
|
||||
descend_tol_m,
|
||||
hole_entry_verdict,
|
||||
relief_tol_m,
|
||||
tool_clearance_m,
|
||||
wrench_live,
|
||||
wrench_touch,
|
||||
is_joint_limit_error,
|
||||
pixel_stable,
|
||||
points_stable,
|
||||
hover_z_guard,
|
||||
hover_z_for_tool_clearance,
|
||||
resolve_hover_z,
|
||||
ratchet_cycles,
|
||||
resolve_next,
|
||||
rotation_verdict,
|
||||
screw_stop,
|
||||
tangential_jump,
|
||||
drag_verdict,
|
||||
ratchet_delta_rad,
|
||||
ratchet_travel_ok,
|
||||
SNAP_DRAG_TOL_M,
|
||||
)
|
||||
|
||||
from arm_application.gauge_config import DEFAULTS
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 主线：k1 → k2 → 复位
|
||||
# =============================================================================
|
||||
def test_happy_path_covers_every_state_in_order():
|
||||
"""一次完整装配的**确切**状态序列。
|
||||
|
||||
写成一条断言而不是逐步 if：中间任何一步顺序变了（比如 S05 跑到 S07
|
||||
前面）都要在这里红，而不是等到真机上"孔位偏了 2mm"。
|
||||
"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||
seq = [
|
||||
(Event.BOOT_OK, State.HOME_EXTREME),
|
||||
(Event.POSE_OK, State.GRIP_CHECK),
|
||||
(Event.GRIP_SKIPPED, State.TRACK),
|
||||
(Event.TRACK_LOCKED, State.SNAP),
|
||||
(Event.SNAP_OK, State.HOVER_K),
|
||||
(Event.ALIGN_OK, State.CONTACT),
|
||||
(Event.CONTACT_OK, State.SCREW_IN),
|
||||
(Event.SCREW_STOP, State.UNSCREW),
|
||||
(Event.UNSCREW_DONE, State.NEXT_HOLE), # k1 拧出 → 换 k2
|
||||
(Event.POSE_OK, State.HOVER_K), # 已在 k2 正悬停
|
||||
(Event.ALIGN_OK, State.CONTACT),
|
||||
(Event.CONTACT_OK, State.SCREW_IN),
|
||||
(Event.SCREW_DONE, State.UNSCREW),
|
||||
(Event.UNSCREW_DONE, State.RESET), # 最后一个孔 → 复位
|
||||
(Event.POSE_OK, State.DONE),
|
||||
]
|
||||
for i, (ev, want) in enumerate(seq, start=1):
|
||||
got = fsm.handle(ev)
|
||||
assert got is want, f"第 {i} 步（{ev.value}）应到 {want}，实际到 {got}"
|
||||
assert fsm.is_terminal and fsm.ok
|
||||
assert [s.next for s in fsm.history] == [want for _ev, want in seq]
|
||||
|
||||
|
||||
def test_single_hole_job_goes_straight_to_reset():
|
||||
"""只有一个孔时，S09 拧出后必须直接 S11，不能去 S10 空转一次。"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=1))
|
||||
for ev in (
|
||||
Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED,
|
||||
Event.SNAP_OK, Event.ALIGN_OK, Event.CONTACT_OK, Event.SCREW_STOP,
|
||||
):
|
||||
fsm.handle(ev)
|
||||
assert fsm.handle(Event.UNSCREW_DONE) is State.RESET
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 结构性质：不靠人肉通读转移表
|
||||
# =============================================================================
|
||||
def test_every_state_can_be_aborted_or_reach_terminal():
|
||||
"""每个非终态都必须能**走得到**终态。
|
||||
|
||||
漏掉的那个状态，在真机上表现为"按了中止，流程还在跑"。
|
||||
ABORT/TIMEOUT 是全局兜底，这条断言顺带验证 GLOBAL 真的覆盖了全部状态。
|
||||
"""
|
||||
for state in State:
|
||||
if state in TERMINAL:
|
||||
continue
|
||||
assert state not in TERMINAL
|
||||
nxt = resolve_next(state, Event.ABORT, GaugeContext())
|
||||
assert nxt is State.ABORT, f"{state} 不能中止"
|
||||
nxt = resolve_next(state, Event.TIMEOUT, GaugeContext())
|
||||
assert nxt is State.ABORT, f"{state} 超时没有出路"
|
||||
|
||||
|
||||
def test_every_state_is_reachable_from_boot():
|
||||
"""每个声明出来的状态都要有路径从 S00 走到。
|
||||
|
||||
否则就是一个"写在文档里、代码里永远进不去"的阶段 —— 比没有更糟：
|
||||
文档说它在跑，现场却在等一个不会发生的事。
|
||||
"""
|
||||
seen = {State.BOOT}
|
||||
frontier = [State.BOOT]
|
||||
while frontier:
|
||||
cur = frontier.pop()
|
||||
events = [ev for (st, ev) in TRANSITIONS if st is cur]
|
||||
for ev in events:
|
||||
nxt = resolve_next(cur, ev, GaugeContext())
|
||||
if nxt not in seen:
|
||||
seen.add(nxt)
|
||||
frontier.append(nxt)
|
||||
# S09 的主出口是 UNSCREW_DONE（上下文相关），单独补上
|
||||
seen.add(State.UNSCREW)
|
||||
seen.add(State.NEXT_HOLE)
|
||||
seen.add(State.RESET)
|
||||
seen.add(State.DONE)
|
||||
seen.add(State.ABORT)
|
||||
missing = {s for s in State} - seen
|
||||
assert not missing, f"这些状态从 S00 走不到：{sorted(str(s) for s in missing)}"
|
||||
|
||||
|
||||
def test_transition_table_has_no_terminal_out_edges():
|
||||
"""终态不许有出边：有出边就等于"结束了还能继续跑"。"""
|
||||
for (state, _ev) in TRANSITIONS:
|
||||
assert state not in TERMINAL, f"{state} 是终态却出现在转移表里"
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# S05 自环 / S06 回退：不推错孔号
|
||||
# =============================================================================
|
||||
def test_unstable_snap_keeps_same_hole():
|
||||
"""S05 等稳定是自环，不能把孔号推走（否则会在 k1 上等，然后去装 k2）。"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||
for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED):
|
||||
fsm.handle(ev)
|
||||
assert fsm.ctx.hole_index == 0
|
||||
for _ in range(5):
|
||||
fsm.handle(Event.SNAP_UNSTABLE)
|
||||
assert fsm.state is State.SNAP and fsm.ctx.hole_index == 0
|
||||
assert fsm.ctx.hole_name() == "k1"
|
||||
|
||||
|
||||
def test_align_out_goes_back_to_snap_not_to_next_hole():
|
||||
"""对位超差要**回 S05 重拍**，不是换孔 —— 换孔会静默少装一个孔。"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||
for ev in (
|
||||
Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED,
|
||||
Event.SNAP_OK,
|
||||
):
|
||||
fsm.handle(ev)
|
||||
assert fsm.handle(Event.ALIGN_OUT) is State.SNAP
|
||||
assert fsm.ctx.hole_index == 0
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# S03/S04 互为兜底
|
||||
# =============================================================================
|
||||
def test_track_loss_falls_back_to_coarse_and_back():
|
||||
"""S04 丢失 → S03 粗定位；S03 识别不到 → 回 S04。两条路都要能来回。"""
|
||||
fsm = GaugeFsm(GaugeContext())
|
||||
for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED):
|
||||
fsm.handle(ev)
|
||||
assert fsm.state is State.TRACK
|
||||
assert fsm.handle(Event.TRACK_LOST) is State.COARSE
|
||||
assert fsm.handle(Event.COARSE_FAIL) is State.TRACK
|
||||
assert fsm.handle(Event.TRACK_LOST) is State.COARSE
|
||||
assert fsm.handle(Event.COARSE_OK) is State.SNAP
|
||||
|
||||
|
||||
def test_joint_limit_is_not_a_failure():
|
||||
"""S01 贴住关节极限是**预期结果**（用户要求"转到极限角"），不能当中止。"""
|
||||
fsm = GaugeFsm(GaugeContext())
|
||||
fsm.handle(Event.BOOT_OK)
|
||||
assert fsm.state is State.HOME_EXTREME
|
||||
assert fsm.handle(Event.JOINT_LIMIT) is State.GRIP_CHECK
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# E1 掉件重放
|
||||
# =============================================================================
|
||||
def _to_screw_in(fsm: GaugeFsm) -> None:
|
||||
for ev in (
|
||||
Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED, Event.TRACK_LOCKED,
|
||||
Event.SNAP_OK, Event.ALIGN_OK, Event.CONTACT_OK,
|
||||
):
|
||||
fsm.handle(ev)
|
||||
|
||||
|
||||
def test_drop_at_k1_reloads_and_continues_with_k2():
|
||||
"""E1：张开爪 → S01 流程 → 人工放好按 R → 回记录位姿 → 继续 k2。"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=2))
|
||||
_to_screw_in(fsm)
|
||||
assert fsm.state is State.SCREW_IN and fsm.ctx.hole_index == 0
|
||||
|
||||
assert fsm.handle(Event.GAUGE_DROPPED) is State.HOME_EXTREME
|
||||
assert fsm.ctx.relocations == 1
|
||||
# 人工重放后直接回记录位姿（等价于换孔动作），坑已跳过，所以是去 k2
|
||||
assert fsm.handle(Event.RELOADED) is State.NEXT_HOLE
|
||||
assert fsm.ctx.hole_index == 1
|
||||
assert fsm.handle(Event.POSE_OK) is State.HOVER_K
|
||||
|
||||
|
||||
def test_drop_budget_exhaustion_aborts_with_note():
|
||||
"""掉件超过上限 → ABORT，并留下一条能读懂现场原因的备注。"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=1, max_relocations=2))
|
||||
fsm.handle(Event.BOOT_OK)
|
||||
for attempt in range(3):
|
||||
fsm.state = State.SCREW_IN # 直接摆到"正在拧"，掉件只可能发生在这一步
|
||||
nxt = fsm.handle(Event.GAUGE_DROPPED)
|
||||
if attempt < 2:
|
||||
assert nxt is State.HOME_EXTREME, f"第 {attempt + 1} 次掉件应还在预算内"
|
||||
fsm.handle(Event.RELOADED)
|
||||
else:
|
||||
assert nxt is State.ABORT
|
||||
assert fsm.ctx.relocations == 3
|
||||
assert any("掉" in n for n in fsm.ctx.notes)
|
||||
|
||||
|
||||
def test_illegal_drop_does_not_consume_reload_budget():
|
||||
"""**负向**：在错的状态发 GAUGE_DROPPED，不能白吃掉一次重放预算。
|
||||
|
||||
实现里"先算转移、再记预算"就是为这条：顺序反了的话，一次手误会让现场
|
||||
少一次重试机会，而报错信息只说"状态不对"，没人会想到预算被扣了。
|
||||
"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=1, max_relocations=1))
|
||||
assert fsm.state is State.BOOT
|
||||
with pytest.raises(FsmError):
|
||||
fsm.handle(Event.GAUGE_DROPPED)
|
||||
assert fsm.ctx.relocations == 0
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 非法转移要当场炸
|
||||
# =============================================================================
|
||||
def test_illegal_transition_raises_with_actionable_message():
|
||||
fsm = GaugeFsm(GaugeContext())
|
||||
with pytest.raises(FsmError) as ei:
|
||||
fsm.handle(Event.SCREW_DONE) # S00 不可能已经在拧
|
||||
msg = str(ei.value)
|
||||
assert "S00" in msg and "screw_done" in msg
|
||||
assert "gauge_fsm.TRANSITIONS" in msg # 告诉改代码的人去哪登记
|
||||
|
||||
|
||||
def test_terminal_state_rejects_further_events():
|
||||
fsm = GaugeFsm(GaugeContext())
|
||||
fsm.handle(Event.BOOT_FAIL)
|
||||
assert fsm.state is State.ABORT
|
||||
with pytest.raises(FsmError):
|
||||
fsm.handle(Event.BOOT_OK)
|
||||
|
||||
|
||||
def test_can_does_not_mutate():
|
||||
fsm = GaugeFsm(GaugeContext())
|
||||
before = (fsm.state, len(fsm.history))
|
||||
assert fsm.can(Event.BOOT_OK) is True
|
||||
assert fsm.can(Event.SCREW_DONE) is False
|
||||
assert (fsm.state, len(fsm.history)) == before
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 判据
|
||||
# =============================================================================
|
||||
def test_pixel_stable_rejects_zero_mean_jitter():
|
||||
"""**负向**：相邻两帧很近、但整体在晃，不能被判成"稳定"。
|
||||
|
||||
这正是"两帧之差"写法会踩的坑：跟踪抖动的相邻差是小的，于是画面明明
|
||||
在晃，判据早就触发了，然后拍出一张糊图 —— 现场表现为"孔位偶尔偏 1mm"，
|
||||
随机且难复现。这里用一条锯齿状的抖动序列把这个坑钉死。
|
||||
"""
|
||||
jitter = [(100.0 + 6.0 * (-1) ** i, 50.0) for i in range(8)]
|
||||
# 相邻差都是 12px，很大；但下面这条更阴：相邻差 0、整体摆幅 ±6px
|
||||
sawtooth = [(100.0, 50.0), (100.0, 50.0), (106.0, 50.0), (106.0, 50.0),
|
||||
(94.0, 50.0), (94.0, 50.0)]
|
||||
assert not pixel_stable(jitter, tol_px=2.0, min_frames=6)
|
||||
assert not pixel_stable(sawtooth, tol_px=2.0, min_frames=6)
|
||||
# 正向：真的不动
|
||||
still = [(100.0 + 0.5 * i % 1.0, 50.0) for i in range(8)]
|
||||
assert pixel_stable(still, tol_px=2.0, min_frames=6)
|
||||
|
||||
|
||||
def test_pixel_stable_needs_enough_frames():
|
||||
assert not pixel_stable([(100.0, 50.0)], tol_px=5.0, min_frames=6)
|
||||
assert not pixel_stable([(100.0, 50.0)] * 5, tol_px=5.0, min_frames=6)
|
||||
assert pixel_stable([(100.0, 50.0)] * 6, tol_px=5.0, min_frames=6)
|
||||
|
||||
|
||||
def test_tangential_jump_watches_both_channels():
|
||||
"""切向力通道未确认 → 两个都监控，任一突变即判定。"""
|
||||
ref = (100.0, 100.0)
|
||||
assert not tangential_jump((110.0, 130.0), ref, delta=50.0)
|
||||
assert tangential_jump((160.0, 100.0), ref, delta=50.0) # X 突变
|
||||
assert tangential_jump((100.0, 160.0), ref, delta=50.0) # Y 突变
|
||||
# 反向突变（规被顶回来）同样算
|
||||
assert tangential_jump((40.0, 100.0), ref, delta=50.0)
|
||||
# 边界：恰好等于阈值算触发（>=，避免"永远差那么一点"）
|
||||
assert tangential_jump((150.0, 100.0), ref, delta=50.0)
|
||||
|
||||
|
||||
def test_screw_stop_requires_both_conditions():
|
||||
"""S08 停止判据是**且**：只看一个会误停。"""
|
||||
# 只有切向力大（规刚蹭到螺纹）→ 不停
|
||||
assert not screw_stop(normal_delta=0.0, shear_max=999.0, normal_th=100.0, shear_th=200.0)
|
||||
# 只有法向力突增（规顶到孔底）→ 不停
|
||||
assert not screw_stop(normal_delta=999.0, shear_max=0.0, normal_th=100.0, shear_th=200.0)
|
||||
# 两者同时满足 → 停
|
||||
assert screw_stop(normal_delta=150.0, shear_max=250.0, normal_th=100.0, shear_th=200.0)
|
||||
|
||||
|
||||
def test_ratchet_cycle_count_rounds_up():
|
||||
assert ratchet_cycles(360.0, 180.0) == 2
|
||||
assert ratchet_cycles(361.0, 180.0) == 3 # 差一点也要多做一轮，不能少拧
|
||||
assert ratchet_cycles(0.0, 180.0) == 0
|
||||
with pytest.raises(ValueError):
|
||||
ratchet_cycles(360.0, 0.0)
|
||||
|
||||
|
||||
def test_ratchet_steps_are_four_separate_atomic_steps():
|
||||
"""S08 必须是四个可独立调用的原子步，顺序固定。
|
||||
|
||||
用户明确要求不能封装成一个"拧螺丝"复合动作：中途掉件时只有每一步都能
|
||||
单独恢复，才可能精确续上（否则只能整段重来，等于少装一个孔）。
|
||||
"""
|
||||
assert RATCHET_STEPS == ("rotate_slow", "release", "rotate_back", "regrip")
|
||||
assert len(set(RATCHET_STEPS)) == len(RATCHET_STEPS)
|
||||
|
||||
|
||||
def test_joint_limit_recognition():
|
||||
assert is_joint_limit_error("Joint limit exceeded on joint7")
|
||||
assert is_joint_limit_error("规划失败：超出关节限位")
|
||||
assert not is_joint_limit_error("controller not active")
|
||||
assert not is_joint_limit_error("")
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 文档与代码同步
|
||||
# =============================================================================
|
||||
def test_every_state_and_event_is_documented():
|
||||
"""每个状态/事件都要有一句人话说明。
|
||||
|
||||
没有说明的状态，最后一定会以"日志里只印了个 S07，没人知道 S07 干嘛"
|
||||
的形式回到现场。这条断言让"新增状态忘了写说明"当场报红。
|
||||
"""
|
||||
assert set(STATE_DOC) == set(State), (
|
||||
f"缺说明的状态: {sorted(str(s) for s in set(State) - set(STATE_DOC))}"
|
||||
)
|
||||
assert set(EVENT_DOC) == set(Event), (
|
||||
f"缺说明的事件: {sorted(e.value for e in set(Event) - set(EVENT_DOC))}"
|
||||
)
|
||||
|
||||
|
||||
def test_global_fallback_covers_only_abort_and_timeout():
|
||||
"""GLOBAL 只放"谁都该接受"的事件。
|
||||
|
||||
一旦有人往里加业务事件，就会悄悄多出一条所有状态都合法的边，
|
||||
把"这条转移不存在"这个保护废掉。
|
||||
"""
|
||||
assert set(GLOBAL) == {Event.ABORT, Event.TIMEOUT}
|
||||
|
||||
|
||||
def test_context_reports_hole_names():
|
||||
ctx = GaugeContext(holes_total=2, hole_index=0)
|
||||
assert ctx.hole_name() == "k1"
|
||||
ctx.hole_index = 1
|
||||
assert ctx.hole_name() == "k2"
|
||||
assert ctx.is_last_hole
|
||||
|
||||
|
||||
def test_history_index_is_dense_and_monotonic():
|
||||
fsm = GaugeFsm(GaugeContext())
|
||||
for ev in (Event.BOOT_OK, Event.POSE_OK, Event.GRIP_SKIPPED):
|
||||
fsm.handle(ev)
|
||||
assert [s.index for s in fsm.history] == [1, 2, 3]
|
||||
assert str(fsm.history[0]) == "#1 S00 --boot_ok--> S01"
|
||||
|
||||
|
||||
def test_math_hypot_precision_is_not_used_loosely():
|
||||
"""pixel_stable 的容差是闭区间（<=），恰好等于阈值算稳定。
|
||||
|
||||
留一条边界断言，是因为改成 ``<`` 会让"刚好卡在容差上"的稳定画面永远
|
||||
等不到 —— 现场表现为"卡在 S05 不动"，而日志只说"还在等稳定"。
|
||||
"""
|
||||
samples = [(100.0, 50.0), (102.0, 50.0), (98.0, 50.0)]
|
||||
assert math.isclose(2.0, 2.0)
|
||||
assert pixel_stable(samples, tol_px=2.0, min_frames=3)
|
||||
assert not pixel_stable(samples, tol_px=1.9, min_frames=3)
|
||||
|
||||
|
||||
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_relief_tolerance_is_strictly_smaller_than_the_relief_move():
|
||||
"""卸载回退的容差必须**严格小于**回退量（与下压步长同一条纪律）。
|
||||
|
||||
2026-09-24 实测踩到：回退 0.50mm 而容差被写死 0.0005 m（同量级），
|
||||
目标 z 的浮点回减让 ``dist`` 比 0.0005 略小 ⇒ ``_goto`` 入口就判"已到位"
|
||||
⇒ 卸载成为空操作，而残余力判据比的是两个相同读数 ⇒ 假报"已放干净"。
|
||||
之后 S08 带着 ~70N 轴向力去转（夹持摩擦上限 ≈73N）→ 掉件(E1)。
|
||||
"""
|
||||
for dz_mm in (0.05, 0.2, 0.5, 1.0, 3.0):
|
||||
dz = dz_mm / 1000.0
|
||||
tol = relief_tol_m(dz)
|
||||
assert 0.0 < tol < dz, f"回退 {dz_mm}mm 的容差 {tol * 1000:.4f}mm 不小于回退量"
|
||||
|
||||
|
||||
def test_relief_tolerance_has_a_floor_but_never_swallows_the_move():
|
||||
"""回退量极小时容差退不下去，但**仍不许**大于等于回退量。"""
|
||||
dz = 1e-5
|
||||
tol = relief_tol_m(dz)
|
||||
assert 0.0 < tol <= dz * 0.5
|
||||
|
||||
|
||||
def test_relief_tolerance_rejects_nonpositive_move():
|
||||
with pytest.raises(ValueError):
|
||||
relief_tol_m(0.0)
|
||||
with pytest.raises(ValueError):
|
||||
relief_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_tool_clearance_is_negative_when_tool_already_inside():
|
||||
"""悬停高度按相机定、工具比相机长 → 悬停那一刻工具已经插进工件。
|
||||
|
||||
这是 2026-09-24 仿真里实测到的局面（也是真机同参数同结果）：
|
||||
悬停法兰 0.632 − 工具长 0.243 = 尖端 0.389，工件上表面 0.412
|
||||
→ 尖端在面下 23mm
|
||||
现象是 S07 一路压到顶都判不到接触，看起来像力阈值标错了 —— 其实是几何错。
|
||||
"""
|
||||
gap = tool_clearance_m(0.632, 0.243, 0.412)
|
||||
assert gap == pytest.approx(-0.023, abs=1e-9)
|
||||
|
||||
|
||||
def test_tool_clearance_positive_when_tool_is_short():
|
||||
assert tool_clearance_m(0.632, 0.100, 0.412) == pytest.approx(0.120, abs=1e-9)
|
||||
|
||||
|
||||
def test_descend_needed_measures_shoulder_not_tip():
|
||||
"""判"到底了"的是**台肩落面**，所以需要的下压量按台肩算，不按尖端算。
|
||||
|
||||
台肩在尖端上方 30mm：尖端要往下 30mm，台肩才落到面上。
|
||||
按尖端算会少算 30mm —— 那正是"预算 25mm 永远到不了落面"那个 bug。
|
||||
"""
|
||||
# 尖端正好在面上（法兰 = 面 0.412 + 工具长 0.243 = 0.655）
|
||||
# → 还要再下压 30mm 台肩才落面
|
||||
assert descend_needed_m(0.655, 0.243, 0.412, 0.030) == pytest.approx(0.030)
|
||||
|
||||
# 悬停在 0.632（= 仿真里那个按相机定的值）→ 台肩还差 7mm
|
||||
assert descend_needed_m(0.632, 0.243, 0.412, 0.030) == pytest.approx(0.007)
|
||||
|
||||
# 按尖端算会少算台肩那 30mm —— 差的就是出问题的那个量
|
||||
tip_only = tool_clearance_m(0.655, 0.243, 0.412)
|
||||
assert tip_only == pytest.approx(0.0)
|
||||
assert descend_needed_m(0.655, 0.243, 0.412, 0.030) - tip_only == pytest.approx(0.030)
|
||||
|
||||
|
||||
def test_hole_entry_verdict_flags_gauge_landing_on_the_surface():
|
||||
"""2026-09-28 实测：同一命令三跑里坏一跑（DONE / ABORT / DONE）。
|
||||
|
||||
坏的那跑：k1 装好了，k2 只下压 0.90mm 就"接触"（正常要 28.6~31.9mm）——
|
||||
因为工件在 k1 之后被工具推走了 ~20mm，而 k2 的孔位是 S05 一次性定下的。
|
||||
不拦的代价是干转 5 轮 + 最后报一句"转轴偏差…先确认工具系有没有装转接件"，
|
||||
把人引去查工具系（那几个都是好的）。
|
||||
"""
|
||||
needed = 0.0319 # 台肩落面还需下压 31.9mm（实测）
|
||||
# 坏那跑：0.90 / 31.9 = 3% ⇒ 必须报"没进孔"
|
||||
assert hole_entry_verdict(0.0009, needed) == ENTRY_HIGH
|
||||
# 好那跑：28.6 / 31.9 = 90% ⇒ 必须放行（否则白扔一整个孔）
|
||||
assert hole_entry_verdict(0.0286, needed) == ENTRY_OK
|
||||
# 另一跑 k1：31.45 / 31.9 ⇒ 放行
|
||||
assert hole_entry_verdict(0.03145, needed) == ENTRY_OK
|
||||
|
||||
|
||||
def test_hole_entry_verdict_never_flags_the_healthy_band():
|
||||
"""健康的比值区间（台肩倒角没完全落面、板厚公差）绝不能被判成"没进孔"。
|
||||
|
||||
这是"宁可漏报也不能误报"那一边：误报一次就白扔一整个孔，
|
||||
而漏报只是回到旧行为（多烧几轮 + 一句误导）。
|
||||
"""
|
||||
needed = 0.032
|
||||
for ratio in (0.85, 0.90, 0.95, 1.00, 1.10):
|
||||
assert hole_entry_verdict(needed * ratio, needed) == ENTRY_OK, (
|
||||
f"比值 {ratio} 是正常范围，不该判成没进孔"
|
||||
)
|
||||
# 明确该报的一侧
|
||||
for ratio in (0.0, 0.03, 0.10, 0.30, 0.49):
|
||||
assert hole_entry_verdict(needed * ratio, needed) == ENTRY_HIGH, (
|
||||
f"比值 {ratio} 差着一个台肩高度，必须报"
|
||||
)
|
||||
|
||||
|
||||
def test_hole_entry_verdict_stays_quiet_when_geometry_is_unusable():
|
||||
"""``needed_m <= 0`` 时不许抢着报第二个错。
|
||||
|
||||
那种情况上面那条几何自检（tool_clearance_m / descend_needed_m）已经在报了，
|
||||
这里再报一次只会把真正的第一因埋掉。
|
||||
"""
|
||||
assert hole_entry_verdict(0.0005, 0.0) == ENTRY_OK
|
||||
assert hole_entry_verdict(0.0005, -0.01) == ENTRY_OK
|
||||
|
||||
|
||||
def test_hole_lost_event_aborts_and_has_an_honest_label():
|
||||
"""事件本身与它的中文标签都必须是实话。
|
||||
|
||||
复用 FORCE_OVER_LIMIT 会让结论报成"力超安全上限"，而这种情况下力根本没超限 ——
|
||||
又是一句误导。所以单列 HOLE_LOST，并且标签要能一眼看出是孔位问题。
|
||||
"""
|
||||
assert resolve_next(State.CONTACT, Event.HOLE_LOST, GaugeContext()) == State.ABORT
|
||||
label = EVENT_DOC[Event.HOLE_LOST]
|
||||
assert "孔" in label, "标签要指明是孔位问题"
|
||||
assert "力" not in label, "别把它说成力的问题 —— 力是好的"
|
||||
|
||||
|
||||
def test_descend_budget_has_margin_and_floor():
|
||||
"""预算要有余量（容纳对位残差/板厚公差），且再小也有下限。
|
||||
|
||||
下限的理由：几何算出来"只差 2mm"时，如果预算就填 2mm，一点对位残差就会
|
||||
让"预算用完"先于"判到接触"触发 —— 报出来的原因是假的。
|
||||
"""
|
||||
# 需求很小时吃下限：0.002×1.5 = 0.003 < 0.008
|
||||
assert descend_budget_m(0.002) == pytest.approx(DESCEND_BUDGET_FLOOR_M)
|
||||
# 需求正常时按系数放大：0.030×1.5 = 0.045
|
||||
assert descend_budget_m(0.030) == pytest.approx(0.045)
|
||||
|
||||
|
||||
def test_shipped_budget_covers_shipped_gauge_and_hover():
|
||||
"""出厂配置必须自洽：预算要够走完"台肩落面"。
|
||||
|
||||
这是那个 bug 的回归测试 —— 当时预算 25mm 而台肩在尖端上方 30mm，
|
||||
永远到不了落面那一下，报"下探 25mm 都没触发接触判据"（假原因）。
|
||||
以后谁把 contact_max_depth_mm 调回去、或把 tool_tip_below_flange_m 改大
|
||||
到不匹配，这里就会红。
|
||||
"""
|
||||
budget_m = DEFAULTS["contact_max_depth_mm"] / 1000.0
|
||||
tip_below = DEFAULTS["tool_tip_below_flange_m"]
|
||||
# 出厂配置里 workpiece_surface_z_m = 0（运行期用 stage2 实测孔位兜底），
|
||||
# 所以这里取一个"最坏"的悬停-面高差：仿真里实测法兰 0.632、面 0.412。
|
||||
needed = descend_needed_m(0.632, tip_below, 0.412, GAUGE_SHOULDER_ABOVE_TIP_M)
|
||||
assert budget_m >= needed, (
|
||||
f"下探预算 {budget_m*1000:.1f}mm 不够走完台肩落面所需的 "
|
||||
f"{needed*1000:.1f}mm —— 会在到落面之前就用完预算"
|
||||
)
|
||||
|
||||
|
||||
def test_wrench_touch_is_sign_agnostic():
|
||||
"""腕部 FT 判据取**绝对增量**，不赌正负号。
|
||||
|
||||
正负号方向取决于传感器安装（真机法兰系、仿真 link7 系，压下去是 +z 还是 -z
|
||||
都可能）。赌错了就是"整段流程静默失效"：下发正常、日志正常、就是停不住。
|
||||
取绝对值只会更早停一点点，这个方向的错是安全的 —— 与既有
|
||||
stage2_locate_node._contact_detected 的口径（abs(fz - baseline)）也一致。
|
||||
"""
|
||||
assert wrench_touch(20.0, 0.0, 8.0)
|
||||
assert wrench_touch(-20.0, 0.0, 8.0)
|
||||
# 基线本身不是 0（带载）时同样只看增量
|
||||
assert wrench_touch(54.0, 46.0, 8.0)
|
||||
assert wrench_touch(38.0, 46.0, 8.0)
|
||||
|
||||
|
||||
def test_wrench_touch_below_threshold_does_not_fire():
|
||||
assert not wrench_touch(7.9, 0.0, 8.0)
|
||||
assert not wrench_touch(46.0, 46.0, 8.0)
|
||||
# 边界：等于阈值算触发（与 tangential_jump 的 >= 口径一致）
|
||||
assert wrench_touch(8.0, 0.0, 8.0)
|
||||
|
||||
|
||||
def test_wrench_touch_threshold_is_reachable_before_grip_slips():
|
||||
"""阈值的量级必须**远低于**夹持摩擦能传的上限，否则等于设了个永远达不到的数。
|
||||
|
||||
这是"用 300 当阈值"那个 bug 的一般化回归：夹持段 µ=0.25、实测握力 ≈46N
|
||||
⇒ 两指合计能传约 µ·N ≈ 23N。接触刚度 5e4 N/m，所以 8N 阈值 ≈ 0.16mm
|
||||
过压就判到，而打滑要到 ≈0.46mm —— 留了约 3 倍余量。
|
||||
"""
|
||||
mu, grip_n = 0.25, 46.0
|
||||
slip_limit_n = mu * grip_n * 2 # 两指
|
||||
assert DEFAULTS["contact_axial_delta_n"] < slip_limit_n / 2, (
|
||||
f"阈值 {DEFAULTS['contact_axial_delta_n']}N 太接近夹持摩擦上限 "
|
||||
f"{slip_limit_n:.1f}N —— 打滑前可能来不及判到"
|
||||
)
|
||||
|
||||
|
||||
def test_wrench_live_uses_freshness_not_magnitude():
|
||||
"""判活看"够不够新"，**不看数值大小**。
|
||||
|
||||
静止时轴向力本来就接近 0。拿数值判活会把"安静的传感器"误判成掉线 ——
|
||||
这正是 2026-09-24 在触觉上踩过的同一个坑（判据从 |fnormal|>1.0 改成
|
||||
"够新 + connected"）。所以这里专门断言 age 有值但数值为 0 → 活的。
|
||||
"""
|
||||
assert wrench_live(0.0)
|
||||
assert wrench_live(WRENCH_MAX_AGE_S)
|
||||
assert not wrench_live(WRENCH_MAX_AGE_S + 1e-6)
|
||||
# None = 一帧都没收到过（话题名错/传感器没起来）→ 不是活的
|
||||
assert not wrench_live(None)
|
||||
|
||||
|
||||
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) # 往上跑 ≠ 推进
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 绕轴旋转的验收判据（S01/S08/S09 共用）
|
||||
# =============================================================================
|
||||
def test_rotation_verdict_accepts_a_full_turn():
|
||||
assert rotation_verdict(-180.0, -179.7, 0.0, 5.0) == "ok"
|
||||
assert rotation_verdict(+180.0, +175.1, 0.0, 5.0) == "ok" # 5° 之内算到位
|
||||
|
||||
|
||||
def test_rotation_verdict_calls_a_partial_turn_a_joint_limit():
|
||||
"""2026-09-24 实测的那一条：**只转了 16.4°**（被关节7 的上限截住）。
|
||||
|
||||
期望 +180、实测 +16.4：方向对、只是没走完 ⇒ 判"贴限位"（调用方张开爪
|
||||
让出余量再转），**不是**"偏差过大"直接 ABORT —— 后者把一整轮白扔，
|
||||
而且日志看起来像"这台臂坏了"。
|
||||
"""
|
||||
assert rotation_verdict(+180.0, +16.4, 0.1, 5.0) == "limit"
|
||||
# 完全没动也是同一类（只是余量更少）
|
||||
assert rotation_verdict(+180.0, +0.4, 0.0, 5.0) == "limit"
|
||||
|
||||
|
||||
def test_rotation_verdict_still_stops_on_wrong_direction_or_overshoot():
|
||||
"""判"贴限位"必须**卡住方向**：反向转/过冲都是真问题，不能拿去让余量重试。"""
|
||||
assert rotation_verdict(+180.0, -170.0, 0.0, 5.0) == "short" # 转反了
|
||||
assert rotation_verdict(+180.0, +260.0, 0.0, 5.0) == "short" # 过冲
|
||||
# 小幅指令（≤10°）几乎没动时**不算**"完全没动"那条特例，于是落进
|
||||
# "方向对但没走完" ⇒ 同样判 limit。这是有意的：让余量那一步是免费的
|
||||
# （张开爪时规在打滑），宁可白让一次，也别把"其实贴住了"判成"臂坏了"。
|
||||
assert rotation_verdict(+8.0, +0.2, 0.0, 5.0) == "limit"
|
||||
|
||||
|
||||
def test_rotation_verdict_axis_misalignment_beats_everything():
|
||||
"""转轴不共线（>5°）时，任何角度数字都没有意义 —— 必须先报它。"""
|
||||
assert rotation_verdict(+180.0, +179.0, 6.0, 5.0) == "axis"
|
||||
assert rotation_verdict(+180.0, +16.4, 6.0, 5.0) == "axis"
|
||||
|
||||
|
||||
def test_ratchet_step_of_180_never_fits_the_rizon_joint7_range():
|
||||
"""棘轮一步 180° 在 Rizon4 的关节7 行程（±175°）里**永远**放不下。
|
||||
|
||||
这是 S09 中止的物理根源，也解释了为什么转腕前要先让余量：
|
||||
贴着上限时反向那一步只剩十几度（实测 16.4°）。
|
||||
"""
|
||||
from arm_application.gauge_kin import prewind
|
||||
|
||||
lim = (-3.0543, 3.0543) # Rizon4 关节7：±175°
|
||||
# 贴在上限（棘轮回转每轮都停在这儿）→ 想再往 + 转：放不下，必须让余量
|
||||
plan = prewind(q_now_rad=math.radians(171.0), want_deg=180.0, direction=+1,
|
||||
limits=lim, margin_deg=8.0)
|
||||
assert plan.clamped
|
||||
# 同一个位置往 - 转：放得下（这正是"让余量"那一步）
|
||||
plan = prewind(q_now_rad=math.radians(171.0), want_deg=180.0, direction=-1,
|
||||
limits=lim, margin_deg=8.0)
|
||||
assert not plan.clamped
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 悬停高度自检（悬停按相机定、工具比相机长 ⇒ 会捅进工件）
|
||||
# =============================================================================
|
||||
def test_hover_guard_fires_for_the_exact_case_we_hit():
|
||||
"""2026-09-24 实测：仿真悬停 0.632、工具长 0.245、工件面 0.412。
|
||||
|
||||
尖端 0.632−0.245=0.387，比工件面低 25mm ⇒ 悬停这一步就把工具捅进去了，
|
||||
紧接着 S05 的横向对准把工件拖走 92mm。这里必须报"要改"并给出几何值。
|
||||
"""
|
||||
fixed, why = hover_z_guard(0.632, 0.412, 0.245, 0.002)
|
||||
assert why, "这种悬停必须被判为不安全"
|
||||
assert fixed == pytest.approx(0.412 + 0.245 + 0.002)
|
||||
assert "钻进工件" in why
|
||||
assert "hole_hover_z_m = 0.659" in why # 要能直接照着改配置
|
||||
|
||||
|
||||
def test_hover_guard_is_silent_when_the_tool_stays_clear():
|
||||
"""几何对的配置一个字都不许说（否则每次启动都刷一堆无用告警）。"""
|
||||
fixed, why = hover_z_guard(0.659, 0.412, 0.245, 0.002)
|
||||
assert why == ""
|
||||
assert fixed == pytest.approx(0.659)
|
||||
# 更高也照过（悬停高一点只是拍得远，不算错）
|
||||
assert hover_z_guard(0.700, 0.412, 0.245, 0.002)[1] == ""
|
||||
|
||||
|
||||
def test_hover_guard_uses_want_clearance_when_configuring():
|
||||
"""修正值必须落在"刚好 want_clearance"的位置，而不是随手抬一点。"""
|
||||
fixed, _ = hover_z_guard(0.50, 0.412, 0.245, 0.005)
|
||||
assert fixed == pytest.approx(0.412 + 0.245 + 0.005)
|
||||
assert hover_z_for_tool_clearance(0.412, 0.245, 0.005) == pytest.approx(fixed)
|
||||
|
||||
|
||||
def test_user_hover_is_refused_instead_of_rewritten():
|
||||
"""启动参数写了悬停高：不安全就拒绝，不许改成另一个数再往下走。"""
|
||||
used, why = resolve_hover_z(0.632, 0.412, 0.245, 0.002, True)
|
||||
assert used == pytest.approx(0.632)
|
||||
assert "不会改成别的高度" in why
|
||||
# 没写启动参数：仍按几何抬高，不当成拒绝。
|
||||
used_site, why_site = resolve_hover_z(0.632, 0.412, 0.245, 0.002, False)
|
||||
assert why_site == ""
|
||||
assert used_site == pytest.approx(0.412 + 0.245 + 0.002)
|
||||
# 写了且本身安全：原样用。
|
||||
used_ok, why_ok = resolve_hover_z(0.700, 0.412, 0.245, 0.002, True)
|
||||
assert why_ok == ""
|
||||
assert used_ok == pytest.approx(0.700)
|
||||
|
||||
|
||||
def test_hover_guard_tolerates_millimetre_level_shortfall():
|
||||
"""夹在目标间隙附近（差不到 1mm）不算错：工件面 z 本身就有几毫米不确定度。"""
|
||||
assert hover_z_guard(0.6580, 0.412, 0.245, 0.002)[1] == "" # 差 1.0mm 之内
|
||||
assert hover_z_guard(0.6560, 0.412, 0.245, 0.002)[1] != "" # 差 3mm：判错
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# S05 精对位时"工件被工具推走"的看门狗
|
||||
# =============================================================================
|
||||
def test_drag_verdict_says_nothing_on_the_first_frame():
|
||||
"""第一拍没有上一拍可比 —— 不能因为"没有历史"就报警。"""
|
||||
assert drag_verdict((0.49, 0.0), None)[0] is False
|
||||
|
||||
|
||||
def test_drag_verdict_ignores_measurement_jitter():
|
||||
"""正常对准时两孔中点不动（动的是目标孔，不是中点）—— 亚毫米抖动不算。"""
|
||||
ok, d = drag_verdict((0.4905, 0.0002), (0.4900, 0.0000), (0.4899, 0.0000))
|
||||
assert ok is False
|
||||
assert d == pytest.approx(math.hypot(0.0005, 0.0002), abs=1e-9)
|
||||
|
||||
|
||||
def test_drag_verdict_fires_on_one_big_jump():
|
||||
"""单拍就挪了 8mm：刚体不会自己挪回去，不必等第二拍确认。"""
|
||||
ok, d = drag_verdict((0.4820, 0.0), (0.4900, 0.0))
|
||||
assert ok is True
|
||||
assert d == pytest.approx(0.008)
|
||||
|
||||
|
||||
def test_drag_verdict_needs_two_frames_when_it_is_only_3_plus_mm():
|
||||
"""只大过 3mm 一点点：要求上一拍也朝同方向挪过，避免一次误检就中止流程。"""
|
||||
mid = (0.4860, 0.0)
|
||||
prev = (0.4900, 0.0)
|
||||
# 上一拍没动（d2 = 0）⇒ 只当噪声
|
||||
assert drag_verdict(mid, prev, (0.4901, 0.0))[0] is False
|
||||
# 上一拍也挪了 4mm、方向一致 ⇒ 判被推走
|
||||
assert drag_verdict(mid, prev, (0.4940, 0.0))[0] is True
|
||||
# 上一拍也挪了 4mm，但方向**相反**（工件抖动/误检）⇒ 不判
|
||||
assert drag_verdict(mid, prev, (0.4820, 0.0))[0] is False
|
||||
|
||||
|
||||
def test_drag_verdict_matches_what_we_actually_saw_in_gazebo():
|
||||
"""2026-09-24 实测那一轮的真实数字：工件每拍被推 11.8mm（残差恒 11mm）。
|
||||
|
||||
这条是回归测试：当时的现场没有任何判据报警，日志只说"检查手眼/相机安装"，
|
||||
于是一路追到迭代用满。现在同样的数列必须在**第一拍**就判出来。
|
||||
"""
|
||||
ks = [0.4910, 0.4910, 0.4792, 0.4674, 0.4566, 0.4458, 0.4350, 0.4239, 0.4121]
|
||||
mids = [(k - 0.029, 0.0) for k in ks] # 中点 = k1 左移两孔间距的一半
|
||||
flags = [
|
||||
drag_verdict(mids[i], mids[i - 1] if i >= 1 else None,
|
||||
mids[i - 2] if i >= 2 else None)[0]
|
||||
for i in range(len(mids))
|
||||
]
|
||||
assert flags == [False, False, True, True, True, True, True, True, True]
|
||||
assert SNAP_DRAG_TOL_M < 0.0118
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 棘轮：目标角（绝对值）→ 增量。2026-09-24 S09 第一步 ABORT 的根因
|
||||
# =============================================================================
|
||||
def test_ratchet_delta_wraps_a_target_that_is_past_the_limit():
|
||||
"""实测那一笔：关节在 +158.9°、目标 +338.9° ⇒ 增量是 +180°（不是 +338.9°）。
|
||||
|
||||
差值的折算是整个棘轮的地基：把它当成 338.9° 会让"行程够不够"算错、
|
||||
让余量那一步算到限位外面，最后报一句与真因无关的"偏差过大"。
|
||||
"""
|
||||
d = ratchet_delta_rad(math.radians(338.9), math.radians(158.9))
|
||||
assert math.degrees(d) == pytest.approx(180.0, abs=1e-6)
|
||||
|
||||
|
||||
def test_ratchet_rewind_target_stays_inside_the_limit():
|
||||
"""让余量那一步必须落在限位内 —— 这正是当时失败的地方（−180° < −175°）。"""
|
||||
lo, up = math.radians(-175.0), math.radians(175.0)
|
||||
q_now = math.radians(158.9)
|
||||
delta = ratchet_delta_rad(math.radians(338.9), q_now)
|
||||
rewind = q_now - delta # 让出整步
|
||||
assert lo < rewind < up
|
||||
assert math.degrees(rewind) == pytest.approx(-21.1, abs=1e-6)
|
||||
# 让完余量再前进整步 ⇒ 关节回到原处、工具净转 +180°
|
||||
retry = rewind + delta
|
||||
assert math.degrees(retry) == pytest.approx(158.9, abs=1e-6)
|
||||
|
||||
|
||||
def test_ratchet_delta_is_the_same_number_for_both_wrapped_aims():
|
||||
"""+338.9° 与 −21.1° 是同一个关节位置（差一整圈）—— 增量必须一致。"""
|
||||
q_now = math.radians(158.9)
|
||||
a = ratchet_delta_rad(math.radians(338.9), q_now)
|
||||
b = ratchet_delta_rad(math.radians(-21.1), q_now)
|
||||
assert a == pytest.approx(b, abs=1e-9)
|
||||
|
||||
|
||||
def test_ratchet_delta_covers_a_plain_back_and_forth_cycle():
|
||||
"""正常的棘轮一步（不贴限位）：从 +150° 拧 −180°、再回转 +180°。"""
|
||||
q = math.radians(150.0)
|
||||
d = ratchet_delta_rad(math.radians(-30.0), q)
|
||||
assert math.degrees(d) == pytest.approx(-180.0, abs=1e-6)
|
||||
d2 = ratchet_delta_rad(math.radians(150.0), q + d)
|
||||
assert math.degrees(d2) == pytest.approx(180.0, abs=1e-6)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 棘轮的**转向**必须由工艺意图决定，不能交给"谁更短"
|
||||
# 2026-09-24 实测：S08 第 3 轮 ABORT（步长 180° 的刀口）
|
||||
# =============================================================================
|
||||
def test_ratchet_180_degree_step_flips_direction_on_a_tenth_of_a_degree_read():
|
||||
"""**实测那一笔**：调用方读到 q=+162.0 ⇒ 目标 −18.0；棘轮晚 0.1° 读到
|
||||
q=+162.1 ⇒ "最短路径"给出 **+179.9°**（方向反了）。
|
||||
|
||||
180° 正好落在 ±180° 的刀口上：0.1° 的读数差就能决定"往前"还是"往后"。
|
||||
结果是棘轮朝螺纹反方向走 → 贴限位 → 让余量 → 重试 → **净位移 0** ⇒ ABORT，
|
||||
而现场什么都没坏。所以转向必须由调用方按工艺意图给定。
|
||||
"""
|
||||
d_naive = ratchet_delta_rad(math.radians(-18.0), math.radians(162.1))
|
||||
assert math.degrees(d_naive) == pytest.approx(+179.9, abs=0.05) # 旧口径：翻向
|
||||
d_screw = ratchet_delta_rad(math.radians(-18.0), math.radians(162.1), -1)
|
||||
assert math.degrees(d_screw) == pytest.approx(-180.1, abs=0.05), "拧入必须往 − 转"
|
||||
d_back = ratchet_delta_rad(math.radians(342.0), math.radians(162.1), +1)
|
||||
assert math.degrees(d_back) == pytest.approx(+179.9, abs=0.05), "回转必须往 + 转"
|
||||
|
||||
|
||||
def test_directed_delta_always_lands_on_the_target_and_keeps_the_sign():
|
||||
"""给了方向：增量符号一定等于指定方向，且转过去**就是**目标角（差整圈）。"""
|
||||
q = math.radians(162.1)
|
||||
for want_sign, target_deg in ((+1, 342.0), (+1, -18.0), (-1, -18.0), (-1, 342.0)):
|
||||
d = ratchet_delta_rad(math.radians(target_deg), q, want_sign)
|
||||
assert d != 0.0
|
||||
assert math.copysign(1.0, d) == want_sign, (want_sign, target_deg, math.degrees(d))
|
||||
# 转过去与目标角**同一个位置**（差整数圈）
|
||||
turns = (q + d - math.radians(target_deg)) / (2.0 * math.pi)
|
||||
assert abs(turns - round(turns)) < 1e-9
|
||||
# 且是那一侧**最小**的一步（再多转一整圈就不是"最小"了）
|
||||
assert abs(d) <= 2.0 * math.pi + 1e-9
|
||||
|
||||
|
||||
def test_directed_delta_does_not_waste_a_full_turn_when_already_there():
|
||||
"""已经在目标角上时不许"为了凑方向"白转一整圈 —— 那是 1 秒多的空转。"""
|
||||
q = math.radians(30.0)
|
||||
assert ratchet_delta_rad(q, q, +1) == 0.0
|
||||
assert ratchet_delta_rad(q, q, -1) == 0.0
|
||||
# 差一点点也算"就在那儿"（1e-12 rad 量级）而不是 359.999…°
|
||||
assert abs(ratchet_delta_rad(q, q - 1e-12, +1)) < 1e-9
|
||||
|
||||
|
||||
def test_plain_shortest_path_is_still_available_for_non_ratchet_callers():
|
||||
"""``direction=0`` 保留"最短路径"语义（给目标本来就明确在某一侧的调用方）。"""
|
||||
d = ratchet_delta_rad(math.radians(338.9), math.radians(158.9))
|
||||
assert math.degrees(d) == pytest.approx(180.0, abs=1e-6)
|
||||
assert math.degrees(ratchet_delta_rad(math.radians(10.0), math.radians(350.0))) == pytest.approx(20.0)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 棘轮步长 vs 关节行程：S08 开头就要判，不能等第 3 轮贴限位
|
||||
# =============================================================================
|
||||
def test_ratchet_travel_rejects_180_on_the_rizon_joint7():
|
||||
"""180° 的步长在 ±175° 的关节 7 上**永远**放不下（实测第 3 轮 ABORT）。
|
||||
|
||||
一个来回要 2×180° = 360°，比行程 350° 还大 —— 无论怎么让余量都腾不出
|
||||
空间，所以这不是"调调就好"，是**配置错**，必须在 S08 开头就拦下来。
|
||||
"""
|
||||
lim = (-3.0543, 3.0543) # Rizon4/10 关节7：±175°
|
||||
v = ratchet_travel_ok(180.0, lim, 8.0)
|
||||
assert not v.ok
|
||||
assert v.need_deg == pytest.approx(376.0)
|
||||
assert v.travel_deg == pytest.approx(350.0, abs=0.1)
|
||||
assert v.max_step_deg == pytest.approx(167.0, abs=0.1)
|
||||
assert "screw_step_deg" in v.why, "报错要直接指出该改哪个键"
|
||||
assert "screw_max_cycles" in v.why, "还要提醒累计必须是整数圈"
|
||||
|
||||
|
||||
def test_ratchet_travel_accepts_the_shipped_step_and_margin():
|
||||
"""出厂值（120°）必须过 —— 否则 S08 一进门就 ABORT，仿真永远跑不完。"""
|
||||
from arm_application.gauge_config import DEFAULTS
|
||||
|
||||
lim = (-3.0543, 3.0543)
|
||||
v = ratchet_travel_ok(DEFAULTS["screw_step_deg"], lim, 8.0)
|
||||
assert v.ok, v.why
|
||||
# 建议值必须落在上限内，否则"照着改"会再撞一次
|
||||
assert v.max_step_deg >= DEFAULTS["screw_step_deg"]
|
||||
|
||||
|
||||
def test_ratchet_travel_passes_when_limits_are_unknown_but_says_so():
|
||||
"""拿不到限位不能猜着拦：放行，理由交给驱动器报 JOINT_LIMIT 兜底。"""
|
||||
v = ratchet_travel_ok(180.0, None, 8.0)
|
||||
assert v.ok
|
||||
assert v.travel_deg == float("inf")
|
||||
assert v.max_step_deg == float("inf")
|
||||
assert v.why == ""
|
||||
@@ -0,0 +1,190 @@
|
||||
"""``gauge_gripper`` 的单元测试：**同名服务、两种类型**怎么选。
|
||||
|
||||
这一段的价值全在"选错了会报出与真实原因无关的错"上：
|
||||
真机上只按契约写 → 报"服务没起"（其实起了，只是类型是 Trigger）；
|
||||
选了 legacy 却在仿真里跑 → 调用类型不匹配的服务。
|
||||
所以每条选择路径都要有断言，包括"两种都在线"和"一个都不在"。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import pytest
|
||||
|
||||
from arm_application.gauge_gripper import (
|
||||
CONTRACT_CLOSE,
|
||||
CONTRACT_OPEN,
|
||||
CONTRACT_STATUS_TOPIC,
|
||||
CONTRACT_TYPE,
|
||||
GRIP_STATE_CAUGHT,
|
||||
GRIP_STATE_DROPPED,
|
||||
GRIP_STATE_MOVING,
|
||||
GRIP_STATE_REACHED,
|
||||
LEGACY_STATUS_TOPIC,
|
||||
LEGACY_TYPE,
|
||||
GripperInterfaceError,
|
||||
describe_backend_choice,
|
||||
is_dropped_from_state,
|
||||
is_gripped_from_state,
|
||||
legacy_states_to_fields,
|
||||
pick_backend,
|
||||
states_frame_online,
|
||||
)
|
||||
|
||||
|
||||
def _services(open_types, close_types=None, extra=None):
|
||||
close_types = open_types if close_types is None else close_types
|
||||
out = {CONTRACT_OPEN: list(open_types), CONTRACT_CLOSE: list(close_types)}
|
||||
if extra:
|
||||
out.update(extra)
|
||||
return out
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# auto：按实际类型选
|
||||
# =============================================================================
|
||||
def test_contract_backend_when_services_are_gripper_execute():
|
||||
choice = pick_backend(_services([CONTRACT_TYPE]))
|
||||
assert choice.backend == "contract"
|
||||
assert CONTRACT_TYPE in choice.reason
|
||||
assert choice.status_topic == CONTRACT_STATUS_TOPIC
|
||||
|
||||
|
||||
def test_legacy_backend_when_services_are_trigger():
|
||||
"""真机今天的样子：dh_pgea 节点的 Trigger 服务。"""
|
||||
choice = pick_backend(_services([LEGACY_TYPE]))
|
||||
assert choice.backend == "legacy"
|
||||
assert "dh_pgea" in choice.reason
|
||||
assert choice.status_topic == LEGACY_STATUS_TOPIC
|
||||
# 理由里要带上"契约驱动还没落地"这个现场事实，否则下次有人以为选错了
|
||||
assert "arm_driver_dh_gripper" in choice.reason
|
||||
|
||||
|
||||
def test_missing_services_reports_how_to_start_them():
|
||||
with pytest.raises(GripperInterfaceError) as ei:
|
||||
pick_backend({})
|
||||
msg = str(ei.value)
|
||||
assert "run_gripper.sh" in msg and "run_sim_gz.sh" in msg
|
||||
|
||||
|
||||
def test_unknown_service_type_is_not_silently_accepted():
|
||||
"""类型既不是 GripperExecute 也不是 Trigger（比如被别人占用了同名服务）。
|
||||
|
||||
这时必须是"认不出来"的明确报错，而不是随便选一个后端然后调用失败。
|
||||
"""
|
||||
with pytest.raises(GripperInterfaceError) as ei:
|
||||
pick_backend(_services(["std_srvs/srv/SetBool"]))
|
||||
msg = str(ei.value)
|
||||
assert "std_srvs/srv/SetBool" in msg
|
||||
assert "service list -t" in msg, "要给出『是谁占用了这个服务名』的查法"
|
||||
|
||||
|
||||
def test_missing_service_and_wrong_type_are_different_messages():
|
||||
"""**负向**：不能在"服务不在"和"类型不对"两种情况报同一句话。
|
||||
|
||||
两者都报"夹爪节点没起来"，现场就会去反复重启一个根本没问题的节点
|
||||
（规则 M1：报错信息必须指向真实原因）。
|
||||
"""
|
||||
with pytest.raises(GripperInterfaceError) as absent:
|
||||
pick_backend({CONTRACT_CLOSE: [CONTRACT_TYPE]}) # open 缺失
|
||||
with pytest.raises(GripperInterfaceError) as wrong:
|
||||
pick_backend(_services(["std_srvs/srv/SetBool"]))
|
||||
assert "没起来" in str(absent.value)
|
||||
assert "没起来" not in str(wrong.value)
|
||||
assert "被别的节点占用" in str(wrong.value) or "占用了" in str(wrong.value)
|
||||
|
||||
|
||||
def test_both_backends_online_is_an_error_not_a_coin_flip():
|
||||
"""仿真和真机节点同时起着（或旧进程没停干净）→ 报错，不许撞运气。"""
|
||||
with pytest.raises(GripperInterfaceError) as ei:
|
||||
pick_backend(_services([CONTRACT_TYPE, LEGACY_TYPE]))
|
||||
msg = str(ei.value)
|
||||
assert "同时存在两种类型" in msg
|
||||
assert "stop_sim_gz.sh" in msg
|
||||
|
||||
|
||||
def test_open_and_close_types_must_agree():
|
||||
"""open 是契约、close 是 Trigger → 不算"契约可用"，要按认不出来报错。"""
|
||||
with pytest.raises(GripperInterfaceError):
|
||||
pick_backend(_services([CONTRACT_TYPE], [LEGACY_TYPE]))
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 显式指定后端：不符就报错，不静默降级
|
||||
# =============================================================================
|
||||
def test_explicit_contract_on_legacy_hardware_fails_with_fix_list():
|
||||
with pytest.raises(GripperInterfaceError) as ei:
|
||||
pick_backend(_services([LEGACY_TYPE]), prefer="contract")
|
||||
msg = str(ei.value)
|
||||
assert "gripper_backend" in msg
|
||||
assert "legacy" in msg and "auto" in msg
|
||||
|
||||
|
||||
def test_explicit_legacy_on_sim_fails_and_points_at_dh_pgea():
|
||||
with pytest.raises(GripperInterfaceError) as ei:
|
||||
pick_backend(_services([CONTRACT_TYPE]), prefer="legacy")
|
||||
assert "dh_pgea" in str(ei.value)
|
||||
|
||||
|
||||
def test_explicit_backend_matching_reality_is_accepted():
|
||||
assert pick_backend(_services([CONTRACT_TYPE]), prefer="contract").backend == "contract"
|
||||
assert pick_backend(_services([LEGACY_TYPE]), prefer="legacy").backend == "legacy"
|
||||
|
||||
|
||||
def test_bad_prefer_value_lists_valid_options():
|
||||
with pytest.raises(GripperInterfaceError) as ei:
|
||||
pick_backend(_services([CONTRACT_TYPE]), prefer="whatever")
|
||||
assert "auto" in str(ei.value) and "contract" in str(ei.value)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 状态解读
|
||||
# =============================================================================
|
||||
def test_gripped_only_when_state_is_definitive():
|
||||
""""到位"不等于"夹住"：空爪闭合也会到位。
|
||||
|
||||
把 1（到位）当成"夹住了"会让 E1 掉件检测在正常开合时误报；
|
||||
把"不知道"当成"没夹住"会让流程一开始就报错。
|
||||
"""
|
||||
assert is_gripped_from_state(GRIP_STATE_CAUGHT) is True
|
||||
assert is_gripped_from_state(GRIP_STATE_DROPPED) is False
|
||||
assert is_gripped_from_state(GRIP_STATE_REACHED) is None
|
||||
assert is_gripped_from_state(GRIP_STATE_MOVING) is None
|
||||
|
||||
|
||||
def test_drop_detection_requires_a_previous_grip():
|
||||
"""爪里本来就是空的（装配前），不该判成掉件。"""
|
||||
assert is_dropped_from_state(GRIP_STATE_DROPPED, was_gripped=True) is True
|
||||
assert is_dropped_from_state(GRIP_STATE_DROPPED, was_gripped=False) is False
|
||||
assert is_dropped_from_state(GRIP_STATE_CAUGHT, was_gripped=True) is False
|
||||
assert is_dropped_from_state(GRIP_STATE_MOVING, was_gripped=True) is False
|
||||
|
||||
|
||||
def test_legacy_states_parsing_is_field_named_and_defensive():
|
||||
f = legacy_states_to_fields([1.0, 2.0, 500.0, 0.0, 50.0])
|
||||
assert f["initialized"] == 1.0 and f["grip_state"] == 2.0
|
||||
assert f["position"] == 500.0 and f["target_force"] == 50.0
|
||||
# 畸形/空数据 → 空字典（上层走"不知道"分支），不抛异常
|
||||
assert legacy_states_to_fields([]) == {}
|
||||
assert legacy_states_to_fields([1.0, 2.0]) == {}
|
||||
|
||||
# 离线帧（第 6 位 0）里的前 5 个是上一次的值，不能当现在的状态用。
|
||||
# 2026-10-20 读 0x0201 连 264 次无应答，这就是流水线把"读不到"说成
|
||||
# "夹爪没到位"的入口，必须有负向测试。
|
||||
assert legacy_states_to_fields([1.0, 2.0, 500.0, 0.0, 50.0, 0.0]) == {}
|
||||
|
||||
|
||||
def test_states_online_marks_only_fresh_frames():
|
||||
assert states_frame_online([1.0, 2.0, 500.0, 0.0, 50.0]) is True
|
||||
assert states_frame_online([1.0, 2.0, 500.0, 0.0, 50.0, 1.0]) is True
|
||||
assert states_frame_online([1.0, 2.0, 500.0, 0.0, 50.0, 0.0]) is False
|
||||
# 第 6 位是垃圾 → 当离线，不许当有效帧
|
||||
assert states_frame_online([1.0, 2.0, 500.0, 0.0, 50.0, "x"]) is False
|
||||
|
||||
|
||||
def test_describe_mentions_the_unit_trap_for_legacy():
|
||||
"""legacy 的"开口"是千分比不是毫米，日志里必须写明。
|
||||
|
||||
这是最容易误用的地方：拿到 500 以为是 500mm，实际是 10mm 行程的一半。
|
||||
"""
|
||||
text = "\n".join(describe_backend_choice(pick_backend(_services([LEGACY_TYPE]))))
|
||||
assert "1000‰" in text and "不是毫米" in text
|
||||
@@ -0,0 +1,151 @@
|
||||
"""``GripperClient.dropped()``：E1 掉件判据只认**开口塌陷**，不认状态位抖动。
|
||||
|
||||
为什么值得单独钉住（2026-09-24 实测踩到）：
|
||||
仿真里 ``GripperStatus.is_gripped`` 是**推断值**（"指令到了合拢侧 + 实际开口
|
||||
比指令大出容差"）。S08 拧规时会在"合到某个开口 / 全合"之间发指令，只要某一帧
|
||||
的指令落到与实际开口同一档，``is_gripped`` 就翻成 False —— 于是
|
||||
**S08 第 2/12 轮报「通止规掉了」**（爪里其实好好的），整条流程转去等人工
|
||||
"放好通止规"，而仿真里没有这个人。
|
||||
|
||||
判据改成"夹住时的开口 − 当前开口 ≥ 5mm"以后，这种抖动不再报掉件；而真的掉了
|
||||
（两指合到底、开口塌几十毫米）仍然会报。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, List
|
||||
|
||||
from arm_msgs.msg import GripperStatus
|
||||
|
||||
|
||||
class _StubNode:
|
||||
"""``GripperClient`` 只用到这三个方法；测试里不建任何 ROS 实体。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.warnings: List[str] = []
|
||||
|
||||
def create_client(self, srv_type: Any, name: str) -> None:
|
||||
return None
|
||||
|
||||
def create_subscription(self, msg_type: Any, topic: str, cb: Any, qos: Any) -> None:
|
||||
self._cb = cb
|
||||
return None
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return _StubLogger(self.warnings)
|
||||
|
||||
|
||||
class _StubLogger:
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def info(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
def warn(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
def error(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
|
||||
def _status(is_gripped: bool, position: float) -> GripperStatus:
|
||||
msg = GripperStatus()
|
||||
msg.is_gripped = bool(is_gripped)
|
||||
msg.position = float(position)
|
||||
return msg
|
||||
|
||||
|
||||
def _client():
|
||||
from arm_application.gauge_assembly_node import GripperClient
|
||||
|
||||
node = _StubNode()
|
||||
return GripperClient(node, "contract", "/gripper/status"), node
|
||||
|
||||
|
||||
def test_no_drop_before_ever_gripped():
|
||||
"""装配前爪里本来就空：状态位说什么都不算掉件（否则一开场就 E1）。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(False, 0.0))
|
||||
assert client.dropped() is False
|
||||
|
||||
|
||||
def test_a_wide_open_flap_does_not_poison_the_baseline():
|
||||
"""**本次踩到的那个假 E1 的真因**：全开时 is_gripped 也闪 True（实测 99.29mm）。
|
||||
|
||||
以前"第一次判到夹住"就记基准 ⇒ 基准被记成 99.29mm ⇒ 塌陷量算成 77mm ⇒
|
||||
报"通止规掉了"，而规好好地在爪里、真实开口一直是 21.9mm。
|
||||
现在基准只由明确调用 adopt_current_grip 记录：状态位怎么闪都不动它。
|
||||
"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 99.29)) # 全开时的假 GRIPPING
|
||||
assert client._grip_aperture is None, "状态位不许写基准"
|
||||
assert client.dropped() is False
|
||||
client._on_contract(_status(True, 21.9)) # 真正夹住规之后
|
||||
client.adopt_current_grip("测试：确认夹住")
|
||||
assert client._grip_aperture == 21.9
|
||||
client._on_contract(_status(False, 21.8))
|
||||
assert client.dropped() is False, "开口没塌就不是掉件"
|
||||
|
||||
|
||||
def test_is_gripped_flap_is_not_a_drop():
|
||||
"""**本次踩到的那个假 E1**：指令切换让 is_gripped 翻 False，开口没塌。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 24.9)) # 夹住 φ30 的规，开口停在 24.9mm
|
||||
client.adopt_current_grip("测试：确认夹住")
|
||||
assert client.dropped() is False
|
||||
client._on_contract(_status(False, 24.8)) # 指令落到同一档 → 推断值翻 False
|
||||
assert client.dropped() is False, "开口没塌就不是掉件"
|
||||
|
||||
|
||||
def test_real_drop_is_detected_by_aperture_collapse():
|
||||
"""真的掉了：两指一路合到底，开口塌到几毫米。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 24.9))
|
||||
client.adopt_current_grip("测试：确认夹住")
|
||||
client._on_contract(_status(False, 0.2))
|
||||
assert client.dropped() is True
|
||||
|
||||
|
||||
def test_aperture_jitter_within_the_threshold_is_not_a_drop():
|
||||
"""实测抖动是亚毫米量级；阈值 5mm 不该被它触发。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 24.9))
|
||||
client.adopt_current_grip("测试：确认夹住")
|
||||
client._on_contract(_status(False, 24.9 - 4.9))
|
||||
assert client.dropped() is False
|
||||
|
||||
|
||||
def test_without_aperture_reading_falls_back_to_the_old_criterion():
|
||||
"""拿不到 position（老固件/别的夹爪）时退回旧判据：会抖，但不至于"永远判不了掉件"。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, float("nan")))
|
||||
assert client.adopt_current_grip("测试：确认夹住") is None
|
||||
assert client._grip_aperture is None
|
||||
client._on_contract(_status(False, float("nan")))
|
||||
assert client.dropped() is True, "没有开口读数时不能变成「永远不报掉件」"
|
||||
|
||||
|
||||
def test_settle_waits_for_the_motion_to_finish():
|
||||
"""``settle`` 不能读"动作还没做完"的旧值。
|
||||
|
||||
实测：``close`` 服务返回时手指还在动，立刻读开口读到的是**全开 99.93mm** ——
|
||||
于是"第 1 轮夹紧后：开口 99.93mm（夹住时 99.29mm）"这种荒唐日志。
|
||||
``settle`` 要一直等到连续两帧一样才认。
|
||||
"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 99.93))
|
||||
assert client.settle(settle_s=0.2) == 99.93
|
||||
# 动作进行中：值一直在变 ⇒ 不到稳定就不返回（这里只验证它会返回**最终**值）
|
||||
client._on_contract(_status(False, 30.0))
|
||||
assert client.settle(settle_s=0.2) == 30.0
|
||||
|
||||
|
||||
def test_adopt_current_grip_records_what_is_in_the_jaws_now():
|
||||
"""基准 = "确认夹住那一刻"的开口；之后状态位再翻也不改参照。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 21.9))
|
||||
assert client.adopt_current_grip("测试") == 21.9
|
||||
client._on_contract(_status(False, 21.4))
|
||||
assert client.grip_aperture() == 21.9
|
||||
assert client.collapse_mm() == 0.5
|
||||
@@ -0,0 +1,225 @@
|
||||
"""``gauge_kin`` 的单元测试：S01 的"转到极限角"与"确实绕对了轴"。
|
||||
|
||||
两个都在测"算错了不会报错、只会做错动作"的东西：
|
||||
限位夹取算错 → 要么撞限位被驱动器拒绝，要么离极限还差很远（流程看起来一样）；
|
||||
转轴验证算错 → 无论实际绕哪根轴转都报"没问题"，判据等于没有。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.gauge_kin import (
|
||||
DEFAULT_LIMIT_MARGIN_DEG,
|
||||
a7_limit_target,
|
||||
axis_misalignment_deg,
|
||||
match_limits,
|
||||
parse_joint_limits,
|
||||
prewind,
|
||||
rotation_about_axis_deg,
|
||||
unit_tool_z,
|
||||
vertical_down_error_deg,
|
||||
)
|
||||
|
||||
URDF = """<?xml version="1.0"?>
|
||||
<robot name="rizon">
|
||||
<link name="Rizon4-063989_link6"/>
|
||||
<link name="Rizon4-063989_link7"/>
|
||||
<joint name="Rizon4-063989_joint6" type="revolute">
|
||||
<parent link="Rizon4-063989_link5"/>
|
||||
<child link="Rizon4-063989_link6"/>
|
||||
<limit lower="-1.4835" upper="4.6251" effort="39" velocity="4.8869"/>
|
||||
</joint>
|
||||
<joint name="Rizon4-063989_joint7" type="revolute">
|
||||
<parent link="Rizon4-063989_link6"/>
|
||||
<child link="Rizon4-063989_link7"/>
|
||||
<limit lower="-3.0543" upper="3.0543" effort="39" velocity="4.8869"/>
|
||||
</joint>
|
||||
<joint name="Rizon4-063989_roll" type="continuous">
|
||||
<parent link="Rizon4-063989_link7"/>
|
||||
<child link="Rizon4-063989_tool"/>
|
||||
</joint>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# URDF 解析
|
||||
# =============================================================================
|
||||
def test_parse_limits_keeps_full_joint_names():
|
||||
"""关节名原样保留（含 SN 前缀）。
|
||||
|
||||
"顺手去掉前缀"这种清洗会在换臂时静默失配：所有关节都查不到限位，
|
||||
于是限位夹取悄悄失效，现象是"贴着限位下发、驱动器拒绝"。
|
||||
"""
|
||||
lim = parse_joint_limits(URDF)
|
||||
assert lim["Rizon4-063989_joint7"] == (-3.0543, 3.0543)
|
||||
assert lim["Rizon4-063989_joint6"] == (-1.4835, 4.6251)
|
||||
# continuous 关节没有 limit，不该被当成解析结果
|
||||
assert "Rizon4-063989_roll" not in lim
|
||||
|
||||
|
||||
def test_parse_limits_on_garbage_returns_empty_not_partial():
|
||||
assert parse_joint_limits("") == {}
|
||||
assert parse_joint_limits("not xml at all") == {}
|
||||
# 未展开的 xacro：既不是合法 XML，也没有可用的 limit 行
|
||||
assert parse_joint_limits("<robot xmlns:xacro='x'>${bad}</robot>") == {}
|
||||
|
||||
|
||||
def test_parse_limits_falls_back_to_line_regex_on_malformed_xml():
|
||||
"""URDF 里有未转义的 ``&`` 时整份 XML 解析会失败，但限位行仍然可读。
|
||||
|
||||
降级到逐行正则能救回"拿到的是半成品 URDF"这种常见情况；救不回就返回空表
|
||||
（= 不做限位夹取，由调用方告警），**不会**返回半套数据。
|
||||
"""
|
||||
broken = URDF.replace('<link name="Rizon4-063989_link6"/>', '<link name="a & b"/>')
|
||||
lim = parse_joint_limits(broken)
|
||||
assert lim["Rizon4-063989_joint7"] == (-3.0543, 3.0543)
|
||||
|
||||
|
||||
def test_match_limits_is_all_or_nothing():
|
||||
lim = parse_joint_limits(URDF)
|
||||
assert match_limits(lim, ["Rizon4-063989_joint7"]) == (-3.0543, 3.0543)
|
||||
# 缺一个就整体不可用：半套限位会让"能不能转到极限"的结论无法解释
|
||||
assert match_limits(lim, ["Rizon4-063989_joint7", "不存在_joint8"]) is None
|
||||
assert match_limits({}, ["Rizon4-063989_joint7"]) is None
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# S01 预绕
|
||||
# =============================================================================
|
||||
def test_prewind_without_limits_applies_full_angle():
|
||||
r = prewind(q_now_rad=0.0, want_deg=150.0, direction=1, limits=None)
|
||||
assert not r.has_limits and not r.clamped
|
||||
assert math.isclose(r.applied_deg, 150.0, abs_tol=1e-9)
|
||||
assert math.isclose(r.target_rad, math.radians(150.0), abs_tol=1e-12)
|
||||
|
||||
|
||||
def test_prewind_direction_sign():
|
||||
"""方向只由 ``extreme_direction`` 决定，不靠代码里的负号。"""
|
||||
up = prewind(0.0, 90.0, 1, None)
|
||||
down = prewind(0.0, 90.0, -1, None)
|
||||
assert up.applied_deg > 0 and down.applied_deg < 0
|
||||
assert math.isclose(up.applied_deg, -down.applied_deg, abs_tol=1e-9)
|
||||
with pytest.raises(ValueError):
|
||||
prewind(0.0, 90.0, 0, None)
|
||||
with pytest.raises(ValueError):
|
||||
prewind(0.0, -90.0, 1, None)
|
||||
|
||||
|
||||
def test_prewind_clamps_inside_the_limit_with_margin():
|
||||
"""上限 3.0543 rad（175°），余量 3° → 最多转到 172°，且报 clamped。"""
|
||||
r = prewind(q_now_rad=0.0, want_deg=150.0, direction=1, limits=(-3.0543, 3.0543))
|
||||
assert math.isclose(r.applied_deg, 150.0, abs_tol=1e-9)
|
||||
assert not r.clamped
|
||||
|
||||
r2 = prewind(q_now_rad=0.0, want_deg=179.0, direction=1, limits=(-3.0543, 3.0543))
|
||||
assert r2.clamped and r2.has_limits
|
||||
assert r2.applied_deg < 179.0
|
||||
# 关键断言：目标必须落在 "上限 − 余量" 之内（余量是给驱动器留的，不是装饰）
|
||||
upper = 3.0543 - math.radians(DEFAULT_LIMIT_MARGIN_DEG)
|
||||
assert r2.target_rad <= upper + 1e-12
|
||||
assert math.isclose(r2.target_rad, upper, abs_tol=1e-12)
|
||||
|
||||
|
||||
def test_prewind_clamps_toward_lower_limit_for_negative_direction():
|
||||
r = prewind(q_now_rad=0.0, want_deg=179.0, direction=-1, limits=(-3.0543, 3.0543))
|
||||
lower = -3.0543 + math.radians(DEFAULT_LIMIT_MARGIN_DEG)
|
||||
assert r.clamped and math.isclose(r.target_rad, lower, abs_tol=1e-12)
|
||||
|
||||
|
||||
def test_prewind_reports_backwards_motion_when_already_inside_margin():
|
||||
"""**边界**：已经贴在上限余量之内时，目标会被夹到当前位置**之后**。
|
||||
|
||||
这时 ``applied_deg`` 是负的（臂会往回退一点点）。这不是 bug，是"宁可退
|
||||
一点也不要撞限位"；但必须如实报出来，否则日志里会出现"要转 150°、
|
||||
实际转了 -2°"却没人解释。
|
||||
"""
|
||||
q = 3.0543 - math.radians(1.0) # 已经在余量(3°)之内
|
||||
r = prewind(q_now_rad=q, want_deg=150.0, direction=1, limits=(-3.0543, 3.0543))
|
||||
assert r.clamped and r.applied_deg < 0.0
|
||||
assert r.target_rad <= 3.0543 - math.radians(DEFAULT_LIMIT_MARGIN_DEG) + 1e-12
|
||||
|
||||
|
||||
def test_prewind_handles_degenerate_limits():
|
||||
"""上下限比余量还窄（配置错）时，退到区间中点而不是静默走偏。"""
|
||||
r = prewind(0.0, 90.0, 1, limits=(-0.01, 0.01), margin_deg=3.0)
|
||||
assert math.isclose(r.target_rad, 0.0, abs_tol=1e-12)
|
||||
assert r.clamped
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 绕轴旋转的验证
|
||||
# =============================================================================
|
||||
def _rot_z(deg: float) -> np.ndarray:
|
||||
t = math.radians(deg)
|
||||
return np.array(
|
||||
[[math.cos(t), -math.sin(t), 0.0], [math.sin(t), math.cos(t), 0.0], [0.0, 0.0, 1.0]]
|
||||
)
|
||||
|
||||
|
||||
def _rot_x(deg: float) -> np.ndarray:
|
||||
t = math.radians(deg)
|
||||
return np.array(
|
||||
[[1.0, 0.0, 0.0], [0.0, math.cos(t), -math.sin(t)], [0.0, math.sin(t), math.cos(t)]]
|
||||
)
|
||||
|
||||
|
||||
def test_rotation_about_axis_reads_the_signed_angle():
|
||||
R0 = np.eye(3)
|
||||
assert math.isclose(rotation_about_axis_deg(R0, _rot_z(30.0), [0, 0, 1]), 30.0, abs_tol=1e-9)
|
||||
assert math.isclose(rotation_about_axis_deg(R0, _rot_z(-30.0), [0, 0, 1]), -30.0, abs_tol=1e-9)
|
||||
# 转了很多圈也要给出同一个角（atan2 值域 ±180°，这里只用来核对单步）
|
||||
assert math.isclose(rotation_about_axis_deg(R0, _rot_z(179.0), [0, 0, 1]), 179.0, abs_tol=1e-9)
|
||||
|
||||
|
||||
def test_rotation_about_axis_ignores_an_orthogonal_rotation_axis():
|
||||
"""绕 X 转 60°，在 Z 轴上的投影应为 0 —— 这正是"绕错轴"的读数特征。"""
|
||||
val = rotation_about_axis_deg(np.eye(3), _rot_x(60.0), [0, 0, 1])
|
||||
assert abs(val) < 1e-9
|
||||
|
||||
|
||||
def test_axis_misalignment_separates_right_axis_from_wrong_axis():
|
||||
"""S01 的判错依据：转对轴 ≈0°，转错轴 ≈90°。
|
||||
|
||||
只看"转了多少度"不够 —— 绕正确轴转 90° 和绕错误轴转 90° 的角度读数一样。
|
||||
"""
|
||||
R0 = np.eye(3)
|
||||
assert axis_misalignment_deg(R0, _rot_z(45.0), [0, 0, 1]) < 1e-6
|
||||
assert axis_misalignment_deg(R0, _rot_x(45.0), [0, 0, 1]) > 89.0
|
||||
# 轴线取反方向应视为同一根轴（cos 取绝对值）
|
||||
assert axis_misalignment_deg(R0, _rot_z(45.0), [0, 0, -1]) < 1e-6
|
||||
# 几乎没转时谈不上"转错轴"
|
||||
assert axis_misalignment_deg(R0, np.eye(3), [0, 0, 1]) == 0.0
|
||||
|
||||
|
||||
def test_rotation_helpers_reject_zero_axis():
|
||||
with pytest.raises(ValueError):
|
||||
rotation_about_axis_deg(np.eye(3), _rot_z(10.0), [0.0, 0.0, 0.0])
|
||||
|
||||
|
||||
def test_a7_limit_moves_only_the_last_joint():
|
||||
"""逆时针到上限、顺时针到下限。前六轴必须原样，没有限位就拒绝。"""
|
||||
q = np.array([0.1, -0.2, 0.3, -0.4, 0.5, -0.6, 0.2])
|
||||
limits = (-3.0543, 3.0543)
|
||||
ccw = a7_limit_target(q, limits, direction=1, margin_deg=3.0)
|
||||
cw = a7_limit_target(q, limits, direction=-1, margin_deg=3.0)
|
||||
assert ccw is not None and cw is not None
|
||||
np.testing.assert_allclose(ccw[:6], q[:6])
|
||||
np.testing.assert_allclose(cw[:6], q[:6])
|
||||
assert ccw[6] == pytest.approx(3.0543 - math.radians(3.0))
|
||||
assert cw[6] == pytest.approx(-3.0543 + math.radians(3.0))
|
||||
assert a7_limit_target(q, None, 1) is None
|
||||
assert a7_limit_target(q, limits, 1, margin_deg=200.0) is None
|
||||
|
||||
|
||||
def test_tool_z_and_vertical_down_error():
|
||||
"""工具 Z 朝世界 -Z = 0°；水平 = 90°；朝上 = 180°。"""
|
||||
R_down = np.diag([1.0, -1.0, -1.0])
|
||||
assert np.allclose(unit_tool_z(R_down), [0.0, 0.0, -1.0])
|
||||
assert vertical_down_error_deg(R_down) < 1e-6
|
||||
assert math.isclose(vertical_down_error_deg(_rot_x(90.0) @ R_down), 90.0, abs_tol=1e-6)
|
||||
assert math.isclose(vertical_down_error_deg(_rot_x(180.0) @ R_down), 180.0, abs_tol=1e-6)
|
||||
@@ -0,0 +1,159 @@
|
||||
"""主流程节点与状态机的**接缝**测试（不需要 ROS、不需要真机）。
|
||||
|
||||
为什么要有这一层
|
||||
----------------
|
||||
``gauge_assembly_node`` 的每个 ``_step_*`` 方法会返回一个 :class:`Event`，
|
||||
由 ``gauge_fsm.TRANSITIONS`` 决定它去哪。这两份东西是**分开写的**：
|
||||
|
||||
* 状态机里没有的转移 → 运行到那一步抛 :class:`FsmError`（好：当场炸）；
|
||||
* 但如果是**最危险的那一步**才炸（力超限、掉件、关节极限），代价就大了。
|
||||
|
||||
所以这里直接读节点源码的 AST，把"每个阶段可能返回哪些事件"抽出来，
|
||||
逐个问状态机"这一步合法吗"。这样新增/修改流程时，忘了登记转移会在
|
||||
``pytest`` 里就报出来，而不是等到真机上。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import pathlib
|
||||
from typing import Dict, Set
|
||||
|
||||
import pytest
|
||||
|
||||
from arm_application.gauge_fsm import Event, GaugeContext, GaugeFsm, State
|
||||
|
||||
NODE_SRC = (
|
||||
pathlib.Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "gauge_assembly_node.py"
|
||||
)
|
||||
|
||||
|
||||
def _node_ast() -> ast.Module:
|
||||
return ast.parse(NODE_SRC.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _step_map(tree: ast.Module) -> Dict[str, str]:
|
||||
"""``State.X -> "_step_y"``（读节点的 ``_STEPS`` 表）。"""
|
||||
for node in ast.walk(tree):
|
||||
# 表是带类型标注的（``_STEPS: Dict[State, str] = {...}``）→ AnnAssign
|
||||
target = None
|
||||
if isinstance(node, ast.AnnAssign) and isinstance(node.target, ast.Name):
|
||||
target = node.target.id
|
||||
elif isinstance(node, ast.Assign) and len(node.targets) == 1:
|
||||
tgt = node.targets[0]
|
||||
target = tgt.id if isinstance(tgt, ast.Name) else None
|
||||
if target != "_STEPS":
|
||||
continue
|
||||
value = node.value
|
||||
assert isinstance(value, ast.Dict), "_STEPS 必须是字面量字典（测试要读它）"
|
||||
out: Dict[str, str] = {}
|
||||
for k, v in zip(value.keys, value.values):
|
||||
assert isinstance(k, ast.Attribute) and isinstance(v, ast.Constant)
|
||||
out[k.attr] = str(v.value)
|
||||
return out
|
||||
raise AssertionError("节点里找不到 _STEPS 表")
|
||||
|
||||
|
||||
def _events_in(node: ast.AST) -> Set[str]:
|
||||
"""表达式里出现的所有 ``Event.XXX`` 名字。"""
|
||||
return {
|
||||
n.attr
|
||||
for n in ast.walk(node)
|
||||
if isinstance(n, ast.Attribute)
|
||||
and isinstance(n.value, ast.Name)
|
||||
and n.value.id == "Event"
|
||||
}
|
||||
|
||||
|
||||
def _returned_events(func: ast.FunctionDef) -> Set[str]:
|
||||
"""一个 ``_step_*`` 方法可能返回的事件。
|
||||
|
||||
收集两处，别的地方不算（比如异常处理里顺手记一条 ``Event.ABORT`` 做日志，
|
||||
那不是"返回"）：
|
||||
1. 所有 ``return <expr>`` 里的 ``Event.X``；
|
||||
2. 所有 ``fsm_event=Event.X`` 关键字实参（喂给 ``_rotate_joint_and_verify``，
|
||||
它会把成功事件原样返回）。
|
||||
"""
|
||||
out: Set[str] = set()
|
||||
for node in ast.walk(func):
|
||||
if isinstance(node, ast.Return) and node.value is not None:
|
||||
out |= _events_in(node.value)
|
||||
elif isinstance(node, ast.Call):
|
||||
for kw in node.keywords:
|
||||
if kw.arg == "fsm_event":
|
||||
out |= _events_in(kw.value)
|
||||
return out
|
||||
|
||||
|
||||
def _func_by_name(tree: ast.Module, name: str) -> ast.FunctionDef:
|
||||
for node in ast.walk(tree):
|
||||
if isinstance(node, ast.FunctionDef) and node.name == name:
|
||||
return node
|
||||
raise AssertionError(f"节点里找不到方法 {name}")
|
||||
|
||||
|
||||
def test_steps_table_covers_every_non_terminal_state() -> None:
|
||||
"""每个非终态都要有处理方法 —— 漏一个就是"流程走到这儿不知道干什么"。"""
|
||||
tree = _node_ast()
|
||||
steps = _step_map(tree)
|
||||
for st in State:
|
||||
if st in (State.DONE, State.ABORT):
|
||||
continue
|
||||
assert st.name in steps, f"{st} 没有登记处理方法"
|
||||
assert len(steps) == len(set(steps.values())), "一个方法不该被两个状态复用"
|
||||
|
||||
|
||||
def test_every_returned_event_is_legal_in_that_state() -> None:
|
||||
"""本阶段可能返回的每个事件，在状态机里必须合法。
|
||||
|
||||
这是本文件的主要断言：它把"节点写的事件"和"状态机登记的转移"对齐，
|
||||
免得在真机上跑到那一步才抛 FsmError。
|
||||
"""
|
||||
tree = _node_ast()
|
||||
problems = []
|
||||
for state_name, method in _step_map(tree).items():
|
||||
state = State[state_name]
|
||||
func = _func_by_name(tree, method)
|
||||
for ev_name in sorted(_returned_events(func)):
|
||||
event = Event[ev_name]
|
||||
# 每个事件都在"干净上下文 + 该状态"下问一次状态机
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=2), start=state)
|
||||
if not fsm.can(event):
|
||||
problems.append(f"{state.value} 的 {method} 会返回 {ev_name}，但状态机不接受")
|
||||
assert not problems, "节点与状态机不一致：\n " + "\n ".join(problems)
|
||||
|
||||
|
||||
def test_s08_drop_goes_to_home_extreme() -> None:
|
||||
"""E1 的落点必须还是 S01：它要在那里张开爪、等人工放好。
|
||||
|
||||
这条单列出来，是因为"掉件后去哪"很容易被顺手改成 S11（"反正收工"），
|
||||
那样掉在孔里的规就没人管了。
|
||||
"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=2), start=State.SCREW_IN)
|
||||
assert fsm.handle(Event.GAUGE_DROPPED) is State.HOME_EXTREME
|
||||
assert fsm.handle(Event.RELOADED) is State.NEXT_HOLE
|
||||
# 掉件后孔序号推进（看板：该孔记为完成，换下一个孔）
|
||||
assert fsm.ctx.hole_index == 1
|
||||
|
||||
|
||||
def test_contact_force_over_limit_aborts_not_retries() -> None:
|
||||
"""法向力超上限 → 中止，不是"抬起来再来一次"。
|
||||
|
||||
超限说明已经压进去了（位置下限或规没对准），原地重试会把工件压变形。
|
||||
"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=2), start=State.CONTACT)
|
||||
assert fsm.handle(Event.FORCE_OVER_LIMIT) is State.ABORT
|
||||
|
||||
|
||||
@pytest.mark.parametrize("state_name", ["SNAP", "HOVER_K"])
|
||||
def test_snap_and_hover_have_a_back_edge(state_name: str) -> None:
|
||||
"""S05/S06 必须能退回去：不稳定 → 再等；对位超差 → 重拍。
|
||||
|
||||
没有回边的结果是：判据没过就一路往下走（去接触、去拧），
|
||||
而这是"孔没对准"最危险的表达方式。
|
||||
"""
|
||||
fsm = GaugeFsm(GaugeContext(holes_total=2), start=State[state_name])
|
||||
back = Event.SNAP_UNSTABLE if state_name == "SNAP" else Event.ALIGN_OUT
|
||||
assert fsm.can(back)
|
||||
assert fsm.handle(back) is State.SNAP
|
||||
@@ -0,0 +1,73 @@
|
||||
"""Station gripper node must not open the bus until probe, and must recover a dead session."""
|
||||
from __future__ import annotations
|
||||
|
||||
import pytest
|
||||
|
||||
rclpy = pytest.importorskip("rclpy")
|
||||
|
||||
from arm_application.dh_pgea import DhPgeaError, GripperState
|
||||
from arm_application import gripper_node as gripper
|
||||
|
||||
|
||||
class FakeGripper:
|
||||
def __init__(self, **kwargs):
|
||||
self.opens = 0
|
||||
self.closes = 0
|
||||
self.reads = 0
|
||||
self.fail_until_open = 0
|
||||
self.state = GripperState(init_state=1, grip_state=1, position=1000, target_position=1000, target_force=20)
|
||||
|
||||
def open(self):
|
||||
self.opens += 1
|
||||
|
||||
def close(self):
|
||||
self.closes += 1
|
||||
|
||||
def get_run_states(self, retries=8):
|
||||
self.reads += 1
|
||||
if self.opens <= self.fail_until_open:
|
||||
raise DhPgeaError("在 /dev/ttyUSB0 上 Modbus 读 0x0201 失败（无有效应答 last=(empty)）")
|
||||
return self.state
|
||||
|
||||
|
||||
@pytest.fixture()
|
||||
def ros(monkeypatch):
|
||||
monkeypatch.setattr(gripper, "DhPgeaGripper", FakeGripper)
|
||||
rclpy.init()
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
def test_startup_does_not_open_serial(ros):
|
||||
node = gripper.GripperNode()
|
||||
try:
|
||||
assert node.driver.opens == 0
|
||||
assert node._online is False
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
def test_connect_closes_then_opens_before_reading(ros):
|
||||
node = gripper.GripperNode()
|
||||
try:
|
||||
assert node._connect() is True
|
||||
assert node.driver.closes >= 1
|
||||
assert node.driver.opens == 1
|
||||
assert node._online is True
|
||||
assert node._reading.position == 1000
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
def test_poll_reopens_after_empty_reply(ros):
|
||||
node = gripper.GripperNode()
|
||||
try:
|
||||
assert node._connect() is True
|
||||
node.driver.fail_until_open = node.driver.opens
|
||||
node._poll()
|
||||
assert node._online is True
|
||||
assert node.driver.opens >= 2
|
||||
finally:
|
||||
node.destroy_node()
|
||||
@@ -0,0 +1,462 @@
|
||||
"""handeye_lib 的纯函数测试 —— 不需要机器人、相机或标定板。
|
||||
|
||||
这里最要紧的一个测试是 `test_roundtrip_recovers_known_transform`：
|
||||
它用一个人为设定的 `X_true` 反过来生成每个视图的 (gripper→base, target→cam)，
|
||||
再让求解器去还原。这是唯一能在没有硬件的情况下真正验证"标定算得对不对"的办法 ——
|
||||
如果求解链路有符号/转置/坐标系方向的错误，它一定会失败。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.handeye_lib import (
|
||||
BoardConfig,
|
||||
CameraIntrinsics,
|
||||
ChessboardStabilizer,
|
||||
Rt_to_T,
|
||||
T_from_flexiv_tcp,
|
||||
board_object_points,
|
||||
calibrate_hand_eye,
|
||||
cam_to_flange_via_tool,
|
||||
format_transform_yaml,
|
||||
invert_Rt,
|
||||
is_pose_filled,
|
||||
pose_to_Rt,
|
||||
quat_xyzw_to_R,
|
||||
)
|
||||
|
||||
|
||||
def _rot(axis: str, deg: float) -> np.ndarray:
|
||||
a = math.radians(deg)
|
||||
c, s = math.cos(a), math.sin(a)
|
||||
if axis == "x":
|
||||
return np.array([[1, 0, 0], [0, c, -s], [0, s, c]], float)
|
||||
if axis == "y":
|
||||
return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], float)
|
||||
return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], float)
|
||||
|
||||
|
||||
def _rand_transform(rng: np.random.Generator) -> np.ndarray:
|
||||
"""随机但物理上合法的刚体变换（旋转正交且 det=+1）。"""
|
||||
R = _rot("x", rng.uniform(-180, 180)) @ _rot("y", rng.uniform(-180, 180)) @ _rot(
|
||||
"z", rng.uniform(-180, 180)
|
||||
)
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = rng.uniform(-0.6, 0.6, 3)
|
||||
return T
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 旋转/位姿基础
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_quat_identity_is_identity_matrix():
|
||||
R = quat_xyzw_to_R([0.0, 0.0, 0.0, 1.0])
|
||||
assert np.allclose(R, np.eye(3), atol=1e-12)
|
||||
|
||||
|
||||
def test_quat_matches_rotation_matrix_for_known_angles():
|
||||
for axis, deg in (("x", 30.0), ("y", -47.0), ("z", 121.0)):
|
||||
q = _axis_angle_to_quat(axis, deg)
|
||||
assert np.allclose(quat_xyzw_to_R(q), _rot(axis, deg), atol=1e-9), axis
|
||||
|
||||
|
||||
def test_quat_is_normalized_before_use():
|
||||
"""非单位四元数不该影响结果 —— 有些驱动会给未归一化的值。"""
|
||||
q = np.array(_axis_angle_to_quat("z", 90.0)) * 3.7
|
||||
assert np.allclose(quat_xyzw_to_R(list(q)), _rot("z", 90.0), atol=1e-9)
|
||||
|
||||
|
||||
def _axis_angle_to_quat(axis: str, deg: float):
|
||||
a = math.radians(deg) / 2.0
|
||||
s, c = math.sin(a), math.cos(a)
|
||||
if axis == "x":
|
||||
return [s, 0.0, 0.0, c]
|
||||
if axis == "y":
|
||||
return [0.0, s, 0.0, c]
|
||||
return [0.0, 0.0, s, c]
|
||||
|
||||
|
||||
def test_pose_to_Rt_shapes():
|
||||
R, t = pose_to_Rt([1.0, 2.0, 3.0], [0.0, 0.0, 0.0, 1.0])
|
||||
assert R.shape == (3, 3)
|
||||
assert t.shape == (3, 1)
|
||||
assert np.allclose(t.reshape(3), [1.0, 2.0, 3.0])
|
||||
|
||||
|
||||
def test_Rt_to_T_round_trip():
|
||||
rng = np.random.default_rng(0)
|
||||
R = _rot("z", 33.0)
|
||||
t = np.array([[0.1], [-0.2], [0.3]])
|
||||
T = Rt_to_T(R, t)
|
||||
assert T.shape == (4, 4)
|
||||
assert np.allclose(T[3], [0, 0, 0, 1])
|
||||
assert np.allclose(T[:3, :3], R)
|
||||
assert np.allclose(T[:3, 3], t.reshape(3))
|
||||
|
||||
|
||||
def test_invert_Rt_is_true_inverse():
|
||||
rng = np.random.default_rng(1)
|
||||
R = _rot("x", 12.0) @ _rot("y", 88.0)
|
||||
t = rng.uniform(-1, 1, (3, 1))
|
||||
Ri, ti = invert_Rt(R, t)
|
||||
T = Rt_to_T(R, t)
|
||||
Ti = Rt_to_T(Ri, ti)
|
||||
assert np.allclose(T @ Ti, np.eye(4), atol=1e-12)
|
||||
assert np.allclose(Ti @ T, np.eye(4), atol=1e-12)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 标定板几何
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_board_object_points_count_and_layout():
|
||||
cfg = BoardConfig(cols=8, rows=11, square_size=0.015)
|
||||
pts = board_object_points(cfg)
|
||||
assert pts.shape == (88, 3)
|
||||
# z 全为 0（板平面），x/y 是网格
|
||||
assert np.allclose(pts[:, 2], 0.0)
|
||||
assert np.isclose(pts[:, 0].max(), 7 * 0.015)
|
||||
assert np.isclose(pts[:, 1].max(), 10 * 0.015)
|
||||
# 第一行是 0..7 个 x
|
||||
assert np.allclose(pts[:8, 0], np.arange(8) * 0.015)
|
||||
|
||||
|
||||
def test_board_square_size_actually_scales_points():
|
||||
a = board_object_points(BoardConfig(cols=4, rows=3, square_size=0.01))
|
||||
b = board_object_points(BoardConfig(cols=4, rows=3, square_size=0.03))
|
||||
assert np.allclose(b, a * 3.0)
|
||||
|
||||
|
||||
def test_camera_intrinsics_holds_k_and_dist():
|
||||
K = np.array([[600.0, 0.0, 320.0], [0.0, 601.0, 240.0], [0.0, 0.0, 1.0]])
|
||||
dist = np.zeros(5)
|
||||
ci = CameraIntrinsics(K=K, dist=dist)
|
||||
assert ci.K.shape == (3, 3)
|
||||
assert ci.K[0, 0] == 600.0
|
||||
assert ci.K[1, 2] == 240.0
|
||||
assert ci.dist.size == 5
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 稳定性判据
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _fake_corners(n: int = 88, jitter: float = 0.0, seed: int = 0):
|
||||
rng = np.random.default_rng(seed)
|
||||
base = np.stack(
|
||||
[np.linspace(0, 100, n), np.linspace(0, 70, n), np.zeros(n)], axis=1
|
||||
).astype(np.float32)
|
||||
if jitter:
|
||||
base = base + rng.normal(0, jitter, base.shape).astype(np.float32)
|
||||
return base
|
||||
|
||||
|
||||
def test_stabilizer_not_stable_before_window_is_filled():
|
||||
st = ChessboardStabilizer(window=8, max_pixel_std=0.45)
|
||||
for _ in range(3):
|
||||
ok, _std = st.push(_fake_corners(jitter=0.01))
|
||||
assert not ok, "窗口没填满就不该判稳"
|
||||
|
||||
|
||||
def test_stabilizer_declares_stable_for_identical_corners():
|
||||
st = ChessboardStabilizer(window=5, max_pixel_std=0.45)
|
||||
fixed = _fake_corners()
|
||||
ok = False
|
||||
for _ in range(6):
|
||||
ok, std = st.push(fixed)
|
||||
assert ok
|
||||
assert std == pytest.approx(0.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_stabilizer_rejects_noisy_corners():
|
||||
st = ChessboardStabilizer(window=5, max_pixel_std=0.45)
|
||||
ok = False
|
||||
for i in range(6):
|
||||
ok, std = st.push(_fake_corners(jitter=5.0, seed=i))
|
||||
assert not ok, "角点抖动 5px 远超 0.45px 阈值，不该判稳"
|
||||
assert std > 0.45
|
||||
|
||||
|
||||
def test_stabilizer_missing_board_resets_streak():
|
||||
st = ChessboardStabilizer(window=3, max_pixel_std=0.45)
|
||||
fixed = _fake_corners()
|
||||
st.push(fixed)
|
||||
st.push(fixed)
|
||||
ok, _ = st.push(None) # 看不到板
|
||||
assert not ok
|
||||
# 重新开始计数，而不是接着之前的 2 次
|
||||
ok, _ = st.push(fixed)
|
||||
assert not ok
|
||||
ok, _ = st.push(fixed)
|
||||
assert not ok
|
||||
ok, _ = st.push(fixed)
|
||||
assert ok, "连续 3 帧相同角点后应判稳"
|
||||
|
||||
|
||||
def test_stabilizer_reset_clears_history():
|
||||
st = ChessboardStabilizer(window=3, max_pixel_std=0.45)
|
||||
fixed = _fake_corners()
|
||||
for _ in range(3):
|
||||
st.push(fixed)
|
||||
st.reset()
|
||||
ok, _ = st.push(fixed)
|
||||
assert not ok
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 核心：合成数据往返（真正验证标定算法）
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _synthesize_views(
|
||||
X_true: np.ndarray, T_target2base: np.ndarray, n: int, seed: int = 7
|
||||
):
|
||||
"""由已知 X_true 生成 n 组视图观测。
|
||||
|
||||
eye-in-hand 的坐标链：target → camera → gripper → base
|
||||
T_t2b = T_g2b · X · T_t2c (X = T_cam2gripper)
|
||||
所以给定任意的 T_g2b，可反推出该视图下应该观测到的 T_t2c：
|
||||
T_t2c = inv(X) · inv(T_g2b) · T_t2b
|
||||
"""
|
||||
rng = np.random.default_rng(seed)
|
||||
Rg, tg, Rc, tc = [], [], [], []
|
||||
Xinv = np.linalg.inv(X_true)
|
||||
for _ in range(n):
|
||||
T_g2b = _rand_transform(rng)
|
||||
T_t2c = Xinv @ np.linalg.inv(T_g2b) @ T_target2base
|
||||
Rg.append(T_g2b[:3, :3])
|
||||
tg.append(T_g2b[:3, 3].reshape(3, 1))
|
||||
Rc.append(T_t2c[:3, :3])
|
||||
tc.append(T_t2c[:3, 3].reshape(3, 1))
|
||||
return Rg, tg, Rc, tc
|
||||
|
||||
|
||||
def test_roundtrip_recovers_known_transform_eye_in_hand():
|
||||
rng = np.random.default_rng(42)
|
||||
X_true = _rand_transform(rng)
|
||||
T_target2base = _rand_transform(rng)
|
||||
Rg, tg, Rc, tc = _synthesize_views(X_true, T_target2base, 12)
|
||||
|
||||
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=False)
|
||||
X_est = Rt_to_T(R, t)
|
||||
|
||||
assert np.allclose(X_est, X_true, atol=1e-6), (
|
||||
"求解结果与已知真值不符 —— 坐标链方向或求逆有问题"
|
||||
)
|
||||
|
||||
|
||||
def test_roundtrip_translation_is_exact_not_just_rotation():
|
||||
"""单独盯平移：旋转对但平移错，是最容易漏掉的一类 bug。"""
|
||||
rng = np.random.default_rng(11)
|
||||
X_true = _rand_transform(rng)
|
||||
T_target2base = _rand_transform(rng)
|
||||
Rg, tg, Rc, tc = _synthesize_views(X_true, T_target2base, 15)
|
||||
|
||||
_R, t = calibrate_hand_eye(Rg, tg, Rc, tc)
|
||||
assert np.allclose(np.asarray(t).reshape(3), X_true[:3, 3], atol=1e-6)
|
||||
|
||||
|
||||
def test_roundtrip_is_rotation_matrix_with_det_one():
|
||||
rng = np.random.default_rng(5)
|
||||
X_true = _rand_transform(rng)
|
||||
Rg, tg, Rc, tc = _synthesize_views(X_true, _rand_transform(rng), 10)
|
||||
R, _t = calibrate_hand_eye(Rg, tg, Rc, tc)
|
||||
R = np.asarray(R, float)
|
||||
assert np.isclose(np.linalg.det(R), 1.0, atol=1e-8), "det 必须是 +1，不是镜像"
|
||||
assert np.allclose(R @ R.T, np.eye(3), atol=1e-8), "必须是正交阵"
|
||||
|
||||
|
||||
def _synthesize_views_eye_to_hand(
|
||||
X_true: np.ndarray, T_target2gripper: np.ndarray, n: int, seed: int = 7
|
||||
):
|
||||
"""由已知 X_true 生成 n 组**眼在手外**的视图观测。
|
||||
|
||||
眼在手外的坐标链：相机固定在工位外不动，**标定板装在法兰上随臂动**。
|
||||
T_t2b = T_g2b · T_t2g （板在基座 = 法兰在基座 · 板在法兰，T_t2g 固定）
|
||||
T_t2b = X · T_t2c （X = T_cam2base）
|
||||
所以给定任意的 T_g2b，可反推出该视图下应该观测到的 T_t2c：
|
||||
T_t2c = inv(X) · T_g2b · T_t2g
|
||||
|
||||
**为什么板必须在法兰上**：相机与基座都不动，唯一会变的就是带着板走的法兰；
|
||||
板若像眼在手上那样摆在台面上不动，T_t2c 就恒定不变，方程直接退化。
|
||||
"""
|
||||
rng = np.random.default_rng(seed)
|
||||
Rg, tg, Rc, tc = [], [], [], []
|
||||
Xinv = np.linalg.inv(X_true)
|
||||
for _ in range(n):
|
||||
T_g2b = _rand_transform(rng)
|
||||
T_t2c = Xinv @ T_g2b @ T_target2gripper
|
||||
Rg.append(T_g2b[:3, :3])
|
||||
tg.append(T_g2b[:3, 3].reshape(3, 1))
|
||||
Rc.append(T_t2c[:3, :3])
|
||||
tc.append(T_t2c[:3, 3].reshape(3, 1))
|
||||
return Rg, tg, Rc, tc
|
||||
|
||||
|
||||
def test_roundtrip_recovers_known_transform_eye_to_hand():
|
||||
"""眼在手外：X = cam → base，**与真值比对**（不是只查正交/det）。
|
||||
|
||||
这条分支内部把 gripper2base 求了逆。求逆方向或坐标链写错时，结果照样是一个
|
||||
合法的旋转矩阵 —— 只查 det/正交的测试**抓不到**，所以必须比真值。
|
||||
"""
|
||||
rng = np.random.default_rng(3)
|
||||
X_true = _rand_transform(rng) # T_cam2base
|
||||
T_target2gripper = _rand_transform(rng) # 板刚性固定在法兰上（每视图不变）
|
||||
Rg, tg, Rc, tc = _synthesize_views_eye_to_hand(X_true, T_target2gripper, 14)
|
||||
|
||||
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=True)
|
||||
X_est = Rt_to_T(R, t)
|
||||
|
||||
assert np.allclose(X_est, X_true, atol=1e-6), (
|
||||
"eye_to_hand 没解回已知的 T_cam2base —— 求逆方向或坐标链错了"
|
||||
)
|
||||
R = np.asarray(R, float)
|
||||
assert np.isclose(np.linalg.det(R), 1.0, atol=1e-8), "det 必须是 +1，不是镜像"
|
||||
|
||||
|
||||
def test_eye_to_hand_flag_actually_changes_the_answer():
|
||||
"""负向：同一份眼在手外数据当成眼在手上解，**必须解出不同的东西**。
|
||||
|
||||
给上面那条把关：万一哪天 `eye_to_hand` 分支被改成"其实什么也没做"
|
||||
（两个模式走同一条路），上面那条测试就会失去意义 —— 而这条会红。
|
||||
"""
|
||||
rng = np.random.default_rng(3)
|
||||
X_true = _rand_transform(rng)
|
||||
T_target2gripper = _rand_transform(rng)
|
||||
Rg, tg, Rc, tc = _synthesize_views_eye_to_hand(X_true, T_target2gripper, 14)
|
||||
|
||||
R_ok, t_ok = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=True)
|
||||
R_bad, t_bad = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=False)
|
||||
|
||||
assert np.allclose(Rt_to_T(R_ok, t_ok), X_true, atol=1e-6)
|
||||
assert not np.allclose(Rt_to_T(R_bad, t_bad), X_true, atol=1e-3), (
|
||||
"眼在手上分支也解出了同一个真值 —— 说明 eye_to_hand 这个开关根本没起作用"
|
||||
)
|
||||
|
||||
|
||||
def test_calibration_fails_loudly_on_degenerate_input():
|
||||
"""全同位姿（没有旋转多样性）时不该悄悄给个"看起来能用"的结果。"""
|
||||
rng = np.random.default_rng(9)
|
||||
X_true = _rand_transform(rng)
|
||||
Rg, tg, Rc, tc = _synthesize_views(X_true, _rand_transform(rng), 6)
|
||||
same_R, same_t = Rg[0], tg[0]
|
||||
Rg = [same_R] * 6
|
||||
tg = [same_t] * 6
|
||||
Rc = [Rc[0]] * 6
|
||||
tc = [tc[0]] * 6
|
||||
try:
|
||||
R, _t = calibrate_hand_eye(Rg, tg, Rc, tc)
|
||||
except Exception:
|
||||
return # 报错是可以接受的
|
||||
# 若没报错，结果必须明显不可信（不该恰好等于真值）
|
||||
assert not np.allclose(Rt_to_T(np.asarray(R, float), np.zeros(3)), X_true, atol=1e-6)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 输出格式
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_format_transform_yaml_shape_and_parseable():
|
||||
import yaml
|
||||
|
||||
R = _rot("z", 90.0)
|
||||
t = np.array([[0.01], [0.02], [0.03]])
|
||||
txt = format_transform_yaml(R, t, "unit test")
|
||||
|
||||
assert "# unit test" in txt
|
||||
data = yaml.safe_load(txt)
|
||||
T = np.array(data["T_4x4"], float)
|
||||
assert T.shape == (4, 4)
|
||||
assert np.allclose(T, Rt_to_T(R, t), atol=1e-7)
|
||||
assert np.allclose(data["translation_m"], [0.01, 0.02, 0.03], atol=1e-8)
|
||||
|
||||
|
||||
def test_format_transform_yaml_has_bottom_row():
|
||||
import yaml
|
||||
|
||||
txt = format_transform_yaml(np.eye(3), np.zeros((3, 1)), "x")
|
||||
data = yaml.safe_load(txt)
|
||||
assert np.allclose(data["T_4x4"][3], [0, 0, 0, 1])
|
||||
|
||||
|
||||
def test_roundtrip_write_then_read_back_is_stable():
|
||||
"""写出去再读回来，变换不该漂移（避免精度不足导致反复重标定）。"""
|
||||
import yaml
|
||||
|
||||
rng = np.random.default_rng(21)
|
||||
R = _rot("x", 37.0) @ _rot("z", -14.0)
|
||||
t = rng.uniform(-0.2, 0.2, (3, 1))
|
||||
txt = format_transform_yaml(R, t, "roundtrip")
|
||||
T = np.array(yaml.safe_load(txt)["T_4x4"], float)
|
||||
assert np.allclose(T[:3, :3], R, atol=1e-7)
|
||||
assert np.allclose(T[:3, 3], t.reshape(3), atol=1e-7)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 位姿占位检测（决定哪些点参与标定）
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_is_pose_filled_rejects_placeholder_zeros():
|
||||
assert not is_pose_filled([0.0] * 7)
|
||||
|
||||
|
||||
def test_is_pose_filled_rejects_nan():
|
||||
assert not is_pose_filled([float("nan")] * 7)
|
||||
|
||||
|
||||
def test_is_pose_filled_rejects_wrong_length():
|
||||
assert not is_pose_filled([0.1, 0.2, 0.3])
|
||||
assert not is_pose_filled([0.1] * 8)
|
||||
|
||||
|
||||
def test_is_pose_filled_accepts_real_pose():
|
||||
assert is_pose_filled([-1.107, -0.744, 0.982, 1.856, -0.735, 0.947, -0.118])
|
||||
|
||||
|
||||
def test_flexiv_tcp_identity_is_pure_translation():
|
||||
"""Studio tool10120938：姿态单位四元数，只沿法兰 Z 偏 224.5 mm。"""
|
||||
T = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
|
||||
assert np.allclose(T[:3, :3], np.eye(3), atol=1e-12)
|
||||
assert T[2, 3] == pytest.approx(0.2245)
|
||||
|
||||
|
||||
def test_cam_to_flange_via_tool_must_not_leave_t_unchanged():
|
||||
"""忘了乘时 T_4x4 停在 TCP；乘反了相机会到法兰后方。"""
|
||||
T_cam_tool = np.eye(4)
|
||||
T_cam_tool[:3, 3] = [-0.05961995, -0.01523625, -0.09761517]
|
||||
T_flange_tool = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
|
||||
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, T_flange_tool)
|
||||
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
|
||||
tool_len = float(np.linalg.norm(T_flange_tool[:3, 3]))
|
||||
assert delta == pytest.approx(tool_len, abs=1e-9)
|
||||
assert tool_len > 0.20
|
||||
# 相机在法兰前方（+Z ≈ 126 mm），与 2026-09-04 旧结果同量级。
|
||||
assert T_cam_flange[2, 3] == pytest.approx(0.12688483, abs=1e-8)
|
||||
assert T_cam_flange[0, 3] == pytest.approx(-0.06011995, abs=1e-8)
|
||||
# 负向：乘反了会到法兰后方约 −296 mm。
|
||||
wrong = T_cam_tool @ np.linalg.inv(T_flange_tool)
|
||||
assert wrong[2, 3] < 0.0
|
||||
# 负向：直接把 cam→工具当成 cam→法兰，平移几乎不动。
|
||||
assert float(np.linalg.norm(T_cam_tool[:3, 3] - T_cam_tool[:3, 3])) < 1e-12
|
||||
# 折回去必须还原：T_ct = inv(T_ft) · T_cf
|
||||
T_back = np.linalg.inv(T_flange_tool) @ T_cam_flange
|
||||
assert np.allclose(T_back, T_cam_tool, atol=1e-12)
|
||||
|
||||
|
||||
def test_cam_at_tcp_is_the_tool_pose_in_flange():
|
||||
"""相机与 TCP 重合时，cam→法兰必须等于法兰→工具，不能是它的逆。"""
|
||||
T_ft = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
|
||||
T_cf = cam_to_flange_via_tool(np.eye(4), T_ft)
|
||||
assert np.allclose(T_cf, T_ft, atol=1e-12)
|
||||
assert T_cf[2, 3] == pytest.approx(0.2245)
|
||||
@@ -0,0 +1,146 @@
|
||||
"""手眼文件读取与四元数换算的回环测试（用真实标定产物）。
|
||||
|
||||
为什么值得单测这个
|
||||
------------------
|
||||
``R_to_quat_xyzw`` 这段换算在仓库里被抄过好几遍，并且**抄错过**：某个分支的
|
||||
公式写歪之后，四元数还是"像模像样的数字"，静态 TF 也照发。症状是手眼外参
|
||||
差了 30°：定位总能落到工件附近（因为平移是对的），但永远差一点，
|
||||
看起来像"相机标定不准"，很难查到是 launch 里的一行除法。
|
||||
|
||||
所以判据必须是**数学回环**（``quat → R`` 要能还原 ``R``），而不是"看数字合不合理"。
|
||||
真实标定文件（``handeye/results/*.yaml``）也一起测：文件是人手拷来拷去的，
|
||||
形状/字段被改坏时，这里直接报错比发一个全零 TF 好。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import pathlib
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.handeye_lib import (
|
||||
R_to_quat_xyzw,
|
||||
quat_xyzw_to_R,
|
||||
read_transform_yaml,
|
||||
)
|
||||
|
||||
|
||||
def _repo_root() -> pathlib.Path:
|
||||
here = pathlib.Path(__file__).resolve()
|
||||
for p in here.parents:
|
||||
if (p / "config" / "site.yaml").is_file():
|
||||
return p
|
||||
raise AssertionError("找不到仓库根（config/site.yaml）")
|
||||
|
||||
|
||||
def _rot(axis: str, deg: float) -> np.ndarray:
|
||||
a = math.radians(deg)
|
||||
c, s = math.cos(a), math.sin(a)
|
||||
if axis == "x":
|
||||
return np.array([[1, 0, 0], [0, c, -s], [0, s, c]], dtype=float)
|
||||
if axis == "y":
|
||||
return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], dtype=float)
|
||||
return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], dtype=float)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("axis", ["x", "y", "z"])
|
||||
@pytest.mark.parametrize("deg", [0.0, 1.0, 45.0, 90.0, 179.0, 180.0, 181.0, 270.0])
|
||||
def test_quaternion_roundtrip_by_axis(axis: str, deg: float) -> None:
|
||||
"""绕单轴转任意角度：``quat → R`` 必须还原原矩阵。
|
||||
|
||||
覆盖 0/180/270 这些"某个分量恰好为零"的角度 —— 四元数换算的分支
|
||||
正是在这些角度上切换的，抄错分支的公式只会在这些角度露出来。
|
||||
"""
|
||||
R = _rot(axis, deg)
|
||||
q = R_to_quat_xyzw(R)
|
||||
assert abs(math.sqrt(sum(v * v for v in q)) - 1.0) < 1e-9, "四元数未归一化"
|
||||
R2 = quat_xyzw_to_R(q)
|
||||
assert np.allclose(R, R2, atol=1e-9), f"轴 {axis} 转角 {deg}：回环不一致"
|
||||
|
||||
|
||||
def test_quaternion_roundtrip_on_composite_rotations() -> None:
|
||||
"""复合旋转（三个分支都会被走到）+ 随机旋转。"""
|
||||
mats = [
|
||||
_rot("x", 30.0) @ _rot("y", -70.0) @ _rot("z", 120.0),
|
||||
_rot("z", 179.9) @ _rot("x", 90.0),
|
||||
_rot("y", 180.0) @ _rot("x", 180.0),
|
||||
]
|
||||
rng = np.random.default_rng(20260924)
|
||||
for _ in range(50):
|
||||
A = rng.normal(size=(3, 3))
|
||||
Q, _R = np.linalg.qr(A)
|
||||
if np.linalg.det(Q) < 0:
|
||||
Q[:, 0] *= -1.0
|
||||
mats.append(Q)
|
||||
for R in mats:
|
||||
assert np.allclose(R, quat_xyzw_to_R(R_to_quat_xyzw(R)), atol=1e-8)
|
||||
|
||||
|
||||
def test_read_real_handeye_files_match_their_own_matrix() -> None:
|
||||
"""真实标定文件：读出来的 xyz/quat 必须和文件里的 T_4x4 自洽。
|
||||
|
||||
这条断言的价值在于"文件 + 读取代码 + 换算"三者一旦有一个变味就会报。
|
||||
"""
|
||||
results = _repo_root() / "handeye" / "results"
|
||||
files = sorted(results.glob("*_eye_in_hand.yaml"))
|
||||
if not files:
|
||||
pytest.skip("仓库里没有手眼标定产物（还没标定过）")
|
||||
import yaml
|
||||
|
||||
for f in files:
|
||||
xyz, quat = read_transform_yaml(f)
|
||||
# 直接拿文件里的 T_4x4 当基准（不手写解析：解析器本身就是被测对象之一，
|
||||
# 用手写解析比对等于让两份解析互相印错）
|
||||
T_file = np.asarray(yaml.safe_load(f.read_text(encoding="utf-8"))["T_4x4"], dtype=float)
|
||||
assert T_file.shape == (4, 4), f"{f.name} 的 T_4x4 形状不对"
|
||||
R = quat_xyzw_to_R(quat)
|
||||
assert np.allclose(xyz, T_file[:3, 3], atol=1e-6), f"{f.name} 平移不一致"
|
||||
assert np.allclose(R, T_file[:3, :3], atol=1e-6), f"{f.name} 旋转不一致"
|
||||
|
||||
|
||||
def test_read_transform_yaml_rejects_broken_files(tmp_path: pathlib.Path) -> None:
|
||||
"""缺字段 / 形状不对 → 明确报错，而不是回一堆 nan 去发 TF。"""
|
||||
(tmp_path / "no_field.yaml").write_text("translation_m: [0, 0, 0]\n", encoding="utf-8")
|
||||
with pytest.raises(ValueError, match="T_4x4"):
|
||||
read_transform_yaml(tmp_path / "no_field.yaml")
|
||||
|
||||
(tmp_path / "bad_shape.yaml").write_text(
|
||||
"T_4x4:\n - [1, 0, 0, 0]\n - [0, 1, 0, 0]\n", encoding="utf-8"
|
||||
)
|
||||
with pytest.raises(ValueError, match="4x4"):
|
||||
read_transform_yaml(tmp_path / "bad_shape.yaml")
|
||||
|
||||
|
||||
def test_identical_rotation_gives_same_quaternion_up_to_sign() -> None:
|
||||
"""同一个姿态的四元数允许差一个整体符号（q 与 -q 同一个旋转）。
|
||||
|
||||
写这条是因为很容易顺手加一句"四元数必须逐位相等"的断言，然后在某次
|
||||
标定重跑后莫名其妙地红 —— 那不是回归，是符号约定。
|
||||
"""
|
||||
R = _rot("z", 100.0)
|
||||
q1 = np.array(R_to_quat_xyzw(R))
|
||||
q2 = np.array(R_to_quat_xyzw(R))
|
||||
assert np.allclose(q1, q2)
|
||||
assert np.allclose(quat_xyzw_to_R(-q1), quat_xyzw_to_R(q1))
|
||||
|
||||
|
||||
def test_launch_files_do_not_reimplement_the_quaternion_math() -> None:
|
||||
"""launch 文件里不许再出现自己写的四元数换算。
|
||||
|
||||
这段换算曾在 4 个 launch 里各抄一份，而且**四份已经抄得不完全一样**
|
||||
（退化分支的公式各不相同）。共用的那一份在 ``handeye_lib`` 里，有回环测试；
|
||||
自己再写一份就等于加了一处没人测的数学。要用就 ``read_transform_yaml``。
|
||||
"""
|
||||
launch_dir = _repo_root() / "src" / "arm_application" / "launch"
|
||||
bad = [
|
||||
f.name
|
||||
for f in sorted(launch_dir.glob("*.launch.py"))
|
||||
if "_quat_from_R" in f.read_text(encoding="utf-8")
|
||||
]
|
||||
assert not bad, (
|
||||
"这些 launch 自己实现了四元数换算："
|
||||
+ ", ".join(bad)
|
||||
+ "；请改用 arm_application.handeye_lib.read_transform_yaml"
|
||||
)
|
||||
@@ -0,0 +1,459 @@
|
||||
"""眼在手外 / 眼在手上必须按模式分开：板尺寸、相机、结果文件名、TF 父子。
|
||||
|
||||
这些不是风格问题。两种装法用两块不同的物理板、两台不同的相机，解出的
|
||||
矩阵语义也不同（cam→法兰 vs cam→基座）。共用一份配置的后果不是报错，
|
||||
而是"标定跑完、结果系统性偏"——现场几乎联想不到是板尺寸/相机/文件名
|
||||
串了。所以这里每条测试都配一条**负向**：故意走错模式必须红。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
import yaml
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
SCRIPTS_LIB = ROOT / "scripts" / "lib"
|
||||
SCRIPT = ROOT / "scripts" / "run_handeye.sh"
|
||||
|
||||
sys.path.insert(0, str(SCRIPTS_LIB))
|
||||
import export_site_env # type: ignore # noqa: E402
|
||||
|
||||
from arm_application.site_io import ( # noqa: E402
|
||||
eye_to_hand_result_name,
|
||||
handeye_static_tf_frames,
|
||||
overhead_camera,
|
||||
resolve_eye_to_hand_file,
|
||||
)
|
||||
|
||||
|
||||
def _site(**over) -> dict:
|
||||
"""最小可 flatten 的虚构站点。调用方可覆盖任意段。"""
|
||||
base = {
|
||||
"robot": {
|
||||
"sn": "Rizon4-777001",
|
||||
"robots": [
|
||||
{
|
||||
"sn": "Rizon4-777001",
|
||||
"arm": "left",
|
||||
"label": "左臂01",
|
||||
"camera": {"name": "cam_a", "serial": "99900001"},
|
||||
},
|
||||
{
|
||||
"sn": "Rizon4-777002",
|
||||
"arm": "right",
|
||||
"label": "右臂02",
|
||||
"camera": {"name": "cam_b", "serial": "99900002"},
|
||||
},
|
||||
],
|
||||
},
|
||||
"cameras": {
|
||||
"d435": {
|
||||
"name": "d435",
|
||||
"role": "overhead_eye_to_hand",
|
||||
"serial": "99900003",
|
||||
"width": 1280,
|
||||
"height": 720,
|
||||
},
|
||||
"sim_d435": {
|
||||
"name": "sim_d435",
|
||||
"role": "overhead_eye_to_hand",
|
||||
"arm": "sim",
|
||||
"serial": "",
|
||||
},
|
||||
},
|
||||
"handeye": {
|
||||
"mode": "eye_in_hand",
|
||||
"boards": {
|
||||
"eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015},
|
||||
"eye_to_hand": {"cols": 11, "rows": 8, "square_size_m": 0.020},
|
||||
},
|
||||
},
|
||||
}
|
||||
for k, v in over.items():
|
||||
if isinstance(v, dict) and isinstance(base.get(k), dict):
|
||||
merged = dict(base[k])
|
||||
merged.update(v)
|
||||
base[k] = merged
|
||||
else:
|
||||
base[k] = v
|
||||
return base
|
||||
|
||||
|
||||
def _flat(monkeypatch, site: dict, sn: str = "Rizon4-777001") -> dict:
|
||||
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||
monkeypatch.setenv("ROBOT_SN", sn)
|
||||
return export_site_env.flatten(site)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 板尺寸按模式
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
class TestHandeyeBoardPerMode:
|
||||
def test_two_modes_keep_different_boards(self, monkeypatch):
|
||||
flat = _flat(monkeypatch, _site())
|
||||
assert flat["SITE_HANDEYE_BOARD_COLS_EYE_IN_HAND"] == "8"
|
||||
assert flat["SITE_HANDEYE_BOARD_ROWS_EYE_IN_HAND"] == "11"
|
||||
assert flat["SITE_HANDEYE_SQUARE_EYE_IN_HAND"] == "0.015"
|
||||
assert flat["SITE_HANDEYE_BOARD_SRC_EYE_IN_HAND"] == "boards"
|
||||
assert flat["SITE_HANDEYE_BOARD_COLS_EYE_TO_HAND"] == "11"
|
||||
assert flat["SITE_HANDEYE_BOARD_ROWS_EYE_TO_HAND"] == "8"
|
||||
assert flat["SITE_HANDEYE_SQUARE_EYE_TO_HAND"] == "0.02"
|
||||
assert flat["SITE_HANDEYE_BOARD_SRC_EYE_TO_HAND"] == "boards"
|
||||
|
||||
def test_current_mode_board_follows_handeye_mode(self, monkeypatch):
|
||||
site = _site()
|
||||
site["handeye"]["mode"] = "eye_to_hand"
|
||||
flat = _flat(monkeypatch, site)
|
||||
assert flat["SITE_HANDEYE_BOARD_COLS"] == "11"
|
||||
assert flat["SITE_HANDEYE_SQUARE_M"] == "0.02"
|
||||
|
||||
def test_legacy_top_level_keys_still_work(self):
|
||||
he = {"board_cols": 9, "board_rows": 6, "square_size_m": 0.025}
|
||||
board, src = export_site_env.handeye_board(he, "eye_in_hand")
|
||||
assert src == "legacy"
|
||||
assert board == {"cols": 9, "rows": 6, "square_size_m": 0.025}
|
||||
|
||||
def test_missing_board_is_default_not_silent_guess_disguised_as_config(self):
|
||||
"""没配过这块板 → 来源必须是 default。run_handeye.sh 见 default 会拦下。
|
||||
|
||||
如果这里返回 boards / legacy，脚本会以为"配过了"，带着内置 8×11/15mm
|
||||
去标 A4 板 —— 标定照样跑完，结果系统性偏。
|
||||
"""
|
||||
board, src = export_site_env.handeye_board({}, "eye_to_hand")
|
||||
assert src == "default"
|
||||
# 默认值本身仍给出来（给 show 打印），但来源必须让调用方能拦。
|
||||
assert board["cols"] == 8
|
||||
|
||||
def test_unknown_mode_is_default(self):
|
||||
he = {
|
||||
"boards": {
|
||||
"eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015},
|
||||
}
|
||||
}
|
||||
_, src = export_site_env.handeye_board(he, "eye_on_foot")
|
||||
assert src == "default"
|
||||
|
||||
def test_negative_shared_keys_cannot_serve_both_modes(self):
|
||||
"""负向：只有一组顶层键时，两种模式会读到**同一块板**。
|
||||
|
||||
这正是旧配置的失败模式。新配置（boards 段）必须让这条不成立。
|
||||
"""
|
||||
he_old = {"board_cols": 8, "board_rows": 11, "square_size_m": 0.015}
|
||||
a, _ = export_site_env.handeye_board(he_old, "eye_in_hand")
|
||||
b, _ = export_site_env.handeye_board(he_old, "eye_to_hand")
|
||||
assert a == b, "旧口径下两种模式本就共用一块板（这条钉住旧逻辑会红的前提）"
|
||||
|
||||
he_new = {
|
||||
"boards": {
|
||||
"eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015},
|
||||
"eye_to_hand": {"cols": 11, "rows": 8, "square_size_m": 0.020},
|
||||
}
|
||||
}
|
||||
a2, _ = export_site_env.handeye_board(he_new, "eye_in_hand")
|
||||
b2, _ = export_site_env.handeye_board(he_new, "eye_to_hand")
|
||||
assert a2 != b2
|
||||
assert a2["square_size_m"] == 0.015
|
||||
assert b2["square_size_m"] == 0.020
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 手外相机按 role + arm
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
class TestOverheadCamera:
|
||||
def test_real_arm_picks_shared_d435_not_sim(self):
|
||||
site = _site()
|
||||
cam, others = overhead_camera(site, {"arm": "left"})
|
||||
assert cam.get("name") == "d435"
|
||||
assert others == []
|
||||
|
||||
def test_sim_arm_picks_sim_d435(self):
|
||||
site = _site()
|
||||
cam, _ = overhead_camera(site, {"arm": "sim"})
|
||||
assert cam.get("name") == "sim_d435"
|
||||
|
||||
def test_no_overhead_returns_empty(self):
|
||||
cam, others = overhead_camera({"cameras": {"d405": {"role": "left_wrist"}}})
|
||||
assert cam == {}
|
||||
assert others == []
|
||||
|
||||
def test_ambiguous_cameras_surface_the_others(self):
|
||||
"""两台都没标 arm → 取第一台，但其余名字必须返回，让调用方出声。"""
|
||||
site = {
|
||||
"cameras": {
|
||||
"a": {"name": "a", "role": "overhead_eye_to_hand"},
|
||||
"b": {"name": "b", "role": "overhead_eye_to_hand"},
|
||||
}
|
||||
}
|
||||
cam, others = overhead_camera(site, {"arm": "left"})
|
||||
assert cam.get("name") in ("a", "b")
|
||||
assert len(others) == 1
|
||||
assert others[0] != cam.get("name")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 结果文件名
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
class TestEyeToHandResultName:
|
||||
def test_name_carries_arm_and_camera(self):
|
||||
assert eye_to_hand_result_name("89", "d435") == "89_d435_eye_to_hand.yaml"
|
||||
|
||||
def test_two_arms_do_not_share_a_file(self, monkeypatch):
|
||||
site = _site()
|
||||
left = _flat(monkeypatch, site, sn="Rizon4-777001")
|
||||
right = _flat(monkeypatch, site, sn="Rizon4-777002")
|
||||
assert left["SITE_HANDEYE_FILE_EYE_TO_HAND"] != right["SITE_HANDEYE_FILE_EYE_TO_HAND"]
|
||||
assert left["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("01_d435_eye_to_hand.yaml")
|
||||
assert right["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("02_d435_eye_to_hand.yaml")
|
||||
|
||||
def test_eye_in_hand_file_is_untouched(self, monkeypatch):
|
||||
"""换模式不能改腕部标定那份路径 —— S05 精拍定位一直读它。"""
|
||||
site = _site()
|
||||
site["handeye"]["mode"] = "eye_to_hand"
|
||||
flat = _flat(monkeypatch, site)
|
||||
assert flat["SITE_HANDEYE_FILE"].endswith("01_eye_in_hand.yaml")
|
||||
assert flat["SITE_HANDEYE_FILE_EYE_TO_HAND"].endswith("01_d435_eye_to_hand.yaml")
|
||||
|
||||
def test_resolve_prefers_explicit_over_legacy(self, tmp_path: Path):
|
||||
explicit = tmp_path / "89_d435_eye_to_hand.yaml"
|
||||
legacy = tmp_path / "handeye" / "results" / "d435_eye_to_hand.yaml"
|
||||
legacy.parent.mkdir(parents=True)
|
||||
explicit.write_text("explicit\n")
|
||||
legacy.write_text("legacy\n")
|
||||
got = resolve_eye_to_hand_file(
|
||||
tmp_path, explicit=str(explicit), camera_name="d435"
|
||||
)
|
||||
assert got == explicit
|
||||
|
||||
def test_resolve_falls_back_to_camera_named_file(self, tmp_path: Path):
|
||||
legacy = tmp_path / "handeye" / "results" / "d435_eye_to_hand.yaml"
|
||||
legacy.parent.mkdir(parents=True)
|
||||
legacy.write_text("legacy\n")
|
||||
got = resolve_eye_to_hand_file(tmp_path, explicit="", camera_name="d435")
|
||||
assert got == legacy
|
||||
|
||||
def test_resolve_missing_returns_none_not_a_guess(self, tmp_path: Path):
|
||||
assert resolve_eye_to_hand_file(tmp_path, camera_name="d435") is None
|
||||
|
||||
def test_resolve_relative_explicit_is_under_root_not_cwd(self, tmp_path: Path):
|
||||
"""相对路径必须按仓库根解析。跟 CWD 走会在换目录启动时静默找不到。"""
|
||||
(tmp_path / "handeye" / "results").mkdir(parents=True)
|
||||
target = tmp_path / "handeye" / "results" / "89_d435_eye_to_hand.yaml"
|
||||
target.write_text("ok\n")
|
||||
got = resolve_eye_to_hand_file(
|
||||
tmp_path,
|
||||
explicit="handeye/results/89_d435_eye_to_hand.yaml",
|
||||
camera_name="d435",
|
||||
)
|
||||
assert got == target
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# TF 父子
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
class TestHandeyeStaticTfFrames:
|
||||
def test_eye_in_hand_is_flange_parent(self):
|
||||
parent, child = handeye_static_tf_frames(
|
||||
mode="eye_in_hand",
|
||||
ee_frame="Rizon4-063989_flange",
|
||||
base_frame="Rizon4-063989_base_link",
|
||||
cam_frame="d405_left_color_optical_frame",
|
||||
)
|
||||
assert parent == "Rizon4-063989_flange"
|
||||
assert child == "d405_left_color_optical_frame"
|
||||
|
||||
def test_eye_to_hand_is_base_parent(self):
|
||||
parent, child = handeye_static_tf_frames(
|
||||
mode="eye_to_hand",
|
||||
ee_frame="Rizon4-063989_flange",
|
||||
base_frame="Rizon4-063989_base_link",
|
||||
cam_frame="d435_color_optical_frame",
|
||||
)
|
||||
assert parent == "Rizon4-063989_base_link"
|
||||
assert child == "d435_color_optical_frame"
|
||||
|
||||
def test_negative_wrong_mode_puts_camera_under_flange(self):
|
||||
"""负向：眼在手外的文件若按眼在手上发 TF，父系会变成法兰。
|
||||
|
||||
这正是旧 launch 的失败模式。新函数必须让这条不成立。
|
||||
"""
|
||||
wrong, _ = handeye_static_tf_frames(
|
||||
mode="eye_in_hand",
|
||||
ee_frame="flange",
|
||||
base_frame="base",
|
||||
cam_frame="d435_color_optical_frame",
|
||||
)
|
||||
right, _ = handeye_static_tf_frames(
|
||||
mode="eye_to_hand",
|
||||
ee_frame="flange",
|
||||
base_frame="base",
|
||||
cam_frame="d435_color_optical_frame",
|
||||
)
|
||||
assert wrong == "flange"
|
||||
assert right == "base"
|
||||
assert wrong != right
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# run_handeye.sh --mode 真的切到对应的板 / 相机 / 文件
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _show(mode: str, site: Path) -> str:
|
||||
env = os.environ.copy()
|
||||
env["SITE_CONFIG"] = str(site)
|
||||
env["ROBOT_SN"] = "Rizon4-063989"
|
||||
proc = subprocess.run(
|
||||
["bash", str(SCRIPT), "--arm", "89", "show", "--mode", mode],
|
||||
cwd=str(ROOT),
|
||||
env=env,
|
||||
text=True,
|
||||
capture_output=True,
|
||||
check=False,
|
||||
)
|
||||
return proc.stdout + proc.stderr
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def _real_site() -> Path:
|
||||
"""脚本走 load_site_config.sh，必须是一份能选出 89 臂的真实形状配置。"""
|
||||
return ROOT / "config" / "site.yaml"
|
||||
|
||||
|
||||
@pytest.mark.skip(reason="只同步 ROS 节点；远程 UI、shell 和整站配置不在本次范围")
|
||||
class TestRunHandeyeScriptMode:
|
||||
def test_eye_to_hand_uses_d435_and_a4_board(self, _real_site: Path):
|
||||
out = _show("eye_to_hand", _real_site)
|
||||
assert "mode=eye_to_hand" in out
|
||||
assert "camera=d435" in out
|
||||
assert "11x8 inner, square=0.02 m" in out
|
||||
assert "89_d435_eye_to_hand.yaml" in out
|
||||
assert "tool10120938" in out
|
||||
# 负向：不能再去开腕部相机。
|
||||
assert "camera=d405" not in out
|
||||
|
||||
def test_eye_in_hand_keeps_wrist_camera_and_a5_board(self, _real_site: Path):
|
||||
out = _show("eye_in_hand", _real_site)
|
||||
assert "mode=eye_in_hand" in out
|
||||
assert "d405" in out
|
||||
assert "11x8 inner, square=0.015 m" in out
|
||||
assert "tool10120938" in out
|
||||
assert "89_eye_in_hand.yaml" in out
|
||||
|
||||
def test_missing_board_blocks_calibrate(self, tmp_path: Path):
|
||||
"""没配 eye_to_hand 板尺寸时，calibrate 必须当场红，不能去赌默认值。"""
|
||||
site = {
|
||||
"robot": {
|
||||
"sn": "Rizon4-063989",
|
||||
"robots": [
|
||||
{
|
||||
"sn": "Rizon4-063989",
|
||||
"arm": "left",
|
||||
"camera": {"name": "d405_left", "serial": "1"},
|
||||
}
|
||||
],
|
||||
},
|
||||
"handeye": {
|
||||
"mode": "eye_in_hand",
|
||||
"boards": {
|
||||
"eye_in_hand": {"cols": 8, "rows": 11, "square_size_m": 0.015},
|
||||
},
|
||||
},
|
||||
"cameras": {
|
||||
"d435": {
|
||||
"name": "d435",
|
||||
"role": "overhead_eye_to_hand",
|
||||
"serial": "9",
|
||||
}
|
||||
},
|
||||
}
|
||||
p = tmp_path / "site.yaml"
|
||||
p.write_text(yaml.safe_dump(site, allow_unicode=True), encoding="utf-8")
|
||||
env = os.environ.copy()
|
||||
env["SITE_CONFIG"] = str(p)
|
||||
env["ROBOT_SN"] = "Rizon4-063989"
|
||||
proc = subprocess.run(
|
||||
["bash", str(SCRIPT), "--arm", "89", "calibrate", "--mode", "eye_to_hand"],
|
||||
cwd=str(ROOT),
|
||||
env=env,
|
||||
text=True,
|
||||
capture_output=True,
|
||||
check=False,
|
||||
)
|
||||
assert proc.returncode != 0
|
||||
assert "没有配置 [eye_to_hand] 用的标定板尺寸" in (proc.stdout + proc.stderr)
|
||||
|
||||
def test_left_arm_calibrate_wires_tool10120938(self):
|
||||
"""89 必须按 tool10120938 求解；脚本和节点都要接到这份 TCP。"""
|
||||
cal = (
|
||||
ROOT / "src" / "arm_application" / "arm_application" / "handeye_calibrate.py"
|
||||
).read_text(encoding="utf-8")
|
||||
sh = SCRIPT.read_text(encoding="utf-8")
|
||||
assert "cam_to_flange_via_tool(" in cal
|
||||
assert "gripper_tool" in cal
|
||||
assert "gripper_tool:=" in sh
|
||||
assert "T_4x4 仍是 cam→基座，不折" in sh
|
||||
backup = ROOT / "handeye" / "results" / "backup" / "89_eye_in_hand_flange_tool1_20260904.yaml"
|
||||
assert backup.is_file()
|
||||
e2h_bak = ROOT / "handeye" / "results" / "backup" / "89_d435_eye_to_hand_20261009.yaml"
|
||||
assert e2h_bak.is_file()
|
||||
e2h = (ROOT / "handeye" / "results" / "89_d435_eye_to_hand.yaml").read_text(
|
||||
encoding="utf-8"
|
||||
)
|
||||
assert "gripper_tool: tool10120938" in e2h
|
||||
assert "cam→基座" in e2h
|
||||
import re
|
||||
|
||||
m_e2h = re.search(r"translation_m:\s*\[([^]]+)\]", e2h)
|
||||
assert m_e2h, e2h
|
||||
xyz_e2h = [float(x) for x in m_e2h.group(1).split(",")]
|
||||
# 顶置相机在基座上方约 0.7 m；折错或写到法兰会差几十厘米。
|
||||
assert 0.60 < xyz_e2h[2] < 0.90, xyz_e2h
|
||||
# 眼在手外不得走 cam_to_flange_via_tool（相机和基座都在世界里）。
|
||||
e2h_block = cal.split("if eye_to_hand:")[1].split("elif self.gripper_tool:")[0]
|
||||
assert "cam_to_flange_via_tool" not in e2h_block
|
||||
live = ROOT / "handeye" / "results" / "89_eye_in_hand.yaml"
|
||||
text = live.read_text(encoding="utf-8")
|
||||
assert "BACKUP" in text or "backup/" in text
|
||||
# 乘反了 tz 会是负的（法兰后方）；现行必须在法兰前方。
|
||||
import re
|
||||
|
||||
m = re.search(r"translation_m:\s*\[([^]]+)\]", text)
|
||||
assert m, text
|
||||
xyz = [float(x) for x in m.group(1).split(",")]
|
||||
assert xyz[2] > 0.10, xyz
|
||||
assert "inv(T_flange_to_tool)" not in text
|
||||
|
||||
def test_teach_and_merge_share_the_same_taught_file(self):
|
||||
"""示教写 A、merge 读 B → 采了点也报 got 0。两边必须用 TAUGHT。"""
|
||||
sh = SCRIPT.read_text(encoding="utf-8")
|
||||
teach = (
|
||||
ROOT / "src" / "arm_application" / "arm_application" / "handeye_teach.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert 'poses_file:="${TAUGHT}"' in sh
|
||||
assert "taught_poses_${HANDEYE_MODE}" in sh
|
||||
assert "declare_parameter('poses_file'" in teach
|
||||
assert "get_parameter('poses_file')" in teach
|
||||
|
||||
def test_illegal_mode_exits_immediately(self):
|
||||
proc = subprocess.run(
|
||||
["bash", str(SCRIPT), "--arm", "89", "show", "--mode", "eye_on_foot"],
|
||||
cwd=str(ROOT),
|
||||
text=True,
|
||||
capture_output=True,
|
||||
check=False,
|
||||
)
|
||||
assert proc.returncode != 0
|
||||
assert "eye_in_hand 或 eye_to_hand" in (proc.stdout + proc.stderr)
|
||||
@@ -0,0 +1,139 @@
|
||||
"""`hole_detector` 权重路径解析的回归测试。
|
||||
|
||||
重点是挡住一类**悬空引用**：代码里写死的默认权重路径指向一个根本不存在的文件。
|
||||
这种 bug 特别难查，因为
|
||||
|
||||
* 配置齐全时永远走不到那条分支，日常运行完全正常；
|
||||
* 一旦 site.yaml 缺键才会触发，而报错信息里打印的路径看起来非常合理
|
||||
（`.../models/hole_insert/best.pt`），很容易被误判成"文件被人误删了"。
|
||||
|
||||
这里**不断言文件存在** —— 二进制权重不进 git，新克隆的仓库里 `models/` 是空的。
|
||||
只断言"解析结果指向规范模型名"，这样在任何环境下都成立。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from arm_application.hole_detector import _default_model_path, weights_from_site
|
||||
|
||||
CANONICAL = "gongjian0929.onnx"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# weights_from_site：site.yaml 缺键时的兜底
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_weights_from_site_prefers_explicit_config():
|
||||
data = {
|
||||
"hole_detect": {"backend": "yolo", "weights": "/custom/my_holes.onnx"},
|
||||
"paths": {"hole_weights": "/custom/ignored.onnx"},
|
||||
}
|
||||
assert weights_from_site(data) == "/custom/my_holes.onnx"
|
||||
|
||||
|
||||
def test_weights_from_site_falls_back_to_paths():
|
||||
data = {
|
||||
"hole_detect": {"backend": "yolo"},
|
||||
"paths": {"hole_weights": "/custom/from_paths.onnx"},
|
||||
}
|
||||
assert weights_from_site(data) == "/custom/from_paths.onnx"
|
||||
|
||||
|
||||
def test_weights_from_site_yolo_fallback_is_not_dangling():
|
||||
"""回归：以前这里兜底到不存在 best.pt，现在必须指向规范模型。"""
|
||||
out = weights_from_site({"hole_detect": {"backend": "yolo"}})
|
||||
assert out.endswith(CANONICAL)
|
||||
assert "best.pt" not in out
|
||||
|
||||
|
||||
def test_weights_from_site_two_stage_uses_workpiece_weights():
|
||||
data = {
|
||||
"hole_detect": {"backend": "two_stage", "workpiece_weights": "/w/wp.onnx"},
|
||||
"paths": {"hole_weights": "/should/not/be/used.onnx"},
|
||||
}
|
||||
assert weights_from_site(data) == "/w/wp.onnx"
|
||||
|
||||
|
||||
def test_weights_from_site_two_stage_falls_back_to_yolo_weights():
|
||||
data = {
|
||||
"hole_detect": {"backend": "two_stage"},
|
||||
"paths": {"yolo_weights": "/w/track.onnx"},
|
||||
}
|
||||
assert weights_from_site(data) == "/w/track.onnx"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# _default_model_path：没有任何配置时的兜底
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_default_model_path_is_not_the_dangling_best_pt():
|
||||
"""回归：以前枚举的三个候选全不存在，函数却 `return candidates[0]`。
|
||||
|
||||
于是它永远返回一个不存在的 `hole_insert/best.pt`，而调用方拿到之后
|
||||
只会在真正加载模型时才炸，报错路径还很有迷惑性。
|
||||
"""
|
||||
p = _default_model_path()
|
||||
assert p.name not in ("best.pt", "hole_best.pt"), f"又指回悬空文件了: {p}"
|
||||
|
||||
|
||||
def test_default_model_path_is_canonical_name():
|
||||
assert _default_model_path().name == CANONICAL
|
||||
|
||||
|
||||
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,215 @@
|
||||
"""``image_utils`` 的单测：数组 -> ``sensor_msgs/Image`` 的打包是否正确。
|
||||
|
||||
这类代码的错误是**静默**的：编码写错、step 算错、通道顺序颠倒，都不会抛异常，
|
||||
只是下游拿到的图裂开或颜色错位 —— 而现场很难判断是相机坏了还是打包错了。
|
||||
所以这里把每种编码的字节布局都钉死。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.image_utils import (
|
||||
UnsupportedImageError,
|
||||
expected_step,
|
||||
infer_encoding,
|
||||
ndarray_to_image,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 编码推断
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
class TestInferEncoding:
|
||||
def test_mono8(self):
|
||||
assert infer_encoding(np.zeros((4, 5), np.uint8)) == "mono8"
|
||||
|
||||
def test_mono16(self):
|
||||
assert infer_encoding(np.zeros((4, 5), np.uint16)) == "mono16"
|
||||
|
||||
def test_float_depth_is_32fc1(self):
|
||||
"""深度图常以米为单位的 float32 出现，必须是 32FC1 而不是 mono。"""
|
||||
assert infer_encoding(np.zeros((4, 5), np.float32)) == "32FC1"
|
||||
|
||||
def test_vector_field_is_32fc2(self):
|
||||
"""力矢量场（每像素 dx, dy）用 32FC2 表达，这是 ROS 里的标准做法。"""
|
||||
assert infer_encoding(np.zeros((4, 5, 2), np.float32)) == "32FC2"
|
||||
|
||||
def test_rgb8(self):
|
||||
assert infer_encoding(np.zeros((4, 5, 3), np.uint8)) == "rgb8"
|
||||
|
||||
def test_rgba8(self):
|
||||
assert infer_encoding(np.zeros((4, 5, 4), np.uint8)) == "rgba8"
|
||||
|
||||
def test_single_channel_3d_is_mono(self):
|
||||
"""(H,W,1) 等价于灰度，不要因为没有 1 通道的编码表项就报错。"""
|
||||
assert infer_encoding(np.zeros((4, 5, 1), np.uint8)) == "mono8"
|
||||
|
||||
def test_1d_is_rejected(self):
|
||||
with pytest.raises(UnsupportedImageError) as exc:
|
||||
infer_encoding(np.zeros(10, np.uint8))
|
||||
assert "2D" in str(exc.value)
|
||||
|
||||
def test_4d_is_rejected(self):
|
||||
with pytest.raises(UnsupportedImageError):
|
||||
infer_encoding(np.zeros((2, 3, 4, 5), np.uint8))
|
||||
|
||||
def test_uint16_rgb_is_rejected(self):
|
||||
"""16 位 RGB 没有标准编码；宁可报错也不要按 8 位打包（会静默错位）。"""
|
||||
with pytest.raises(UnsupportedImageError) as exc:
|
||||
infer_encoding(np.zeros((4, 5, 3), np.uint16))
|
||||
assert "形状" in str(exc.value)
|
||||
|
||||
def test_float64_is_rejected(self):
|
||||
"""float64 图很常见（默认 dtype），必须显式转换，不能猜。"""
|
||||
with pytest.raises(UnsupportedImageError):
|
||||
infer_encoding(np.zeros((4, 5), np.float64))
|
||||
|
||||
def test_error_lists_supported_combinations(self):
|
||||
with pytest.raises(UnsupportedImageError) as exc:
|
||||
infer_encoding(np.zeros((3, 3, 5), np.uint8))
|
||||
assert "mono8" in str(exc.value) and "rgb8" in str(exc.value)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 打包
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
class TestPackaging:
|
||||
def test_mono8_layout(self):
|
||||
arr = np.arange(12, dtype=np.uint8).reshape(3, 4)
|
||||
msg = ndarray_to_image(arr, frame_id="left_tactile")
|
||||
assert (msg.height, msg.width) == (3, 4)
|
||||
assert msg.encoding == "mono8"
|
||||
assert msg.step == 4
|
||||
assert msg.header.frame_id == "left_tactile"
|
||||
assert msg.is_bigendian is False
|
||||
assert bytes(msg.data) == arr.tobytes()
|
||||
|
||||
def test_mono16_step_and_bytes(self):
|
||||
arr = np.arange(12, dtype=np.uint16).reshape(3, 4)
|
||||
msg = ndarray_to_image(arr)
|
||||
assert msg.encoding == "mono16"
|
||||
assert msg.step == 4 * 2 # 每像素 2 字节
|
||||
assert len(msg.data) == 3 * 4 * 2
|
||||
assert bytes(msg.data) == arr.tobytes()
|
||||
|
||||
def test_rgb8_step_is_three_bytes_per_pixel(self):
|
||||
arr = np.zeros((5, 7, 3), np.uint8)
|
||||
msg = ndarray_to_image(arr)
|
||||
assert msg.encoding == "rgb8"
|
||||
assert msg.step == 7 * 3
|
||||
assert msg.height == 5 and msg.width == 7
|
||||
|
||||
def test_rgba8_step(self):
|
||||
msg = ndarray_to_image(np.zeros((5, 7, 4), np.uint8))
|
||||
assert msg.step == 7 * 4
|
||||
|
||||
def test_vector_field_32fc2(self):
|
||||
"""矢量场：每像素 2 个 float32，step 必须是 W*8。"""
|
||||
arr = np.zeros((6, 8, 2), np.float32)
|
||||
msg = ndarray_to_image(arr)
|
||||
assert msg.encoding == "32FC2"
|
||||
assert msg.step == 8 * 4 * 2
|
||||
assert len(msg.data) == 6 * 8 * 2 * 4
|
||||
|
||||
def test_float_depth_32fc1(self):
|
||||
msg = ndarray_to_image(np.zeros((6, 8), np.float32))
|
||||
assert msg.encoding == "32FC1"
|
||||
assert msg.step == 8 * 4
|
||||
|
||||
def test_3d_single_channel_flattens_fine(self):
|
||||
"""(H,W,1) 压平后与 (H,W) 的字节完全一致。"""
|
||||
a = np.arange(12, dtype=np.uint8).reshape(3, 4, 1)
|
||||
b = np.arange(12, dtype=np.uint8).reshape(3, 4)
|
||||
assert bytes(ndarray_to_image(a).data) == bytes(ndarray_to_image(b).data)
|
||||
|
||||
def test_non_contiguous_input_is_handled(self):
|
||||
"""切片产生的非连续数组必须先转连续，否则 tobytes 的顺序是错的。"""
|
||||
base = np.arange(24, dtype=np.uint8).reshape(4, 6)
|
||||
view = base[:, ::2] # 非连续
|
||||
assert not view.flags["C_CONTIGUOUS"]
|
||||
msg = ndarray_to_image(view)
|
||||
assert msg.width == 3
|
||||
assert bytes(msg.data) == np.ascontiguousarray(view).tobytes()
|
||||
|
||||
def test_width_and_height_are_ints_not_numpy(self):
|
||||
"""ROS 消息字段是 int32；塞 numpy 标量在某些版本会失败。"""
|
||||
msg = ndarray_to_image(np.zeros((3, 4), np.uint8))
|
||||
assert isinstance(msg.height, int) and isinstance(msg.width, int)
|
||||
assert isinstance(msg.step, int)
|
||||
|
||||
def test_stamp_is_forwarded(self):
|
||||
"""stamp 必须原样传下去 —— 下游靠它做时间对齐。"""
|
||||
from builtin_interfaces.msg import Time
|
||||
|
||||
stamp = Time(sec=123, nanosec=456)
|
||||
msg = ndarray_to_image(np.zeros((2, 2), np.uint8), stamp=stamp)
|
||||
assert msg.header.stamp.sec == 123
|
||||
assert msg.header.stamp.nanosec == 456
|
||||
|
||||
def test_stamp_defaults_to_zero_when_omitted(self):
|
||||
msg = ndarray_to_image(np.zeros((2, 2), np.uint8))
|
||||
assert msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0
|
||||
|
||||
def test_fortran_order_is_normalised(self):
|
||||
arr = np.asfortranarray(np.arange(12, dtype=np.uint8).reshape(3, 4))
|
||||
msg = ndarray_to_image(arr)
|
||||
assert bytes(msg.data) == np.ascontiguousarray(arr).tobytes()
|
||||
|
||||
def test_empty_image(self):
|
||||
"""零尺寸图：别崩，正常给出 height=0。"""
|
||||
msg = ndarray_to_image(np.zeros((0, 0), np.uint8))
|
||||
assert msg.height == 0 and msg.width == 0
|
||||
assert len(msg.data) == 0
|
||||
|
||||
def test_explicit_encoding_override_is_respected(self):
|
||||
"""显式指定编码时要照用 —— 有些相机给出的 BGR 需要按 rgb8 打包。"""
|
||||
arr = np.zeros((3, 4, 3), np.uint8)
|
||||
msg = ndarray_to_image(arr, encoding="rgb8")
|
||||
assert msg.encoding == "rgb8"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# step 计算与 data 长度的一致性
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
class TestStepConsistency:
|
||||
"""``step`` 与 ``len(data)`` 必须自洽：``len(data) == step * height``。
|
||||
|
||||
这是下游按行读图时唯一依赖的不变量。不一致会导致整幅图错位。
|
||||
"""
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"shape,dtype",
|
||||
[
|
||||
((3, 4), np.uint8),
|
||||
((3, 4), np.uint16),
|
||||
((3, 4), np.float32),
|
||||
((3, 4, 1), np.uint8),
|
||||
((3, 4, 2), np.float32),
|
||||
((3, 4, 3), np.uint8),
|
||||
((3, 4, 4), np.uint8),
|
||||
((1, 1), np.uint8),
|
||||
((17, 23, 3), np.uint8),
|
||||
],
|
||||
)
|
||||
def test_data_length_matches_step_times_height(self, shape, dtype):
|
||||
arr = np.zeros(shape, dtype)
|
||||
msg = ndarray_to_image(arr)
|
||||
assert len(msg.data) == msg.step * msg.height, (
|
||||
f"shape={shape} dtype={dtype.name} encoding={msg.encoding} "
|
||||
f"step={msg.step} h={msg.height} len={len(msg.data)}"
|
||||
)
|
||||
|
||||
def test_expected_step_helper_agrees(self):
|
||||
for shape, dtype in [((3, 4), np.uint8), ((3, 4, 3), np.uint8),
|
||||
((3, 4, 2), np.float32)]:
|
||||
arr = np.zeros(shape, dtype)
|
||||
msg = ndarray_to_image(arr)
|
||||
assert msg.step == expected_step(arr)
|
||||
@@ -0,0 +1,170 @@
|
||||
"""换一台 Linux 上的机械臂，界面和命令必须认 site.yaml，不能只认本机 89/91。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from arm_application.site_io import arm_short_id, listed_arms, load_site_dict, usb_expectations
|
||||
|
||||
|
||||
def test_short_id_is_the_last_two_digits():
|
||||
assert arm_short_id("Rizon4-777001") == "01"
|
||||
assert arm_short_id("Rizon4-063989") == "89"
|
||||
assert arm_short_id("sim") == ""
|
||||
assert arm_short_id("") == ""
|
||||
|
||||
|
||||
def test_another_station_is_listed_and_this_stations_ids_are_not_invented():
|
||||
site = {
|
||||
"robot": {
|
||||
"robots": [
|
||||
{
|
||||
"sn": "Rizon4-777001",
|
||||
"arm": "left",
|
||||
"label": "甲",
|
||||
"gripper": {"model": "PGEA-9", "port": "/dev/ttyFAKE0"},
|
||||
},
|
||||
{
|
||||
"sn": "Rizon4-777002",
|
||||
"arm": "right",
|
||||
"label": "乙",
|
||||
"gripper": {"model": "RGI-9", "port": "/dev/ttyFAKE1"},
|
||||
},
|
||||
{"sn": "sim", "arm": "sim", "gripper": {"model": "RGI-9"}},
|
||||
]
|
||||
}
|
||||
}
|
||||
rows = listed_arms(site)
|
||||
assert [row["id"] for row in rows] == ["01", "02"]
|
||||
assert rows[0]["rotary"] is False
|
||||
assert rows[0]["side"] == "left"
|
||||
assert rows[1]["rotary"] is True
|
||||
assert "89" not in {row["id"] for row in rows}
|
||||
|
||||
|
||||
def test_right_arm_with_a_parallel_gripper_is_not_treated_as_rotary():
|
||||
site = {
|
||||
"robot": {
|
||||
"robots": [
|
||||
{"sn": "Rizon4-555091", "arm": "right", "gripper": {"model": "PGEA-100-20-W"}},
|
||||
]
|
||||
}
|
||||
}
|
||||
rows = listed_arms(site)
|
||||
assert rows[0]["id"] == "91"
|
||||
assert rows[0]["rotary"] is False
|
||||
|
||||
|
||||
def test_duplicate_short_ids_fail_instead_of_dropping_an_arm():
|
||||
site = {
|
||||
"robot": {
|
||||
"robots": [
|
||||
{"sn": "Rizon4-100089", "arm": "left"},
|
||||
{"sn": "Rizon4-200089", "arm": "right"},
|
||||
]
|
||||
}
|
||||
}
|
||||
with pytest.raises(ValueError, match="89"):
|
||||
listed_arms(site)
|
||||
|
||||
|
||||
def test_usb_count_follows_the_camera_list():
|
||||
site = {
|
||||
"cameras": {"d405_only": {"model": "d405"}},
|
||||
"tactile": {"left_video": 0},
|
||||
}
|
||||
expect = usb_expectations(site)
|
||||
assert expect == {"d405": 1, "d435": 0, "orisys": 1}
|
||||
|
||||
|
||||
def test_this_station_still_lists_89_and_91():
|
||||
rows = listed_arms(load_site_dict())
|
||||
ids = [row["id"] for row in rows]
|
||||
assert ids == ["89", "91"]
|
||||
by_id = {row["id"]: row for row in rows}
|
||||
assert by_id["89"]["rotary"] is False
|
||||
assert by_id["91"]["rotary"] is True
|
||||
|
||||
|
||||
@pytest.mark.skip(reason="只同步 ROS 节点；远程 UI、shell 和整站配置不在本次范围")
|
||||
def test_main_page_holds_status_sim_cameras_tactile_and_actions():
|
||||
import os
|
||||
|
||||
os.environ.setdefault("QT_QPA_PLATFORM", "offscreen")
|
||||
pytest.importorskip("PySide6")
|
||||
from PySide6.QtWidgets import QApplication, QPushButton, QScrollArea
|
||||
|
||||
from arm_application.operator_app import StationApp
|
||||
|
||||
app = QApplication.instance() or QApplication([])
|
||||
win = StationApp()
|
||||
try:
|
||||
assert [key for key, _button in win._nav] == ["main", "handeye", "teach"]
|
||||
assert set(win._pages) == {"main", "handeye", "teach"}
|
||||
assert win._stack.currentWidget() is win._pages["main"]
|
||||
assert win.minimumWidth() <= 1600
|
||||
assert win.minimumHeight() <= 900
|
||||
main = win._pages["main"]
|
||||
assert main.findChildren(QScrollArea) == []
|
||||
win.resize(1440, 900)
|
||||
app.processEvents()
|
||||
assert main.minimumSizeHint().height() <= 860
|
||||
texts = [button.text() for button in main.findChildren(QPushButton)]
|
||||
for name in ("跟踪并对孔", "只测旋转装配", "复位到 home", "本臂腕部相机"):
|
||||
assert any(name in text for text in texts)
|
||||
power = [button for button in main.findChildren(QPushButton) if "上电控制栈" in button.text()]
|
||||
assert power and not power[0].icon().isNull()
|
||||
assert win._cam_pic
|
||||
assert all(label.minimumHeight() <= 96 for label in win._cam_pic.values())
|
||||
from arm_application.operator_camera_view import targets
|
||||
|
||||
cams = targets()
|
||||
assert [cam["role"] for cam in cams] == ["wrist", "overhead"]
|
||||
assert "right" not in cams[0]["id"] and not cams[0]["id"].startswith("sim")
|
||||
assert not cams[1]["id"].startswith("sim")
|
||||
assert set(win._cam_pic) == {
|
||||
f"{cam['id']}_{kind}" for cam in cams for kind in ("color", "depth")
|
||||
}
|
||||
assert win._color_box.hasHeightForWidth()
|
||||
assert 240 <= win._color_box.heightForWidth(960) <= 340
|
||||
work = win._color_box.parentWidget().layout()
|
||||
assert work.stretch(work.indexOf(win._color_box)) == 0
|
||||
assert work.stretch(work.indexOf(win._tac_box)) >= 1
|
||||
assert win._tac_box.minimumHeight() >= 160
|
||||
assert win._tac_box.maximumHeight() > 400
|
||||
split = main.layout()
|
||||
assert split.stretch(0) < split.stretch(1)
|
||||
assert set(win._pic) == {"left_shear", "left_thermal", "right_shear", "right_thermal"}
|
||||
assert win._arm_sketch is not None
|
||||
assert win._grip_sketch.parent() is main or main.isAncestorOf(win._grip_sketch)
|
||||
assert 140 <= win._grip_sketch.height() or win._grip_sketch.maximumHeight() <= 180
|
||||
assert win._grip_sketch.maximumHeight() <= 180
|
||||
assert win._pos.value() == 500
|
||||
assert any("按此夹持" in text for text in texts)
|
||||
assert not win._rotary.isVisible()
|
||||
back = [
|
||||
button.text()
|
||||
for button in win._pages["handeye"].findChildren(QPushButton)
|
||||
]
|
||||
assert any("返回主界面" in text for text in back)
|
||||
assert "眼在手上" in back or any("眼在手上" in text for text in back)
|
||||
finally:
|
||||
win.close()
|
||||
app.processEvents()
|
||||
|
||||
|
||||
@pytest.mark.skip(reason="只同步 ROS 节点；远程 UI、shell 和整站配置不在本次范围")
|
||||
def test_operator_code_does_not_keep_a_local_serial_table():
|
||||
root = Path(__file__).resolve().parents[1] / "arm_application"
|
||||
for name in (
|
||||
"operator_console.py",
|
||||
"operator_gripper_state.py",
|
||||
"operator_arm_state.py",
|
||||
"operator_teach.py",
|
||||
"operator_app.py",
|
||||
):
|
||||
text = (root / name).read_text(encoding="utf-8")
|
||||
assert 'endswith("063989")' not in text
|
||||
assert 'not in ("89", "91")' not in text
|
||||
@@ -0,0 +1,484 @@
|
||||
"""model_labels 的测试。
|
||||
|
||||
刻意**不依赖仓库里那些模型二进制**（`models/**/*.onnx` 都在 .gitignore 里，
|
||||
新克隆的仓库根本没有），全部用 tmp_path 自建 fixture。ONNX 用 onnx.helper
|
||||
现场造一个最小的，这样"从 ONNX metadata 读标签"这条路径也能被测到。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
import yaml
|
||||
|
||||
from arm_application.model_labels import (
|
||||
SOURCE_MODEL,
|
||||
SOURCE_ONNX,
|
||||
SOURCE_PLACEHOLDER,
|
||||
SOURCE_SIDECAR,
|
||||
LabelSet,
|
||||
describe,
|
||||
format_labels_yaml,
|
||||
label_file_model,
|
||||
label_file_status,
|
||||
labels_path_for,
|
||||
parse_label_list,
|
||||
parse_names_field,
|
||||
placeholder_labels,
|
||||
read_label_file,
|
||||
read_label_text_file,
|
||||
read_labels,
|
||||
write_labels,
|
||||
)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# fixture 工具
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _make_onnx(path, names_json=None, nc=None, metadata=None):
|
||||
"""造一个最小可读的 ONNX，用于测试 metadata 读取。"""
|
||||
# onnx 只在真机环境（ros_flexiv）里装了，仿真环境（ros_gz_sim）没有。
|
||||
# 缺它时**跳过**而不是失败：仿真环境里每次 pytest 都多十几条红字，
|
||||
# 很快就没有人看测试结果了（"狼来了"），而真正需要 onnx 的用例
|
||||
# 本来也没法在没有 onnx 的环境里验证。
|
||||
pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）")
|
||||
import onnx
|
||||
from onnx import TensorProto, helper
|
||||
|
||||
inp = helper.make_tensor_value_info("images", TensorProto.FLOAT, [1, 3, 8, 8])
|
||||
out = helper.make_tensor_value_info("output0", TensorProto.FLOAT, [1, 3, 8, 8])
|
||||
node = helper.make_node("Identity", ["images"], ["output0"])
|
||||
graph = helper.make_graph([node], "g", [inp], [out])
|
||||
model = helper.make_model(graph)
|
||||
model.opset_import[0].version = 13
|
||||
if names_json is not None:
|
||||
model.metadata_props.add(key="names", value=names_json)
|
||||
if nc is not None:
|
||||
model.metadata_props.add(key="nc", value=str(nc))
|
||||
for k, v in (metadata or {}).items():
|
||||
model.metadata_props.add(key=k, value=v)
|
||||
onnx.save(model, str(path))
|
||||
return path
|
||||
|
||||
|
||||
def _touch(path):
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
path.write_bytes(b"not a real model")
|
||||
return path
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 旁挂路径推导
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_labels_path_replaces_suffix_only():
|
||||
assert labels_path_for("models/yolo/m.engine") == pytest.approx(
|
||||
labels_path_for("models/yolo/m.engine")
|
||||
)
|
||||
from pathlib import Path
|
||||
|
||||
assert Path(labels_path_for("models/yolo/m.engine")) == Path(
|
||||
"models/yolo/m.labels.yaml"
|
||||
)
|
||||
assert Path(labels_path_for("models/yolo/m.onnx")) == Path("models/yolo/m.labels.yaml")
|
||||
assert Path(labels_path_for("a/b/m.pt")) == Path("a/b/m.labels.yaml")
|
||||
|
||||
|
||||
def test_labels_path_same_for_all_formats_of_one_model():
|
||||
"""关键契约：同一模型的三种格式必须算出同一个标签文件。
|
||||
|
||||
否则"换后端名字就变了"这个坑会再次出现。
|
||||
"""
|
||||
from pathlib import Path
|
||||
|
||||
a = Path(labels_path_for("d/m.pt"))
|
||||
b = Path(labels_path_for("d/m.onnx"))
|
||||
c = Path(labels_path_for("d/m.engine"))
|
||||
assert a == b == c
|
||||
|
||||
|
||||
def test_labels_path_handles_dotted_stem():
|
||||
"""含点的文件名（a.b.onnx）不能被截成 a.labels.yaml。
|
||||
|
||||
这正是 Path.with_suffix('.labels.yaml') 容易踩的坑，所以手工拼 stem。
|
||||
"""
|
||||
from pathlib import Path
|
||||
|
||||
assert Path(labels_path_for("a.b.onnx")) == Path("a.b.labels.yaml")
|
||||
|
||||
|
||||
def test_labels_path_without_extension():
|
||||
from pathlib import Path
|
||||
|
||||
assert Path(labels_path_for("models/bare")) == Path("models/bare.labels.yaml")
|
||||
|
||||
|
||||
def test_labels_path_expands_user():
|
||||
assert "~" not in str(labels_path_for("~/ros2/models/x.onnx"))
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# names 字段解析：三种写法都要吃
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_parse_names_dict_int_keys():
|
||||
assert parse_names_field({0: "workpiece", 1: "hole"}) == {0: "workpiece", 1: "hole"}
|
||||
|
||||
|
||||
def test_parse_names_dict_string_keys():
|
||||
"""YAML 里键常常是字符串形式的数字（'0'），必须吃。"""
|
||||
assert parse_names_field({"0": "workpiece", "1": "hole"}) == {
|
||||
0: "workpiece",
|
||||
1: "hole",
|
||||
}
|
||||
|
||||
|
||||
def test_parse_names_list_index_is_position():
|
||||
assert parse_names_field(["workpiece", "hole"]) == {0: "workpiece", 1: "hole"}
|
||||
|
||||
|
||||
def test_parse_names_list_skips_blank_but_keeps_index():
|
||||
"""空项被跳过，但后面的项索引不能因此前移 —— 否则类别就整体错位了。"""
|
||||
assert parse_names_field(["a", "", "c"]) == {0: "a", 2: "c"}
|
||||
assert parse_names_field(["a", None, "c"]) == {0: "a", 2: "c"}
|
||||
assert parse_names_field(["a", " ", "c"]) == {0: "a", 2: "c"}
|
||||
|
||||
|
||||
def test_parse_names_string():
|
||||
assert parse_names_field("workpiece, hole") == {0: "workpiece", 1: "hole"}
|
||||
|
||||
|
||||
def test_parse_names_none_and_garbage():
|
||||
assert parse_names_field(None) == {}
|
||||
assert parse_names_field(42) == {}
|
||||
assert parse_names_field({}) == {}
|
||||
assert parse_names_field([]) == {}
|
||||
|
||||
|
||||
def test_parse_names_ignores_unparsable_dict_keys():
|
||||
"""键不是整数时跳过这一项，而不是抛异常让推理进程崩掉。"""
|
||||
assert parse_names_field({"x": "bad", 0: "good"}) == {0: "good"}
|
||||
|
||||
|
||||
def test_parse_names_strips_whitespace():
|
||||
assert parse_names_field([" workpiece "]) == {0: "workpiece"}
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 逗号串解析（CLI --set 用）
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_parse_label_list_ascii_comma():
|
||||
assert parse_label_list("a,b,c") == {0: "a", 1: "b", 2: "c"}
|
||||
|
||||
|
||||
def test_parse_label_list_chinese_comma_and_semicolon():
|
||||
assert parse_label_list("工件，通止规") == {0: "工件", 1: "通止规"}
|
||||
assert parse_label_list("a;b") == {0: "a", 1: "b"}
|
||||
|
||||
|
||||
def test_parse_label_list_with_explicit_index():
|
||||
"""从别处拷配置常带索引，要能认。"""
|
||||
assert parse_label_list("0:workpiece, 5:hole") == {0: "workpiece", 5: "hole"}
|
||||
assert parse_label_list("0：workpiece") == {0: "workpiece"} # 全角冒号
|
||||
|
||||
|
||||
def test_parse_label_list_tolerates_sloppy_input():
|
||||
assert parse_label_list("") == {}
|
||||
assert parse_label_list(" ") == {}
|
||||
assert parse_label_list("a,,b") == {0: "a", 1: "b"}
|
||||
assert parse_label_list(" a , b ") == {0: "a", 1: "b"}
|
||||
assert parse_label_list("a\nb") == {0: "a", 1: "b"}
|
||||
|
||||
|
||||
def test_parse_label_list_mixed_forms_stay_ordered():
|
||||
out = parse_label_list("a, 1:b, c")
|
||||
assert out[0] == "a"
|
||||
assert out[1] == "b"
|
||||
assert out[2] == "c"
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 纯文本标签文件（classes.txt 风格）
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_read_label_text_file_comments_do_not_consume_index(tmp_path):
|
||||
p = tmp_path / "classes.txt"
|
||||
p.write_text("# 这是注释\nworkpiece\n\n# 又一条注释\nhole\n", encoding="utf-8")
|
||||
assert read_label_text_file(p) == {0: "workpiece", 1: "hole"}
|
||||
|
||||
|
||||
def test_read_label_text_file_chinese(tmp_path):
|
||||
p = tmp_path / "classes.txt"
|
||||
p.write_text("工件\n通止规\n", encoding="utf-8")
|
||||
assert read_label_text_file(p) == {0: "工件", 1: "通止规"}
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 占位符
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_placeholder_is_labelled_not_bare_digit():
|
||||
"""占位名必须一眼看出是占位，不能退化成裸数字 '0'。
|
||||
|
||||
原先的代码是 _id_to_name.get(cid, str(cid))，也就是类名变成 "0"，
|
||||
和"真的有个叫 0 的类"无法区分 —— 标签丢了都看不出来。
|
||||
"""
|
||||
assert placeholder_labels(3) == {0: "class_0", 1: "class_1", 2: "class_2"}
|
||||
assert placeholder_labels(1)[0] == "class_0"
|
||||
assert placeholder_labels(0) == {}
|
||||
assert placeholder_labels(-5) == {}
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 旁挂文件：写 → 读 往返
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_format_labels_yaml_is_parseable_and_ordered():
|
||||
txt = format_labels_yaml({0: "workpiece", 1: "hole"}, model="m")
|
||||
data = yaml.safe_load(txt)
|
||||
assert data["names"] == ["workpiece", "hole"]
|
||||
assert data["model"] == "m"
|
||||
|
||||
|
||||
def test_format_labels_yaml_includes_usage_hint():
|
||||
"""注释头是给直接打开文件的人看的，必须存在。"""
|
||||
txt = format_labels_yaml({0: "a"})
|
||||
assert "第 1 行对应类别 0" in txt
|
||||
assert "scripts/weights.sh labels" in txt
|
||||
|
||||
|
||||
def test_format_labels_yaml_empty():
|
||||
data = yaml.safe_load(format_labels_yaml({}))
|
||||
assert data["names"] == []
|
||||
|
||||
|
||||
def test_write_then_read_round_trip(tmp_path):
|
||||
w = _touch(tmp_path / "detect_best_0.engine")
|
||||
written = write_labels(w, {0: "workpiece", 1: "tongzhigui"})
|
||||
assert written.name == "detect_best_0.labels.yaml"
|
||||
assert read_label_file(written) == {0: "workpiece", 1: "tongzhigui"}
|
||||
|
||||
|
||||
def test_write_labels_records_model_name(tmp_path):
|
||||
w = _touch(tmp_path / "abc.onnx")
|
||||
p = write_labels(w, {0: "x"})
|
||||
assert label_file_model(p) == "abc"
|
||||
|
||||
|
||||
def test_read_label_file_tolerates_dict_form(tmp_path):
|
||||
"""用户可能按 Ultralytics data.yaml 的字典写法手写。"""
|
||||
p = tmp_path / "m.labels.yaml"
|
||||
p.write_text("names:\n 0: a\n 1: b\n", encoding="utf-8")
|
||||
assert read_label_file(p) == {0: "a", 1: "b"}
|
||||
|
||||
|
||||
def test_read_label_file_tolerates_comma_string(tmp_path):
|
||||
p = tmp_path / "m.labels.yaml"
|
||||
p.write_text('names: "a, b"\n', encoding="utf-8")
|
||||
assert read_label_file(p) == {0: "a", 1: "b"}
|
||||
|
||||
|
||||
def test_read_label_file_tolerates_bare_text_file(tmp_path):
|
||||
"""有人直接把手写的 classes.txt 改个名字就用了，也该能读。"""
|
||||
p = tmp_path / "m.labels.yaml"
|
||||
p.write_text("a\nb\n", encoding="utf-8")
|
||||
assert read_label_file(p) == {0: "a", 1: "b"}
|
||||
|
||||
|
||||
def test_read_label_file_missing_returns_none(tmp_path):
|
||||
assert read_label_file(tmp_path / "nope.labels.yaml") is None
|
||||
|
||||
|
||||
def test_read_label_file_malformed_yaml_returns_none(tmp_path):
|
||||
"""文件坏了不该让推理进程崩 —— 退回"没有标签"即可。"""
|
||||
p = tmp_path / "m.labels.yaml"
|
||||
p.write_text("names: [a, b\n bad: : :\n", encoding="utf-8")
|
||||
assert read_label_file(p) is None
|
||||
|
||||
|
||||
def test_read_label_file_empty_names_returns_none(tmp_path):
|
||||
p = tmp_path / "m.labels.yaml"
|
||||
p.write_text("names: []\n", encoding="utf-8")
|
||||
assert read_label_file(p) is None
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 从 ONNX metadata 读
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_labels_from_onnx_via_read_labels(tmp_path):
|
||||
w = _make_onnx(tmp_path / "m.onnx", names_json="{0: 'workpiece'}", nc=1)
|
||||
ls = read_labels(w)
|
||||
assert ls.names == {0: "workpiece"}
|
||||
assert ls.source == SOURCE_ONNX
|
||||
assert not ls.is_placeholder
|
||||
|
||||
|
||||
def test_onnx_multiclass_names(tmp_path):
|
||||
w = _make_onnx(tmp_path / "m.onnx", names_json="{0: 'a', 1: 'b', 2: 'c'}")
|
||||
assert read_labels(w).names == {0: "a", 1: "b", 2: "c"}
|
||||
|
||||
|
||||
def test_onnx_without_names_gives_placeholder(tmp_path):
|
||||
w = _make_onnx(tmp_path / "m.onnx")
|
||||
ls = read_labels(w)
|
||||
assert ls.is_placeholder
|
||||
assert ls.path is None
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 优先级：sidecar 必须赢
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_sidecar_beats_onnx_metadata(tmp_path):
|
||||
"""这是本模块最要紧的行为。
|
||||
|
||||
ONNX 里内嵌的是导出时的旧名字。用户改了旁挂文件却看到旧名字，
|
||||
会变成极难排查的问题（"我明明改了为什么不生效"）。
|
||||
"""
|
||||
w = _make_onnx(tmp_path / "m.onnx", names_json="{0: 'OLD_NAME'}")
|
||||
assert read_labels(w).names == {0: "OLD_NAME"}
|
||||
|
||||
write_labels(w, {0: "NEW_NAME"})
|
||||
ls = read_labels(w)
|
||||
assert ls.names == {0: "NEW_NAME"}
|
||||
|
||||
|
||||
def test_sidecar_beats_placeholder_for_engine(tmp_path):
|
||||
"""engine 读不到任何 metadata，旁挂文件是它唯一的标签来源。"""
|
||||
w = _touch(tmp_path / "m.engine")
|
||||
assert read_labels(w).is_placeholder
|
||||
|
||||
write_labels(w, {0: "workpiece"})
|
||||
ls = read_labels(w)
|
||||
assert ls.names == {0: "workpiece"}
|
||||
assert ls.source == SOURCE_SIDECAR
|
||||
|
||||
|
||||
def test_sidecar_shared_across_formats_in_same_dir(tmp_path):
|
||||
""".onnx 和 .engine 同名同目录 → 共用一份标签。"""
|
||||
onnx = _make_onnx(tmp_path / "m.onnx", names_json="{0: 'x'}")
|
||||
engine = _touch(tmp_path / "m.engine")
|
||||
write_labels(engine, {0: "shared", 1: "two"})
|
||||
|
||||
assert read_labels(onnx).names == {0: "shared", 1: "two"}
|
||||
assert read_labels(engine).names == {0: "shared", 1: "two"}
|
||||
assert read_labels(onnx).source == SOURCE_SIDECAR
|
||||
|
||||
|
||||
def test_engine_without_sidecar_does_not_guess(tmp_path):
|
||||
"""engine 缺标签时必须显式报占位符，不能假装知道。"""
|
||||
w = _touch(tmp_path / "m.engine")
|
||||
ls = read_labels(w)
|
||||
assert ls.is_placeholder
|
||||
assert ls.source == SOURCE_PLACEHOLDER
|
||||
|
||||
|
||||
def test_read_labels_tolerates_missing_weights_file(tmp_path):
|
||||
"""权重不存在也不该崩（由调用方负责检查存在性）。"""
|
||||
ls = read_labels(tmp_path / "ghost.engine")
|
||||
assert isinstance(ls, LabelSet)
|
||||
assert ls.is_placeholder
|
||||
|
||||
|
||||
def test_existing_sidecar_wins_even_if_weights_absent(tmp_path):
|
||||
"""标签文件独立于权重存在，有权重没权重都能读出来。"""
|
||||
write_labels(tmp_path / "m.engine", {0: "only_labels"})
|
||||
ls = read_labels(tmp_path / "m.engine")
|
||||
assert ls.names == {0: "only_labels"}
|
||||
assert ls.source == SOURCE_SIDECAR
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# LabelSet 行为
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_labelset_get_falls_back_to_placeholder_name():
|
||||
ls = LabelSet(names={0: "a"}, source=SOURCE_SIDECAR)
|
||||
assert ls.get(0) == "a"
|
||||
assert ls.get(7) == "class_7"
|
||||
assert ls.get(7, "unknown") == "unknown"
|
||||
|
||||
|
||||
def test_labelset_sorted_names():
|
||||
ls = LabelSet(names={2: "c", 0: "a", 1: "b"}, source=SOURCE_SIDECAR)
|
||||
assert ls.sorted_names() == ["a", "b", "c"]
|
||||
|
||||
|
||||
def test_describe_mentions_source_in_chinese():
|
||||
"""来源必须出现在日志里 —— 这是"标签丢了"能被发现的关键。"""
|
||||
ls = LabelSet(names={0: "workpiece"}, source=SOURCE_SIDECAR)
|
||||
assert "workpiece" in describe(ls)
|
||||
assert "旁挂标签文件" in describe(ls)
|
||||
|
||||
ls = LabelSet(names={0: "class_0"}, source=SOURCE_PLACEHOLDER)
|
||||
assert "占位符" in describe(ls)
|
||||
|
||||
|
||||
def test_describe_handles_empty():
|
||||
assert "(空)" in describe(LabelSet(names={}, source=SOURCE_PLACEHOLDER))
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 错配检测（doctor 用）
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_label_file_status_reports_missing(tmp_path):
|
||||
exists, path, hint = label_file_status(tmp_path / "m.engine")
|
||||
assert exists is False
|
||||
assert hint is not None and "缺标签文件" in hint
|
||||
|
||||
|
||||
def test_label_file_status_ok(tmp_path):
|
||||
w = _touch(tmp_path / "m.engine")
|
||||
write_labels(w, {0: "a"})
|
||||
exists, _path, hint = label_file_status(w)
|
||||
assert exists is True
|
||||
assert hint is None
|
||||
|
||||
|
||||
def test_label_file_status_detects_mismatched_copy(tmp_path):
|
||||
"""把 A 的标签文件拷给 B —— 这种错配必须被指出来。"""
|
||||
write_labels(tmp_path / "A.onnx", {0: "a"})
|
||||
(tmp_path / "A.labels.yaml").rename(tmp_path / "B.labels.yaml")
|
||||
b = _touch(tmp_path / "B.onnx")
|
||||
|
||||
exists, _path, hint = label_file_status(b)
|
||||
assert exists is True
|
||||
assert hint is not None
|
||||
assert "A" in hint and "B" in hint
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 中文标签（实际会用到的场景）
|
||||
# --------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_chinese_labels_round_trip(tmp_path):
|
||||
w = _touch(tmp_path / "m.onnx")
|
||||
write_labels(w, {0: "工件", 1: "通止规"})
|
||||
ls = read_labels(w)
|
||||
assert ls.names == {0: "工件", 1: "通止规"}
|
||||
# 写出的文件必须是 UTF-8，否则中文会乱码
|
||||
txt = labels_path_for(w).read_text(encoding="utf-8")
|
||||
assert "通止规" in txt
|
||||
|
||||
|
||||
def test_numpy_int_keys_are_accepted(tmp_path):
|
||||
"""调用方从 numpy 结果里拿到的索引可能是 np.int64，不能因此失败。"""
|
||||
w = _touch(tmp_path / "m.onnx")
|
||||
write_labels(w, {np.int64(0): "a", np.int64(1): "b"})
|
||||
assert read_labels(w).names == {0: "a", 1: "b"}
|
||||
@@ -0,0 +1,190 @@
|
||||
"""回归：节点类之间的**回调方法重名**必须被拦住。
|
||||
|
||||
为什么值得一条专门的测试（2026-09-24 踩到的真实故障，代价是两轮排查方向全错）：
|
||||
|
||||
Stage2Locate.__init__ 里建了
|
||||
create_subscription(WrenchStamped, ft_wrench_topic, self._on_ft, ...)
|
||||
Stage3Search（子类）里定义了
|
||||
def _on_ft(self, msg): self._ft = (time.monotonic(), msg)
|
||||
|
||||
Python 的方法解析顺序让**子类覆盖父类**，而两个订阅都已经在 __init__ 里
|
||||
建好了。于是那条 WrenchStamped 订阅的回调实际是子类那个 —— 它把消息塞进
|
||||
stage3 的 RobotStates 缓存，而父类的 `_ft_fz` 永远不被赋值。
|
||||
|
||||
现象："S07 腕部 FT 没有数据（话题 /sim/ft_sensor_wrench）"，
|
||||
而在 shell 里 `ros2 topic echo` 那条话题**数据完全正常**、
|
||||
QoS 也完全兼容（实测 BEST_EFFORT 与 RELIABLE 订阅者都收得到）。
|
||||
于是排查方向被引到"话题名写错 / QoS 不匹配 / 桥接没起来"上，全部走偏。
|
||||
|
||||
真机上更阴：两个话题都存在，`self._ft` 里 RobotStates 与 WrenchStamped
|
||||
交替覆盖 → `_ft_mag()` 时有时无 → 腕部力看门狗时灵时不灵（偶发，最难查）。
|
||||
|
||||
规则：**整条继承链上，`_on_*` 形式的方法名必须唯一 —— 除非子类显式
|
||||
``super().<同名>(...)`` 委托回父类**。后者是"扩展"而不是"遮蔽"，是允许的
|
||||
（``GaugeAssembly._on_color`` 就属于这种：先 ``super()._on_color(msg)`` 存帧，
|
||||
再补记"收到时刻"）。判定标准就是有没有那一次委托调用 ——
|
||||
没有委托的同名覆盖，一定会让父类的那份状态**永远不更新**，而且不报任何错。
|
||||
|
||||
命名空间不够用就改名字（`_on_wrench` / `_on_robot_states` 比 `_on_ft` 好，
|
||||
后者里那个 "ft" 到底指腕部六维力还是手指触觉，本来就含糊）——
|
||||
不要靠"反正签名不同"来赌，Python 不检查签名。
|
||||
|
||||
为什么用 AST 读源码而不是 import 后反射：
|
||||
这几个节点模块 import 了 ROS 生成的消息包（``arm_msgs`` 等），
|
||||
在没有 source 过工作空间的 pytest 环境里 import 不进来。
|
||||
而这条规则是**纯源码层面**的，不需要真的构造节点。
|
||||
这也是本仓库既有的做法（见 ``test_gauge_step_events.py``）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import pathlib
|
||||
from typing import Dict, List
|
||||
|
||||
_APP = pathlib.Path(__file__).resolve().parents[1] / "arm_application"
|
||||
|
||||
#: 继承链上的源文件。
|
||||
_CHAIN_FILES = (
|
||||
"stage2_locate_node.py",
|
||||
"stage3_search_node.py",
|
||||
"pipeline_node.py",
|
||||
"gauge_assembly_node.py",
|
||||
)
|
||||
|
||||
#: 从哪个类开始往上走（装配流程真正实例化的那个）。
|
||||
_LEAF = "GaugeAssembly"
|
||||
|
||||
|
||||
class _Cls:
|
||||
"""一个类的：基类名、本类定义的 `_on_*` 方法（名字 → AST 节点）。"""
|
||||
|
||||
def __init__(self, name: str, bases: List[str], handlers: Dict[str, ast.AST]) -> None:
|
||||
self.name = name
|
||||
self.bases = bases
|
||||
self.handlers = handlers
|
||||
|
||||
|
||||
def _classes_in(path: pathlib.Path) -> Dict[str, _Cls]:
|
||||
tree = ast.parse(path.read_text(encoding="utf-8"))
|
||||
out: Dict[str, _Cls] = {}
|
||||
for node in tree.body:
|
||||
if not isinstance(node, ast.ClassDef):
|
||||
continue
|
||||
bases = [b.id for b in node.bases if isinstance(b, ast.Name)]
|
||||
handlers = {
|
||||
item.name: item
|
||||
for item in node.body
|
||||
if isinstance(item, (ast.FunctionDef, ast.AsyncFunctionDef))
|
||||
and item.name.startswith("_on_")
|
||||
}
|
||||
out[node.name] = _Cls(node.name, bases, handlers)
|
||||
return out
|
||||
|
||||
|
||||
def _table() -> Dict[str, _Cls]:
|
||||
table: Dict[str, _Cls] = {}
|
||||
for name in _CHAIN_FILES:
|
||||
table.update(_classes_in(_APP / name))
|
||||
return table
|
||||
|
||||
|
||||
def _chain() -> List[str]:
|
||||
"""从 ``_LEAF`` 沿基类往上，返回类名列表（子类在前）。"""
|
||||
table = _table()
|
||||
order: List[str] = []
|
||||
cur = _LEAF
|
||||
while cur and cur in table:
|
||||
order.append(cur)
|
||||
bases = table[cur].bases
|
||||
cur = bases[0] if bases else ""
|
||||
assert len(order) >= 3, f"继承链没解析出来（只找到 {order}）—— 改过类名？"
|
||||
return order
|
||||
|
||||
|
||||
def _delegates_to_super(func: ast.AST, name: str) -> bool:
|
||||
"""``func`` 的方法体里有没有 ``super().<name>(...)`` 这一次委托调用。"""
|
||||
for node in ast.walk(func):
|
||||
if not isinstance(node, ast.Call):
|
||||
continue
|
||||
callee = node.func
|
||||
if not (isinstance(callee, ast.Attribute) and callee.attr == name):
|
||||
continue
|
||||
owner = callee.value
|
||||
if (
|
||||
isinstance(owner, ast.Call)
|
||||
and isinstance(owner.func, ast.Name)
|
||||
and owner.func.id == "super"
|
||||
):
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def test_leaf_class_is_reachable_from_every_chain_file():
|
||||
"""先确认这条测试自己在看对文件 —— 文件改名/移动时别静默变成空跑。"""
|
||||
chain = _chain()
|
||||
assert _LEAF in chain
|
||||
for expected in ("Stage2Locate", "Stage3Search", "AssemblyPipeline"):
|
||||
assert expected in chain, f"继承链里没有 {expected}（链 = {chain}）"
|
||||
|
||||
|
||||
def test_no_silent_callback_shadowing_across_inheritance_chain():
|
||||
"""同名 `_on_*` 只允许两种情形：唯一，或子类显式 ``super()`` 委托。
|
||||
|
||||
挂了怎么修（二选一）：
|
||||
a) 在子类那个方法体里加 ``super().<同名>(msg)`` —— 表示"我是扩展"；
|
||||
b) 改掉其中一个的名字（带来源，如 ``_on_wrench`` / ``_on_robot_states``）。
|
||||
别什么都不做：父类那份状态会永远不更新，而**不会有任何报错**。
|
||||
"""
|
||||
table = _table()
|
||||
owner: Dict[str, str] = {}
|
||||
problems: List[str] = []
|
||||
|
||||
# _chain() 是"子类在前"，所以后出现的才是父类。
|
||||
for cls_name in _chain():
|
||||
cls = table.get(cls_name)
|
||||
if cls is None:
|
||||
continue
|
||||
for handler in sorted(cls.handlers):
|
||||
owner.setdefault(handler, cls_name)
|
||||
|
||||
# 再逐个查：凡是有多个类定义了的，子类必须委托。
|
||||
for handler, owner_cls in owner.items():
|
||||
definers = [c for c in _chain() if c in table and handler in table[c].handlers]
|
||||
if len(definers) < 2:
|
||||
continue
|
||||
for cls_name in definers:
|
||||
if cls_name == definers[-1]:
|
||||
continue # 链最靠上的那个（基类）不需要委托
|
||||
cls = table[cls_name]
|
||||
if _delegates_to_super(cls.handlers[handler], handler):
|
||||
continue
|
||||
problems.append(
|
||||
f"{handler}: 定义在 {', '.join(definers)} —— {cls_name} 覆盖了"
|
||||
f"{owner_cls} 的同名回调，但方法体里没有 super().{handler}(...) 委托。\n"
|
||||
f" 实际生效的只有 {cls_name} 那个，父类那份状态永远不会更新。"
|
||||
)
|
||||
|
||||
assert not problems, (
|
||||
"回调被静默遮蔽。症状是「某路数据永远收不到」而话题本身完全正常，"
|
||||
"极难从现象倒推到这里：\n " + "\n ".join(problems)
|
||||
)
|
||||
|
||||
|
||||
def test_wrench_and_robot_states_callbacks_are_not_the_same_method():
|
||||
"""专门钉住这一对名字 —— 它们是最容易被合并成 `_on_ft` 的两个。
|
||||
|
||||
腕部 FT（WrenchStamped，100Hz，法兰系）与整机状态（RobotStates，含外部
|
||||
力/力矩）是两个**不同的**话题、不同的消息类型，缓存也必须是两个字段：
|
||||
``_ft_fz``（轴向力标量）与 ``_ft``（整帧 RobotStates）。
|
||||
"""
|
||||
table = _table()
|
||||
s2 = set(table["Stage2Locate"].handlers)
|
||||
s3 = set(table["Stage3Search"].handlers)
|
||||
|
||||
assert "_on_wrench" in s2, (
|
||||
"Stage2Locate 里腕部 FT 的回调应叫 _on_wrench（不能再叫 _on_ft："
|
||||
"会被 Stage3Search 的同名方法遮蔽）"
|
||||
)
|
||||
assert "_on_robot_states" in s3, "Stage3Search 里整机状态的回调应叫 _on_robot_states"
|
||||
assert not (s2 & s3), f"两个类还有重名的回调: {sorted(s2 & s3)}"
|
||||
@@ -0,0 +1,50 @@
|
||||
"""``OnnxWorkpieceDetector`` 必须**当场拒绝**非检测模型，而不是解出垃圾框。
|
||||
|
||||
为什么值得一条测试（2026-10-08 实测）
|
||||
-------------------------------------
|
||||
把一个 ``task=pose`` 的模型（输出 ``[1, 4+nc+3K, 8400]``）喂给这个"只认
|
||||
``[1, N, 6]``"的解码器，会得到 12 个**看起来像检测结果**的东西：
|
||||
|
||||
conf=70.870 cls=class_73 xyxy=[28.7, 0.0, 75.8, 0.0]
|
||||
conf=41.092 cls=class_47 xyxy=[11.0, 0.0, 35.4, 0.0]
|
||||
...
|
||||
conf=0.324 cls=workpiece xyxy=[ 0.2, 0.0, 0.3, 0.0]
|
||||
|
||||
坐标被当成置信度（>1）、类号不存在、框是退化的（``y1 == y2``）—— 但没有异常、
|
||||
没有告警，流水线照跑。这类"静默失效"正是本仓库花最多代价在防的东西，所以在
|
||||
``__init__`` 里硬失败。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
pytest.importorskip("onnxruntime", reason="没装 onnxruntime，属于环境缺依赖")
|
||||
|
||||
from arm_application.onnx_workpiece import OnnxWorkpieceDetector # noqa: E402
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
YOLO_DIR = ROOT / "models" / "yolo"
|
||||
POSE_ONNX = YOLO_DIR / "gongjian0929.onnx" # task=pose, 输出 [1,14,8400]
|
||||
DETECT_ONNX = YOLO_DIR / "detect_best_0.onnx" # task=detect, end2end, 输出 [1,N,6]
|
||||
|
||||
|
||||
def test_pose_model_is_refused_loudly():
|
||||
"""带关键点的 pose 模型进了检测解码器 ⇒ 必须报错，且报错要指出正确做法。"""
|
||||
if not POSE_ONNX.is_file():
|
||||
pytest.skip(f"没有 {POSE_ONNX.name}（模型不在本机）")
|
||||
with pytest.raises(RuntimeError) as ei:
|
||||
OnnxWorkpieceDetector(POSE_ONNX, conf=0.25, imgsz=640)
|
||||
msg = str(ei.value)
|
||||
assert "不是 6" in msg, f"报错要说出真实输出维度，实际：{msg}"
|
||||
# 必须告诉人该往哪走，否则只会被当成"模型坏了"
|
||||
assert "pose" in msg.lower()
|
||||
|
||||
|
||||
def test_end2end_detect_model_still_loads():
|
||||
"""守卫不能过严：真正的 end2end 检测模型必须照常加载。"""
|
||||
if not DETECT_ONNX.is_file():
|
||||
pytest.skip(f"没有 {DETECT_ONNX.name}（模型不在本机）")
|
||||
OnnxWorkpieceDetector(DETECT_ONNX, conf=0.25, imgsz=640)
|
||||
@@ -0,0 +1,146 @@
|
||||
"""``read_vector_map`` 的单测。
|
||||
|
||||
这个函数存在的唯一原因是：Orisys 的 Python 绑定是编译过的，公开接口里没有
|
||||
一个明确叫 "force vector map" 的方法，只能按语义去试 ``compute_flow`` /
|
||||
``compute_hhd`` / ``compute_deformation``，而且要判断返回值到底是数组
|
||||
还是 bool/可视化图。
|
||||
|
||||
但它接的是真传感器，没法在单测里调。所以这里用假传感器把每条分支钉死：
|
||||
**试错的顺序**和**接受的返回类型**一旦写错，表现是"矢量场话题永远没消息"
|
||||
或"发了一张全零假图"，两者都很难从现场现象反推回代码。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.orisys_tactile import read_vector_map
|
||||
|
||||
|
||||
class FakeSensor:
|
||||
"""可控假传感器。
|
||||
|
||||
传入 ``方法名=返回值``；想让某个方法抛异常就传异常实例。
|
||||
没有列出的方法属性一律不存在 —— 这正好模拟"老版本 SDK 没这个方法"。
|
||||
"""
|
||||
|
||||
def __init__(self, **methods: Any) -> None:
|
||||
for name, value in methods.items():
|
||||
if isinstance(value, BaseException):
|
||||
def _raise(_exc=value):
|
||||
raise _exc
|
||||
|
||||
setattr(self, name, _raise)
|
||||
else:
|
||||
setattr(self, name, (lambda _v=value: _v))
|
||||
|
||||
|
||||
class TestPreferredSource:
|
||||
def test_uses_compute_flow_first(self):
|
||||
"""compute_flow 语义上最接近"切向力矢量场"，优先级最高。"""
|
||||
flow = np.ones((8, 6, 2), np.float32)
|
||||
sensor = FakeSensor(
|
||||
compute_flow=flow,
|
||||
compute_hhd=np.zeros((8, 6, 3), np.uint8),
|
||||
)
|
||||
assert read_vector_map(sensor) is flow
|
||||
|
||||
def test_falls_back_to_hhd_when_flow_is_bool(self):
|
||||
"""有些版本 compute_flow 只返回 bool（"有没有流动"）—— 那不是矢量场，
|
||||
必须继续往下试，而不是把 True 当成图。"""
|
||||
hhd = np.ones((4, 4), np.float32)
|
||||
sensor = FakeSensor(compute_flow=True, compute_hhd=hhd)
|
||||
assert read_vector_map(sensor) is hhd
|
||||
|
||||
def test_falls_back_to_deformation(self):
|
||||
deform = np.ones((5, 5, 2), np.float32)
|
||||
sensor = FakeSensor(compute_flow=False, compute_hhd=None, compute_deformation=deform)
|
||||
assert read_vector_map(sensor) is deform
|
||||
|
||||
def test_skips_flow_that_raises(self):
|
||||
hhd = np.ones((3, 3), np.float32)
|
||||
sensor = FakeSensor(compute_flow=RuntimeError("no cuda"), compute_hhd=hhd)
|
||||
assert read_vector_map(sensor) is hhd
|
||||
|
||||
|
||||
class TestRejectedValues:
|
||||
"""不是"二维以上数组"的返回值一律不能当矢量场用。"""
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"bad",
|
||||
[True, False, None, 0, 0.0, "flow", (1, 2), [1.0, 2.0], {"a": 1}],
|
||||
)
|
||||
def test_non_array_values_are_rejected(self, bad):
|
||||
assert read_vector_map(FakeSensor(compute_flow=bad)) is None
|
||||
|
||||
def test_1d_array_is_rejected(self):
|
||||
"""1D 没法按 (H,W) 解释成图。"""
|
||||
assert read_vector_map(FakeSensor(compute_flow=np.zeros(10, np.float32))) is None
|
||||
|
||||
def test_empty_array_is_rejected(self):
|
||||
"""空数组同样是"没有数据"，不能发一张 0x0 的图。"""
|
||||
assert read_vector_map(FakeSensor(compute_flow=np.zeros((0, 0), np.uint8))) is None
|
||||
|
||||
def test_2d_array_is_accepted(self):
|
||||
"""2D 灰度图是合法结果（某些版本把形变强度图当成矢量场返回）。"""
|
||||
arr = np.zeros((4, 5), np.uint8)
|
||||
assert read_vector_map(FakeSensor(compute_flow=arr)) is arr
|
||||
|
||||
def test_scalar_ndarray_is_rejected(self):
|
||||
"""0 维 ndarray 没有 (H,W) 语义。"""
|
||||
assert read_vector_map(FakeSensor(compute_flow=np.array(3.0))) is None
|
||||
|
||||
|
||||
class TestMissingMethods:
|
||||
def test_all_missing_returns_none(self):
|
||||
"""老版本 SDK 三个方法都没有 —— 应当安静返回 None，让上层跳过话题。"""
|
||||
assert read_vector_map(FakeSensor()) is None
|
||||
|
||||
def test_all_raising_returns_none(self):
|
||||
sensor = FakeSensor(
|
||||
compute_flow=RuntimeError("x"),
|
||||
compute_hhd=RuntimeError("y"),
|
||||
compute_deformation=RuntimeError("z"),
|
||||
)
|
||||
assert read_vector_map(sensor) is None
|
||||
|
||||
def test_never_raises_even_when_everything_fails(self):
|
||||
"""这是它的契约：探测失败只能返回 None，不能把异常抛给 30fps 的读取循环。"""
|
||||
sensor = FakeSensor(compute_flow=KeyboardInterrupt())
|
||||
# KeyboardInterrupt 是 BaseException；探测逻辑捕获 Exception，
|
||||
# 所以它会被放行 —— 记录当前行为，提醒别在这里拿 BaseException 当普通错误。
|
||||
with pytest.raises(KeyboardInterrupt):
|
||||
read_vector_map(sensor)
|
||||
|
||||
|
||||
class TestResultIsUsableByImageUtils:
|
||||
"""探测到的数组必须能被 image_utils 打包 —— 否则等于白探测。"""
|
||||
|
||||
def test_flow_shaped_array_packs_as_32fc2(self):
|
||||
from arm_application.image_utils import ndarray_to_image
|
||||
|
||||
flow = np.zeros((8, 6, 2), np.float32)
|
||||
sensor = FakeSensor(compute_flow=flow)
|
||||
arr = read_vector_map(sensor)
|
||||
msg = ndarray_to_image(arr)
|
||||
assert msg.encoding == "32FC2"
|
||||
assert (msg.height, msg.width) == (8, 6)
|
||||
|
||||
def test_grayscale_array_packs_as_mono8(self):
|
||||
from arm_application.image_utils import ndarray_to_image
|
||||
|
||||
arr = read_vector_map(FakeSensor(compute_flow=np.zeros((8, 6), np.uint8)))
|
||||
assert ndarray_to_image(arr).encoding == "mono8"
|
||||
|
||||
def test_float64_result_is_rejected_by_packer_not_silently_truncated(self):
|
||||
"""float64 是 numpy 的默认 dtype，SDK 很可能返回它。image_utils 会
|
||||
明确报错而不是按 float32 打包 —— 那样会让每个像素错位 4 字节。"""
|
||||
from arm_application.image_utils import UnsupportedImageError, ndarray_to_image
|
||||
|
||||
arr = read_vector_map(FakeSensor(compute_flow=np.zeros((8, 6, 2), np.float64)))
|
||||
assert arr is not None
|
||||
with pytest.raises(UnsupportedImageError):
|
||||
ndarray_to_image(arr)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,247 @@
|
||||
"""一阶段跟踪切到顶置 D435：相机、手眼文件、TF 父子、点变换都必须对。
|
||||
|
||||
把眼在手外的 T（cam→基座）再乘法兰，定位总能到附近、永远差一截。
|
||||
所以这里每条都配负向：走错模式 / 借腕部量程 / 非法角色必须红。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
SCRIPTS_LIB = ROOT / "scripts" / "lib"
|
||||
sys.path.insert(0, str(SCRIPTS_LIB))
|
||||
import export_site_env # type: ignore # noqa: E402
|
||||
|
||||
from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T # noqa: E402
|
||||
from arm_application.site_io import ( # noqa: E402
|
||||
handeye_static_tf_frames,
|
||||
track_camera_role,
|
||||
track_handeye_mode,
|
||||
)
|
||||
|
||||
from test_handeye_mode import _site # noqa: E402
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _isolate_arm(monkeypatch):
|
||||
"""别让本机 env 里的 ROBOT_SN=89 把虚构站点解析到真机相机。"""
|
||||
monkeypatch.setenv("SITE_CONFIG", "/nonexistent/site.yaml")
|
||||
monkeypatch.setenv("ROBOT_SN", "Rizon4-777001")
|
||||
|
||||
|
||||
def _track_site(**over) -> dict:
|
||||
site = _site()
|
||||
site["camera"] = {"name": "d405_left"}
|
||||
site["paths"] = {"handeye_file": "/tmp/wrist_eye_in_hand.yaml"}
|
||||
site["yolo"] = {"max_depth_m": 0.55, "min_depth_m": 0.04, "feature_min_range_m": 0.14}
|
||||
site["servo"] = {
|
||||
"xy_track": "hover",
|
||||
"lock_still_sec": 3.0,
|
||||
"d435_handback_sec": 3.0,
|
||||
"wrist_align_sec": 0.5,
|
||||
"lock_still_move_m": 0.015,
|
||||
}
|
||||
site["perception"] = {
|
||||
"track_camera": "wrist",
|
||||
"wrist_handoff_conf": 0.5,
|
||||
"overhead": {
|
||||
"min_depth_m": 0.25,
|
||||
"max_depth_m": 1.80,
|
||||
"feature_min_range_m": 0.30,
|
||||
"oneshot_conf": 0.5,
|
||||
"cooldown_s": 1.5,
|
||||
"frame_max_age_s": 0.4,
|
||||
"oneshot_hold_s": 8.0,
|
||||
},
|
||||
}
|
||||
for k, v in over.items():
|
||||
if isinstance(v, dict) and isinstance(site.get(k), dict):
|
||||
merged = dict(site[k])
|
||||
merged.update(v)
|
||||
site[k] = merged
|
||||
else:
|
||||
site[k] = v
|
||||
return site
|
||||
|
||||
|
||||
def _overhead_ok() -> dict:
|
||||
return _track_site(
|
||||
perception={
|
||||
"track_camera": "overhead",
|
||||
"overhead": {
|
||||
"min_depth_m": 0.25,
|
||||
"max_depth_m": 1.80,
|
||||
"feature_min_range_m": 0.30,
|
||||
},
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
class TestTrackCameraRole:
|
||||
def test_missing_defaults_to_wrist(self):
|
||||
assert track_camera_role({}) == "wrist"
|
||||
assert track_camera_role({"perception": {}}) == "wrist"
|
||||
|
||||
def test_illegal_value_raises_not_fallback(self):
|
||||
with pytest.raises(ValueError, match="wrist 或 overhead"):
|
||||
track_camera_role({"perception": {"track_camera": "d405"}})
|
||||
|
||||
def test_overhead_maps_to_eye_to_hand(self):
|
||||
assert track_handeye_mode("overhead") == "eye_to_hand"
|
||||
assert track_handeye_mode("wrist") == "eye_in_hand"
|
||||
|
||||
|
||||
class TestPerceptionServoWiring:
|
||||
def test_overhead_uses_d435_not_wrist_camera(self):
|
||||
p = export_site_env.perception_overrides(_overhead_ok())
|
||||
assert p["camera_source"] == "d435"
|
||||
assert p["camera_frame"] == "d435_color_optical_frame"
|
||||
assert p["depth_topic"] == "/d435/aligned_depth_to_color/image_raw"
|
||||
assert "d405" not in p["camera_source"]
|
||||
assert "d405" not in p["depth_topic"]
|
||||
|
||||
def test_overhead_does_not_borrow_wrist_depth_range(self):
|
||||
p = export_site_env.perception_overrides(_overhead_ok())
|
||||
assert p["max_depth_m"] == pytest.approx(1.80)
|
||||
assert p["min_depth_m"] == pytest.approx(0.25)
|
||||
assert p["feature_min_range_m"] == pytest.approx(0.30)
|
||||
# 负向：腕部 0.55 绝不能漏过来
|
||||
assert p["max_depth_m"] != pytest.approx(0.55)
|
||||
|
||||
def test_overhead_handeye_file_is_e2h_not_wrist(self):
|
||||
s = export_site_env.servo_overrides(_overhead_ok())
|
||||
assert s["handeye_mode"] == "eye_to_hand"
|
||||
assert s["handeye_file"].endswith("01_d435_eye_to_hand.yaml")
|
||||
assert "wrist_eye_in_hand" not in s["handeye_file"]
|
||||
parent, child = handeye_static_tf_frames(
|
||||
mode=s["handeye_mode"],
|
||||
ee_frame="flange",
|
||||
base_frame="base",
|
||||
cam_frame=s["camera_frame"],
|
||||
)
|
||||
assert parent == "base"
|
||||
assert child == "d435_color_optical_frame"
|
||||
|
||||
def test_wrist_track_keeps_arm_camera_and_wrist_file(self):
|
||||
site = _track_site(perception={"track_camera": "wrist"})
|
||||
p = export_site_env.perception_overrides(site)
|
||||
s = export_site_env.servo_overrides(site)
|
||||
assert p["camera_source"] == "cam_a"
|
||||
assert s["handeye_mode"] == "eye_in_hand"
|
||||
assert s["handeye_file"] == "/tmp/wrist_eye_in_hand.yaml"
|
||||
assert s["handeye_file"].endswith("eye_in_hand.yaml")
|
||||
|
||||
def test_overhead_without_range_raises(self):
|
||||
site = _track_site(perception={"track_camera": "overhead", "overhead": {}})
|
||||
with pytest.raises(RuntimeError, match="perception.overhead"):
|
||||
export_site_env.perception_overrides(site)
|
||||
|
||||
def test_wrist_hybrid_wires_oneshot_without_overwriting_wrist_range(self):
|
||||
site = _track_site(perception={"track_camera": "wrist"})
|
||||
p = export_site_env.perception_overrides(site)
|
||||
s = export_site_env.servo_overrides(site)
|
||||
assert p["camera_source"] == "cam_a"
|
||||
assert p["max_depth_m"] == pytest.approx(0.55)
|
||||
assert p["overhead_oneshot_enable"] is True
|
||||
assert p["wrist_handoff_conf"] == pytest.approx(0.5)
|
||||
assert p["overhead_max_depth_m"] == pytest.approx(1.80)
|
||||
assert p["overhead_handeye_file"].endswith("01_d435_eye_to_hand.yaml")
|
||||
assert "d405" not in p["overhead_camera_source"]
|
||||
assert s["handeye_mode"] == "eye_in_hand"
|
||||
assert s["lock_still_sec"] == pytest.approx(3.0)
|
||||
assert s["d435_handback_sec"] == pytest.approx(3.0)
|
||||
assert s["wrist_align_sec"] == pytest.approx(0.5)
|
||||
assert s["wrist_handoff_conf"] == pytest.approx(0.5)
|
||||
assert s["overhead_hold_s"] == pytest.approx(8.0)
|
||||
|
||||
def test_wrist_hybrid_missing_handoff_conf_raises(self):
|
||||
site = _track_site()
|
||||
del site["perception"]["wrist_handoff_conf"]
|
||||
with pytest.raises(RuntimeError, match="wrist_handoff_conf"):
|
||||
export_site_env.perception_overrides(site)
|
||||
|
||||
def test_wrist_hybrid_missing_still_keys_raises(self):
|
||||
site = _track_site()
|
||||
del site["servo"]["lock_still_sec"]
|
||||
with pytest.raises(RuntimeError, match="lock_still_sec"):
|
||||
export_site_env.servo_overrides(site)
|
||||
|
||||
def test_wrist_hybrid_missing_handback_raises(self):
|
||||
site = _track_site()
|
||||
del site["servo"]["d435_handback_sec"]
|
||||
with pytest.raises(RuntimeError, match="d435_handback_sec"):
|
||||
export_site_env.servo_overrides(site)
|
||||
|
||||
def test_overhead_lookat_raises(self):
|
||||
site = _overhead_ok()
|
||||
site["servo"] = {"xy_track": "lookat"}
|
||||
with pytest.raises(RuntimeError, match="lookat"):
|
||||
export_site_env.servo_overrides(site)
|
||||
|
||||
def test_flatten_track_vars_follow_role(self):
|
||||
flat = export_site_env.flatten(_overhead_ok())
|
||||
assert flat["SITE_TRACK_CAMERA"] == "overhead"
|
||||
assert flat["SITE_TRACK_CAM_NAME"] == "d435"
|
||||
assert flat["SITE_CAM_NAME"] == "cam_a" # 腕部精拍路不变
|
||||
assert flat["SITE_TRACK_HANDEYE_MODE"] == "eye_to_hand"
|
||||
assert flat["SITE_TRACK_HANDEYE_FILE"].endswith("01_d435_eye_to_hand.yaml")
|
||||
assert "wrist_eye_in_hand" not in flat["SITE_TRACK_HANDEYE_FILE"]
|
||||
|
||||
|
||||
class TestCamPointToBase:
|
||||
def test_eye_to_hand_ignores_flange(self):
|
||||
T_he = np.eye(4)
|
||||
T_he[:3, 3] = [0.4, -0.5, 0.7]
|
||||
T_ee = np.eye(4)
|
||||
T_ee[:3, 3] = [0.6, 0.0, 0.3]
|
||||
p = np.array([0.0, 0.0, 0.8])
|
||||
got = cam_point_to_base(p, T_he, T_ee, "eye_to_hand")
|
||||
np.testing.assert_allclose(got, [0.4, -0.5, 1.5], atol=1e-9)
|
||||
# 负向：若误乘法兰，点会多出法兰平移
|
||||
wrong = cam_point_to_base(p, T_he, T_ee, "eye_in_hand")
|
||||
assert np.linalg.norm(got - wrong) > 0.5
|
||||
|
||||
def test_eye_in_hand_still_left_multiplies_ee(self):
|
||||
T_he = np.eye(4)
|
||||
T_he[:3, 3] = [0.0, 0.0, 0.12]
|
||||
T_ee = np.eye(4)
|
||||
T_ee[:3, 3] = [0.5, -0.1, 0.3]
|
||||
p = np.array([0.01, 0.0, 0.2])
|
||||
got = cam_point_to_base(p, T_he, T_ee, "eye_in_hand")
|
||||
np.testing.assert_allclose(got, [0.51, -0.1, 0.62], atol=1e-9)
|
||||
|
||||
def test_illegal_mode_raises(self):
|
||||
with pytest.raises(ValueError, match="eye_in_hand 或 eye_to_hand"):
|
||||
cam_to_base_T(np.eye(4), np.eye(4), "eye_on_foot")
|
||||
|
||||
|
||||
class TestRealSiteBranchDefault:
|
||||
"""main 上 site.yaml 必须是 D405 主跟踪 + D435 一次性，跟踪外参仍是腕部。"""
|
||||
|
||||
@pytest.mark.skip(reason="远程整站配置/硬件实现未同步；本地有独立回归测试")
|
||||
def test_site_yaml_hybrid_wrist_plus_d435_oneshot(self, monkeypatch):
|
||||
monkeypatch.setenv("SITE_CONFIG", str(ROOT / "config" / "site.yaml"))
|
||||
monkeypatch.setenv("ROBOT_SN", "Rizon4-063989")
|
||||
data = export_site_env.load_site()
|
||||
p = export_site_env.perception_overrides(data)
|
||||
s = export_site_env.servo_overrides(data)
|
||||
assert p["camera_source"] in ("d405_left", "d405")
|
||||
assert p["max_depth_m"] == pytest.approx(0.55)
|
||||
assert p["wrist_handoff_conf"] == pytest.approx(0.5)
|
||||
assert p["overhead_oneshot_enable"] is True
|
||||
assert p["overhead_handeye_file"].endswith("89_d435_eye_to_hand.yaml")
|
||||
assert p["overhead_max_depth_m"] == pytest.approx(1.80)
|
||||
assert s["handeye_mode"] == "eye_in_hand"
|
||||
assert s["handeye_file"].endswith("89_eye_in_hand.yaml")
|
||||
assert s["lock_still_sec"] == pytest.approx(3.0)
|
||||
assert s["d435_handback_sec"] == pytest.approx(2.0)
|
||||
assert s["wrist_align_sec"] == pytest.approx(0.5)
|
||||
assert s["lock_still_move_m"] == pytest.approx(0.015)
|
||||
# 负向：跟踪外参不能被换成顶视文件
|
||||
assert "eye_to_hand" not in s["handeye_file"]
|
||||
assert "eye_in_hand" not in p["overhead_handeye_file"]
|
||||
@@ -0,0 +1,180 @@
|
||||
"""一阶段"重新捕获"的两道守卫：置信度门槛 + 特征点相机量程下限。
|
||||
|
||||
为什么单拎出来测（2026-10-08 真机，arm 89，一阶段把臂带离工件 500mm）
|
||||
--------------------------------------------------------------------
|
||||
``/tmp/pipeline_stage1.log`` 原话：
|
||||
|
||||
0659.551 死区保持 |e_xy|=3.0mm ← 已经收敛到 3mm
|
||||
0659.598 k0 置信度 0.00 < 0.30，本帧不用它
|
||||
0659.6xx 重新捕获 conf=0.15 dpx=503 ← 503px 外、conf 0.15 的假框
|
||||
feat_base=(0.231,-0.484) z=-0.021 ← 跟踪点瞬间横移 427mm
|
||||
0670.985 === 一阶段已锁定 === ← "锁定"在离工件 500mm 处
|
||||
|
||||
视觉模型（真 k0 conf 0.87）与控制（伺服收敛到 3mm）都正常，坏的是逻辑：
|
||||
``pick_best`` 的"重新捕获"分支把门槛接成了 ``min_conf=0.12``，于是 0.15 的远框
|
||||
被原样采信、perception 自己 ``kf.reset()`` 跳过去；伺服侧的 FeatureJumpGate 只能
|
||||
拦一帧（假框帧间稳定，3 帧后照样认）。拦不住还有一个更根本的原因：那个假目标
|
||||
就在**相机前 0.08m**（爪上工具/相机座，随臂一起动），跟踪其误差恒为 0 —— 伺服
|
||||
"收敛"得完美，锁的却是自己。所以还要一道**量纲是米**的判据。
|
||||
|
||||
前两条是**负向**用例：把门槛调回旧值，第一条必须红。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
import yaml
|
||||
|
||||
from arm_application.yolo_detector import Detection, pick_best
|
||||
|
||||
# 测试在 src/<包>/test/ 下 → 仓库根是 parents[3]
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
SITE = ROOT / "config" / "site.yaml"
|
||||
HANDEYE = ROOT / "handeye" / "results" / "89_eye_in_hand.yaml"
|
||||
|
||||
# 真机实测数字（见本文件头与 site.yaml 的对应注释）——
|
||||
# 写死在这里是**故意**的：它们是被测事实，不是配置。
|
||||
REAL_K0_CONF = 0.87 # 真 k0 的置信度
|
||||
FALSE_BOX_CONF = 0.15 # 被误采信的那个假框
|
||||
FALSE_BOX_CONF_HI = 0.47 # 那串假框的实测上沿
|
||||
# 相机光轴视距的两条带 [m]（p_cam.z）
|
||||
ROBOT_NEAR_BAND = (0.078, 0.087) # 随臂一起动：爪上工具 / 相机座
|
||||
WORKPIECE_RANGE_BAND = (0.19, 0.29) # 工件面在锁高下
|
||||
|
||||
# 假框：503px 外、尺寸和真工件框同量级（否则会被 min_area 先挡掉，测不到本意）
|
||||
PREV = np.array([300.0, 300.0, 460.0, 440.0]) # 上一帧框，中心 (380, 370)
|
||||
FALSE_FAR = np.array([800.0, 20.0, 960.0, 160.0]) # 中心 (880, 90) → 503px 外
|
||||
|
||||
|
||||
def _det(xyxy: np.ndarray, conf: float) -> Detection:
|
||||
return Detection(xyxy=np.asarray(xyxy, dtype=float), conf=float(conf),
|
||||
cls_id=0, cls_name="workpiece")
|
||||
|
||||
|
||||
def _pick(conf: float, reacquire_conf: float):
|
||||
return pick_best(
|
||||
[_det(FALSE_FAR, conf)],
|
||||
prev_uv=np.array([380.0, 370.0]),
|
||||
prev_xyxy=PREV,
|
||||
max_track_px=400.0,
|
||||
reacquire_conf=reacquire_conf,
|
||||
min_conf=0.12,
|
||||
lock_iou=0.05,
|
||||
)
|
||||
|
||||
|
||||
def test_old_wiring_would_adopt_the_false_box():
|
||||
"""负向基线：门槛=min_conf（旧接线）时，conf 0.15 的 503px 远框**会被采信**。
|
||||
|
||||
这条是"这个检查能红"的证明 —— 它若开始返回 None，说明前提变了，
|
||||
下面"新接线挡得住"就不再是在测同一件事。
|
||||
"""
|
||||
best = _pick(FALSE_BOX_CONF, reacquire_conf=0.12)
|
||||
assert best is not None, "旧接线本该采信这个假框 —— 前提变了，用例已失真"
|
||||
assert best.conf == pytest.approx(FALSE_BOX_CONF)
|
||||
|
||||
|
||||
def test_new_wiring_rejects_the_false_box():
|
||||
"""新接线：门槛 0.55 > 假框上沿 0.47 ⇒ 低置信度远框一律不采信。"""
|
||||
assert _pick(FALSE_BOX_CONF, reacquire_conf=0.55) is None
|
||||
|
||||
|
||||
def test_new_wiring_rejects_whole_false_band():
|
||||
"""实测那串假框 conf 0.13~0.47，全带都要挡住（不能只挡 0.15 那一个）。"""
|
||||
for conf in (0.13, 0.25, 0.38, FALSE_BOX_CONF_HI):
|
||||
assert _pick(conf, reacquire_conf=0.55) is None, f"conf={conf} 漏过去了"
|
||||
|
||||
|
||||
def test_high_conf_relocation_is_still_adopted():
|
||||
"""守卫不能过严：真 k0（0.87）级的重定位必须仍然立刻生效。
|
||||
|
||||
否则"工件确实被挪了"时臂会僵在原地不动 —— 那是另一种失效。
|
||||
"""
|
||||
best = _pick(REAL_K0_CONF, reacquire_conf=0.55)
|
||||
assert best is not None and best.conf == pytest.approx(REAL_K0_CONF)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 接线：site.yaml 真的把这两个门槛接到了位，且取值区间有实测/几何依据
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _site() -> dict:
|
||||
assert SITE.is_file(), f"找不到站点配置 {SITE}"
|
||||
return yaml.safe_load(SITE.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _left_arm89(site: dict) -> dict:
|
||||
for entry in site["robot"]["robots"]:
|
||||
if str(entry.get("label", "")) == "左臂89":
|
||||
return entry
|
||||
pytest.fail("site.yaml 里找不到 左臂89 的条目")
|
||||
|
||||
|
||||
def test_site_yaml_wires_reacquire_conf_between_false_band_and_real_k0():
|
||||
yolo = _site()["yolo"]
|
||||
# 键必须存在 —— 缺了会静默退回 min_conf（代码默认值兜着，配置看着"改了"却没用）
|
||||
assert "reacquire_conf" in yolo, "site.yaml 没接 reacquire_conf（会退回 min_conf）"
|
||||
v = float(yolo["reacquire_conf"])
|
||||
assert FALSE_BOX_CONF_HI < v < REAL_K0_CONF, (
|
||||
f"reacquire_conf={v} 必须高于假框带 {FALSE_BOX_CONF_HI}、低于真 k0 {REAL_K0_CONF}"
|
||||
)
|
||||
|
||||
|
||||
def test_feature_min_range_sits_between_robot_and_workpiece():
|
||||
"""量程下限必须夹在"机械臂自身"与"工件面"两条实测带之间。"""
|
||||
site = _site()
|
||||
assert "feature_min_range_m" in site["yolo"], "site.yaml 没接 feature_min_range_m"
|
||||
gate = float(site["yolo"]["feature_min_range_m"])
|
||||
assert ROBOT_NEAR_BAND[1] < gate < WORKPIECE_RANGE_BAND[0], (
|
||||
f"量程下限 {gate} 必须高于随臂近带 {ROBOT_NEAR_BAND[1]}、"
|
||||
f"低于工件面 {WORKPIECE_RANGE_BAND[0]}"
|
||||
)
|
||||
|
||||
|
||||
def test_gauge_rod_extends_below_camera_so_near_band_is_the_robot():
|
||||
"""近带为什么会被占满：规尖比相机更靠下 ⇒ 规杆整段都在相机近场。"""
|
||||
arm = _left_arm89(_site())
|
||||
tip = float(arm["gauge_assembly"]["tool_tip_below_flange_m"])
|
||||
doc = yaml.safe_load(HANDEYE.read_text(encoding="utf-8"))
|
||||
cam_z = abs(float(doc["translation_m"][2])) # 相机在法兰 Z 下方的距离
|
||||
assert tip > cam_z + 0.05, (
|
||||
f"规尖 {tip:.3f} 应比相机 {cam_z:.3f} 明显更靠下 —— 否则近带论证不成立"
|
||||
)
|
||||
|
||||
|
||||
def test_feature_min_range_is_not_looser_than_depth_floor():
|
||||
"""不许把量程下限调到解码下限以下 —— 那等于这道守卫没接。"""
|
||||
yolo = _site()["yolo"]
|
||||
assert float(yolo["feature_min_range_m"]) > float(yolo["min_depth_m"])
|
||||
assert float(yolo["feature_min_range_m"]) < float(yolo["max_depth_m"])
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 接线（代码侧）：site.yaml 写了键还不够，必须真的传进 pick_best / 量程判断
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _perception_src() -> str:
|
||||
p = ROOT / "src" / "arm_application" / "arm_application" / "perception_node.py"
|
||||
assert p.is_file(), f"找不到 {p}"
|
||||
return p.read_text(encoding="utf-8")
|
||||
|
||||
|
||||
def test_perception_wires_reacquire_conf_into_pick_best():
|
||||
src = _perception_src()
|
||||
assert "reacquire_conf=self.reacquire_conf" in src, "参数没传进 pick_best，配置等于没接"
|
||||
# 旧接线不许回来：这就是 2026-10-08 出事的写法
|
||||
assert "reacquire_conf=self.min_conf" not in src, "重新捕获门槛又退回 min_conf 了"
|
||||
assert 'declare_parameter("reacquire_conf"' in src
|
||||
|
||||
|
||||
def test_perception_uses_feature_min_range_not_min_depth_for_features():
|
||||
src = _perception_src()
|
||||
assert 'declare_parameter("feature_min_range_m"' in src
|
||||
assert "self.feature_min_range = max(" in src, "量程下限没有取严（可能被调得比 min_depth 还松）"
|
||||
# 特征点量程判断必须用 feature_min_range，不能用回 min_depth
|
||||
assert "self.feature_min_range < z < self.max_depth" in src
|
||||
assert "(meters > self.feature_min_range)" in src
|
||||
assert "(meters2 > self.feature_min_range)" in src
|
||||
assert "min_depth=self.min_depth," not in src, "还有特征测量在用 min_depth"
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
"""A failed child must not turn into a successful ros2 launch result."""
|
||||
import importlib.util
|
||||
from pathlib import Path
|
||||
import sys
|
||||
|
||||
import pytest
|
||||
from launch import LaunchDescription, LaunchService
|
||||
from launch.actions import ExecuteProcess, RegisterEventHandler
|
||||
from launch.event_handlers import OnProcessExit
|
||||
|
||||
spec=importlib.util.spec_from_file_location('pipeline_launch',Path(__file__).resolve().parents[1]/'launch/pipeline.launch.py')
|
||||
module=importlib.util.module_from_spec(spec);spec.loader.exec_module(module)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('child_exit,expected',[(0,0),(2,1)])
|
||||
def test_launch_reports_child_failure(child_exit,expected):
|
||||
child=ExecuteProcess(cmd=[sys.executable,'-c',f'raise SystemExit({child_exit})'])
|
||||
service=LaunchService(noninteractive=True)
|
||||
service.include_launch_description(LaunchDescription([
|
||||
RegisterEventHandler(OnProcessExit(target_action=child,on_exit=module._pipeline_exited)),child]))
|
||||
assert service.run()==expected
|
||||
@@ -0,0 +1,269 @@
|
||||
"""一阶段超时的**归因**：是"感知没给点"还是"给了点没收敛"。
|
||||
|
||||
两者现象完全一样（都是"没锁定"），排查方向却相反：
|
||||
|
||||
* 一个点都没收到 → 感知侧（缺依赖 / 真值节点没起 / 没人发）；
|
||||
* 收到了点没收敛 → 控制与几何侧（工件不在视野、手眼、工作空间）。
|
||||
|
||||
以前共用一句"没锁定（视野里没工件？）"，于是"perception_node 缺
|
||||
ultralytics 早退了"被读成"算法不收敛"，白查半天。这里把这句话钉住。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP
|
||||
from arm_application.pipeline_node import (
|
||||
UNWIND_ROUNDS,
|
||||
close_ready,
|
||||
gripper_wait_timeout_reason,
|
||||
hold_at_insert_z,
|
||||
init_z_refusal,
|
||||
stage1_log_fatal,
|
||||
stage1_timeout_reason,
|
||||
track_lock_z,
|
||||
unwind_round_label,
|
||||
)
|
||||
|
||||
|
||||
def test_gripper_timeout_does_not_call_a_dead_bus_not_in_position():
|
||||
"""2026-10-20：读 0x0201 无应答时只发离线帧，不能报成"夹爪没到位"。
|
||||
|
||||
三种情况说法必须不同，否则会去查夹爪开合，而真正的问题是总线读不回来。
|
||||
"""
|
||||
no_frame = gripper_wait_timeout_reason(
|
||||
"pos", 8.0, None, saw_any=False, saw_offline=False
|
||||
)
|
||||
assert "都没收到" in no_frame and "节点没在发" in no_frame
|
||||
offline = gripper_wait_timeout_reason(
|
||||
"pos", 8.0, None, saw_any=True, saw_offline=True
|
||||
)
|
||||
assert "读不到状态" in offline and "不是夹爪没张开" in offline
|
||||
assert "未到位" not in offline
|
||||
late = gripper_wait_timeout_reason(
|
||||
"pos", 8.0, (0, 120.0), saw_any=True, saw_offline=False
|
||||
)
|
||||
assert "未到位" in late and "grip=0 pos=120" in late
|
||||
|
||||
|
||||
def test_close_at_zero_continues_even_when_firmware_says_arrived():
|
||||
"""2026-10-19：合到 0 时规还在，固件写 grip=1。只认 grip=2 就永远不顺时针。
|
||||
|
||||
停在张开位（pos=200）仍然不能往下走。
|
||||
"""
|
||||
assert close_ready(2, 80) is True
|
||||
assert close_ready(1, 0) is True
|
||||
assert close_ready(1, 40) is True
|
||||
assert close_ready(1, 41) is False
|
||||
assert close_ready(1, 200) is False
|
||||
assert close_ready(0, 0) is False
|
||||
assert close_ready(3, 0) is False
|
||||
|
||||
|
||||
def test_fast_a7_duration_keeps_spline_peak_under_the_drive_cap():
|
||||
"""空回 342° 不能按 0.90 rad/s 的平均速度排。样条峰速会超上限，到位还差几十度。"""
|
||||
from arm_application.pipeline_node import (
|
||||
A7_MAX_SPEED_DEG_S,
|
||||
CUBIC_PEAK_OVER_AVG,
|
||||
NRT_VMAX_RAD_S,
|
||||
a7_move_duration_s,
|
||||
)
|
||||
|
||||
delta = math.radians(169.9 - (-172.0))
|
||||
dur = a7_move_duration_s(delta, A7_MAX_SPEED_DEG_S)
|
||||
peak = CUBIC_PEAK_OVER_AVG * abs(delta) / dur
|
||||
assert peak == pytest.approx(NRT_VMAX_RAD_S, rel=1e-6)
|
||||
assert dur == pytest.approx(abs(delta) / (NRT_VMAX_RAD_S / CUBIC_PEAK_OVER_AVG))
|
||||
# 10°/s 的峰速本来就低于上限，排程不得被放慢。
|
||||
slow = math.radians(137.6)
|
||||
assert a7_move_duration_s(slow, 10.0) == pytest.approx(abs(slow) / math.radians(10.0))
|
||||
|
||||
|
||||
def test_unwind_is_four_rounds_and_labels_cannot_lie():
|
||||
"""首轮咬合之后第三步整段固定 4 轮。轮次标签越界必须红，不能打出第 0 轮或第 5 轮。"""
|
||||
assert UNWIND_ROUNDS == 4
|
||||
assert unwind_round_label(1) == "空回第1/4轮"
|
||||
assert unwind_round_label(4) == "空回第4/4轮"
|
||||
for bad in (0, 5, -1):
|
||||
with pytest.raises(ValueError):
|
||||
unwind_round_label(bad)
|
||||
with pytest.raises(ValueError):
|
||||
unwind_round_label(1, total=3)
|
||||
|
||||
|
||||
def test_insert_hold_does_not_return_to_hover():
|
||||
"""贴面后停住。三阶段开局会抬回悬高；没有传感器，不另设一个 Z。"""
|
||||
assert hold_at_insert_z({"insert_held": True}, skip3=False) is True
|
||||
assert hold_at_insert_z({}, skip3=False) is False
|
||||
assert hold_at_insert_z(None, skip3=False) is False
|
||||
assert hold_at_insert_z({}, skip3=True) is True
|
||||
src = (
|
||||
Path(__file__).resolve().parents[1] / "arm_application" / "stage2_locate_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
hold = src[src.find("# ---- 5) 锁定保持 ----") : src.find("def _linger_hold")]
|
||||
assert 'final["insert_held"] = True' in hold
|
||||
assert 'final["insert_z_source"]' in hold
|
||||
assert "hole_entry_mm" in hold
|
||||
assert "self.insert_gap_m" in hold
|
||||
pipe = (
|
||||
Path(__file__).resolve().parents[1] / "arm_application" / "pipeline_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
run = pipe[pipe.find("def run") :]
|
||||
assert "hold_at_insert_z(" in run
|
||||
assert "不抬回悬高" in run
|
||||
assert "_run_a7_seat(" in run
|
||||
seat = pipe[pipe.find("def _run_a7_seat") : pipe.find("def _on_lock")]
|
||||
marks = (
|
||||
"首轮咬合结束",
|
||||
'_gripper_cli("pos", str(open_permille))',
|
||||
"空回前微抬",
|
||||
"松爪回转",
|
||||
"空回后降回",
|
||||
'_gripper_cli("close")',
|
||||
"夹紧再进",
|
||||
)
|
||||
pos = [seat.find(m) for m in marks]
|
||||
assert all(i >= 0 for i in pos)
|
||||
assert pos == sorted(pos)
|
||||
assert seat.count("A7_MAX_SPEED_DEG_S") == 3
|
||||
# 首轮：出孔用上限，顺时针进孔用 site 的 extreme_speed_deg_s（现场 10°/s）。
|
||||
# 空回循环里的正反转仍是上限，不能把 10°/s 写进循环。
|
||||
assert 'ga["extreme_speed_deg_s"]' in seat
|
||||
first = seat[
|
||||
seat.find("第一步出孔走接口上限") : seat.find("首轮咬合结束：A7 已顺时针转到限位")
|
||||
]
|
||||
assert "A7_MAX_SPEED_DEG_S" in first
|
||||
assert "seat_cw_deg_s" in first
|
||||
assert first.find("A7_MAX_SPEED_DEG_S") < first.find("seat_cw_deg_s")
|
||||
loop = seat[seat.find("for rnd in range(1, UNWIND_ROUNDS + 1)") :]
|
||||
assert "seat_cw_deg_s" not in loop
|
||||
assert loop.count("A7_MAX_SPEED_DEG_S") == 2
|
||||
# 抬和降写在循环里面，各一处。循环外再写一份就会有的轮次不抬。
|
||||
loop_at = seat.find("for rnd in range(1, UNWIND_ROUNDS + 1)")
|
||||
assert loop_at > seat.find("首轮咬合结束")
|
||||
# 抬、降、开、合都在循环里，而且各只调用一次（靠循环重复，不在循环外再写一份）。
|
||||
assert seat.find("_shift_flange_z(lift_mm") > loop_at
|
||||
assert seat.count("_shift_flange_z(lift_mm") == 1
|
||||
assert seat.count('_shift_flange_z(-dz_up') == 1
|
||||
assert seat.count('_gripper_cli("pos"') == 1
|
||||
assert seat.count('_gripper_cli("close")') == 1
|
||||
assert "没走完，后面的轮次不再做" in seat
|
||||
grip = pipe[pipe.find("def _gripper_cli") : pipe.find("def _wait_gripper")]
|
||||
assert "run_gripper.sh" in grip
|
||||
assert '"--cli"' in grip
|
||||
assert "_send_a7_only" in pipe[pipe.find("def _a7_to_limit") : pipe.find("def _run_a7_seat")]
|
||||
wait = pipe[pipe.find("def _wait_gripper") : pipe.find("def _a7_to_limit")]
|
||||
assert "close_ready(" in wait
|
||||
assert 'ga["unwind_open_permille"]' in seat
|
||||
shift = pipe[pipe.find("def _shift_flange_z") : pipe.find("def _arm_short_id")]
|
||||
assert "_vendor_movel_tcp" in shift
|
||||
assert "movel_tcp_z_only" in shift
|
||||
assert 'zone="ZFine"' in shift
|
||||
assert "toler=1" in shift
|
||||
assert "_goto(" not in shift
|
||||
# /robot_description 是锁存话题。VOLATILE 订阅会错过已经发出的 URDF。
|
||||
sub = pipe[pipe.find('"/robot_description"') : pipe.find('"/robot_description"') + 400]
|
||||
assert "TRANSIENT_LOCAL" in sub
|
||||
|
||||
|
||||
def test_zero_points_blames_perception_not_calibration(tmp_path: Path):
|
||||
msg = stage1_timeout_reason(0, 150.0, str(tmp_path / "missing.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(tmp_path: Path):
|
||||
msg = stage1_timeout_reason(123, 150.0, str(tmp_path / "missing.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(tmp_path: Path):
|
||||
log = str(tmp_path / "x.log")
|
||||
msg = stage1_timeout_reason(0, 42.0, log)
|
||||
assert "42s" in msg
|
||||
assert log in msg
|
||||
|
||||
|
||||
def test_real_arm_locks_track_height_at_hover():
|
||||
"""真机 89：悬停时规尖已经离面约 75mm，跟踪锁在悬停高，对孔前不再抬。"""
|
||||
z = track_lock_z(0.320, 0.223, 0.022)
|
||||
assert z == pytest.approx(0.320)
|
||||
|
||||
|
||||
def test_sim_still_lifts_when_hover_barely_clears_the_part():
|
||||
"""仿真悬停只留约 2mm，不抬就会在跟踪时刮到工件。"""
|
||||
z = track_lock_z(0.657, 0.243, 0.412)
|
||||
assert z == pytest.approx(0.707)
|
||||
|
||||
|
||||
def test_init_z_refuses_a_value_that_would_enter_the_part():
|
||||
"""用户指定的初始高度会钻进工件时必须拒绝，不许悄悄抬高。"""
|
||||
why = init_z_refusal(0.20, 0.2245, 0.022, 0.002, 0.18, 0.65)
|
||||
assert "进入工件" in why
|
||||
assert "不会悄悄抬高" in why
|
||||
assert init_z_refusal(0.320, 0.2245, 0.022, 0.002, 0.18, 0.65) == ""
|
||||
assert "工作盒" in init_z_refusal(0.10, 0.2245, 0.022, 0.002, 0.18, 0.65)
|
||||
assert "未知" in init_z_refusal(0.320, 0.0, 0.022, 0.002, 0.18, 0.65)
|
||||
|
||||
|
||||
def test_movel_ipc_roundtrip():
|
||||
lib = __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts" / "lib"
|
||||
import sys
|
||||
if str(lib) not in sys.path:
|
||||
sys.path.insert(0, str(lib))
|
||||
from flexiv_movel_ipc import format_request, parse_done
|
||||
|
||||
line = format_request(7, 0.4, -0.2, 0.32, True, 0.05)
|
||||
assert line.startswith("7 0.400000 -0.200000 0.320000 1 0.0500")
|
||||
assert "ZFine" not in line
|
||||
fine = format_request(8, 0.7, -0.1, 0.02, False, 0.01, "ZFine", 1)
|
||||
assert fine.strip().endswith("ZFine 1")
|
||||
try:
|
||||
format_request(8, 0.7, -0.1, 0.02, False, 0.01, "Z50", 3)
|
||||
except ValueError as exc:
|
||||
assert "ZFine" in str(exc)
|
||||
else:
|
||||
raise AssertionError("默认过渡区 Z50 不许写进微升降请求")
|
||||
cpp = (
|
||||
Path(__file__).resolve().parents[3]
|
||||
/ "src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp"
|
||||
).read_text(encoding="utf-8")
|
||||
assert 'movel_args.emplace("zoneRadius"' in cpp
|
||||
assert 'if (!zone.empty())' in cpp
|
||||
assert "微升降只接受过渡区 ZFine 和到位等级 1" in cpp
|
||||
assert parse_done("7 ok\n") == (7, True, "")
|
||||
assert parse_done("7 fail mode\n") == (7, False, "mode")
|
||||
assert parse_done("nope\n") is None
|
||||
|
||||
|
||||
def test_movel_gave_up_in_log_is_fatal_not_a_vision_timeout(tmp_path: Path):
|
||||
"""2026-10-16 17:27：MoveL 控制模式失败后空等 150s 被读成粗定位卡住。"""
|
||||
p = tmp_path / "stage1.log"
|
||||
p.write_text(f"foo\n{D435_MOVEL_GAVE_UP} —— 一阶段中止\n", encoding="utf-8")
|
||||
why = stage1_log_fatal(str(p))
|
||||
assert why
|
||||
assert "控制模式" in why
|
||||
assert "不是视野" in why
|
||||
msg = stage1_timeout_reason(80, 150.0, str(p))
|
||||
assert "控制模式" in msg
|
||||
assert "没收敛" not in msg
|
||||
assert stage1_log_fatal(str(tmp_path / "nope.log")) == ""
|
||||
|
||||
|
||||
def test_missing_geometry_does_not_pretend_the_hover_is_clear():
|
||||
"""工具长或工件面缺失时必须抬升。缺数还锁在悬停高，等于没检查。"""
|
||||
assert track_lock_z(0.320, 0.0, 0.022) == pytest.approx(0.370)
|
||||
assert track_lock_z(0.320, 0.223, 0.0) == pytest.approx(0.370)
|
||||
@@ -0,0 +1,292 @@
|
||||
"""pose 模型支持（k0 跟踪 / k1,k2 对孔）的测试。
|
||||
|
||||
刻意**不依赖仓库里的模型二进制**（`models/**/*.onnx` 都不进 git）：
|
||||
ONNX 用 `onnx.helper` 现场造，解码用合成张量。所以纯逻辑都能离线跑，
|
||||
GPU / TensorRT 都不需要 —— engine 那条路只能真机验（`weights.sh doctor --check-engines`）。
|
||||
|
||||
要挡住的那类 bug（2026-09-29 实测踩到的）：
|
||||
ultralytics 按 `task="detect"` 去解 pose 的 14 通道输出，会吐 300 个垃圾框，
|
||||
`conf` 直接是坐标值（600+）—— 现场表现是"跟踪锁到空气上"，在真机上很危险。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.hole_detect_hough import _keypoint_holes
|
||||
from arm_application.model_labels import SOURCE_SIDECAR, LabelSet
|
||||
from arm_application.pose_detector import (
|
||||
PoseDetector,
|
||||
is_pose_model,
|
||||
keypoint_roles,
|
||||
letterbox,
|
||||
nms_xyxy,
|
||||
read_pose_info,
|
||||
)
|
||||
from arm_application.yolo_detector import Detection
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# fixture 工具
|
||||
# --------------------------------------------------------------------------
|
||||
def _make_onnx(path, metadata):
|
||||
"""造一个只有 metadata 的最小 ONNX（图内容无所谓，只测 metadata 解析）。"""
|
||||
pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）")
|
||||
import onnx
|
||||
from onnx import TensorProto, helper
|
||||
|
||||
inp = helper.make_tensor_value_info("images", TensorProto.FLOAT, [1, 3, 8, 8])
|
||||
out = helper.make_tensor_value_info("output0", TensorProto.FLOAT, [1, 14, 4])
|
||||
graph = helper.make_graph(
|
||||
[helper.make_node("Identity", ["images"], ["output0"])], "g", [inp], [out]
|
||||
)
|
||||
model = helper.make_model(graph)
|
||||
model.opset_import[0].version = 13
|
||||
for k, v in metadata.items():
|
||||
model.metadata_props.add(key=k, value=v)
|
||||
onnx.save(model, str(path))
|
||||
return path
|
||||
|
||||
|
||||
def _bare_detector(*, kpt_count=3, conf=0.25, iou=0.45, max_det=20, class_names=None):
|
||||
"""只装解码需要的属性 —— 绕开 __init__ 里的 engine 加载（要 GPU）。"""
|
||||
det = object.__new__(PoseDetector)
|
||||
det.kpt_count = kpt_count
|
||||
det.conf = conf
|
||||
det.iou = iou
|
||||
det.max_det = max_det
|
||||
det.class_ids = None
|
||||
det.class_names = class_names
|
||||
det._labels = LabelSet(names={0: "workpiece"}, source=SOURCE_SIDECAR, path=None)
|
||||
return det
|
||||
|
||||
|
||||
def _raw(boxes, kpts, confs, n_anchor=4, nc=1):
|
||||
"""拼一个 (1, 4+nc+3K, A) 的合成输出。boxes=xywh，kpts=[[(x,y,c),...], ...]"""
|
||||
k = kpts.shape[1]
|
||||
ch = 4 + nc + 3 * k
|
||||
o = np.zeros((ch, n_anchor), dtype=np.float32)
|
||||
n = boxes.shape[0]
|
||||
o[0:4, :n] = boxes.T
|
||||
o[4:4 + nc, :n] = confs[None, :] if nc > 1 else confs[None, :]
|
||||
o[4 + nc:, :n] = kpts.reshape(n, -1).T
|
||||
return o[None]
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 模型识别：从旁挂 .onnx 的 metadata 判断是不是 pose
|
||||
# --------------------------------------------------------------------------
|
||||
def test_pose_model_is_recognized(tmp_path):
|
||||
_make_onnx(
|
||||
tmp_path / "m.onnx",
|
||||
{
|
||||
"task": "pose",
|
||||
"kpt_shape": "[3, 3]",
|
||||
"kpt_names": "{0: ['0', '1', '2']}",
|
||||
"names": "{0: 'workpiece'}",
|
||||
},
|
||||
)
|
||||
info = read_pose_info(tmp_path / "m.onnx")
|
||||
assert info is not None
|
||||
assert info["kpt_count"] == 3
|
||||
assert info["names"] == {0: "workpiece"}
|
||||
assert is_pose_model(tmp_path / "m.onnx")
|
||||
|
||||
|
||||
def test_detect_model_is_not_pose(tmp_path):
|
||||
_make_onnx(tmp_path / "d.onnx", {"task": "detect", "names": "{0: 'workpiece'}"})
|
||||
assert read_pose_info(tmp_path / "d.onnx") is None
|
||||
assert not is_pose_model(tmp_path / "d.onnx")
|
||||
|
||||
|
||||
def test_pose_without_kpt_shape_is_not_guessed(tmp_path):
|
||||
"""没有 kpt_shape 就别猜关键点个数 —— 猜错是"静默错位"，比报错难查得多。"""
|
||||
_make_onnx(tmp_path / "p.onnx", {"task": "pose", "names": "{0: 'workpiece'}"})
|
||||
assert read_pose_info(tmp_path / "p.onnx") is None
|
||||
|
||||
|
||||
def test_engine_reads_pose_info_from_sibling_onnx(tmp_path):
|
||||
"""engine 没有 metadata（TRT 10 Python API 就没有接口），要靠同名的 .onnx。"""
|
||||
_make_onnx(
|
||||
tmp_path / "m.onnx", {"task": "pose", "kpt_shape": "[3, 3]", "names": "{0: 'workpiece'}"}
|
||||
)
|
||||
(tmp_path / "m.engine").write_bytes(b"not a real engine")
|
||||
assert is_pose_model(tmp_path / "m.engine")
|
||||
|
||||
|
||||
def test_pose_info_missing_onnx_returns_none(tmp_path):
|
||||
(tmp_path / "m.engine").write_bytes(b"not a real engine")
|
||||
assert read_pose_info(tmp_path / "m.engine") is None
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# letterbox：坐标还原全靠它，错了整幅图的框都会偏
|
||||
# --------------------------------------------------------------------------
|
||||
def test_letterbox_preserves_aspect_and_reports_padding():
|
||||
img = np.zeros((480, 848, 3), dtype=np.uint8)
|
||||
out, gain, (dw, dh) = letterbox(img, 640)
|
||||
assert out.shape == (640, 640, 3)
|
||||
# 848x480 → gain = 640/848；等比缩到 640x362，纵向补 139
|
||||
assert gain == pytest.approx(640 / 848, rel=1e-6)
|
||||
assert dw == pytest.approx(0.0, abs=0.5)
|
||||
assert dh == pytest.approx(139.0, abs=1.0)
|
||||
|
||||
|
||||
def test_letterbox_square_input_is_untouched():
|
||||
img = np.zeros((640, 640, 3), dtype=np.uint8)
|
||||
out, gain, (dw, dh) = letterbox(img, 640)
|
||||
assert out.shape == (640, 640, 3)
|
||||
assert gain == pytest.approx(1.0)
|
||||
assert (dw, dh) == (0.0, 0.0)
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# NMS
|
||||
# --------------------------------------------------------------------------
|
||||
def test_nms_suppresses_overlap_and_keeps_highest():
|
||||
boxes = np.array([[0, 0, 100, 100], [5, 5, 105, 105], [500, 500, 600, 600]], dtype=float)
|
||||
scores = np.array([0.9, 0.8, 0.7])
|
||||
keep = nms_xyxy(boxes, scores, 0.5, max_det=10)
|
||||
assert list(keep) == [0, 2]
|
||||
|
||||
|
||||
def test_nms_respects_max_det():
|
||||
boxes = np.array([[0, 0, 10, 10], [100, 100, 110, 110], [200, 200, 210, 210]], dtype=float)
|
||||
assert len(nms_xyxy(boxes, np.array([0.9, 0.8, 0.7]), 0.5, max_det=2)) == 2
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 解码：框 + 关键点，并去 letterbox 还原到原图
|
||||
# --------------------------------------------------------------------------
|
||||
def test_decode_returns_box_and_keypoints_unletterboxed():
|
||||
det = _bare_detector()
|
||||
boxes = np.array([[320.0, 320.0, 100.0, 80.0]])
|
||||
kpts = np.array([[[300.0, 300.0, 0.9], [360.0, 300.0, 0.8], [280.0, 310.0, 0.7]]])
|
||||
raw = _raw(boxes, kpts, np.array([0.90]))
|
||||
|
||||
out = det._decode(raw, gain=0.5, dw=10.0, dh=20.0)
|
||||
assert len(out) == 1
|
||||
d = out[0]
|
||||
assert d.cls_name == "workpiece"
|
||||
assert d.conf == pytest.approx(0.90, abs=1e-5)
|
||||
# ((cx±w/2) - dw) / gain
|
||||
assert list(np.round(d.xyxy, 3)) == [520.0, 520.0, 720.0, 680.0]
|
||||
assert d.kpts.shape == (3, 3)
|
||||
assert list(np.round(d.kpts[0][:2], 3)) == [580.0, 560.0]
|
||||
assert d.kpts[0][2] == pytest.approx(0.9, abs=1e-5)
|
||||
assert list(np.round(d.kpts[1][:2], 3)) == [700.0, 560.0]
|
||||
assert list(np.round(d.kpts[2][:2], 3)) == [540.0, 580.0]
|
||||
|
||||
|
||||
def test_decode_drops_low_conf_anchors():
|
||||
det = _bare_detector(conf=0.5)
|
||||
boxes = np.array([[100.0, 100.0, 20.0, 20.0], [400.0, 400.0, 20.0, 20.0]])
|
||||
kpts = np.zeros((2, 3, 3), dtype=float)
|
||||
raw = _raw(boxes, kpts, np.array([0.9, 0.1]))
|
||||
out = det._decode(raw, gain=1.0, dw=0.0, dh=0.0)
|
||||
assert len(out) == 1
|
||||
assert out[0].conf == pytest.approx(0.9, abs=1e-5)
|
||||
|
||||
|
||||
def test_decode_raises_when_channels_do_not_match_kpt_count():
|
||||
"""通道数和 kpt_shape 对不上时必须报错，不能默默解出一堆垃圾。"""
|
||||
det = _bare_detector(kpt_count=3)
|
||||
raw = np.zeros((1, 6, 4), dtype=np.float32) # 6 通道装不下 3 个关键点
|
||||
with pytest.raises(RuntimeError, match="通道数不对"):
|
||||
det._decode(raw, gain=1.0, dw=0.0, dh=0.0)
|
||||
|
||||
|
||||
def test_decode_of_detect_style_output_would_be_garbage():
|
||||
"""回归：把 pose 的 14 通道输出当检测解 → conf 会变成 600+ 的坐标值。
|
||||
|
||||
这条测的是"我们**没有**走那条路"：同一份合成输出，按 pose 解出来的 conf
|
||||
必须在 (0,1]，绝不能是坐标量级。
|
||||
"""
|
||||
det = _bare_detector()
|
||||
boxes = np.array([[600.0, 640.0, 100.0, 80.0]])
|
||||
kpts = np.array([[[610.0, 300.0, 0.9], [620.0, 300.0, 0.8], [630.0, 310.0, 0.7]]])
|
||||
raw = _raw(boxes, kpts, np.array([0.9]))
|
||||
out = det._decode(raw, gain=1.0, dw=0.0, dh=0.0)
|
||||
assert len(out) == 1
|
||||
assert 0.0 < out[0].conf <= 1.0
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# k1/k2：模型语义顺序，**不按图像 x 交换**
|
||||
# --------------------------------------------------------------------------
|
||||
def _det_with_kpts(kpts, conf=0.9):
|
||||
return Detection(
|
||||
xyxy=np.array([300.0, 300.0, 500.0, 400.0]),
|
||||
conf=conf,
|
||||
cls_id=0,
|
||||
cls_name="workpiece",
|
||||
kpts=np.asarray(kpts, dtype=float),
|
||||
)
|
||||
|
||||
|
||||
def _kw(**over):
|
||||
cfg = dict(conf_thr=0.25, min_kconf=0.30, radius_px=12.0, min_dist_px=15.0)
|
||||
cfg.update(over)
|
||||
return cfg
|
||||
|
||||
|
||||
def test_keypoint_holes_keeps_model_semantic_order():
|
||||
"""k1 在画面右边、k2 在左边时，**不能**交换。
|
||||
|
||||
Hough 路径的 k1/k2 是"图像左/右"（位置定义）；pose 的 k1/k2 是训练时标注的
|
||||
固定物理孔。按 x 交换会让 target_hole=k1 在工件转个角度后指向**另一个**孔 ——
|
||||
现场表现为"有时对左孔、有时对右孔"，非常难查。
|
||||
"""
|
||||
det = _det_with_kpts([[400, 350, 1.0], [470, 340, 1.0], [330, 345, 1.0]])
|
||||
out = _keypoint_holes([det], **_kw())
|
||||
assert out is not None
|
||||
(k1x, k1y), (k2x, k2y), r1, r2 = out
|
||||
assert (k1x, k1y) == (470.0, 340.0), "k1 必须还是模型的 k1（画面右）"
|
||||
assert (k2x, k2y) == (330.0, 345.0), "k2 必须还是模型的 k2（画面左）"
|
||||
assert (r1, r2) == (12.0, 12.0)
|
||||
|
||||
|
||||
def test_keypoint_holes_rejects_low_confidence():
|
||||
det = _det_with_kpts([[400, 350, 1.0], [470, 340, 0.10], [330, 345, 0.05]])
|
||||
assert _keypoint_holes([det], **_kw()) is None
|
||||
|
||||
|
||||
def test_keypoint_holes_rejects_coincident_points():
|
||||
det = _det_with_kpts([[400, 350, 1.0], [401, 350, 1.0], [402, 350, 1.0]])
|
||||
assert _keypoint_holes([det], **_kw(min_dist_px=15.0)) is None
|
||||
|
||||
|
||||
def test_keypoint_holes_returns_none_for_plain_detection():
|
||||
"""普通检测模型没有 kpts → 必须返回 None，让 Hough 老路径接手。"""
|
||||
det = Detection(xyxy=np.array([0.0, 0.0, 10.0, 10.0]), conf=0.9, cls_id=0, cls_name="workpiece")
|
||||
assert _keypoint_holes([det], **_kw()) is None
|
||||
|
||||
|
||||
def test_keypoint_holes_skips_low_conf_detection():
|
||||
det = _det_with_kpts([[400, 350, 1.0], [470, 340, 1.0], [330, 345, 1.0]], conf=0.05)
|
||||
assert _keypoint_holes([det], **_kw(conf_thr=0.25)) is None
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------
|
||||
# 角色约定
|
||||
# --------------------------------------------------------------------------
|
||||
def test_keypoint_roles_defaults_to_k0_track_k1k2_holes():
|
||||
assert keypoint_roles(3) == (0, (1, 2))
|
||||
|
||||
|
||||
def test_keypoint_roles_handles_models_with_fewer_points():
|
||||
assert keypoint_roles(1) == (0, ())
|
||||
assert keypoint_roles(2) == (0, (1,))
|
||||
|
||||
|
||||
def test_resolve_weights_skips_engine_when_tensorrt_missing(tmp_path, monkeypatch):
|
||||
from arm_application import yolo_detector as yd
|
||||
|
||||
onnx = tmp_path / "gongjian0929.onnx"
|
||||
engine = tmp_path / "gongjian0929.engine"
|
||||
onnx.write_bytes(b"onnx")
|
||||
engine.write_bytes(b"engine")
|
||||
monkeypatch.setattr(yd, "tensorrt_importable", lambda: False)
|
||||
assert yd.resolve_weights(onnx, "auto") == onnx
|
||||
assert yd.resolve_weights(onnx, "tensorrt") == onnx
|
||||
@@ -0,0 +1,167 @@
|
||||
"""`TolerantNode` 的回归锚点 + 「谁都不许直接用 rclpy 的 Node」守卫。
|
||||
|
||||
背景（2026-09-28 真机落地实测）：现场 `config/site.yaml` 的
|
||||
``cameras.d435.ae_limit_us: 8000`` 是 YAML **整数**，节点里声明的是 double，
|
||||
rclpy 严格较类型 → 顶置 D435 节点**启动即崩**，报错里还完全不提 site.yaml：
|
||||
|
||||
InvalidParameterTypeException: Trying to set parameter 'ae_limit_us'
|
||||
to '8000' of type 'INTEGER', expecting type 'DOUBLE'
|
||||
|
||||
这组测试钉住三件事：
|
||||
1. `TolerantNode` 对「数字宽窄不一致」是宽容的（8000 / 8000.0 / "8000" 都能用）；
|
||||
2. 宽容**只**发生在数值族，不是"什么都收"；
|
||||
3. 应用层不再有直接继承 `rclpy.node.Node` 的节点类 —— 否则这个坑会悄悄回来。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
rclpy = pytest.importorskip("rclpy")
|
||||
|
||||
from rclpy.node import Node # noqa: E402
|
||||
from rclpy.parameter import Parameter # noqa: E402
|
||||
|
||||
from arm_application.ros_node import TolerantNode, param_as_str # noqa: E402
|
||||
|
||||
APP_DIR = Path(__file__).resolve().parents[1] / "arm_application"
|
||||
|
||||
|
||||
@pytest.fixture()
|
||||
def ros():
|
||||
rclpy.init()
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
def _overrides(**kwargs):
|
||||
return [Parameter(k, value=v) for k, v in kwargs.items()]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 1. 数值族宽容
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_float_param_accepts_integer_override(ros):
|
||||
"""site.yaml 写 8000（整数）→ double 参数必须能起来，并按 8000.0 用。"""
|
||||
node = TolerantNode("t_float", parameter_overrides=_overrides(ae_limit_us=8000))
|
||||
try:
|
||||
node.declare_parameter("ae_limit_us", 3000.0)
|
||||
assert float(node.get_parameter("ae_limit_us").value) == 8000.0
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
def test_int_param_accepts_float_override(ros):
|
||||
"""反过来也要能起：site.yaml 写 848.0 → int 参数（宽度）。"""
|
||||
node = TolerantNode("t_int", parameter_overrides=_overrides(width=848.0))
|
||||
try:
|
||||
node.declare_parameter("width", 1280)
|
||||
assert int(node.get_parameter("width").value) == 848
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
def test_bool_param_accepts_int_override(ros):
|
||||
"""site.yaml 写 1 / 0（YAML 布尔的一种常见手写）→ bool 参数。"""
|
||||
node = TolerantNode("t_bool", parameter_overrides=_overrides(emitter_enabled=1))
|
||||
try:
|
||||
node.declare_parameter("emitter_enabled", False)
|
||||
assert bool(node.get_parameter("emitter_enabled").value) is True
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
def test_number_array_param_accepts_integer_array(ros):
|
||||
"""`workspace_xyz_min: [0, -1, 0]` 这种整数字面量列表 → DOUBLE_ARRAY。"""
|
||||
node = TolerantNode("t_arr", parameter_overrides=_overrides(ws=[0, -1, 0]))
|
||||
try:
|
||||
node.declare_parameter("ws", [0.1, -0.5, 0.2])
|
||||
assert [float(v) for v in node.get_parameter("ws").value] == [0.0, -1.0, 0.0]
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
def test_param_as_str_accepts_numeric_override(ros):
|
||||
"""SN 写成不带引号的 26610799（YAML 整数）→ 字符串参数。"""
|
||||
node = TolerantNode("t_str", parameter_overrides=_overrides(serial_number=26610799))
|
||||
try:
|
||||
assert param_as_str(node, "serial_number", "") == "26610799"
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
def test_missing_override_falls_back_to_default(ros):
|
||||
node = TolerantNode("t_default")
|
||||
try:
|
||||
node.declare_parameter("ae_limit_us", 3000.0)
|
||||
assert float(node.get_parameter("ae_limit_us").value) == 3000.0
|
||||
assert param_as_str(node, "camera_serial", "abc") == "abc"
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 2. 「为什么需要这个子类」——防止有人把宽容读成"本来就没问题"
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_plain_rclpy_node_really_is_strict(ros):
|
||||
"""原生 Node 遇到宽窄不一致会抛异常。
|
||||
|
||||
这条测试是**原因**的记录：如果哪天 rclpy 改了行为，这条会红，
|
||||
我们就知道 `TolerantNode` 已经没有存在意义（可以删掉，而不是留着当谜语）。
|
||||
"""
|
||||
from rclpy.exceptions import InvalidParameterTypeException
|
||||
|
||||
node = Node("t_strict", parameter_overrides=_overrides(ae_limit_us=8000))
|
||||
try:
|
||||
with pytest.raises(InvalidParameterTypeException):
|
||||
node.declare_parameter("ae_limit_us", 3000.0)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 3. 守卫：应用层不许再直接继承 rclpy 的 Node
|
||||
# ---------------------------------------------------------------------------
|
||||
def _direct_node_subclasses() -> list[str]:
|
||||
hits: list[str] = []
|
||||
for path in sorted(APP_DIR.glob("*.py")):
|
||||
if path.name == "ros_node.py":
|
||||
continue # TolerantNode 自己就是干这个的
|
||||
tree = ast.parse(path.read_text(encoding="utf-8"))
|
||||
imports_rclpy_node = False
|
||||
for node in ast.walk(tree):
|
||||
if isinstance(node, ast.ImportFrom) and (node.module or "") == "rclpy.node":
|
||||
if any(alias.name == "Node" for alias in node.names):
|
||||
imports_rclpy_node = True
|
||||
if not imports_rclpy_node:
|
||||
continue
|
||||
for node in ast.walk(tree):
|
||||
if isinstance(node, ast.ClassDef) and any(
|
||||
isinstance(b, ast.Name) and b.id == "Node" for b in node.bases
|
||||
):
|
||||
hits.append(f"{path.name}::{node.name}")
|
||||
return hits
|
||||
|
||||
|
||||
def test_no_node_subclasses_rclpy_node_directly():
|
||||
hits = _direct_node_subclasses()
|
||||
assert not hits, (
|
||||
"这些节点类直接继承了 rclpy 的 Node，会重新踩「site.yaml 写 8000、"
|
||||
"节点要 double」的启动崩溃：\n " + "\n ".join(hits) +
|
||||
"\n改法：继承 arm_application.ros_node.TolerantNode。"
|
||||
)
|
||||
|
||||
|
||||
def test_tolerant_node_is_the_thing_nodes_subclass():
|
||||
"""至少得有人真的在用它，否则这条规则只是摆设。"""
|
||||
users = [
|
||||
p.name
|
||||
for p in sorted(APP_DIR.glob("*.py"))
|
||||
if "TolerantNode" in p.read_text(encoding="utf-8") and p.name != "ros_node.py"
|
||||
]
|
||||
assert len(users) >= 10, f"用 TolerantNode 的节点太少了：{users}"
|
||||
@@ -0,0 +1,149 @@
|
||||
"""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 os
|
||||
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}"
|
||||
|
||||
|
||||
def _flatten_for(sn: str) -> dict:
|
||||
import export_site_env # type: ignore
|
||||
|
||||
previous = os.environ.get("ROBOT_SN")
|
||||
os.environ["ROBOT_SN"] = sn
|
||||
try:
|
||||
return export_site_env.flatten(export_site_env.load_site())
|
||||
finally:
|
||||
if previous is None:
|
||||
os.environ.pop("ROBOT_SN", None)
|
||||
else:
|
||||
os.environ["ROBOT_SN"] = previous
|
||||
|
||||
|
||||
def test_live_left_gripper_does_not_auto_home(arm_section_keys):
|
||||
"""左臂现场 auto_init 是 false。拉起驱动若把它弄成 true，会写 0xA5 回零。"""
|
||||
flat = _flatten_for("89")
|
||||
assert flat["SITE_GRIP_AUTO_INIT"] == "false"
|
||||
assert flat["SITE_GRIP_PORT"] == "/dev/ttyUSB0"
|
||||
|
||||
|
||||
def test_auto_init_override_is_not_ignored(arm_section_keys, tmp_path):
|
||||
"""把现场改成 true 必须真的变成 true。否则上面那条是 exporter 写死的假通过。"""
|
||||
import export_site_env # type: ignore
|
||||
|
||||
doc = yaml.safe_load((ROOT / "config" / "site.yaml").read_text(encoding="utf-8"))
|
||||
changed = False
|
||||
for robot in robot_entries(doc):
|
||||
if str(robot.get("sn") or "").endswith("063989"):
|
||||
robot["gripper"]["auto_init"] = True
|
||||
changed = True
|
||||
assert changed
|
||||
cfg = tmp_path / "site.yaml"
|
||||
cfg.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
previous_cfg = os.environ.get("SITE_CONFIG")
|
||||
previous_sn = os.environ.get("ROBOT_SN")
|
||||
os.environ["SITE_CONFIG"] = str(cfg)
|
||||
os.environ["ROBOT_SN"] = "89"
|
||||
try:
|
||||
flat = export_site_env.flatten(export_site_env.load_site())
|
||||
finally:
|
||||
if previous_cfg is None:
|
||||
os.environ.pop("SITE_CONFIG", None)
|
||||
else:
|
||||
os.environ["SITE_CONFIG"] = previous_cfg
|
||||
if previous_sn is None:
|
||||
os.environ.pop("ROBOT_SN", None)
|
||||
else:
|
||||
os.environ["ROBOT_SN"] = previous_sn
|
||||
assert flat["SITE_GRIP_AUTO_INIT"] == "true"
|
||||
@@ -0,0 +1,50 @@
|
||||
"""Exercise Action failure handling with fake futures; no ROS nodes or hardware."""
|
||||
from concurrent.futures import Future
|
||||
from types import SimpleNamespace
|
||||
from unittest.mock import Mock, patch
|
||||
|
||||
import pytest
|
||||
from trajectory_msgs.msg import JointTrajectory
|
||||
from arm_application.stage2_locate_node import Stage2Locate
|
||||
|
||||
|
||||
def done(value):
|
||||
future=Future();future.set_result(value);return future
|
||||
|
||||
|
||||
def fake_node(reply):
|
||||
clock=[0.]
|
||||
def spin(dt):clock[0]+=dt
|
||||
logger=Mock()
|
||||
node=SimpleNamespace(_cli=SimpleNamespace(send_goal_async=Mock(return_value=reply)),
|
||||
_spin=spin,get_logger=lambda:logger)
|
||||
return node,clock
|
||||
|
||||
|
||||
@pytest.mark.parametrize('status,code,expected',[(4,0,True),(5,0,False),(6,0,False),(4,-1,False)])
|
||||
def test_only_completed_success_is_success(status,code,expected):
|
||||
handle=SimpleNamespace(accepted=True,get_result_async=lambda:done(
|
||||
SimpleNamespace(status=status,result=SimpleNamespace(error_code=code,error_string='test'))))
|
||||
node,clock=fake_node(done(handle))
|
||||
with patch('arm_application.stage2_locate_node.time.monotonic',lambda:clock[0]):
|
||||
assert Stage2Locate._send_action(node,JointTrajectory(),.2) is expected
|
||||
|
||||
|
||||
def test_acceptance_timeout_never_replays_goal_and_cancels_late_acceptance():
|
||||
pending=Future();node,clock=fake_node(pending)
|
||||
with patch('arm_application.stage2_locate_node.time.monotonic',lambda:clock[0]):
|
||||
with pytest.raises(RuntimeError,match='禁止重发'):
|
||||
Stage2Locate._send_action(node,JointTrajectory(),.2)
|
||||
assert node._cli.send_goal_async.call_count==1
|
||||
handle=SimpleNamespace(accepted=True,cancel_goal_async=Mock())
|
||||
pending.set_result(handle)
|
||||
handle.cancel_goal_async.assert_called_once()
|
||||
|
||||
|
||||
def test_execution_timeout_cancels_instead_of_continuing():
|
||||
handle=SimpleNamespace(accepted=True,get_result_async=lambda:Future(),cancel_goal_async=Mock())
|
||||
node,clock=fake_node(done(handle))
|
||||
with patch('arm_application.stage2_locate_node.time.monotonic',lambda:clock[0]):
|
||||
with pytest.raises(RuntimeError,match='中止当前流程'):
|
||||
Stage2Locate._send_action(node,JointTrajectory(),.2)
|
||||
handle.cancel_goal_async.assert_called_once()
|
||||
@@ -0,0 +1,287 @@
|
||||
"""粗居中的两条误检守卫：**框被画面切** + **目标点离谱/盒外**。
|
||||
|
||||
为什么单独立一条（2026-10-08 真机 arm 89，二阶段 exit=4）
|
||||
--------------------------------------------------------
|
||||
``/tmp/run_all3.log`` 原话（一阶段锁定 ✓、二阶段悬停到位 ✓，死在这一步）::
|
||||
|
||||
悬停到位: 法兰 Z=0.320m（目标 0.320，差 +0mm） 残余倾角 0.37deg
|
||||
snap_coarse_0: 工件 bbox=(0, 142, 97, 222) 中心=(48,182)
|
||||
base=(0.280,-0.747)[深度] 夹爪=(0.336,-0.543) 偏差=211.5mm
|
||||
目标超出工作盒，已截断: (0.280,-0.747,0.320) → (0.280,-0.545,0.320)
|
||||
plan coarse Δ=(-56,-2,-0)mm ← 把臂带离工件 56mm
|
||||
snap_coarse_1: 工件 bbox=[0, 0, 847, 479]（整幅画面）偏差=55.9mm
|
||||
snap_fine_0..8: 两孔基座间距 105.8 / 210.0 / 213.2 / …mm
|
||||
（期望 32–85mm）丢弃本拍
|
||||
二阶段失败 exit=4，停在当前位姿
|
||||
|
||||
工件其实在 (0.5,-0.12) 一带。两个缺陷叠在一起：
|
||||
|
||||
1. ``bbox=(0,142,97,222)`` 的 ``x0=0`` —— 被画面**左边缘切掉**的半框。框中心
|
||||
根本不在工件上，深度反投影因此给出盒外的 (0.280,-0.747)；
|
||||
2. 这个盒外目标**没有被判误检**，而是"截断到盒边"后照样移动 —— 越界只被夹到
|
||||
盒边，臂还是走了。这就是规矩里禁的"宽容降级"：把"没检查"伪装成"检查通过"。
|
||||
一挪错，精定位就再也看不到 k1/k2，而流程把它报成"孔检测失败"（exit=4）。
|
||||
|
||||
修法：``_bbox_on_border`` 弃用贴边框、``_coarse_target_reject`` 在**移动前**
|
||||
否掉盒外/离谱目标（判误检 ⇒ 原地不动，交给精定位）；以及 YOLO 无可信框但精定位
|
||||
已看到 k1/k2 时**真的不移动**（旧代码嘴上说"跳过居中"却仍然算偏差横移）。
|
||||
|
||||
这些用例全是**离线**的：两个被判的函数都是纯逻辑，用打桩对象即可，不建 ROS 实体
|
||||
（与 ``test_stage2_descent_clamp`` 同一套做法）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any, List, Optional
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
import yaml
|
||||
|
||||
from arm_application.stage2_locate_node import Stage2Locate
|
||||
|
||||
# 测试在 src/<包>/test/ 下 → 仓库根是 parents[3]
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
SITE = ROOT / "config" / "site.yaml"
|
||||
|
||||
# ---- 真机实测数字（见本文件头，故意写死：它们是被测事实，不是配置）----
|
||||
FRAME_W, FRAME_H = 848, 480 # D405 彩色图尺寸（bbox [0,0,847,479] 可反推）
|
||||
BAD_BBOX_CLIPPED = (0, 142, 97, 222) # 左边缘切掉的半框 —— 事故那一帧
|
||||
DEGENERATE_BBOX = (0, 0, 847, 479) # 孔检测器兜底给的整幅画面
|
||||
GOOD_BBOX = (300, 150, 520, 330) # 一个正常的、不贴边的框
|
||||
BAD_TARGET_BASE = np.array([0.280, -0.747, 0.320]) # 那帧反投影出来的点（盒外）
|
||||
GAUGE_XY = np.array([0.336, -0.543]) # 那帧的夹爪轴线 XY
|
||||
BAD_ERR_M = 0.2115 # 日志里的"偏差=211.5mm"
|
||||
CLAMPED_Y = -0.545 # 日志里的"截断"结果
|
||||
# 工作盒（site.yaml: robots[].servo.workspace_xyz_*）
|
||||
WS_MIN = np.array([0.08, -0.55, 0.18])
|
||||
WS_MAX = np.array([0.85, 0.55, 0.65])
|
||||
|
||||
|
||||
class _StubLogger:
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def _add(self, msg: str) -> None:
|
||||
self._sink.append(str(msg))
|
||||
|
||||
info = _add
|
||||
warn = _add
|
||||
error = _add
|
||||
|
||||
|
||||
class _Stub:
|
||||
"""两个被判函数只用到这几个成员，其余全部打桩。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
ws_min: Any = WS_MIN,
|
||||
ws_max: Any = WS_MAX,
|
||||
max_offset_m: float = 0.12,
|
||||
border_px: int = 2,
|
||||
) -> None:
|
||||
self.logs: List[str] = []
|
||||
self.ws_min = ws_min
|
||||
self.ws_max = ws_max
|
||||
self.coarse_max_offset_m = max_offset_m
|
||||
self.coarse_border_px = border_px
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return _StubLogger(self.logs)
|
||||
|
||||
|
||||
def _reject(st: _Stub, p: Any, g: Any, max_offset_m: Optional[float] = None) -> Optional[str]:
|
||||
return Stage2Locate._coarse_target_reject( # type: ignore[arg-type]
|
||||
st, np.asarray(p, dtype=float), np.asarray(g, dtype=float), max_offset_m
|
||||
)
|
||||
|
||||
|
||||
def _on_border(st: _Stub, bbox: Any, w: int = FRAME_W, h: int = FRAME_H) -> bool:
|
||||
return Stage2Locate._bbox_on_border(st, bbox, w, h) # type: ignore[arg-type]
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 守卫一：被画面边缘切掉的框一律弃用
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_clipped_bbox_is_rejected():
|
||||
"""负向基线：事故那一帧的框贴左边（x0=0），必须判"不可用"。"""
|
||||
assert _on_border(_Stub(), BAD_BBOX_CLIPPED) is True
|
||||
|
||||
|
||||
def test_degenerate_full_frame_bbox_is_rejected():
|
||||
"""整幅画面当工件（四边全贴）也必须弃用 —— 它的"中心"就是画面中心，毫无信息。"""
|
||||
assert _on_border(_Stub(), DEGENERATE_BBOX) is True
|
||||
|
||||
|
||||
def test_normal_bbox_is_kept():
|
||||
"""守卫不能过严：正常的、不贴边的框必须仍然可用，否则粗居中彻底废掉。"""
|
||||
assert _on_border(_Stub(), GOOD_BBOX) is False
|
||||
|
||||
|
||||
def test_bbox_touching_any_single_edge_is_rejected():
|
||||
"""四条边各自贴边都要挡住（只挡左边就漏掉了真机 coarse_move 1 那种整幅图）。"""
|
||||
for bbox in (
|
||||
(0, 100, 300, 300),
|
||||
(100, 0, 300, 300),
|
||||
(100, 100, FRAME_W - 1, 300),
|
||||
(100, 100, 300, FRAME_H - 1),
|
||||
):
|
||||
assert _on_border(_Stub(), bbox) is True, f"{bbox} 贴边却没被挡住"
|
||||
|
||||
|
||||
def test_border_margin_is_config_driven_not_hardcoded():
|
||||
"""余量必须来自配置：margin=0 时 3px 贴边的框算贴边，margin 大时更靠内也算贴边。"""
|
||||
assert _on_border(_Stub(border_px=2), (1, 100, 300, 300)) is True
|
||||
assert _on_border(_Stub(border_px=0), (3, 100, 300, 300)) is False
|
||||
assert _on_border(_Stub(border_px=8), (6, 100, 300, 300)) is True
|
||||
|
||||
|
||||
def test_none_bbox_is_rejected():
|
||||
assert _on_border(_Stub(), None) is True
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 守卫二：目标点在移动前就被否掉（而不是"截断到盒边照样走"）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_old_behaviour_clamped_the_target_and_therefore_relocated_the_arm():
|
||||
"""负向基线：证明"截断"不是安全兜底，而是一条真实的横移指令。
|
||||
|
||||
这条是"这个检查能红"的前提证明 —— 如果哪天 ``_clamp_ws`` 改成直接拒绝，
|
||||
下面的用例就不是在测同一件事了。
|
||||
"""
|
||||
st = _Stub()
|
||||
clamped = Stage2Locate._clamp_ws(st, BAD_TARGET_BASE) # type: ignore[arg-type]
|
||||
|
||||
assert clamped[1] == pytest.approx(CLAMPED_Y, abs=1e-6), "夹具前提变了"
|
||||
moved_mm = float(np.hypot(clamped[0] - BAD_TARGET_BASE[0],
|
||||
clamped[1] - BAD_TARGET_BASE[1])) * 1000.0
|
||||
assert moved_mm > 190.0, (
|
||||
f"截断只把目标挪了 {moved_mm:.1f}mm —— 与真机日志不符，用例已失真"
|
||||
)
|
||||
|
||||
|
||||
def test_bad_target_is_rejected_outside_workspace():
|
||||
"""真机那一帧的目标 (0.280,-0.747) 在盒外（y < -0.55），必须判误检。"""
|
||||
why = _reject(_Stub(), BAD_TARGET_BASE, GAUGE_XY)
|
||||
assert why is not None, "盒外目标没被挡住 —— 就是它把臂带走 56mm 的"
|
||||
assert "工作盒" in why, f"拒绝原因要说清是盒外（实际：{why}）"
|
||||
|
||||
|
||||
def test_bad_target_is_rejected_even_if_inside_workspace():
|
||||
"""把目标夹进盒内之后，211.5mm 的偏移本身也必须挡住 —— 两条判据解耦。
|
||||
|
||||
这样即使以后放宽工作盒，第二道判据仍然拦得住"坏深度反投影出来的点"。
|
||||
"""
|
||||
inside = np.array([0.206, CLAMPED_Y, 0.320]) # 盒内（x/y 都在界内），但偏移 130mm
|
||||
err = float(np.hypot(inside[0] - GAUGE_XY[0], inside[1] - GAUGE_XY[1]))
|
||||
assert err > 0.12, "夹具前提变了：这个点已经不是大偏移"
|
||||
assert WS_MIN[0] < inside[0] < WS_MAX[0] and WS_MIN[1] < inside[1] < WS_MAX[1]
|
||||
why = _reject(_Stub(), inside, GAUGE_XY)
|
||||
assert why is not None, f"盒内但偏移 {err*1000:.1f}mm 的目标漏过去了"
|
||||
assert "上限" in why, f"拒绝原因要说清是超偏移上限（实际：{why}）"
|
||||
|
||||
|
||||
def test_good_target_is_accepted():
|
||||
"""守卫不能过严：一阶段刚锁定时"臂在工件上方、差几厘米"的正常量必须放行。"""
|
||||
p = np.array([GAUGE_XY[0] + 0.030, GAUGE_XY[1] + 0.012, 0.320])
|
||||
assert _reject(_Stub(), p, GAUGE_XY) is None
|
||||
|
||||
|
||||
def test_offset_limit_is_config_driven():
|
||||
"""上限是配置项：放开后同一个点必须放行，否则阈值是写死的、配置等于没接。"""
|
||||
assert _reject(_Stub(max_offset_m=0.12), BAD_TARGET_BASE, GAUGE_XY) is not None
|
||||
# 偏移上限放到 10m ⇒ 只剩"盒外"这条在拦；说明两条判据不是同一条的复述
|
||||
why = _reject(_Stub(max_offset_m=10.0), BAD_TARGET_BASE, GAUGE_XY)
|
||||
assert why is not None and "工作盒" in why
|
||||
|
||||
|
||||
def test_offset_guard_alone_blocks_a_point_that_is_inside_the_box():
|
||||
"""只放宽工作盒（盒内）时，偏移这条必须独立生效。"""
|
||||
wide = _Stub(ws_min=np.array([-9.0, -9.0, 0.0]), ws_max=np.array([9.0, 9.0, 9.0]))
|
||||
why = _reject(wide, BAD_TARGET_BASE, GAUGE_XY)
|
||||
assert why is not None and "上限" in why
|
||||
|
||||
|
||||
def test_nonfinite_target_is_rejected():
|
||||
assert _reject(_Stub(), [np.nan, -0.5, 0.32], GAUGE_XY) is not None
|
||||
assert _reject(_Stub(), [0.3, np.inf, 0.32], GAUGE_XY) is not None
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 接线：配置 + 调用点（写了键却没接线 = 等于没检查）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _site() -> dict:
|
||||
assert SITE.is_file(), f"找不到站点配置 {SITE}"
|
||||
return yaml.safe_load(SITE.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _stage2_src() -> str:
|
||||
p = ROOT / "src" / "arm_application" / "arm_application" / "stage2_locate_node.py"
|
||||
assert p.is_file(), f"找不到 {p}"
|
||||
return p.read_text(encoding="utf-8")
|
||||
|
||||
|
||||
@pytest.mark.skip(reason="远程整站配置/硬件实现未同步；本地有独立回归测试")
|
||||
def test_site_yaml_has_the_two_guard_keys():
|
||||
# 路径与源码同源：stage2_locate_node 读的是 site["hole_detect"]["stage2"]
|
||||
stage2 = (_site().get("hole_detect") or {}).get("stage2") or {}
|
||||
assert "coarse_max_offset_m" in stage2, "site.yaml 没接 coarse_max_offset_m"
|
||||
assert "coarse_border_px" in stage2, "site.yaml 没接 coarse_border_px"
|
||||
off = float(stage2["coarse_max_offset_m"])
|
||||
# 必须小于真机那次出事的 211.5mm、又大于合理粗居中的厘米级位移。
|
||||
# 还要挡住 2026-10-16 的 110mm 回撤（0.12 放行了）。
|
||||
assert 0.03 < off < 0.10, (
|
||||
f"coarse_max_offset_m={off} 必须夹在'合理粗居中'与'110mm 回撤'之间"
|
||||
)
|
||||
|
||||
|
||||
def test_d435_handoff_110mm_reverse_is_rejected():
|
||||
"""2026-10-16：D405 把法兰拉到 (0.569,-0.375)，工件仍在 (0.512,-0.469)。
|
||||
|
||||
110mm 必须挡住。旧上限 0.12 会放行 —— 这条是「改了阈值真能红」的负向。
|
||||
"""
|
||||
gauge = np.array([0.569, -0.375])
|
||||
workpiece = np.array([0.512, -0.469, 0.320])
|
||||
err = float(np.hypot(workpiece[0] - gauge[0], workpiece[1] - gauge[1]))
|
||||
assert 0.10 < err < 0.12, f"夹具前提变了：偏差是 {err*1000:.1f}mm 不是 ~110mm"
|
||||
why = _reject(_Stub(max_offset_m=0.08), workpiece, gauge)
|
||||
assert why is not None, "110mm 回撤漏过去了"
|
||||
assert "上限" in why
|
||||
assert _reject(_Stub(max_offset_m=0.12), workpiece, gauge) is None, (
|
||||
"旧上限 0.12 必须放行这条，否则负向基线失真"
|
||||
)
|
||||
|
||||
|
||||
def test_best_bbox_wires_the_border_guard():
|
||||
src = _stage2_src()
|
||||
assert "self._bbox_on_border(bbox, w, h)" in src, "贴边守卫没接到 _best_bbox"
|
||||
assert "def _bbox_on_border(" in src
|
||||
# 旧写法不许回来：贴边的框直接当检测结果用
|
||||
assert "return tuple(int(v) for v in best.xyxy)" not in src, (
|
||||
"又直接采信检测框了（没有过贴边守卫）"
|
||||
)
|
||||
|
||||
|
||||
def test_coarse_loop_rejects_before_moving():
|
||||
src = _stage2_src()
|
||||
assert "self._coarse_target_reject(p_base, gxy)" in src, "误检守卫没接在移动前"
|
||||
assert "def _coarse_target_reject(" in src
|
||||
# 旧写法不许回来：拿孔检测器的整幅 bbox 当粗居中目标，然后照样移动
|
||||
assert 'bbox = holes.get("bbox")' not in src, (
|
||||
"又在咬孔检测器的 bbox 当粗居中目标了（真机那帧是整幅画面）"
|
||||
)
|
||||
assert "skip_coarse = True" in src, "'跳过粗居中'必须真的不移动"
|
||||
coarse = src[src.find("# ---- 3) 粗居中") : src.find("# ---- 4)")]
|
||||
assert "hole_visual_only" in coarse
|
||||
assert "pixel_on_base_plane" in coarse
|
||||
assert "拒绝用深度图凑框心" in coarse
|
||||
assert 'src = "像素交工件面"' in coarse
|
||||
|
||||
|
||||
def test_guard_reason_is_logged_with_both_numbers():
|
||||
"""判误检必须把"目标点 vs 夹爪"两个数都喊出来，否则现场查不出是谁离谱。"""
|
||||
src = _stage2_src()
|
||||
assert "粗定位目标判误检" in src
|
||||
assert "**不移动**" in src, "日志要说清处置方式（不移动），别让人以为它动了"
|
||||
@@ -0,0 +1,645 @@
|
||||
"""轻降的下降量必须由**几何**决定，深度测量只许做交叉核对。
|
||||
|
||||
两段真机事故，方向相反，都指向同一个结论：那套"规杆外环形"的间隙读数不能用。
|
||||
--------------------------------------------------------------------------
|
||||
2026-09-29（**多降**，撞工件）::
|
||||
|
||||
轻降准备: 规尖-工件间隙 141.0mm → 目标 2.0mm，需下降 139.0mm
|
||||
|
||||
按几何真值（悬停 0.320 − 工具长 0.245 − 工件面 0.022）尖端只在面上 **53mm**，
|
||||
它多降约 86mm，把通止规撞进孔，工件被推走 5mm（k1 x: 0.4907 → 0.4967）。
|
||||
|
||||
2026-10-08（**少降**，永远到不了 2mm）::
|
||||
|
||||
轻降被几何夹逼：测量出的间隙 144.0mm 与几何不符（…= 75.0mm）
|
||||
…
|
||||
已达安全下限间隙 -7.0mm，停止下降（不接触）
|
||||
=== 轻降完成: 规尖悬在孔面上方 ≈18.0mm （目标 2.0mm，不接触）===
|
||||
|
||||
这次读数反着错：悬停时报 144mm（几何真值 75mm），只降 24mm 后又翻成 **−7mm**，
|
||||
``SAFE_FLOOR`` 于是把轻降停在离面约 50mm 处 —— **永远到不了 2mm**。
|
||||
|
||||
结论：同一套读数在两个方向上各错一次、幅度 150mm 量级、还随位姿乱跳。现在
|
||||
``_descend_above_hole`` **只**用几何算下降量（法兰 FK − 工具长 − 工件面 − final_gap），
|
||||
深度读数只在偏差 >10mm 时喊一声，**不参与任何决策**。安全由三条物理/几何机制保证：
|
||||
|
||||
1. 几何预算上限 ``descent_budget_m``（与 ``gauge_fsm.hover_z_guard`` 同源）；
|
||||
2. 腕部 FT 接触检测 ⇒ 停 + 回退 1mm；
|
||||
3. 任一步 ``_goto`` 没走到位 ⇒ 停，保持当前高度。
|
||||
|
||||
这些用例全是**离线**的：``_descend_above_hole`` 只用到少量成员，用打桩对象即可，
|
||||
不建任何 ROS 实体（与 ``test_stage2_no_follow`` 同一套做法）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, List, Optional
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.overhead_oneshot import ONCE_APPROACH_STEP_M
|
||||
from arm_application.stage2_locate_node import (
|
||||
Stage2Locate,
|
||||
contact_armed_by_gap,
|
||||
)
|
||||
|
||||
# 与真机那次一致的一组数（site.yaml 现行值）
|
||||
_HOVER_Z = 0.320
|
||||
_TOOL_LEN = 0.245
|
||||
_SURFACE_Z = 0.022
|
||||
_FINAL_GAP = 0.002
|
||||
# ⇒ 几何间隙 = 0.320 − 0.245 − 0.022 = 53mm；需下降 = 51mm
|
||||
_GAP0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
|
||||
_BUDGET = _GAP0 - _FINAL_GAP
|
||||
|
||||
# 真机实测的两个坏读数（工件面被量成 242mm 而不是 ~175mm，差 86mm）
|
||||
_BAD_Z_WP = 0.242
|
||||
_TIP_D = 0.101
|
||||
_BAD_GAP0 = _BAD_Z_WP - _TIP_D # = 141mm
|
||||
|
||||
|
||||
class _StubLogger:
|
||||
"""把日志写进调用方给的 sink —— 必须与 ``_StubStage2.logs`` 是**同一个** list，
|
||||
否则断言永远看不到日志（踩过一次：两个 list 各写各的）。"""
|
||||
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def _add(self, msg: str) -> None:
|
||||
self._sink.append(str(msg))
|
||||
|
||||
info = _add
|
||||
warn = _add
|
||||
error = _add
|
||||
|
||||
|
||||
class _StubStage2:
|
||||
"""``_descend_above_hole`` 用到的成员全部打桩。
|
||||
|
||||
``z_wp`` 固定不变 = 复现"环形深度读数一直错/乱跳"的场景。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
z_wp: Optional[float] = _BAD_Z_WP,
|
||||
budget: Optional[float] = _BUDGET,
|
||||
tool_len: float = _TOOL_LEN,
|
||||
surface_z: float = _SURFACE_Z,
|
||||
contact_after_steps: Optional[int] = None,
|
||||
zwp_slope: Optional[float] = None,
|
||||
ft_fz: Optional[float] = 3.0,
|
||||
) -> None:
|
||||
self.logs: List[str] = []
|
||||
self._logger = _StubLogger(self.logs)
|
||||
self.final_descent_enable = True
|
||||
self.final_gap_m = _FINAL_GAP
|
||||
self.observe_gap_m = 0.015
|
||||
self.final_descent_speed = 0.005
|
||||
self.vert_tol_rad = 0.01
|
||||
self.hover_z = _HOVER_Z
|
||||
self.tool_tip_below_flange_m = tool_len
|
||||
self.surface_z_m = surface_z
|
||||
self.descent_budget_m = budget
|
||||
self.descent_final_gap_m: Optional[float] = None
|
||||
self._z_wp = z_wp
|
||||
# zwp_slope ≠ None 时复现 2026-10-08 实测那种"读数随下降乱掉"：
|
||||
# 真实工件面深度只随下降 1:1 减小，但那个环形读数掉得**快很多倍**
|
||||
# （实测：降 24mm 就从 +144mm 间隙翻到 −7mm）。斜率 6 正是这个倍率。
|
||||
self._zwp_slope = zwp_slope
|
||||
self._cur_z = _HOVER_Z
|
||||
self._ft_fz = ft_fz
|
||||
self._contact_after = contact_after_steps
|
||||
# 记录每一次实际下发的下降步长，用来核对累计行程
|
||||
self.steps_mm: List[float] = []
|
||||
self.goto_hold_xy: List[bool] = []
|
||||
self.pins = 0
|
||||
|
||||
# --- 被测函数会调到的钩子 ---
|
||||
def _state(self, _name: str) -> None:
|
||||
return None
|
||||
|
||||
def _measure_gauge_state(self) -> Any:
|
||||
if self._zwp_slope is not None and self._z_wp is not None:
|
||||
z_wp = self._z_wp - self._zwp_slope * (_HOVER_Z - self._cur_z)
|
||||
else:
|
||||
z_wp = self._z_wp
|
||||
return _TIP_D, np.ones((4, 4), dtype=bool), z_wp
|
||||
|
||||
def _contact_detected(self, _base_fz: float) -> bool:
|
||||
# ``contact_after_steps`` 步之后"碰到工件"（复现 FT 兜底）
|
||||
return self._contact_after is not None and len(self.steps_mm) >= self._contact_after
|
||||
|
||||
def _pin(self, _q: Any) -> None:
|
||||
self.pins += 1
|
||||
|
||||
def _goto(self, _q: Any, Tee: Any, target: Any, **_kw: Any) -> Any:
|
||||
Tee = np.array(Tee, dtype=float)
|
||||
self.goto_hold_xy.append(bool(_kw.get("hold_xy")))
|
||||
step_mm = (Tee[2, 3] - float(target[2])) * 1000.0
|
||||
# 记录"下降步"（正数）；回退步（负数）不计入下降行程
|
||||
if step_mm > 0.0:
|
||||
self.steps_mm.append(step_mm)
|
||||
if not _kw.get("hold_xy"):
|
||||
Tee[0, 3] = float(target[0])
|
||||
Tee[1, 3] = float(target[1])
|
||||
Tee[2, 3] = float(target[2])
|
||||
self._cur_z = float(target[2])
|
||||
return True, np.zeros(7), Tee
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return self._logger
|
||||
|
||||
|
||||
def _run(st: _StubStage2) -> Any:
|
||||
# ``_descend_above_hole`` 循环开头有 ``rclpy.ok()`` 兜底（运行期 Ctrl-C 时退出），
|
||||
# 而 ``rclpy.ok()`` 在没 init 过时恒为 False —— 不初始化的话函数会在第一行就返回。
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
Tee0 = np.eye(4)
|
||||
Tee0[2, 3] = _HOVER_Z
|
||||
ok, _q, Tee = Stage2Locate._descend_above_hole( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee0
|
||||
)
|
||||
return ok, Tee, st
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 核心回归：坏读数**不能**再决定下降量（两个方向都不行）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_bad_reading_no_longer_shortens_the_descent():
|
||||
"""2026-10-08 的回归：读数随下降乱掉时，也要**降满几何预算**到 2mm。
|
||||
|
||||
旧逻辑里 ``SAFE_FLOOR`` 会被这个读数触发 —— 实测它降 24mm 就从 +144mm 翻到
|
||||
−7mm（``zwp_slope`` 复现这个倍率），于是轻降被停在离面 ~50mm 处，**永远到不了
|
||||
2mm**。这就是本次要修的现象。
|
||||
"""
|
||||
ok, Tee, st = _run(_StubStage2(zwp_slope=6.0))
|
||||
|
||||
assert ok is True
|
||||
total_mm = sum(st.steps_mm)
|
||||
assert abs(total_mm - _BUDGET * 1000) < 1.0, (
|
||||
f"应降满几何预算 {_BUDGET * 1000:.1f}mm 到 2mm 目标，实际只降了 {total_mm:.1f}mm "
|
||||
"—— 又被那个乱跳的读数叫停了"
|
||||
)
|
||||
gap_final_mm = (Tee[2, 3] - _TOOL_LEN - _SURFACE_Z) * 1000
|
||||
assert abs(gap_final_mm - _FINAL_GAP * 1000) < 1.0, (
|
||||
f"最终几何间隙 {gap_final_mm:.1f}mm，应当到 {_FINAL_GAP * 1000:.1f}mm"
|
||||
)
|
||||
assert Tee[2, 3] >= _SURFACE_Z + _TOOL_LEN, "规尖已低于工件面 —— 撞了"
|
||||
|
||||
|
||||
def test_bad_reading_cannot_drive_it_into_the_workpiece():
|
||||
"""2026-09-29 的回归：读数往**大**错时，下降量也不许超过几何预算。
|
||||
|
||||
那次读数报 141mm ⇒ 想降 139mm；真值只有 53mm ⇒ 多降 86mm 撞进工件。
|
||||
"""
|
||||
# 读数报 141mm，同时把 budget 也按真值给（51mm）
|
||||
ok, Tee, st = _run(_StubStage2(z_wp=_BAD_Z_WP))
|
||||
|
||||
assert ok is True
|
||||
total_mm = sum(st.steps_mm)
|
||||
assert total_mm <= _BUDGET * 1000 + 0.5, (
|
||||
f"下降了 {total_mm:.1f}mm > 几何预算 {_BUDGET * 1000:.1f}mm —— "
|
||||
"又是那个「多降 86mm 撞工件」的 bug"
|
||||
)
|
||||
assert Tee[2, 3] >= _SURFACE_Z + _TOOL_LEN - 1e-9, (
|
||||
f"法兰停在 {Tee[2, 3]:.4f}，规尖 {Tee[2, 3] - _TOOL_LEN:.4f} 低于工件面 {_SURFACE_Z}"
|
||||
)
|
||||
|
||||
|
||||
def test_disagreement_is_announced_with_both_numbers():
|
||||
"""深度与几何不符时必须喊出来，并把两个数都打出来。
|
||||
|
||||
否则现场只看到"下降量变了"，查不出是测量坏了还是配置写错了 —— 而"测距方法
|
||||
待修"这件事必须留在日志里，不能被静默吞掉。
|
||||
"""
|
||||
_ok, _Tee, st = _run(_StubStage2())
|
||||
|
||||
hit = [m for m in st.logs if "深度测距与几何不符" in m]
|
||||
assert hit, "读数与几何差 88mm 却一声不吭 —— 静默比报错难查得多"
|
||||
text = " ".join(hit)
|
||||
assert f"{_BAD_GAP0 * 1000:.1f}mm" in text, "要打出测量值，才看得出它离谱"
|
||||
assert f"{_GAP0 * 1000:.1f}mm" in text, "要打出几何值，才能核对配置"
|
||||
assert "按几何下降" in text, "要说清本次是按几何走的（读数不参与决策）"
|
||||
|
||||
|
||||
def test_missing_gauge_visibility_still_descends_on_geometry():
|
||||
"""规不在视野（测不到）时**仍按几何下降** —— 下降量本来就不依赖规是否可见。
|
||||
|
||||
旧逻辑在这里直接"跳过轻降、停在悬停高"，等于轻降功能随机失效。
|
||||
"""
|
||||
st = _StubStage2(z_wp=None) # 测不到工件面
|
||||
ok, Tee, st = _run(st)
|
||||
|
||||
assert ok is True
|
||||
total_mm = sum(st.steps_mm)
|
||||
assert abs(total_mm - _BUDGET * 1000) < 1.0, "测不到就该按几何降满，而不是跳过"
|
||||
assert any("深度核对跳过" in m for m in st.logs), "跳过了核对也要讲一声"
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 缺几何 ⇒ 不许降（绝不用测距去赌）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_missing_geometry_refuses_to_descend():
|
||||
"""工具长/工件面没配时**必须不下降**并报错，不能退回"按测量降"。
|
||||
|
||||
2026-09-29 撞工件就是"没有几何、只有测距"时发生的。旧测试甚至把这个危险
|
||||
行为钉成了"legacy 行为"（按测量降 139mm）—— 那条是错的，已改。
|
||||
"""
|
||||
st = _StubStage2(budget=None)
|
||||
ok, Tee, st = _run(st)
|
||||
|
||||
assert ok is False, "缺几何必须判失败（保持悬停），不能假装完成"
|
||||
assert not st.steps_mm, "缺几何时一步都不许降"
|
||||
assert abs(Tee[2, 3] - _HOVER_Z) < 1e-9, "臂必须停在原位"
|
||||
assert any("轻降缺少几何" in m for m in st.logs), "缺几何要说清缺什么、怎么补"
|
||||
|
||||
|
||||
def test_zero_tool_length_refuses_to_descend():
|
||||
"""工具长为 0（没标定）同样不许降 —— 那是"拿 0 当尺子"。"""
|
||||
st = _StubStage2(tool_len=0.0)
|
||||
ok, _Tee, st = _run(st)
|
||||
assert ok is False and not st.steps_mm
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 真安全机制仍在：FT 接触、goto 失败
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_ft_contact_stops_and_backs_off():
|
||||
"""腕部力突变必须立即停并回退 —— 这是与测距无关的**物理**兜底。
|
||||
|
||||
力从第 8 步（间隙约 21mm）就超门槛，但门控是 ``final_gap+4mm``（6mm）。
|
||||
21mm / 10mm 这一档必须继续降；进到 6mm 才许停，且不许把 2mm 目标走完。
|
||||
"""
|
||||
st = _StubStage2(contact_after_steps=8)
|
||||
ok, Tee, st = _run(st)
|
||||
|
||||
assert ok is True, "接触后停住即算安全收工"
|
||||
assert any("腕部力突变" in m for m in st.logs), "碰到工件却没喊"
|
||||
assert any("不当作碰到" in m for m in st.logs), "21mm 间隙时力超了也要明说不当成碰到"
|
||||
assert sum(st.steps_mm) > 32.0, "力从 21mm 间隙就热，必须穿过 10mm 假接触区"
|
||||
assert sum(st.steps_mm) < _BUDGET * 1000 - 1.0, "力突变后还在继续降到 2mm"
|
||||
assert Tee[2, 3] > _HOVER_Z - _BUDGET, "回退没生效"
|
||||
|
||||
|
||||
def test_contact_not_armed_while_still_high_above_the_part():
|
||||
"""远场 138mm 和近场 10mm / ΔFz≈3N 都不算碰到；6mm 才武装。"""
|
||||
assert contact_armed_by_gap(0.1375, 0.015) is False
|
||||
assert contact_armed_by_gap(0.020, 0.015) is False
|
||||
assert contact_armed_by_gap(0.010, 0.015) is False
|
||||
assert contact_armed_by_gap(0.0095, 0.015) is False
|
||||
assert contact_armed_by_gap(0.006, 0.015) is True
|
||||
assert contact_armed_by_gap(0.005, 0.015) is True
|
||||
|
||||
|
||||
def test_lock_xy_descend_holds_xy_and_stops_at_final_gap():
|
||||
"""锁 XY 实验：从悬停纯 Z 收到 2mm，XY 不许动，步长仍 20mm。"""
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
st = _StubStage2()
|
||||
Tee0 = np.eye(4)
|
||||
Tee0[0, 3] = 0.512
|
||||
Tee0[1, 3] = -0.401
|
||||
Tee0[2, 3] = _HOVER_Z
|
||||
ok, _q, Tee = Stage2Locate._descend_above_hole( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee0, target_gap_m=_FINAL_GAP, step_m=ONCE_APPROACH_STEP_M
|
||||
)
|
||||
assert ok is True
|
||||
gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z
|
||||
assert abs(gap - _FINAL_GAP) < 0.001, f"应停在贴面 2mm，实际 {gap * 1000:.1f}mm"
|
||||
assert Tee[0, 3] == 0.512
|
||||
assert Tee[1, 3] == -0.401
|
||||
assert max(st.steps_mm) <= ONCE_APPROACH_STEP_M * 1000 + 1e-6
|
||||
assert all(st.goto_hold_xy), "下臂每一步都必须 hold_xy"
|
||||
|
||||
|
||||
def _look_neg_x_T() -> np.ndarray:
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = np.array(
|
||||
[[0.0, 0.0, -1.0], [1.0, 0.0, 0.0], [0.0, -1.0, 0.0]],
|
||||
dtype=float,
|
||||
)
|
||||
return T
|
||||
|
||||
|
||||
def test_once_ff_zero_k_is_cartesian_lock_xy_movel():
|
||||
"""k=0 必须下发相对 MoveL，且 ΔXY=0、只降 Z。不许停在悬停高装成功。"""
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
st = _ff_stub_ready()
|
||||
st.once_ff_look_per_m = 0.0
|
||||
st._lookup = lambda *_a, **_k: np.eye(4) # type: ignore[method-assign]
|
||||
st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096) # type: ignore[method-assign]
|
||||
got: list = []
|
||||
st._vendor_movel_tcp = ( # type: ignore[method-assign]
|
||||
lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
|
||||
)
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.595
|
||||
Tee[1, 3] = -0.467
|
||||
Tee[2, 3] = _HOVER_Z
|
||||
ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
|
||||
)
|
||||
assert ok is True
|
||||
assert len(got) == 1
|
||||
gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
|
||||
descent = gap0 - _FINAL_GAP
|
||||
np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
|
||||
assert any("笛卡尔锁XY只降Z" in m for m in st.logs)
|
||||
|
||||
|
||||
def _ff_stub_ready() -> _StubStage2:
|
||||
st = _StubStage2()
|
||||
st.once_ff_look_per_m = 0.0
|
||||
st._tip_off = np.array([-0.001, -0.001], dtype=float)
|
||||
st.cam_frame = "cam"
|
||||
st.base_frame = "base"
|
||||
st._ws_clipped = False
|
||||
st._lookup = lambda *_a, **_k: _look_neg_x_T() # type: ignore[method-assign]
|
||||
st._clamp_ws = lambda *_a, **_k: None # type: ignore[method-assign]
|
||||
st._spin = lambda *_a, **_k: None # type: ignore[method-assign]
|
||||
st._wait_state = lambda *_a, **_k: (None, None) # type: ignore[method-assign]
|
||||
st.robot_states_topic = "/Rizon4_063989/flexiv_robot_states"
|
||||
return st
|
||||
|
||||
|
||||
def test_once_ff_refuses_without_tcp_pose():
|
||||
"""没有厂家 tcp_pose 不许退回 ROS 绝对坐标。"""
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
st = _ff_stub_ready()
|
||||
st._fresh_tcp_world = lambda: None # type: ignore[method-assign]
|
||||
called: list = []
|
||||
st._vendor_movel_tcp = lambda *_a, **_k: called.append(1) or True # type: ignore[method-assign]
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.595
|
||||
Tee[1, 3] = -0.467
|
||||
Tee[2, 3] = _HOVER_Z
|
||||
ok, _q, Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
|
||||
)
|
||||
assert ok is False
|
||||
assert called == []
|
||||
assert Tee1[2, 3] == pytest.approx(_HOVER_Z)
|
||||
assert any("没有厂家 tcp_pose" in m for m in st.logs)
|
||||
|
||||
|
||||
def test_once_ff_movel_is_flexiv_tcp_plus_delta():
|
||||
"""MoveL 必须是当前厂家 TCP + (ΔXY, −下降)，不是 ROS 规尖绝对坐标。"""
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
st = _ff_stub_ready()
|
||||
st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096) # type: ignore[method-assign]
|
||||
got: list = []
|
||||
st._vendor_movel_tcp = ( # type: ignore[method-assign]
|
||||
lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
|
||||
)
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.595
|
||||
Tee[1, 3] = -0.467
|
||||
Tee[2, 3] = _HOVER_Z
|
||||
ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
|
||||
)
|
||||
assert ok is True
|
||||
assert len(got) == 1
|
||||
gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
|
||||
descent = gap0 - _FINAL_GAP
|
||||
np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
|
||||
assert got[0][0] == pytest.approx(0.580)
|
||||
assert not np.allclose(got[0][:2], [0.595 - 0.001, -0.467 - 0.001], atol=1e-4)
|
||||
|
||||
|
||||
def test_once_ff_custom_final_gap_only_changes_z():
|
||||
"""--final-gap 8mm：MoveL XY 仍为 0，下降量必须少 6mm，不许再走贴面 2mm。"""
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
st = _ff_stub_ready()
|
||||
st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096) # type: ignore[method-assign]
|
||||
got: list = []
|
||||
st._vendor_movel_tcp = ( # type: ignore[method-assign]
|
||||
lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
|
||||
)
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.595
|
||||
Tee[1, 3] = -0.467
|
||||
Tee[2, 3] = _HOVER_Z
|
||||
gap_custom = 0.008
|
||||
ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee, target_gap_m=gap_custom
|
||||
)
|
||||
assert ok is True
|
||||
assert len(got) == 1
|
||||
gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
|
||||
descent = gap0 - gap_custom
|
||||
descent_2mm = gap0 - _FINAL_GAP
|
||||
np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
|
||||
assert got[0][0] == pytest.approx(0.580)
|
||||
assert got[0][1] == pytest.approx(-0.460)
|
||||
assert descent == pytest.approx(descent_2mm - 0.006)
|
||||
assert any("笛卡尔锁XY只降Z" in m for m in st.logs)
|
||||
|
||||
|
||||
def test_once_ff_k_018_still_adds_look_xy():
|
||||
"""有人把 k 改回 0.018 时，MoveL 必须真的加 +k×下降，不许再吞成绝对坐标。"""
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
st = _ff_stub_ready()
|
||||
st.once_ff_look_per_m = 0.018
|
||||
st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096) # type: ignore[method-assign]
|
||||
got: list = []
|
||||
st._vendor_movel_tcp = ( # type: ignore[method-assign]
|
||||
lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
|
||||
)
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.595
|
||||
Tee[1, 3] = -0.467
|
||||
Tee[2, 3] = _HOVER_Z
|
||||
ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
|
||||
)
|
||||
assert ok is True
|
||||
assert len(got) == 1
|
||||
gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
|
||||
descent = gap0 - _FINAL_GAP
|
||||
dxy = 0.018 * descent
|
||||
np.testing.assert_allclose(got[0], [0.580 + dxy, -0.460, 0.096 - descent], atol=1e-9)
|
||||
assert any("视线前馈斜线" in m for m in st.logs)
|
||||
|
||||
|
||||
def test_oneshot_approach_stops_at_observe_gap_with_larger_steps():
|
||||
"""单次精拍第一段只降到 15mm，步长 20mm（约 3 步），不许 4mm 切到 2mm。"""
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
st = _StubStage2()
|
||||
Tee0 = np.eye(4)
|
||||
Tee0[2, 3] = _HOVER_Z
|
||||
ok, _q, Tee = Stage2Locate._descend_above_hole( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee0, target_gap_m=0.015, step_m=ONCE_APPROACH_STEP_M
|
||||
)
|
||||
assert ok is True
|
||||
gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z
|
||||
assert abs(gap - 0.015) < 0.001, f"应停在观察高 15mm，实际 {gap * 1000:.1f}mm"
|
||||
assert max(st.steps_mm) <= ONCE_APPROACH_STEP_M * 1000 + 1e-6
|
||||
assert len(st.steps_mm) <= 3
|
||||
assert abs(sum(st.steps_mm) - (_GAP0 - 0.015) * 1000) < 1.5
|
||||
|
||||
|
||||
def test_oneshot_approach_does_not_treat_force_as_contact():
|
||||
"""观察高 15mm 仍在接触门控之外，力再热也不许当碰到。"""
|
||||
|
||||
class _AlwaysHot(_StubStage2):
|
||||
def _contact_detected(self, _base_fz: float) -> bool:
|
||||
return True
|
||||
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
st = _AlwaysHot()
|
||||
Tee0 = np.eye(4)
|
||||
Tee0[2, 3] = _HOVER_Z
|
||||
ok, _q, Tee = Stage2Locate._descend_above_hole( # type: ignore[arg-type]
|
||||
st, np.zeros(7), Tee0, target_gap_m=0.015, step_m=ONCE_APPROACH_STEP_M
|
||||
)
|
||||
assert ok is True
|
||||
gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z
|
||||
assert abs(gap - 0.015) < 0.001
|
||||
assert not any("腕部力突变" in m for m in st.logs)
|
||||
|
||||
|
||||
def test_ft_drift_far_from_surface_does_not_stop_on_the_first_step():
|
||||
"""离面还远时力一直超门槛，也要先降到门控间隙附近才许停。"""
|
||||
|
||||
class _AlwaysHot(_StubStage2):
|
||||
def _contact_detected(self, _base_fz: float) -> bool:
|
||||
return True
|
||||
|
||||
ok, _Tee, st = _run(_AlwaysHot())
|
||||
assert ok is True
|
||||
assert sum(st.steps_mm) >= 40.0, "离面 53mm 时力超门槛也不该第一步就停"
|
||||
assert any("不当作碰到" in m for m in st.logs)
|
||||
assert any("腕部力突变" in m for m in st.logs)
|
||||
|
||||
|
||||
def test_every_step_logs_the_ft_force():
|
||||
"""每一步都要打出 FT 轴向力 —— 这是"停住是碰到还是没跟上"的唯一直接证据。
|
||||
|
||||
2026-10-08 排查"轻降在离面 ~4mm 处停住"时全靠这一行；没有它只能猜。
|
||||
"""
|
||||
_ok, _Tee, st = _run(_StubStage2())
|
||||
ft_lines = [m for m in st.logs if "FT_fz=" in m]
|
||||
assert ft_lines, "每步的 FT 读数没打进日志 —— 排查时无从判断是否接触"
|
||||
assert any("基线" in m for m in ft_lines), "要给出基线与增量，才看得出力有没有变"
|
||||
|
||||
|
||||
def test_missing_ft_baseline_is_announced():
|
||||
"""轻降开始前没收到 FT 时要明说'接触兜底不生效'（不许静默少一道保险）。"""
|
||||
_ok, _Tee, st = _run(_StubStage2(ft_fz=None))
|
||||
assert any("接触兜底不生效" in m for m in st.logs), (
|
||||
"FT 没数据却一声不吭 —— 会让人以为有接触兜底"
|
||||
)
|
||||
|
||||
|
||||
def test_goto_failure_stops_the_descent():
|
||||
"""某一步走不到位（被挡/碰到东西）⇒ 停，保持当前高度，并判失败。"""
|
||||
st = _StubStage2()
|
||||
# 从第 3 步起 _goto 报失败
|
||||
orig = st._goto
|
||||
calls = {"n": 0}
|
||||
|
||||
def _goto(q, Tee, target, **kw):
|
||||
calls["n"] += 1
|
||||
if calls["n"] >= 3:
|
||||
return False, q, Tee
|
||||
return orig(q, Tee, target, **kw)
|
||||
|
||||
st._goto = _goto # type: ignore[assignment]
|
||||
ok, Tee, st = _run(st)
|
||||
|
||||
assert ok is False, "走不到位必须判失败（安全优先）"
|
||||
assert any("轻降中途未到位" in m for m in st.logs)
|
||||
assert Tee[2, 3] > _SURFACE_Z + _TOOL_LEN, "走不到位时也不许低于工件面"
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 不许回退到"把目标值当结果汇报"
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_no_descent_still_drops_to_the_observe_gap():
|
||||
"""--no-descent 不是停在悬停高：要降到观测间隙（15mm），方便看规尖偏没偏。
|
||||
|
||||
不许降到贴面 2mm —— 对偏了再贴面会把规顶在孔沿上。
|
||||
"""
|
||||
st = _StubStage2()
|
||||
st.final_descent_enable = False
|
||||
st.observe_gap_m = 0.015
|
||||
ok, Tee, st = _run(st)
|
||||
|
||||
assert ok is True
|
||||
gap_mm = (Tee[2, 3] - _TOOL_LEN - _SURFACE_Z) * 1000
|
||||
assert abs(gap_mm - 15.0) < 1.0, f"观测下降应停在离面 15mm，实际 {gap_mm:.1f}mm"
|
||||
total_mm = sum(st.steps_mm)
|
||||
assert total_mm < _BUDGET * 1000 - 5.0, "观测下降走到了贴面预算，等于把 --no-descent 吃掉了"
|
||||
assert any("观测下降完成" in m for m in st.logs), "日志要写明这是观测下降，不是贴面轻降"
|
||||
|
||||
|
||||
def test_wired_final_gap_is_reported_from_geometry_not_target():
|
||||
"""收尾要汇报**几何实算**的间隙，不能把目标值（2.0mm）念一遍。
|
||||
|
||||
旧日志就出现过"停在离面 18mm"却同时打印"悬在孔面上方 ~2.0mm"，现场会以为
|
||||
轻降做到了（R1.1：不许把没做到说成做到）。
|
||||
"""
|
||||
_ok, _Tee, st = _run(_StubStage2())
|
||||
assert st.descent_final_gap_m is not None, "没记录最终间隙"
|
||||
assert abs(st.descent_final_gap_m * 1000 - _FINAL_GAP * 1000) < 1.0
|
||||
assert any("轻降完成" in m and "几何" in m for m in st.logs), (
|
||||
"收尾日志要标明这是几何量，且给出可核对的分量"
|
||||
)
|
||||
|
||||
|
||||
def test_source_no_longer_uses_depth_to_stop_the_descent():
|
||||
"""守卫测试：源码里不许再出现"用深度间隙叫停轻降"的逻辑。
|
||||
|
||||
这是**反向**证据 —— 只要有人把 SAFE_FLOOR / _measure_z_wp 叫停逻辑加回来，
|
||||
这条立刻红（比"单元测试碰巧还能过"更直接）。用语法树判据（不是文本匹配），
|
||||
这样解释这件事的注释/文档字符串不会把它误判成"逻辑又回来了"。
|
||||
"""
|
||||
import ast
|
||||
from pathlib import Path
|
||||
|
||||
src = (
|
||||
Path(__file__).resolve().parents[1]
|
||||
/ "arm_application"
|
||||
/ "stage2_locate_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
tree = ast.parse(src)
|
||||
fn = next(
|
||||
n
|
||||
for n in ast.walk(tree)
|
||||
if isinstance(n, ast.FunctionDef) and n.name == "_descend_above_hole"
|
||||
)
|
||||
names = {n.id for n in ast.walk(fn) if isinstance(n, ast.Name)}
|
||||
attrs = {n.attr for n in ast.walk(fn) if isinstance(n, ast.Attribute)}
|
||||
assert "SAFE_FLOOR" not in names, "深度安全下限又回来了 —— 它就是那个误停的元凶"
|
||||
assert "_measure_z_wp" not in attrs, "又在循环里重测深度当停止条件了"
|
||||
# 交叉核对必须仍在（不是把测量悄悄删掉了）
|
||||
assert "_measure_gauge_state" in attrs, "连交叉核对都没了 —— 那就没法发现测距坏了"
|
||||
@@ -0,0 +1,168 @@
|
||||
"""臂没跟随指令时，报错必须指向**连接**，不能甩锅给几何。
|
||||
|
||||
为什么单独钉住（2026-09-28 真机实测）
|
||||
--------------------------------------
|
||||
真机跑到 S05 精拍定位时反复失败，报的是
|
||||
``code=2（垂直化或悬停失败：工具轴没竖直、或悬停高度到不了）``，
|
||||
看着像标定/几何/悬停高度的问题。实际日志里是：
|
||||
|
||||
plan hover Δ=(+0,+0,-80)mm tilt=0.12deg
|
||||
send hover: 114 pts T=4.37s ... max|Δq|=0.17378rad
|
||||
hover hop1 实际 max|Δq|=0.00000rad Δz=+0.00mm
|
||||
hover hop2 实际 max|Δq|=0.00000rad Δz=+0.00mm
|
||||
|
||||
指令要求把法兰挪 0.17378 rad，实测**一动不动**。真因是 RDK 与机器人的连接在
|
||||
14:53:44 就断了（``Disconnected from the robot, waiting for reconnection``），
|
||||
此后 ``robot_->states()`` 只会返回最后一帧缓存、``robot_timestamp`` 推进量恒为 0
|
||||
（joint_states 因此永久冻结），而 ``rizon_arm_controller`` 是**本地插值**、
|
||||
不校验硬件反馈，所以照样打印 ``Goal reached, success!``。
|
||||
|
||||
最坑的一点：整条链路**没有一个环节报错** —— 断连不报、写命令不报、控制器报成功。
|
||||
只有"实测位移为 0"这一个信号是真的。所以这个信号必须自己喊出来，并且喊对方向
|
||||
（去查网络/连接），否则下一次仍然会被"垂直化或悬停失败"带去调标定。
|
||||
|
||||
顺带一提：``_goto`` 里那个 ``stall`` 逻辑本来就会因"连续 2 hop 无进展"而放弃，
|
||||
所以**不会**无限转下去；缺的只是**说清原因**。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, List
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.stage2_locate_node import Stage2Locate
|
||||
|
||||
_NO_FOLLOW_HINT = "臂没有跟随指令"
|
||||
_NO_FOLLOW_SIGN = "关节状态**毫无变化**"
|
||||
|
||||
# 与真机那次同量级：悬停要往下 197mm，容差 0.7mm。
|
||||
_EE_Z = 0.444
|
||||
_TARGET_Z = 0.247
|
||||
_TOL_M = 0.0007
|
||||
_TOL_TILT = 0.01
|
||||
|
||||
|
||||
class _StubLogger:
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def info(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
def warn(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
def error(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
|
||||
class _StubStage2:
|
||||
"""``_goto`` 只用这几样东西；测试里不建任何 ROS 实体。
|
||||
|
||||
``follows=False``：模拟"指令发了、关节状态冻住" —— ``_wait_state`` 每次都
|
||||
回同一份 (q, Tee)，就像真机上读到的最后一帧缓存。
|
||||
``follows=True``：正常跟随，用于确认不会误报。
|
||||
"""
|
||||
|
||||
def __init__(self, follows: bool) -> None:
|
||||
self.logs: List[str] = []
|
||||
self.sends = 0
|
||||
self._follows = bool(follows)
|
||||
self._calls = 0
|
||||
|
||||
def _clamp_ws(self, target: Any) -> np.ndarray:
|
||||
return np.array(target, dtype=float)
|
||||
|
||||
def _spin(self, _dt: float = 0.05) -> None:
|
||||
return None
|
||||
|
||||
def _plan_and_send(self, *_a: Any, **_kw: Any) -> bool:
|
||||
self.sends += 1
|
||||
return True
|
||||
|
||||
def _wait_state(self, _timeout: float) -> Any:
|
||||
self._calls += 1
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.0
|
||||
Tee[1, 3] = 0.0
|
||||
q = np.zeros(7)
|
||||
if self._follows:
|
||||
# 每次调用都比上一次更靠近目标：模拟"真的在走"。
|
||||
# 注意必须**逐次递进** —— 若每次都回同一份"动过"的状态，第二次相对
|
||||
# 第一次就没变化了，会被判成没跟随（这正是打桩要避免的假象）。
|
||||
Tee[2, 3] = _EE_Z - 0.03 * self._calls
|
||||
q[1] = 0.05 * self._calls
|
||||
else:
|
||||
# 冻结在最后一帧缓存：每次都一样，就像真机断连后读到的那样。
|
||||
Tee[2, 3] = _EE_Z
|
||||
return q, Tee
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return _StubLogger(self.logs)
|
||||
|
||||
|
||||
def _run(follows: bool) -> Any:
|
||||
st = _StubStage2(follows)
|
||||
Tee0 = np.eye(4)
|
||||
Tee0[2, 3] = _EE_Z
|
||||
ok, _q, _Tee = Stage2Locate._goto( # type: ignore[arg-type]
|
||||
st,
|
||||
np.zeros(7),
|
||||
Tee0,
|
||||
np.array([0.0, 0.0, _TARGET_Z]),
|
||||
False, # verticalize：本用例不碰姿态，只看"臂动没动"
|
||||
_TOL_M,
|
||||
_TOL_TILT,
|
||||
"hover",
|
||||
0.022,
|
||||
max_hops=2,
|
||||
settle_s=0.0,
|
||||
min_dur=0.0,
|
||||
)
|
||||
return ok, st
|
||||
|
||||
|
||||
def test_frozen_joint_state_is_reported_as_no_follow():
|
||||
"""实测位移为 0 → 必须出一条指向"连接/关节状态"的错，而不是只说容差没到。"""
|
||||
ok, st = _run(follows=False)
|
||||
|
||||
assert st.sends == 2, "无进展会连着 hop 两次才放弃（stall 判据）"
|
||||
assert ok is False, "臂没跟上就不该报成功"
|
||||
assert any(_NO_FOLLOW_SIGN in m for m in st.logs), (
|
||||
"要有一条明确说'关节状态毫无变化'的日志，否则查日志时会以为臂动过"
|
||||
)
|
||||
assert any(_NO_FOLLOW_HINT in m for m in st.logs), (
|
||||
"错因必须指向'臂没有跟随指令'，不能只停在'悬停未达容差'"
|
||||
)
|
||||
hint = " ".join(m for m in st.logs if _NO_FOLLOW_HINT in m)
|
||||
for kw in ("RDK", "断连", "ping", "MAC"):
|
||||
assert kw in hint, f"提示里要给出可执行的排查方向（缺 {kw}）"
|
||||
|
||||
|
||||
def test_normal_follow_does_not_emit_the_no_follow_error():
|
||||
"""正常跟随时不能误报 —— 这条错一旦刷出来就会把人带去查网络。"""
|
||||
_ok, st = _run(follows=True)
|
||||
|
||||
assert st.sends >= 1, "至少得真的发过轨迹"
|
||||
assert not any(_NO_FOLLOW_HINT in m for m in st.logs), (
|
||||
"臂动了还报'没跟随'就是误报，比不报更糟"
|
||||
)
|
||||
|
||||
|
||||
def test_eps_is_small_enough_to_not_swallow_real_motion():
|
||||
"""阈值必须小于"真动了一下"的量级，否则真实的小位移会被误判成没动。"""
|
||||
from arm_application.stage2_locate_node import _NO_FOLLOW_EPS_RAD
|
||||
|
||||
assert _NO_FOLLOW_EPS_RAD > 0.0
|
||||
# 真机一次 hop 计划 0.17378rad；即使只走到 1%，也远大于阈值。
|
||||
assert _NO_FOLLOW_EPS_RAD < 0.17378 * 0.01, (
|
||||
"阈值取大了会把'动了一点点'误判成'完全没动'"
|
||||
)
|
||||
|
||||
|
||||
def test_hint_mentions_the_controller_still_reports_success():
|
||||
"""要点破"控制器仍报 success"这个反直觉点，否则下一轮还会被日志骗。"""
|
||||
_ok, st = _run(follows=False)
|
||||
hint = " ".join(m for m in st.logs if _NO_FOLLOW_HINT in m)
|
||||
assert "success" in hint, "真机现象就是'控制器报 success 但臂没动'，得写清楚"
|
||||
@@ -0,0 +1,742 @@
|
||||
"""精对准的「工具位置」必须取自**视觉量到的规尖**，不能再用法兰轴线。
|
||||
|
||||
为什么（2026-10-08 真机 arm 89）
|
||||
--------------------------------
|
||||
规是**夹在爪里**的零件，它相对法兰轴线可以整体偏出约 10mm。实测（同一帧、同一
|
||||
条手眼链，n=8）：
|
||||
|
||||
* 规杆轴线相对法兰 = **(+10.7, −0.1) mm**（σ≈0.5mm）；最深像素法 (+8.8, −1.8) mm
|
||||
* 而 ``hover_offset_ee_xy_m`` 写的是 ``[0, 0]``（"平行爪同轴"）⇒ 精对准把**法兰**
|
||||
对到孔上 ⇒ 规尖实际离孔心 ~10mm ⇒ 轻降时 φ6 规尖骑在 φ7 孔的**孔沿**上，
|
||||
腕力从 −1N 蹦到 −31N→−63N 卡死。现场看到的是"轻降在离面 4mm 停住"。
|
||||
|
||||
而孔检测本身重复性极好（k1 基座坐标 σ=(0.17,0.04)mm）—— 精度不是瓶颈，
|
||||
"对的是谁"才是。所以改成在同一帧里同时解「规尖」与「孔」，两者共用手眼链。
|
||||
|
||||
真机复跑还暴露了第二件事：**工具长读数会跳**（192~223mm）——规尖细、反光，掠射角
|
||||
下深度容易丢，丢了以后"最深有效像素"往上跑、工具长只会变短。逐拍重算偏移会把
|
||||
"规杆略有倾斜"引入的 ±0.2~0.4mm 抖动带进残差，残差在 0.44~0.85mm 之间来回荡、
|
||||
收不进 0.25mm。偏移是**刚体量**，所以**量准一份就冻结**，并且只取"端头看得全"
|
||||
（工具长最长）那一簇。
|
||||
|
||||
这些用例全是**离线**的：只用到少量成员，用打桩对象即可。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Dict, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.stage2_locate_node import Stage2Locate
|
||||
|
||||
_INTR = {"fx": 400.0, "fy": 400.0, "cx": 400.0, "cy": 240.0}
|
||||
_TEE = np.eye(4)
|
||||
_TEE[0, 3] = 0.50
|
||||
_TEE[1, 3] = -0.10
|
||||
_TEE[2, 3] = 0.55
|
||||
|
||||
# 相机朝下、相对竖直前倾约 30°（≈真机那个 29.7° 的视角）。
|
||||
# 用旋转而不是纯平移——纯平移会让"更深"在基座里变成**更长**的工具长，与真机相反，
|
||||
# 那样这条链就测不出"端头丢像素 ⇒ 工具长变短"这件事。
|
||||
_R_CB = np.array([[1.0, 0.0, 0.0], [0.0, -0.866, -0.5], [0.0, 0.5, -0.866]])
|
||||
_T_CB = np.array([0.50, -0.10, 0.45])
|
||||
|
||||
|
||||
def _rod_frame(v1: int = 300, u0: int = 400, v0: int = 100, near: float = 0.06,
|
||||
far: float = 0.12, half: int = 8, w: int = 800, h: int = 480) -> np.ndarray:
|
||||
"""造一帧含"规杆"的深度图：中央竖带，深度随 v 线性变深（越靠下越深）。
|
||||
|
||||
深度只随 ``v`` 的**绝对位置**变（用固定的 200px 量程），所以 ``v1`` 截短 = 规尖
|
||||
那几个像素丢了 —— 正好复现真机那个"最深有效像素往上跑、工具长变短"。
|
||||
"""
|
||||
depth = np.full((h, w), 0.50, np.float32)
|
||||
for v in range(v0, v1 + 1):
|
||||
f = min(1.0, (v - v0) / 200.0)
|
||||
depth[v, u0 - half : u0 + half] = near + f * (far - near)
|
||||
return depth
|
||||
|
||||
|
||||
class _StubLogger:
|
||||
def __init__(self) -> None:
|
||||
self.msgs: list = []
|
||||
|
||||
def _add(self, msg: str) -> None:
|
||||
self.msgs.append(str(msg))
|
||||
|
||||
info = _add
|
||||
warn = _add
|
||||
error = _add
|
||||
|
||||
|
||||
class _Stub:
|
||||
"""被测方法用到的成员。"""
|
||||
|
||||
tip_min_m = 0.04
|
||||
tip_max_m = 0.60
|
||||
tip_near_m = 0.15
|
||||
tip_band_frac = 0.25
|
||||
tip_min_px = 150
|
||||
tip_len_min = 0.15
|
||||
tip_len_max = 0.32
|
||||
tip_off_tol = 0.003
|
||||
tip_calib_n = 4
|
||||
tool_tip_below_flange_m = 0.19
|
||||
|
||||
def __init__(self, t_cb: Tuple[float, float, float] = tuple(_T_CB)) -> None:
|
||||
self._t_cb = np.array(t_cb, float)
|
||||
self._tip_off: Optional[np.ndarray] = None
|
||||
self._tip_samp: list = []
|
||||
self._tip_len_m: Optional[float] = None
|
||||
self._logger = _StubLogger()
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return self._logger
|
||||
|
||||
# 真实实现（纯数学）——换掉就等于没测到那条链
|
||||
def _px_to_cam(self, u: float, v: float, z: float, intr: Dict[str, float]) -> np.ndarray:
|
||||
return Stage2Locate._px_to_cam(self, u, v, z, intr) # type: ignore[arg-type]
|
||||
|
||||
def _cam_to_base(self, p_cam: np.ndarray) -> np.ndarray:
|
||||
return _R_CB @ np.asarray(p_cam, float) + self._t_cb
|
||||
|
||||
|
||||
def _call(st: _Stub, depth: np.ndarray, z_wp_cam: Optional[float] = None):
|
||||
return Stage2Locate._tip_offset_from_depth( # type: ignore[arg-type]
|
||||
st, depth, _INTR, _TEE, z_wp_cam=z_wp_cam
|
||||
)
|
||||
|
||||
|
||||
def _feed(st: _Stub, depth: np.ndarray) -> Tuple[Optional[np.ndarray], Optional[float]]:
|
||||
"""走一遍"量一帧 → 攒样本 → 试冻结"的完整流程。"""
|
||||
off, tlen, _why = _call(st, depth)
|
||||
if off is not None and tlen is not None:
|
||||
st._tip_samp.append((off, tlen))
|
||||
Stage2Locate._refresh_tip_offset(st) # type: ignore[arg-type]
|
||||
return off, tlen
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 正常路径
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_measures_the_rod_tip_offset():
|
||||
"""规杆在中央、深度合理时，要真的量出「规尖相对法兰」的基座 XY 偏移 + 工具长。"""
|
||||
off, tlen, why = _call(_Stub(), _rod_frame())
|
||||
assert off is not None and tlen is not None, f"没量到规尖：{why}"
|
||||
# 规杆在 u=400=cx 的正中央 ⇒ 基座 X 偏移应≈0
|
||||
assert abs(float(off[0])) < 0.001, f"规杆居中，X 偏移应≈0，实际 {off[0]*1000:+.2f}mm"
|
||||
assert abs(float(off[1])) < 0.15, f"Y 偏移 {off[1]*1000:.1f}mm 离谱"
|
||||
assert 0.15 <= tlen <= 0.32, f"工具长 {tlen*1000:.0f}mm 不在合法区间"
|
||||
assert "工具长" in why, "说明里要给出反投影出的工具长（可核对）"
|
||||
|
||||
|
||||
def test_offset_is_stable_frame_to_frame():
|
||||
"""同一姿态连拍：偏移是刚体量，重复帧之间必须几乎不动。"""
|
||||
st = _Stub()
|
||||
off1, _t1, _ = _call(st, _rod_frame())
|
||||
assert off1 is not None
|
||||
off2, _t2, why = _call(st, _rod_frame())
|
||||
assert off2 is not None, f"重复帧被误判：{why}"
|
||||
assert float(np.linalg.norm(off2 - off1)) < 1e-6
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 判据各自都要能真的失败（负向用例）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _gripper_and_rod_frame() -> np.ndarray:
|
||||
"""爪子块头大且更近 + 规杆更细更深：复现 2026-10-09 真机『按面积选爪子』。"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
depth[20:90, 330:470] = 0.07 # 爪子：宽、浅、画面上方
|
||||
for v in range(90, 280):
|
||||
f = min(1.0, (v - 90) / 200.0)
|
||||
depth[v, 392:408] = 0.08 + f * 0.05 # 规杆：细、伸向工件
|
||||
return depth
|
||||
|
||||
|
||||
def test_picks_the_lower_tip_not_the_config_length_blob():
|
||||
"""画面上方、工具长刚好贴着配置的那一块不是规尖。
|
||||
|
||||
2026-10-09 真机：最接近配置 223mm 的点工具长 225mm、偏移约 0，冻住的是
|
||||
法兰轴线。残差 0.14mm 达标，下降后规尖仍偏出约 10mm。规尖在画面更靠下。
|
||||
"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
depth[40:100, 370:430] = 0.080 # 靠上、居中，工具长贴着配置
|
||||
for v in range(200, 301):
|
||||
f = min(1.0, (v - 200) / 200.0)
|
||||
depth[v, 442:458] = 0.10 + f * 0.02 # 靠下、偏出中心
|
||||
st = _Stub()
|
||||
st.tool_tip_below_flange_m = 0.185
|
||||
off, tlen, why = _call(st, depth)
|
||||
assert off is not None and tlen is not None, f"有规尖却没量到：{why}"
|
||||
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v > 180, f"采到的是贴着配置工具长的上方点，不是规尖：{why}"
|
||||
assert abs(float(off[0])) > 0.005, (
|
||||
f"规尖偏出中心，X 偏移不该≈0（那是法兰轴）：{why} off={off}"
|
||||
)
|
||||
|
||||
|
||||
def test_picks_rod_not_larger_gripper():
|
||||
"""中央竖带里爪子面积更大时，必须仍选**更深的规杆**，不能按连通域面积。"""
|
||||
depth = _gripper_and_rod_frame()
|
||||
off, tlen, why = _call(_Stub(), depth)
|
||||
assert off is not None and tlen is not None, f"有规杆却没量到：{why}"
|
||||
assert "px=" in why
|
||||
# 规尖在杆下端 v≈270，爪子在 v≈20–90；采到爪子 v 会 <100
|
||||
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v > 150, f"采到的是爪子不是规尖：{why}"
|
||||
|
||||
|
||||
def test_workpiece_ceiling_keeps_the_real_tip():
|
||||
"""同一帧工件面深度必须抬高近端上限，否则 150mm 会切掉真规尖。
|
||||
|
||||
2026-10-09 真机：孔 camera-Z ≈168mm，固定 tip_near=150mm 把尖切掉，
|
||||
像素在 v=80（杆上半）和 v=188（尖偶尔漏进来）两档跳，6 拍冻不住。
|
||||
上限要留在工件面前方（8mm）：贴着面的像素是工件，不是规。
|
||||
再往前砍 15mm 会把只比面近十来毫米的真尖切掉。
|
||||
"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
for v in range(80, 200):
|
||||
depth[v, 392:408] = 0.08 + (v - 80) / 120.0 * 0.06 # →0.14，在 150mm 内
|
||||
for v in range(200, 280):
|
||||
depth[v, 392:408] = 0.14 + (v - 200) / 80.0 * 0.024 # →0.164，会被 150mm 切掉
|
||||
depth[300:400, 200:600] = 0.172 # 工件面；近端上限 = 面 − 15mm
|
||||
cut, _t0, why0 = _call(_Stub(), depth) # 无 z_wp → 固定 150mm
|
||||
full, tlen, why = _call(_Stub(), depth, z_wp_cam=0.172)
|
||||
assert full is not None and tlen is not None, f"给了工件面仍没量到尖：{why}"
|
||||
v_full = float(why.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v_full > 200, f"有工件面上限仍没采到杆下端：{why}"
|
||||
if cut is not None:
|
||||
v_cut = float(why0.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v_full > v_cut + 20, (
|
||||
f"工件面上限没有比固定 150mm 采到更靠下的尖：cut={why0} full={why}"
|
||||
)
|
||||
|
||||
|
||||
def test_keeps_the_tip_when_it_is_only_slightly_in_front_of_the_surface():
|
||||
"""规尖只比工件面近十来毫米时，必须采尖，不能退回更近的夹爪。
|
||||
|
||||
2026-10-09 观测高度实测：工件面相机深度 127mm，规尖 115mm，夹爪/杆身 80mm。
|
||||
近端砍到「面 − 15mm」会把尖切掉，冻住的是杆身（偏移约 2mm、工具长贴着
|
||||
配置 223mm）。残差 0.2mm 达标，针却偏在孔沿上。
|
||||
"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
depth[140:190, 360:430] = 0.080 # 夹爪：更近、更靠上
|
||||
depth[210:236, 430:470] = 0.118 # 规尖：只比面近 12mm，画面更靠下
|
||||
depth[250:420, 250:600] = 0.130 # 工件面
|
||||
off, tlen, why = _call(_Stub(), depth, z_wp_cam=0.130)
|
||||
assert off is not None and tlen is not None, f"只比面近 12mm 的尖被切掉了：{why}"
|
||||
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v > 200, f"采到的是夹爪不是规尖：{why}"
|
||||
assert abs(float(off[0])) > 0.005, (
|
||||
f"尖偏出中心，不该量成法兰轴：{why} off={off}"
|
||||
)
|
||||
|
||||
|
||||
def test_rejects_pixels_stuck_to_the_workpiece():
|
||||
"""贴着工件面的近端块必须拒绝 —— 那是面，不是规尖。
|
||||
|
||||
2026-10-09 重跑：上限只让 3mm，噪声把工件漏进来，工具长测成 296–315mm
|
||||
（规实际约 190mm），样本互斥，随后两孔中点一跳就被当成工件被推走。
|
||||
"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
depth[200:420, 360:440] = 0.170 # 大块、深度贴着 0.172 的工件面
|
||||
off, tlen, why = _call(_Stub(), depth, z_wp_cam=0.172)
|
||||
assert off is None and tlen is None, f"贴着工件面的块被当成规尖：{why}"
|
||||
assert "工件" in why
|
||||
|
||||
|
||||
def test_rejects_tip_that_lands_on_the_workpiece_surface():
|
||||
"""反投影落在工件面上的点必须丢掉，哪怕相机深度还在近端上限之内。
|
||||
|
||||
2026-10-09：相机深度留了 15mm，工具长仍测成 298mm（配置 223mm），
|
||||
基座 z 贴在工件面上。偏移冻不住 → exit=8 → 观测下降根本不会执行。
|
||||
"""
|
||||
st = _Stub()
|
||||
st.surface_z_m = 1.0 # 高于任何反投影点
|
||||
off, tlen, why = _call(st, _rod_frame())
|
||||
assert off is None and tlen is None, f"贴在工件面上的点被当成规尖：{why}"
|
||||
assert "工件" in why
|
||||
|
||||
|
||||
def test_keeps_tip_that_clears_the_workpiece_surface():
|
||||
"""规尖明显高于工件面时，表面守卫不许把真规尖一起丢掉。"""
|
||||
st = _Stub()
|
||||
st.surface_z_m = 0.05
|
||||
off, tlen, why = _call(st, _rod_frame())
|
||||
assert off is not None and tlen is not None, f"离面足够却被拒：{why}"
|
||||
|
||||
|
||||
def test_rejects_when_nothing_is_near():
|
||||
"""近端什么都没有（规不在视野）⇒ 必须判失败，不能返回一个假偏移。"""
|
||||
depth = np.full((480, 800), 0.50, np.float32) # 全在远端
|
||||
off, tlen, why = _call(_Stub(), depth)
|
||||
assert off is None and tlen is None, "近端没物体却量出了偏移 —— 那是拿台面当规"
|
||||
assert "近端" in why
|
||||
|
||||
|
||||
def test_rejects_when_rod_is_outside_the_center_band():
|
||||
"""物体在画面边上（不是夹在爪里的规）⇒ 必须拒绝。"""
|
||||
off, _tlen, why = _call(_Stub(), _rod_frame(u0=60))
|
||||
assert off is None, "画面边上的东西不是规，不许采信"
|
||||
assert "近端" in why
|
||||
|
||||
|
||||
def test_rejects_implausible_tool_length():
|
||||
"""反投影出的工具长荒谬（量到台面/工件）⇒ 必须拒绝。
|
||||
|
||||
把相机系平移的 z 抬到 0.75 ⇒ 反投影的基座 z 高于法兰 ⇒ 工具长为负。
|
||||
"""
|
||||
st = _Stub(t_cb=(0.50, -0.10, 0.75))
|
||||
off, _tlen, why = _call(st, _rod_frame())
|
||||
assert off is None, "工具长为负也敢采信 —— 会把臂往工件里带"
|
||||
assert "工具长" in why and "不是规" in why
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 冻结：偏移是刚体量，量准一次就定住
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_freezes_offset_after_consistent_samples():
|
||||
"""攒够一致样本后必须**冻结**，且冻结后不再被后续样本改动。
|
||||
|
||||
为什么要冻结：逐拍重算会把"规杆略有倾斜"引入的 ±0.2~0.4mm 抖动带进残差
|
||||
（2026-10-08 真机残差 0.44~0.85mm 来回荡、收不进 0.25mm）。
|
||||
"""
|
||||
st = _Stub()
|
||||
for _ in range(st.tip_calib_n):
|
||||
_feed(st, _rod_frame())
|
||||
assert st._tip_off is not None, "样本一致却没冻结"
|
||||
frozen = st._tip_off.copy()
|
||||
assert any("已冻结" in m for m in st._logger.msgs), "冻结了要讲一声（否则现场不知道基准何时定的）"
|
||||
for _ in range(3): # 再喂几帧：冻结后不许改
|
||||
_feed(st, _rod_frame(u0=402))
|
||||
assert np.allclose(st._tip_off, frozen), "冻结后又被改了 —— 那冻结就没意义"
|
||||
|
||||
|
||||
def test_does_not_freeze_when_samples_disagree_without_a_majority():
|
||||
"""样本分成**势均力敌的两派**时绝不许冻结 —— 宁可继续用退路 + 报失败。
|
||||
|
||||
（若有一派能凑够 ``tip_calib_n`` 拍一致，那按多数派冻结是对的：外点就是靠这个
|
||||
剔掉的 —— 见 test_freezes_offset_after_consistent_samples。）
|
||||
这是 2026-10-08 那次真机"残差 0.15mm 的假达标"的守卫：只要偏移量不准，
|
||||
对准结果就必须判不可信（见 test_success_is_refused_without_tip_offset）。
|
||||
"""
|
||||
st = _Stub()
|
||||
good = _rod_frame() # 偏移 ≈ x=0
|
||||
far = _rod_frame(u0=470) # 挪到别的 u ⇒ x 偏移差 ~21mm
|
||||
for i in range(st.tip_calib_n): # 各一半，谁都不够多数派
|
||||
_feed(st, good if i % 2 == 0 else far)
|
||||
assert st._tip_off is None, "两派势均力敌（各 2 拍）竟然冻住了 —— 定了个假基准"
|
||||
|
||||
|
||||
def test_reports_tool_length_from_the_consistent_samples():
|
||||
"""冻结时要并记工具长，并在与 site.yaml 差 >3mm 时告警（夹持变动的唯一线索）。"""
|
||||
st = _Stub()
|
||||
st.tool_tip_below_flange_m = 0.30 # 故意与量到的 ~0.19m 差很多
|
||||
for _ in range(st.tip_calib_n):
|
||||
_feed(st, _rod_frame())
|
||||
assert st._tip_off is not None
|
||||
assert st._tip_len_m is not None, "冻结了却没记工具长"
|
||||
warn = [m for m in st._logger.msgs if "site.yaml 的工具长" in m]
|
||||
assert warn, "量的工具长与配置差一大截却一声不吭 —— 夹持变动的线索就这么丢了"
|
||||
|
||||
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 守卫测试：精对准必须真的用规尖，且保留退路
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_align_loop_uses_visual_tip_with_flange_fallback():
|
||||
"""源码守卫（语法树判据）：精对准循环里
|
||||
|
||||
· 必须调用 ``_tip_offset_from_depth``（视觉规尖）；
|
||||
· 必须调用 ``_refresh_tip_offset``（量准一次就冻结）；
|
||||
· 必须仍能用 ``_gauge_xy`` 兜底（规尖量不到时不至于完全没方向）；
|
||||
· 结果里要标出这一拍用的是哪个（``gauge_source``），否则现场分不清。
|
||||
"""
|
||||
import ast
|
||||
from pathlib import Path
|
||||
|
||||
src = (
|
||||
Path(__file__).resolve().parents[1]
|
||||
/ "arm_application"
|
||||
/ "stage2_locate_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
tree = ast.parse(src)
|
||||
attrs = {n.attr for n in ast.walk(tree) if isinstance(n, ast.Attribute)}
|
||||
assert "_tip_offset_from_depth" in attrs, "精对准没用视觉规尖 —— 又回到拿法兰对孔了"
|
||||
assert "_refresh_tip_offset" in attrs, "偏移没走『量准一次就冻结』的路子 —— 残差会被抖动拖住"
|
||||
assert "_gauge_xy" in attrs, "连退路都没了：规尖一时量不到就完全没方向"
|
||||
consts = {n.value for n in ast.walk(tree) if isinstance(n, ast.Constant)}
|
||||
assert "gauge_source" in consts, "结果里没标这一拍用的是规尖还是法兰，现场没法核对"
|
||||
assert "align_trusted" in consts, "结果里没标这次对准可不可信，现场没法核对"
|
||||
|
||||
|
||||
def test_success_is_refused_without_tip_offset():
|
||||
"""**残差达标但规尖偏移没量到**时，必须判失败 —— 不许把没做到说成做到。
|
||||
|
||||
2026-10-08 真机复跑就栽在这：``tip_offset_m=None``、残差 0.15mm 看着漂亮，
|
||||
其实那一拍用的是**法兰轴线**（退路），法兰压在 k1 上、规尖偏出 ~11mm。
|
||||
没有这条守卫，日志会报"二阶段 OK 残差 0.15mm"，而规根本没对上孔。
|
||||
|
||||
判据走**语法树**而不是文本匹配：必须存在一个"残差达标"的分支，其（含嵌套的）
|
||||
分支体里同时写了 ``align_trusted`` 与 ``fail_reason``。文本匹配会被方法里
|
||||
别处的 `_tip_off is None` 蒙混过去（试过，真的会漏）。
|
||||
"""
|
||||
import ast
|
||||
from pathlib import Path
|
||||
|
||||
src = (
|
||||
Path(__file__).resolve().parents[1]
|
||||
/ "arm_application"
|
||||
/ "stage2_locate_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
fn = next(
|
||||
n
|
||||
for n in ast.walk(ast.parse(src))
|
||||
if isinstance(n, ast.FunctionDef) and n.name == "_run_locate"
|
||||
)
|
||||
|
||||
def _consts(node: ast.AST) -> set:
|
||||
return {x.value for x in ast.walk(node) if isinstance(x, ast.Constant)}
|
||||
|
||||
tol_branches = [
|
||||
n for n in ast.walk(fn) if isinstance(n, ast.If) and "align_tol" in ast.unparse(n.test)
|
||||
]
|
||||
assert tol_branches, "没找到『残差达标』那个分支 —— 结构变了，这条守卫要跟着改"
|
||||
guarded = [
|
||||
n for n in tol_branches if {"align_trusted", "fail_reason"} <= _consts(n)
|
||||
]
|
||||
assert guarded, (
|
||||
"『残差达标』分支里没有『规尖没量到 ⇒ align_trusted=False + fail_reason』—— "
|
||||
"假达标（对的是法兰）又会蒙混成成功"
|
||||
)
|
||||
|
||||
|
||||
def test_default_is_tip_vision_on_in_site():
|
||||
"""site.yaml 必须默认开视觉规尖，且容差收到 0.25mm。
|
||||
|
||||
容差是关键：0.7mm 会让"规偏 10mm"这种系统偏差在容差里被放过去
|
||||
（原来残差 0.69mm 看着"通过"，其实是拿法兰对的孔）。
|
||||
"""
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
|
||||
site = yaml.safe_load(
|
||||
(Path(__file__).resolve().parents[3] / "config" / "site.yaml").read_text(encoding="utf-8")
|
||||
)
|
||||
st2 = ((site.get("hole_detect") or {}).get("stage2") or {})
|
||||
robots = ((site.get("robot") or {}).get("robots") or [])
|
||||
arm89 = next(r for r in robots if str(r.get("sn")) == "Rizon4-063989")
|
||||
servo = arm89.get("servo") or {}
|
||||
assert float(servo.get("ee_z_m")) == pytest.approx(
|
||||
float(servo.get("hole_hover_z_m"))
|
||||
), "左臂跟踪锁高必须等于孔悬停高：对孔完成前不许改高度"
|
||||
assert st2.get("align_tip_vision") is True, "默认必须用视觉规尖"
|
||||
assert float(st2.get("align_tol_m")) <= 0.00025 + 1e-9, (
|
||||
f"align_tol_m={st2.get('align_tol_m')} 太松：会放过系统性的对孔偏差"
|
||||
)
|
||||
assert int(st2.get("align_max_iters")) >= 12, "收紧容差后迭代上限要跟上"
|
||||
|
||||
|
||||
class _TrimStub:
|
||||
"""下降后 XY 修正用到的成员。孔在基座里不动，规尖偏移是常数。"""
|
||||
|
||||
tip_vision = True
|
||||
hole_keypoint_once = False
|
||||
k1_label = "left"
|
||||
target_hole = "k1"
|
||||
align_tol = 0.00025
|
||||
align_speed = 0.02
|
||||
vert_tol_rad = 0.02
|
||||
base_frame = "world"
|
||||
ee_frame = "flange"
|
||||
cam_frame = "cam"
|
||||
image_topic = "/cam/color"
|
||||
_tip_off = np.array([0.01, 0.0])
|
||||
_tip_samp: list = []
|
||||
_tip_len_m = 0.22
|
||||
_ws_clipped = False
|
||||
final_descent_enable = False
|
||||
tool_tip_below_flange_m = 0.223
|
||||
surface_z_m = 0.022
|
||||
final_gap_m = 0.002
|
||||
final_descent_speed = 0.005
|
||||
|
||||
def __init__(self, err_xy=(0.0026, -0.0004), tip=None) -> None:
|
||||
self.hole = np.array([0.50 + err_xy[0], -0.10 + err_xy[1], 0.022])
|
||||
self.tip = None if tip is False else np.array([0.0015, -0.0012])
|
||||
self.moves: list = []
|
||||
self.tip_calls = 0
|
||||
self._logger = _StubLogger()
|
||||
self.pair_dy = -0.060
|
||||
self.hole_after_move = None
|
||||
self.pair_dy_after_move = None
|
||||
|
||||
def get_logger(self):
|
||||
return self._logger
|
||||
|
||||
def _lookup(self, *_a):
|
||||
return None
|
||||
|
||||
def _pin(self, _q) -> None:
|
||||
return None
|
||||
|
||||
def _snap_holes(self, *, skip_center_gate: bool):
|
||||
hole = self.hole
|
||||
pair_dy = self.pair_dy
|
||||
if self.moves:
|
||||
if self.hole_after_move is not None:
|
||||
hole = self.hole_after_move
|
||||
if self.pair_dy_after_move is not None:
|
||||
pair_dy = self.pair_dy_after_move
|
||||
res = {
|
||||
"status": True,
|
||||
"hole_base": [hole.tolist(), (hole + np.array([0.0, pair_dy, 0.0])).tolist()],
|
||||
"z_wp": 0.127,
|
||||
}
|
||||
return None, None, {}, res
|
||||
|
||||
def _tip_offset_from_depth(self, *_a, **_kw):
|
||||
self.tip_calls += 1
|
||||
if self.tip is None:
|
||||
return None, None, "没看到规"
|
||||
off = self.tip.copy()
|
||||
if self.hole_keypoint_once:
|
||||
off = off + np.array([0.018 * (self.tip_calls % 3), 0.0])
|
||||
return off, 0.222, "px=(447,226)"
|
||||
|
||||
def _stamp_bias(self, rec, *, tgt_b, gxy, tag):
|
||||
rec = dict(rec)
|
||||
rec["tag"] = tag
|
||||
rec["err_xy_m"] = [
|
||||
round(float(tgt_b[0]) - float(gxy[0]), 5),
|
||||
round(float(tgt_b[1]) - float(gxy[1]), 5),
|
||||
]
|
||||
rec["along_look_mm"] = None
|
||||
rec["along_right_mm"] = None
|
||||
return rec
|
||||
|
||||
def _publish_result(self, _payload) -> None:
|
||||
return None
|
||||
|
||||
def _goto(self, _q, Tee, target, **_kw):
|
||||
Tee = np.array(Tee, float)
|
||||
self.moves.append(np.array(target, float))
|
||||
Tee[0, 3] = float(target[0])
|
||||
Tee[1, 3] = float(target[1])
|
||||
Tee[2, 3] = float(target[2])
|
||||
return True, _q, Tee
|
||||
|
||||
|
||||
def _trim(st: _TrimStub, z: float = 0.260):
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.50
|
||||
Tee[1, 3] = -0.10
|
||||
Tee[2, 3] = z
|
||||
final = {"ok": True, "residual_mm": 0.22, "gauge_source": "视觉规尖(已冻结)"}
|
||||
return Stage2Locate._trim_xy_after_descent(st, np.zeros(7), Tee, final) # type: ignore[arg-type]
|
||||
|
||||
|
||||
def test_trim_after_descent_corrects_xy_and_holds_z():
|
||||
"""下降后孔位漂了 2mm 级，要在当前高度补上，不许改高度。
|
||||
|
||||
2026-10-09：悬停残差 0.22mm，降到观测高度后同一孔的基座 XY 差约 2.6mm。
|
||||
"""
|
||||
st = _TrimStub()
|
||||
_q, Tee, final = _trim(st)
|
||||
assert abs(float(Tee[2, 3]) - 0.260) < 1e-9, "下降后修正不许改高度"
|
||||
tip = np.array([Tee[0, 3] + st.tip[0], Tee[1, 3] + st.tip[1]])
|
||||
err = float(np.hypot(tip[0] - st.hole[0], tip[1] - st.hole[1]))
|
||||
assert err <= 0.00025 + 1e-6, f"补完针还偏 {err*1000:.2f}mm"
|
||||
assert final["residual_mm"] <= 0.25
|
||||
assert "下降后" in final["gauge_source"]
|
||||
|
||||
|
||||
def test_trim_refuses_a_centimeter_scale_correction():
|
||||
"""要补的量到了厘米，就是采到夹爪了，必须停在原地。"""
|
||||
st = _TrimStub(err_xy=(0.014, 0.0))
|
||||
_q, Tee, final = _trim(st)
|
||||
assert not st.moves, "厘米级修正被执行了"
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert final["residual_mm"] == 0.22, "拒绝修正时不能把悬停结果改写成没补上的残差"
|
||||
assert any("不像针" in m for m in st._logger.msgs)
|
||||
|
||||
|
||||
def test_trim_does_not_move_when_the_tip_is_missing():
|
||||
"""下降后没量到规尖，保持悬停对准的位置。"""
|
||||
st = _TrimStub(tip=False)
|
||||
_q, Tee, final = _trim(st)
|
||||
assert not st.moves
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert final["gauge_source"] == "视觉规尖(已冻结)"
|
||||
|
||||
|
||||
def test_once_trim_keeps_hover_tip_when_depth_jumps():
|
||||
"""单次精拍：沿用悬停规尖，只跳一次、步长 ≤3mm，剩下的不许再追。
|
||||
|
||||
2026-10-13：两跳合计 4.3mm 把近距检测带崩。5mm 交平面漂只许吃掉 3mm。
|
||||
"""
|
||||
frozen = np.array([0.0001, -0.0003])
|
||||
st = _TrimStub(err_xy=(0.0044, -0.0024))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = frozen.copy()
|
||||
st.align_tol = 0.00025
|
||||
_q, Tee, final = _trim(st)
|
||||
assert st.tip_calls == 0, "单次精拍下降后不许再走深度重测规尖"
|
||||
assert abs(float(Tee[2, 3]) - 0.260) < 1e-9, "下降后修正不许改高度"
|
||||
assert len(st.moves) == 1, f"只许一跳，实际 {len(st.moves)} 跳"
|
||||
hop = float(np.hypot(float(st.moves[0][0]) - 0.50, float(st.moves[0][1]) + 0.10))
|
||||
assert hop <= 0.003 + 1e-9, f"一跳 {hop*1000:.2f}mm 超过 3mm"
|
||||
gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]])
|
||||
err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1]))
|
||||
assert 0.0015 <= err <= 0.0025, f"3mm 一跳后应剩约 2mm，实际 {err*1000:.2f}mm"
|
||||
assert final["residual_mm"] == pytest.approx(err * 1000.0, abs=0.02)
|
||||
assert "悬停冻结" in final["gauge_source"]
|
||||
assert final["residual_remeasured"] is True
|
||||
assert final.get("trim_reverted") is False
|
||||
|
||||
|
||||
def test_once_trim_refuses_without_frozen_tip():
|
||||
"""没有悬停冻住的规尖，不许用法兰或乱跳的深度尖去补。"""
|
||||
st = _TrimStub(err_xy=(0.0044, 0.0))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = None
|
||||
_q, Tee, final = _trim(st)
|
||||
assert not st.moves
|
||||
assert st.tip_calls == 0
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert any("冻住的规尖" in m for m in st._logger.msgs)
|
||||
|
||||
|
||||
def test_once_trim_refuses_a_centimeter_scale_hole_shift():
|
||||
"""孔 XY 要补到厘米，不是交平面那 4mm，必须停。"""
|
||||
st = _TrimStub(err_xy=(0.014, 0.0))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = np.array([0.0001, -0.0003])
|
||||
_q, Tee, final = _trim(st)
|
||||
assert not st.moves
|
||||
assert final["residual_mm"] == 0.22
|
||||
assert any("不像交平面" in m for m in st._logger.msgs)
|
||||
|
||||
|
||||
def test_once_trim_reverts_when_pair_breaks():
|
||||
"""一跳后两孔间距从 60mm 掉到 45mm：检测漂了，必须撤回。"""
|
||||
frozen = np.array([0.0001, -0.0003])
|
||||
st = _TrimStub(err_xy=(0.0026, 0.0))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = frozen.copy()
|
||||
st.pair_dy_after_move = -0.045
|
||||
_q, Tee, final = _trim(st)
|
||||
assert len(st.moves) == 2, "补完变差必须再走一跳撤回"
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert abs(float(Tee[1, 3]) + 0.10) < 1e-9
|
||||
assert final.get("trim_reverted") is True
|
||||
assert any("撤回" in m for m in st._logger.msgs)
|
||||
gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]])
|
||||
err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1]))
|
||||
assert abs(err - 0.0025) < 2e-4, "撤回后残差应回到走前那一档"
|
||||
assert final["residual_mm"] == pytest.approx(err * 1000.0, abs=0.05)
|
||||
|
||||
|
||||
def test_once_trim_reverts_when_residual_worsens():
|
||||
"""一跳后残差变大：停并撤回，终值不许还是走前那份假装没动。"""
|
||||
frozen = np.array([0.0001, -0.0003])
|
||||
st = _TrimStub(err_xy=(0.0026, 0.0))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = frozen.copy()
|
||||
st.hole_after_move = np.array([0.50 + 0.015, -0.10, 0.022])
|
||||
_q, Tee, final = _trim(st)
|
||||
assert len(st.moves) == 2
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert final.get("trim_reverted") is True
|
||||
assert any("撤回" in m for m in st._logger.msgs)
|
||||
assert final["residual_remeasured"] is True
|
||||
assert float(final["residual_mm"]) < 5.0, "撤回后不许把 15mm 漂检测当终值"
|
||||
|
||||
|
||||
def _enable_blend(st: _TrimStub) -> None:
|
||||
"""观察高 15mm（法兰 0.260）斜线收到贴面 2mm（法兰 0.247）。"""
|
||||
st.final_descent_enable = True
|
||||
st.tool_tip_below_flange_m = 0.223
|
||||
st.surface_z_m = 0.022
|
||||
st.final_gap_m = 0.002
|
||||
st.final_descent_speed = 0.005
|
||||
|
||||
|
||||
def test_once_trim_blend_drops_z_and_xy():
|
||||
"""观察高测到的残差和剩余 Z 合成一跳，不许再切 4mm 贴面。"""
|
||||
frozen = np.array([0.0001, -0.0003])
|
||||
st = _TrimStub(err_xy=(0.0044, -0.0024))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = frozen.copy()
|
||||
st.align_tol = 0.00025
|
||||
_enable_blend(st)
|
||||
_q, Tee, final = _trim(st)
|
||||
assert len(st.moves) == 1, f"斜线只许一跳，实际 {len(st.moves)}"
|
||||
assert abs(float(Tee[2, 3]) - 0.247) < 1e-9, "必须收到贴面法兰高"
|
||||
hop_xy = float(np.hypot(float(st.moves[0][0]) - 0.50, float(st.moves[0][1]) + 0.10))
|
||||
assert hop_xy <= 0.003 + 1e-9
|
||||
gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]])
|
||||
err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1]))
|
||||
assert 0.0015 <= err <= 0.0025
|
||||
assert final.get("trim_reverted") is False
|
||||
|
||||
|
||||
def test_once_trim_blend_z_only_when_xy_already_in_tol():
|
||||
"""观察高 XY 已在容差，斜线仍要收 Z，不许停在 15mm。"""
|
||||
frozen = np.array([0.0001, -0.0003])
|
||||
st = _TrimStub(err_xy=(0.0001, -0.0003))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = frozen.copy()
|
||||
_enable_blend(st)
|
||||
_q, Tee, final = _trim(st)
|
||||
assert len(st.moves) == 1
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert abs(float(Tee[2, 3]) - 0.247) < 1e-9
|
||||
assert final.get("trim_reverted") is False
|
||||
|
||||
|
||||
def test_once_trim_blend_refuses_pair_does_not_drop_z():
|
||||
"""观察高间距已经坏了，不许斜线贴面。"""
|
||||
st = _TrimStub(err_xy=(0.004, 0.0))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = np.array([0.0001, -0.0003])
|
||||
st.pair_dy = -0.045
|
||||
_enable_blend(st)
|
||||
_q, Tee, final = _trim(st)
|
||||
assert not st.moves
|
||||
assert abs(float(Tee[2, 3]) - 0.260) < 1e-9
|
||||
assert final["residual_mm"] == 0.22
|
||||
|
||||
|
||||
def test_once_trim_blend_keeps_pose_when_close_snap_breaks():
|
||||
"""贴面复拍间距漂了：近距不可信，观察高算的斜线位姿必须留下。"""
|
||||
frozen = np.array([0.0001, -0.0003])
|
||||
st = _TrimStub(err_xy=(0.0026, 0.0))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = frozen.copy()
|
||||
st.pair_dy_after_move = -0.045
|
||||
_enable_blend(st)
|
||||
_q, Tee, final = _trim(st)
|
||||
assert len(st.moves) == 1, "贴面复拍漂了不许撤回观察高"
|
||||
assert abs(float(Tee[2, 3]) - 0.247) < 1e-9
|
||||
assert abs(float(Tee[0, 3]) - 0.50) > 1e-4, "XY 补偿被撤回了"
|
||||
assert final.get("trim_reverted") is not True
|
||||
assert final.get("close_snap_untrusted") is True
|
||||
|
||||
|
||||
def test_once_trim_refuses_when_pair_already_broken():
|
||||
"""第一拍间距已经 45mm，检测没立住，不许再动。"""
|
||||
st = _TrimStub(err_xy=(0.004, 0.0))
|
||||
st.hole_keypoint_once = True
|
||||
st._tip_off = np.array([0.0001, -0.0003])
|
||||
st.pair_dy = -0.045
|
||||
_q, Tee, final = _trim(st)
|
||||
assert not st.moves
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert final["residual_mm"] == 0.22
|
||||
assert any("检测漂了" in m for m in st._logger.msgs)
|
||||
@@ -0,0 +1,87 @@
|
||||
"""触觉"有没有数据"的判据：为什么不能拿力的大小当传感器活着的代理。
|
||||
|
||||
背景（真实踩到）：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,
|
||||
should_lift_air_baseline,
|
||||
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
|
||||
|
||||
|
||||
def test_no_tactile_does_not_lift_to_air_baseline():
|
||||
"""触觉没数据时不许先抬到 hover+50mm 再报断流。"""
|
||||
assert should_lift_air_baseline(0.385, 0.470, tactile_live=False) is False
|
||||
assert should_lift_air_baseline(0.249, 0.470, tactile_live=True) is True
|
||||
assert should_lift_air_baseline(0.468, 0.470, tactile_live=True) is False
|
||||
@@ -0,0 +1,247 @@
|
||||
traj_coord_type: ""
|
||||
traj_coord_ref: ""
|
||||
pattern_params {
|
||||
pattern_name: "WPT0"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.26072"
|
||||
y: "-0.14604"
|
||||
z: "0.85707"
|
||||
rx: "6.58"
|
||||
ry: "165.63"
|
||||
rz: "-6.63"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "-6.97"
|
||||
joint2: "29.95"
|
||||
joint3: "1.72"
|
||||
joint4: "92.29"
|
||||
joint5: "-6.48"
|
||||
joint6: "-12.54"
|
||||
joint7: "-0.37"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
pattern_params {
|
||||
pattern_name: "WPT1"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.50244"
|
||||
y: "-0.05458"
|
||||
z: "0.62368"
|
||||
rx: "3.30681"
|
||||
ry: "173.28739"
|
||||
rz: "12.76833"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "-0.39471"
|
||||
joint2: "2.89069"
|
||||
joint3: "7.36936"
|
||||
joint4: "104.4854"
|
||||
joint5: "-2.56594"
|
||||
joint6: "19.41153"
|
||||
joint7: "-5.21738"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
pattern_params {
|
||||
pattern_name: "WPT2"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.38216"
|
||||
y: "-0.07015"
|
||||
z: "0.2772"
|
||||
rx: "-0.76189"
|
||||
ry: "178.83063"
|
||||
rz: "13.77851"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "-0.51679"
|
||||
joint2: "-17.82083"
|
||||
joint3: "6.09068"
|
||||
joint4: "145.99388"
|
||||
joint5: "-3.83264"
|
||||
joint6: "75.52854"
|
||||
joint7: "-5.23384"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
pattern_params {
|
||||
pattern_name: "WPT3"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.42395"
|
||||
y: "-0.04059"
|
||||
z: "0.135"
|
||||
rx: "-4.09647"
|
||||
ry: "175.59924"
|
||||
rz: "22.59976"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "0.80285"
|
||||
joint2: "-48.57114"
|
||||
joint3: "4.29418"
|
||||
joint4: "137.232"
|
||||
joint5: "-14.43138"
|
||||
joint6: "99.59405"
|
||||
joint7: "-5.25622"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
pattern_params {
|
||||
pattern_name: "WPT4"
|
||||
pattern_type: "Scatter"
|
||||
scatterpoint_pattern {
|
||||
pose_params {
|
||||
x: "0.2971"
|
||||
y: "0.06042"
|
||||
z: "0.76959"
|
||||
rx: "4.64468"
|
||||
ry: "173.46657"
|
||||
rz: "27.06819"
|
||||
coord_type: "WORLD"
|
||||
coord_ref: "WORLD_ORIGIN"
|
||||
use_quaternion: false
|
||||
preferJntPos {
|
||||
joint1: "5.13584"
|
||||
joint2: "28.76765"
|
||||
joint3: "17.15688"
|
||||
joint4: "100.21538"
|
||||
joint5: "4.05433"
|
||||
joint6: "-9.64651"
|
||||
joint7: "-5.26232"
|
||||
}
|
||||
externalAxes {
|
||||
axis1: "0"
|
||||
axis2: "0"
|
||||
axis3: "0"
|
||||
axis4: "0"
|
||||
axis5: "0"
|
||||
axis6: "0"
|
||||
}
|
||||
}
|
||||
force_params {
|
||||
fx: "0"
|
||||
fy: "0"
|
||||
fz: "0"
|
||||
mx: "0"
|
||||
my: "0"
|
||||
mz: "0"
|
||||
}
|
||||
path_profile: "line"
|
||||
blend_zone: "Z10"
|
||||
velocity: "0.2"
|
||||
acceleration: "1"
|
||||
vel_var_name: ""
|
||||
wrench_var_name: ""
|
||||
}
|
||||
}
|
||||
group_params {
|
||||
group_name: "RecordGroup"
|
||||
pattern_start_idx: 0
|
||||
pattern_end_idx: 4
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
{
|
||||
"source": "http://git.orisys-tech.cn/orisys/ros_contorl.git",
|
||||
"branch": "exp/hole-keypoint-once",
|
||||
"commit": "561d7da21f20d3e63030f81510ae2f29eed01d46",
|
||||
"date": "2026-10-20",
|
||||
"scope": "ROS nodes and supporting libraries/launch/config/tests only",
|
||||
"upstream_identical": [
|
||||
"arm_application/box_tracker.py",
|
||||
"arm_application/demo_named_poses.py",
|
||||
"arm_application/dh_official.py",
|
||||
"arm_application/dh_pgea.py",
|
||||
"arm_application/dual_gripper.py",
|
||||
"arm_application/dual_gripper_cli.py",
|
||||
"arm_application/dual_gripper_node.py",
|
||||
"arm_application/feature_guard.py",
|
||||
"arm_application/feature_jump_guard.py",
|
||||
"arm_application/gauge_assembly_node.py",
|
||||
"arm_application/gauge_boot.py",
|
||||
"arm_application/gauge_config.py",
|
||||
"arm_application/gauge_fsm.py",
|
||||
"arm_application/gauge_gripper.py",
|
||||
"arm_application/gauge_kin.py",
|
||||
"arm_application/geom_ik.py",
|
||||
"arm_application/goto_ee_plane.py",
|
||||
"arm_application/handeye_calibrate.py",
|
||||
"arm_application/handeye_lib.py",
|
||||
"arm_application/handeye_realsense.py",
|
||||
"arm_application/handeye_teach.py",
|
||||
"arm_application/handeye_waypoints.py",
|
||||
"arm_application/hole_detect_hough.py",
|
||||
"arm_application/hole_detect_node.py",
|
||||
"arm_application/hole_detector.py",
|
||||
"arm_application/image_utils.py",
|
||||
"arm_application/kalman_filter.py",
|
||||
"arm_application/model_labels.py",
|
||||
"arm_application/move_to_pose_client.py",
|
||||
"arm_application/onnx_workpiece.py",
|
||||
"arm_application/orisys_tactile.py",
|
||||
"arm_application/overhead_oneshot.py",
|
||||
"arm_application/path_profile.py",
|
||||
"arm_application/perception_node.py",
|
||||
"arm_application/pid_controller.py",
|
||||
"arm_application/plane_feature.py",
|
||||
"arm_application/pose_detector.py",
|
||||
"arm_application/realsense_sdk_view.py",
|
||||
"arm_application/ros_node.py",
|
||||
"arm_application/run_joint_trajectory.py",
|
||||
"arm_application/servo_node.py",
|
||||
"arm_application/site_io.py",
|
||||
"arm_application/stage3_search_node.py",
|
||||
"arm_application/tactile_cli.py",
|
||||
"arm_application/tactile_interface.py",
|
||||
"arm_application/tactile_node.py",
|
||||
"arm_application/yolo_detector.py",
|
||||
"config/demo.yaml",
|
||||
"config/dual_grippers.yaml",
|
||||
"config/hole_detect.yaml",
|
||||
"config/hole_insert.yaml",
|
||||
"config/perception.yaml",
|
||||
"config/servo.yaml",
|
||||
"config/xunhuan_trajectory.yaml",
|
||||
"launch/demo.launch.py",
|
||||
"launch/dh_pgea.launch.py",
|
||||
"launch/dual_gripper.launch.py",
|
||||
"launch/gauge_assembly.launch.py",
|
||||
"launch/hole_detect.launch.py",
|
||||
"launch/stage2_locate.launch.py",
|
||||
"launch/stage3_search.launch.py",
|
||||
"launch/visual_servo.launch.py",
|
||||
"package.xml",
|
||||
"resource/arm_application",
|
||||
"setup.cfg",
|
||||
"test/test_feature_jump_guard.py",
|
||||
"test/test_ft_source_guard.py",
|
||||
"test/test_gauge_boot.py",
|
||||
"test/test_gauge_config.py",
|
||||
"test/test_gauge_exit_lift.py",
|
||||
"test/test_gauge_fsm.py",
|
||||
"test/test_gauge_gripper.py",
|
||||
"test/test_gauge_gripper_client.py",
|
||||
"test/test_gauge_kin.py",
|
||||
"test/test_gauge_step_events.py",
|
||||
"test/test_handeye_lib.py",
|
||||
"test/test_handeye_lib_roundtrip.py",
|
||||
"test/test_hole_detector.py",
|
||||
"test/test_image_utils.py",
|
||||
"test/test_model_labels.py",
|
||||
"test/test_node_handler_collisions.py",
|
||||
"test/test_onnx_workpiece_guard.py",
|
||||
"test/test_orisys_vector.py",
|
||||
"test/test_perception_acquire_gate.py",
|
||||
"test/test_pose_detector.py",
|
||||
"test/test_ros_node.py",
|
||||
"test/test_site_arm_sections.py",
|
||||
"test/test_stage2_descent_clamp.py",
|
||||
"test/test_stage2_no_follow.py",
|
||||
"test/test_stage2_tip_align.py",
|
||||
"test/test_stage3_tactile_live.py",
|
||||
"trajectories/xunhuan.traj"
|
||||
],
|
||||
"locally_adapted": [
|
||||
"README.md",
|
||||
"arm_application/__init__.py",
|
||||
"arm_application/dh_pgea_node.py",
|
||||
"arm_application/pipeline_node.py",
|
||||
"arm_application/realsense_color_pub.py",
|
||||
"arm_application/stage2_locate_node.py",
|
||||
"launch/pipeline.launch.py",
|
||||
"launch/tactile.launch.py",
|
||||
"setup.py",
|
||||
"test/test_dh_pgea_latch.py",
|
||||
"test/test_feature_guard.py",
|
||||
"test/test_handeye_mode.py",
|
||||
"test/test_listed_arms.py",
|
||||
"test/test_overhead_oneshot.py",
|
||||
"test/test_overhead_track.py",
|
||||
"test/test_pipeline_stage1_diag.py",
|
||||
"test/test_stage2_coarse_guard.py"
|
||||
],
|
||||
"excluded_out_of_scope": [
|
||||
"arm_application/operator_app.py",
|
||||
"arm_application/operator_arm_mesh.py",
|
||||
"arm_application/operator_arm_state.py",
|
||||
"arm_application/operator_camera_view.py",
|
||||
"arm_application/operator_console.html",
|
||||
"arm_application/operator_console.py",
|
||||
"arm_application/operator_console_logo.png",
|
||||
"arm_application/operator_gripper_mesh.py",
|
||||
"arm_application/operator_gripper_state.py",
|
||||
"arm_application/operator_handeye_board.py",
|
||||
"arm_application/operator_tactile_view.py",
|
||||
"arm_application/operator_teach.py",
|
||||
"arm_application/realsense_view.py",
|
||||
"arm_application/weights_layout.py",
|
||||
"test/test_camera_preview.py",
|
||||
"test/test_command_lanes.py",
|
||||
"test/test_gauge_result_gate.py",
|
||||
"test/test_handeye_preview.py",
|
||||
"test/test_merge_handeye_poses.py",
|
||||
"test/test_operator_arm_mesh.py",
|
||||
"test/test_operator_arm_state.py",
|
||||
"test/test_operator_gripper_mesh.py",
|
||||
"test/test_operator_gripper_state.py",
|
||||
"test/test_real_geometry_check.py",
|
||||
"test/test_real_preflight.py",
|
||||
"test/test_ros_domain.py",
|
||||
"test/test_usb_health.py",
|
||||
"test/test_v4l_health.py",
|
||||
"test/test_weights_layout.py"
|
||||
],
|
||||
"external_compatibility": [
|
||||
"scripts/lib/flexiv_movel_ipc.py",
|
||||
"src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp",
|
||||
"station/src/station/control_service.py",
|
||||
"station/src/station/ros_gateway.py"
|
||||
],
|
||||
"station_stage_integration": {
|
||||
"node": "arm_application.a7_rotation_node",
|
||||
"alignment": "updated stage2 with configured final descent; hole_entry_mm=1.0",
|
||||
"rotation": "explicit upstream A7 cycle with local typed GripperExecute; no shell",
|
||||
"full_flow": "tracking -> alignment -> A7; failure/cancel prevents next stage",
|
||||
"hardware_motion_tested": false
|
||||
}
|
||||
}
|
||||
@@ -34,8 +34,8 @@ rizon_arm_controller:
|
||||
allow_partial_joints_goal: false
|
||||
interpolation_method: splines
|
||||
allow_nonzero_velocity_at_trajectory_end: true
|
||||
# Stream from last command (no snap-back to encoder each preempt).
|
||||
open_loop_control: true
|
||||
# New trajectories start at measured joints, including after a native MoveL.
|
||||
open_loop_control: false
|
||||
constraints:
|
||||
stopped_velocity_tolerance: 0.25
|
||||
goal_time: 0.0
|
||||
|
||||
@@ -19,6 +19,14 @@ find_package(hardware_interface REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(flexiv_rdk REQUIRED)
|
||||
find_package(flexiv_drdk QUIET)
|
||||
# RDK is a static archive built against flexiv_install's spdlog 1.9 / fmt v8.
|
||||
# A generic find_package(spdlog) in ros_flexiv resolves conda's 1.14 + fmt 11,
|
||||
# and ros2_control_node then dies with undefined fmt::v8 symbols.
|
||||
set(_flexiv_spdlog_dir "$ENV{HOME}/flexiv_install/lib/cmake/spdlog")
|
||||
if(NOT EXISTS "${_flexiv_spdlog_dir}/spdlogConfig.cmake")
|
||||
message(FATAL_ERROR "flexiv_hardware needs spdlog from ~/flexiv_install (RDK fmt v8). Missing ${_flexiv_spdlog_dir}")
|
||||
endif()
|
||||
find_package(spdlog REQUIRED PATHS "${_flexiv_spdlog_dir}" NO_DEFAULT_PATH)
|
||||
|
||||
add_library(
|
||||
${PROJECT_NAME}
|
||||
|
||||
@@ -28,6 +28,7 @@
|
||||
#include <hardware_interface/types/hardware_interface_return_values.hpp>
|
||||
|
||||
#include "flexiv_hardware/visibility_control.h"
|
||||
#include "flexiv_hardware/position_command_fence.hpp"
|
||||
|
||||
// Flexiv
|
||||
#include "flexiv/rdk/robot.hpp"
|
||||
@@ -144,6 +145,9 @@ private:
|
||||
// D435 粗定位的 MoveL 走已经连上的这份 RDK。write() 里一步一步推进，
|
||||
// 不另开线程，也不在这段里往关节接口灌点。请求文件是 /tmp/flexiv_movel_req。
|
||||
enum class VendorMovelPhase { kIdle, kRunning };
|
||||
MotionExecutionLatch motion_execution_latch_;
|
||||
PositionCommandFence position_command_fence_;
|
||||
bool reset_nrt_gate_ = true;
|
||||
VendorMovelPhase vendor_movel_phase_ = VendorMovelPhase::kIdle;
|
||||
long vendor_movel_seq_ = -1;
|
||||
long vendor_movel_done_seq_ = -1;
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
#pragma once
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
namespace flexiv_hardware {
|
||||
// A released E-stop / restored Enable is not permission to resume an old task.
|
||||
// Only explicit hardware lifecycle activation can reset this latch.
|
||||
class MotionExecutionLatch {
|
||||
public:
|
||||
void activate() { interrupted_ = false; }
|
||||
bool observe(bool connected, bool operational, bool fault) {
|
||||
if (!connected || !operational || fault) interrupted_ = true;
|
||||
return !interrupted_;
|
||||
}
|
||||
private:
|
||||
bool interrupted_ = true;
|
||||
};
|
||||
|
||||
// After activation/mode changes, a command stream must start at measured joints.
|
||||
// An old held trajectory target must never pull the arm back to its previous pose.
|
||||
class PositionCommandFence {
|
||||
public:
|
||||
void reset() { waiting_ = true; }
|
||||
bool accept(const std::vector<double>& command, const std::vector<double>& state) {
|
||||
if (command.empty() || command.size() != state.size()) return false;
|
||||
for (size_t i = 0; i < command.size(); ++i) {
|
||||
if (!std::isfinite(command[i]) || !std::isfinite(state[i])) return false;
|
||||
if (waiting_ && std::abs(command[i] - state[i]) > 0.01) return false;
|
||||
}
|
||||
waiting_ = false;
|
||||
return true;
|
||||
}
|
||||
private:
|
||||
bool waiting_ = true;
|
||||
};
|
||||
} // namespace flexiv_hardware
|
||||
@@ -632,6 +632,9 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_activate(
|
||||
return hardware_interface::CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
// Clear any primitive/trajectory still retained on the robot server BEFORE
|
||||
// Enable. If Stop cannot be confirmed, fail activation rather than resume.
|
||||
robot_->Stop();
|
||||
// Enable the robot (retries while waiting for operational)
|
||||
RCLCPP_INFO(getLogger(), "Enabling robot ...");
|
||||
robot_->Enable();
|
||||
@@ -650,6 +653,9 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_activate(
|
||||
|
||||
RCLCPP_INFO(getLogger(), "System successfully started!");
|
||||
// 启动前磁盘上的旧请求不许再执行一遍。新请求用更晚的序号。
|
||||
motion_execution_latch_.activate();
|
||||
position_command_fence_.reset();
|
||||
reset_nrt_gate_ = true;
|
||||
vendor_movel_done_seq_ = read_movel_seq();
|
||||
vendor_movel_phase_ = VendorMovelPhase::kIdle;
|
||||
|
||||
@@ -688,6 +694,7 @@ hardware_interface::return_type FlexivHardwareInterface::read(
|
||||
// 必须放在 operational() **之前**判 —— 断连后 operational() 会一直保持 true，
|
||||
// 只看它会把"已断连"误判成"正常"，这正是当初臂不动却一路报 success 的原因。
|
||||
if (!robot_->connected()) {
|
||||
motion_execution_latch_.observe(false, false, false);
|
||||
RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000,
|
||||
"RDK 与机器人的连接已断开 —— 关节状态只是最后一帧缓存，下发的命令不会生效。"
|
||||
"检查网络后重启本栈（两台臂出厂 MAC 相同时会在同一 L2 段抢转发，"
|
||||
@@ -695,6 +702,21 @@ hardware_interface::return_type FlexivHardwareInterface::read(
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
// Loss of operational state includes physical E-stop/disable. Stop the ROS
|
||||
// execution too: a paused JTC must not keep interpolating and resume on Enable.
|
||||
if (!motion_execution_latch_.observe(true, robot_->operational(), robot_->fault())) {
|
||||
position_command_fence_.reset();
|
||||
reset_nrt_gate_ = true;
|
||||
if (vendor_movel_phase_ == VendorMovelPhase::kRunning) {
|
||||
finish_vendor_movel(vendor_movel_seq_, false, "robot interrupted; request discarded");
|
||||
}
|
||||
vendor_movel_done_seq_ = read_movel_seq();
|
||||
RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000,
|
||||
"Robot interrupted: abort hardware/controllers; explicit lifecycle activation required. "
|
||||
"Old motion will not resume after Enable.");
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
if (robot_->operational()) {
|
||||
|
||||
hw_flexiv_robot_states_ = robot_->states();
|
||||
@@ -713,6 +735,7 @@ hardware_interface::return_type FlexivHardwareInterface::read(
|
||||
last_state_change_ns_ = now_ns;
|
||||
} else if (last_state_change_ns_ != 0
|
||||
&& now_ns - last_state_change_ns_ > kStateStaleTimeoutNs) {
|
||||
motion_execution_latch_.observe(false, false, false);
|
||||
RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000,
|
||||
"机器人状态帧已停滞 %.1f s —— 关节状态是陈旧值，下发命令不会生效。"
|
||||
"检查网络/RDK 连接后重启本栈。",
|
||||
@@ -754,6 +777,8 @@ void FlexivHardwareInterface::finish_vendor_movel(
|
||||
} else {
|
||||
write_movel_line(kMovelDone, std::to_string(seq) + " fail " + clean);
|
||||
}
|
||||
position_command_fence_.reset();
|
||||
reset_nrt_gate_ = true;
|
||||
vendor_movel_done_seq_ = seq;
|
||||
vendor_movel_phase_ = VendorMovelPhase::kIdle;
|
||||
}
|
||||
@@ -805,6 +830,15 @@ bool FlexivHardwareInterface::poll_vendor_movel()
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string zone;
|
||||
int toler = -1;
|
||||
if (in >> zone) {
|
||||
if (!(in >> toler) || zone != "ZFine" || toler != 1) {
|
||||
finish_vendor_movel(seq, false, "微升降只接受过渡区 ZFine 和到位等级 1");
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// 先应答，伺服才能在 1.5s 内分清「新接口没编进去」和「臂还在走」。
|
||||
write_movel_line(kMovelAck, std::to_string(seq));
|
||||
try {
|
||||
@@ -840,8 +874,14 @@ bool FlexivHardwareInterface::poll_vendor_movel()
|
||||
std::array<double, 3> {x, y, z},
|
||||
euler,
|
||||
std::array<std::string, 2> {"WORLD", "WORLD_ORIGIN"});
|
||||
robot_->ExecutePrimitive(
|
||||
"MoveL", {{"target", target}, {"vel", vel}}, true);
|
||||
std::map<std::string, flexiv::rdk::FlexivDataTypes> movel_args;
|
||||
movel_args.emplace("target", target);
|
||||
movel_args.emplace("vel", vel);
|
||||
if (!zone.empty()) {
|
||||
movel_args.emplace("zoneRadius", zone);
|
||||
movel_args.emplace("targetTolerLevel", toler);
|
||||
}
|
||||
robot_->ExecutePrimitive("MoveL", movel_args, true);
|
||||
vendor_movel_seq_ = seq;
|
||||
vendor_movel_phase_ = VendorMovelPhase::kRunning;
|
||||
vendor_movel_deadline_ = std::chrono::steady_clock::now() + std::chrono::seconds(90);
|
||||
@@ -861,6 +901,14 @@ bool FlexivHardwareInterface::poll_vendor_movel()
|
||||
hardware_interface::return_type FlexivHardwareInterface::write(
|
||||
const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
|
||||
{
|
||||
// Check before polling pending native requests or switching control mode.
|
||||
if (!motion_execution_latch_.observe(robot_->connected(), robot_->operational(), robot_->fault())) {
|
||||
position_command_fence_.reset();
|
||||
reset_nrt_gate_ = true;
|
||||
vendor_movel_done_seq_ = read_movel_seq();
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
// MoveL 期间不能回到关节模式，也不能把旧关节目标再发出去。
|
||||
if (poll_vendor_movel()) {
|
||||
return hardware_interface::return_type::OK;
|
||||
@@ -917,20 +965,9 @@ hardware_interface::return_type FlexivHardwareInterface::write(
|
||||
"Robot not operational (status=%s). Commands not sent. "
|
||||
"Release E-stop, AUTO Remote, Enable.",
|
||||
operational_status_name(robot_->operational_status()));
|
||||
static std::chrono::steady_clock::time_point last_en {};
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
if (now - last_en > std::chrono::seconds(2)) {
|
||||
last_en = now;
|
||||
try {
|
||||
if (robot_->fault()) {
|
||||
robot_->ClearFault(3);
|
||||
}
|
||||
robot_->Enable();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000,
|
||||
"Enable failed: %s", e.what());
|
||||
}
|
||||
}
|
||||
// Re-enabling must be explicit; never resume a cached trajectory here.
|
||||
position_command_fence_.reset();
|
||||
reset_nrt_gate_ = true;
|
||||
} else if (robot_->mode() == rdk_control_mode_) {
|
||||
for (size_t rdk_idx = 0; rdk_idx < robot_->info().DoF; ++rdk_idx) {
|
||||
size_t ros_idx = rdk_to_ros_map_[rdk_idx];
|
||||
@@ -943,6 +980,17 @@ hardware_interface::return_type FlexivHardwareInterface::write(
|
||||
std::fabs(target_pos[rdk_idx] - hw_states_joint_positions_[ros_idx]));
|
||||
}
|
||||
static NrtSendGate nrt_gate;
|
||||
if (reset_nrt_gate_) {
|
||||
nrt_gate = NrtSendGate {};
|
||||
reset_nrt_gate_ = false;
|
||||
}
|
||||
if (!position_command_fence_.accept(
|
||||
hw_commands_joint_positions_, hw_states_joint_positions_)) {
|
||||
RCLCPP_WARN_THROTTLE(getLogger(), steady_clock, 2000,
|
||||
"Suppressed stale joint target after activation/MoveL; "
|
||||
"waiting for a new trajectory starting at measured joints");
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
RCLCPP_INFO_THROTTLE(getLogger(), steady_clock, 2000,
|
||||
"HW write running=%d nan=%d mode=%d want=%d op=%d busy=%d d_state=%.4f",
|
||||
static_cast<int>(position_controller_running_), static_cast<int>(is_pos_nan),
|
||||
@@ -1149,6 +1197,8 @@ hardware_interface::return_type FlexivHardwareInterface::perform_command_mode_sw
|
||||
torque_controller_running_ = false;
|
||||
|
||||
// Hold joints before user commands arrives
|
||||
position_command_fence_.reset();
|
||||
reset_nrt_gate_ = true;
|
||||
std::fill(hw_commands_joint_positions_.begin(), hw_commands_joint_positions_.end(),
|
||||
std::numeric_limits<double>::quiet_NaN());
|
||||
|
||||
|
||||
+207
-4
@@ -19,7 +19,9 @@
|
||||
## 首页动作接口
|
||||
|
||||
- 检测 → `stage_track`，运行现有第一阶段跟踪：D405 优先、丢失目标后 D435 补位；会移动机械臂，不进入对孔或旋入。
|
||||
- 对孔 → `stage_locate`，只运行第二阶段，明确关闭最终下压和后续旋入。
|
||||
- 对孔 → `stage_locate`，运行新版第二阶段：精对孔并下降至配置的进孔高度；不自动转 A7。当前 hole_entry_mm=1.0。
|
||||
- 旋转 → 独立 `a7_rotation` ROS 节点，复用上游首轮 A7 正反转和四轮松爪、微抬、空回、降回、合爪旋转；不重新跟踪/对孔。夹爪通过本站 GripperExecute 服务控制，不调用 shell。起点高度/姿态、夹爪或限位不满足时中止。
|
||||
- 开始全部流程 → 跟踪、对孔、A7 旋转依次执行，失败/取消后不进入下一段。
|
||||
- 回零 → 所选臂命名点文件中的 `servo.home_pose_name`（默认 `home`），经原生轨迹 Action 返回初始姿态，不是关节全零。
|
||||
- 停止 → 取消本站任务、通过控制器管理服务暂停对应臂的运动控制器（硬件接口调用 `Robot::Stop`），停止本站流程，再等待新鲜关节反馈持续稳定。失败时明确报告停止未确认，并阻止继续运动。仅下一次显式动作恢复本站暂停的控制器。
|
||||
|
||||
@@ -54,11 +56,11 @@ PGEA 的常驻驱动提供：
|
||||
|
||||
## 图像链路
|
||||
|
||||
RealSense 采集节点只在显式请求时启动。已有发布者会被复用，工位不会按进程名终止它。
|
||||
RealSense 采集节点只在显式请求时启动。外部发布者不会被接管，工位也不会按进程名终止它。只有本站进程记录与当前唯一发布者身份同时匹配，才允许预览和启动后续算法。
|
||||
采集配置继续来自 `site.yaml`，算法使用的彩色/深度独立于界面预览。
|
||||
|
||||
`cam_bridge.py` 负责 ROS 图像接收与共享内存输出。当前 Qt 界面只请求真机彩色流；
|
||||
没有查看需求的图像与检测订阅会撤销。预览处理最高 15 fps、彩色宽度上限 848，
|
||||
没有查看需求的图像与检测订阅会撤销。彩色预览不再额外限到 15 fps，保留最高 1920×1080 的显示图像，
|
||||
算法输入不受影响，采集端现有原图话题的发布速率不会因此降低。
|
||||
图像或桥进程超过 2 秒未更新时清除旧显示。共享内存由生产者释放，读取者只解除映射。
|
||||
深度、仿真及标定流的后端能力保留给独立工具，不再提供首页切换入口。
|
||||
@@ -74,7 +76,7 @@ RealSense 采集节点只在显式请求时启动。已有发布者会被复用
|
||||
默认不自动拉额外查看器、红外或触觉流。上电的网络/工具准备与手眼标定保留原维护脚本，
|
||||
由后端作为明确的系统/标定工具运行。`operator_console.py` 的旧 HTTP/脚本控制路径已经退役。
|
||||
|
||||
`processes.py` 只登记并停止自己启动的进程组，不扫描和终止外部进程。Linux 子进程设置
|
||||
`processes.py` 只登记并停止自己启动的进程树，不扫描和终止外部进程。Linux 子进程设置
|
||||
父进程退出信号，后端也监测界面退出，避免留下预览或控制孤儿进程。
|
||||
“停止动作”取消工位任务，保持采集和驱动；窗口关闭时取消任务并退出自己持有的进程。
|
||||
软件流程停止不能证明控制器已停稳；轨迹、夹爪停止未确认时会明确报告结果未知。
|
||||
@@ -95,3 +97,204 @@ RealSense 采集节点只在显式请求时启动。已有发布者会被复用
|
||||
|
||||
每次后端启动会生成一份私有站点配置快照，所有子进程使用同一份设备身份与参数。
|
||||
现场 `site.yaml` 发生变化后拒绝新操作并提示重启，已执行任务继续使用原快照，避免运行中换臂或换相机。
|
||||
|
||||
|
||||
## 第一批节点治理
|
||||
|
||||
后端按设备驱动、运动控制、算法任务、界面辅助分类展示 ROS 图，并保留完整命名空间及同名实例数。
|
||||
分类仅表示职责：发现一个节点不等于确认它健康，更不代表工位拥有它。
|
||||
监控页另列出本站实际启动的进程 PID、启动时间和退出状态，以及当前任务占用的资源、臂号和取消请求。
|
||||
后端断联时这些缓存统一降为未知。
|
||||
|
||||
Linux 启动器作为 subreaper 持续监督每条启动命令，主命令结束后先终止并回收其遗留子进程，
|
||||
包括自行创建 session 的节点，再允许任务返回。控制栈、相机、算法继续分进程运行；不按节点名字杀外部进程。
|
||||
日志保留历史，但任务结果仅截取本次启动写入的部分，避免成功任务显示上次错误。
|
||||
|
||||
使能显示同时要求本臂关节反馈新鲜、原生驱动 RobotStates 中的机器人时间戳持续推进；
|
||||
仅 ROS header 更新或重复缓存帧不能确认使能。依据是本项目原生驱动只在 connected/operational 时更新该结构。
|
||||
这属于应用层运行状态判断，不替代机器人自身安全状态或安全控制器。
|
||||
|
||||
本批尚未迁移全局运动接口到双臂命名空间，也未替换旧命令行维护/标定脚本。
|
||||
这些入口仍需后续逐条迁移；目前继续使用全局运动资源互斥，禁止双臂同时下发。
|
||||
相机未连接时不更改采集分辨率、USB 模式或宣称实测帧率。预览改动使用模拟图像验证。
|
||||
|
||||
项目发现配置统一在 `config/ros_runtime.env`：默认 domain 42、仅本机发现，不再继承 shell 里
|
||||
不明来源的 domain 0 或仿真域。显式部署覆盖使用 `REAL_ROS_DOMAIN_ID` 和
|
||||
`REAL_ROS_LOCALHOST_ONLY`，所有子启动脚本继承同一值。CLI 观察节点前需先 source 本项目的
|
||||
`scripts/env.sh`。ROS 域用于防止误混入，不属于访问控制或机器人安全功能。
|
||||
|
||||
运行中的旧工位仍保持原域，代码修改不会热切换它。迁移需要在任务结束后完整关闭旧工位，
|
||||
再重新启动；旧域外部控制栈必须由原入口退出，不能同时对同一硬件启动第二份驱动。
|
||||
|
||||
## 操作并发与收尾
|
||||
|
||||
资源策略集中在 `command_policy.py`：Qt 只合并同一臂的同一按钮重复请求，其他请求交由后端
|
||||
按实际写入设备判断。机械臂使能/跟踪/对孔/回零、相机刷新、夹爪控制和传感器刷新/底图重置
|
||||
可跨模块并行；读取图像和力反馈不等于独占采集模块。A7 与全流程实际控制夹爪，所以执行期间
|
||||
保留该夹爪的互锁。仍共用控制器的机械臂运动保持互斥。同一相机初始化由相机适配器内部串行，
|
||||
只有断流或配置不匹配才重启；机械臂使能不阻止相机重连。触觉预览不占用触觉采集资源。
|
||||
机械臂反馈中断仅记录在 backend.device_errors.arms，不提升为全后端错误，不禁用其他模块。
|
||||
|
||||
冲突提示显示动作名称、臂号和执行/收尾/停止阶段。子进程启动器通过私有阶段文件报告收尾，
|
||||
直到整个所属进程树退出才释放资源。工位 pipeline 的结果展示停留改为 0.3 秒；手工 CLI 默认仍为
|
||||
15 秒。此停留仅发布结果/预览，不更改轨迹、定位精度或控制器保持行为。
|
||||
|
||||
停止 RPC 不经过任务队列；明确停止机械臂时只取消运动控制任务，保留独立的相机连接和只读任务。
|
||||
停止未确认仍锁住后续机械臂控制入口，不能靠并发改动绕过停止确认。
|
||||
|
||||
|
||||
## 高清相机配置
|
||||
|
||||
当前请求 D405 彩色/原生深度 1280×720@15，D435 彩色 1920×1080@15、原生深度
|
||||
1280×720@15，由 SDK 对齐到彩色像素坐标。采集器只尝试本台设备实际枚举支持的配置，
|
||||
不再把 1080p 一律过滤，也不强迫 D435 彩色和原生深度使用同一分辨率。
|
||||
|
||||
Qt 预览通过本站采集节点的 `color/preview/compressed` 接收 JPEG 95 图像，只有订阅者存在
|
||||
时才编码；算法继续订阅完整 `color/image_raw`，无算法订阅者时不序列化这份大图。
|
||||
预览桥支持到 1920×1080，原生分辨率在该范围内保持不缩小。压缩仅用于显示，不用于定位计算。
|
||||
|
||||
两台相机共用 5Gbps Hub 的现场并发测试出现断流，单台可接近 30fps；部署应使用独立 USB3
|
||||
连接并重新核对两路同时采集的实测值，不能仅凭配置数字认为帧率已达标。
|
||||
|
||||
“刷新相机”核对本站采集所有权及新鲜 CameraInfo：正常且分辨率匹配的流保持运行，
|
||||
断流或配置不匹配才重启本站对应进程，并等待 DDS 旧发现记录退出、收到实际新帧后报告成功。
|
||||
相机刷新与机械臂模块独立，不接管外部节点。SDK 留出四个帧缓冲供对齐持有，
|
||||
应用层仍只保留最新一帧；每轮处理后释放 SDK 引用，避免等待新帧时占住旧缓冲。
|
||||
|
||||
### 使能与轨迹恢复
|
||||
|
||||
使能的连接检查不恢复本站暂停的轨迹控制器；只有明确的新动作才恢复控制器。
|
||||
关节控制循环不再自动 ClearFault/Enable。JTC 使用实际关节反馈作为新轨迹起点
|
||||
（open_loop_control=false）。硬件激活、控制器恢复和厂家 MoveL 结束后，
|
||||
旧目标被拦截，直到指令流从实测关节附近重新开始；同时清空 NRT 前馈缓存。
|
||||
更换驱动库使用原子替换，运行中的驱动保持原版本，重启工位后加载更新。
|
||||
离线回归包括暂停后使能不恢复控制器，以及旧目标持续重放不能通过硬件保护。
|
||||
|
||||
### 实体急停后的任务取消
|
||||
|
||||
原生驱动在 connected/operational 丢失、故障或硬件时间戳停滞时锁存中断，
|
||||
read/write 返回 ERROR，要求控制器停止；write 在处理厂家 MoveL 请求前检查锁存。
|
||||
仅恢复 Enable 或清故障不会解除锁存，需要显式重新激活硬件生命周期
|
||||
（当前工位可通过重启控制栈重新连接）。重新激活会丢弃旧 MoveL 请求序号。
|
||||
后端在运动期间监控原生反馈，故障或连续 0.75 秒没有推进时取消机器人控制任务，
|
||||
走既有停止与子进程清理流程；取消后的迟到成功也按 cancelled 记录。
|
||||
显式重新连接时，驱动先调用 Stop 清除机器人端残留动作，再请求 Enable；Stop 失败则不使能。
|
||||
此软件逻辑不替代实体急停。Qt 顶部急停图标仍未接硬件接口，保持禁用。
|
||||
验证使用隔离 ROS 域和纯 C++ 状态锁存测试，没有真实机械臂动作测试。
|
||||
|
||||
### 底部操作日志
|
||||
|
||||
底部日志记录按钮点击、重复点击拦截，以及后端收到请求、拒绝、任务开始、阶段变化、
|
||||
取消和最终结果。时间精确到毫秒，显示臂号和任务编号；短任务的状态变化也保留为事件，
|
||||
不依赖界面轮询碰巧看到 running。任务返回的 detail 和完整进程日志路径也会显示。
|
||||
界面保留最近 3000 行，翻看历史时不强制滚到底部；后端缓存最近 512 条事件，
|
||||
完整操作事件追加保存到 log/station/operations.jsonl，进程输出保留原独立日志。
|
||||
|
||||
### 重写的触觉与平行夹爪模块
|
||||
|
||||
参考 `ctag2f-90d-sdk/gripper/panel.py` 的数据处理方式，自行实现本站节点与界面，
|
||||
不导入、不运行它的 panel、GripperClient、GripperWorker 或自动抓取流程。
|
||||
低层触觉算法使用 `/home/cheng/orisys-sdk/src` 中的 Orisys SDK。
|
||||
|
||||
- `arm_application.tactile_flow_node`：left/right 各自独立 ROS2 节点和进程，
|
||||
每路独占一个触觉相机；CPU 目标 30 Hz。读取 SDK 的 VRAW 矢量场绘制箭头，
|
||||
不重复解算光流；没有画面订阅者时不编码 JPEG。不强制 USB reset、不停止别人的进程。
|
||||
- `/tactile/{left,right}/status`：TactileStatus，含在线、帧率、接触、三向原始力和错误。
|
||||
这里保留 RAW 通道量纲以兼容现有装配阈值，不把未经校准的值标为牛顿。
|
||||
- `/tactile/{left,right}/flow/compressed`：CompressedImage/JPEG。预览与状态使用独立通道，
|
||||
不被 Qt 的低频状态刷新限制。队列只留最新样本，断流显示离线并尝试本侧重连。
|
||||
- `arm_application.gripper_node`：一个串口一个节点，使用现有经过验证的 DH Modbus 协议层，
|
||||
仅暴露 `/station/grippers/arm_<id>/execute` 类型化服务与 `status` 状态话题。
|
||||
无旧 Float32 数组控制入口；连接/探测绝不初始化或移动。当前支持 PGE/PGEA 平行夹爪，
|
||||
右臂 RGI 旋转夹爪仍须独立匹配驱动，不能用 PGE 寄存器替代。
|
||||
- 界面：光流区标题右侧提供“刷新传感器”，显示 left/right 帧率和三向力；
|
||||
夹爪区标题右侧提供“连接 / 探测”，开合使用任务操作按钮和后端默认力/速度。
|
||||
连接与刷新不下发运动，闭合、张开才写位置，并等待实际到位反馈。
|
||||
|
||||
主入口仍是 `bash station/run_station.sh`。重启后在首页分别点“刷新传感器”和夹爪“连接 / 探测”。
|
||||
本站会自管相应节点，不要同时从其他入口重复启动。ROS 独立启动入口为
|
||||
`tactile.launch.py`（左右触觉）与 `station_gripper.launch.py arm:=89`（平行爪）。
|
||||
旧 `dh_pgea.launch.py` / `tactile` executable 保留为兼容入口，主界面不再启动它们。
|
||||
|
||||
站点的 `tactile.arm_id` 明确绑定机械臂（当前 89），`sdk_python_path` 为低层 SDK 的 src 路径；
|
||||
配置 JSON 已复制进本仓库 `src/arm_application/config/tactile/ctag2f90d.json`。
|
||||
`left_video/right_video: auto` 只选择 USB vendor 1bcf 的触觉 UVC 采集接口，排除 Intel RealSense、
|
||||
笔记本摄像头与重复接口。首次插入后需确认物理左右；建议改成各自稳定的 `/dev/v4l/by-path/...`，
|
||||
避免 USB 插口调整导致左右顺序变化。两侧不能指向同一设备，跨进程锁会拒绝重复占用。
|
||||
|
||||
额外 CPU 依赖见 `station/requirements-tactile.txt`，已装入现有 ros_flexiv；未另建 conda 环境。
|
||||
验证为假串口、合成光流、隔离 ROS 域与 Qt 离线回归；未做真实开合或机械臂动作测试。
|
||||
|
||||
### RGI STEP 三维模型
|
||||
|
||||
主界面机械臂与夹爪均使用 Qt QOpenGLWidget 内嵌渲染，不是 RViz；ROS 提供设备状态。
|
||||
`run_real.sh` 默认 `use_rviz:=false`，RViz 可作为独立调试工具按需打开。
|
||||
右臂 RGI 模型来自用户提供的 `RGI-100-30-O-S_简易模型_V3.0_20241010.STEP`（SolidWorks AP203，毫米）。
|
||||
原件保存在 `station/src/station/assets/models/rgi-100-30/source.step`，离线通过
|
||||
`scripts/convert_step_mesh.py` 生成 `mesh.npz` 与含原件 SHA256、单位和边界的 `mesh.json`。
|
||||
当前缓存 35148 个三角面，加载后按米渲染，运行时不依赖 CAD 内核，不在 UI 中转换 STEP。
|
||||
显示颜色做了提亮处理以便观察边缘，几何保持原始装配位姿。
|
||||
|
||||
选择右臂显示真实 RGI CAD 外形，可拖拽旋转观察、双击复位；目前是静态装配模型，
|
||||
未猜测各零件的运动轴，也未把整个夹爪当作手指错误旋转。待建立零件/关节映射并有
|
||||
旋转爪实时反馈后再加入开合与旋转动画。左臂仍显示标注为近似的 PGEA 尺寸模型，
|
||||
不会把 RGI 外形套到左臂。本次没有更改机械臂控制或发出设备命令。
|
||||
|
||||
首页夹爪区已精简：标题右侧只留“连接 / 探测”，移除力、速度、目标位置控件；
|
||||
开合继续使用任务操作按钮及后端默认参数。传感器标题右侧只留“刷新传感器”，
|
||||
负责重启本站拥有的左右触觉采集节点，重新检测设备并恢复预览，不操作机械臂或夹爪。
|
||||
两个入口尺寸均为 92×28，与“刷新相机”一致。
|
||||
|
||||
### 标定力显示与光流布局
|
||||
|
||||
界面 Fx、Fy 置于每张光流的左、右两侧，保留方向正负号；下方显示法向 Fn，均标注 N。
|
||||
列宽和下方行高固定，数值更新不会推动布局。
|
||||
`tactile_flow_node` 在保留 `/tactile/<side>/status` 原始值的同时，新增标准
|
||||
`/tactile/<side>/wrench`（geometry_msgs/WrenchStamped）：force.x=Fx、y=Fy、z=Fn，单位牛顿。
|
||||
装配算法仍接收原始 status，判断阈值不变。没有新鲜标定力话题时界面显示横线，
|
||||
不会把原始数值直接标成 N。
|
||||
|
||||
换算曲线来自用户给定的 CTAG2F-90D 示例项目 `config/gripper.json`，已作为纯数据复制到
|
||||
`src/arm_application/config/tactile/force_calibration.json`。使用分段线性插值、配置的外推和
|
||||
零下限，切向分量按绝对值换算后恢复正负号。这是参考项目的现有曲线，并非本次
|
||||
对左右两只设备逐只做出的实测标定。可在 tactile 配置中设置
|
||||
`left_force_calibration_file` / `right_force_calibration_file` 覆盖各自数据，
|
||||
或用 `force_calibration_file` 指定共用曲线；不导入或运行示例面板代码。
|
||||
|
||||
### 光流旁的切向力曲线与底图重置
|
||||
|
||||
每张光流左右是 X/Y 切向力折线图，显示最近 10 秒（N）；四张小图使用相同时间窗
|
||||
与对称量程，零力在中线。所有图表与画面顶底对齐，下方仍显示 Fn 标定法向力。
|
||||
采样来自标定 wrench 话题的有界历史，不使用原始力替代。
|
||||
|
||||
“重置底图”在“刷新传感器”前。它通过 `/tactile/<side>/reset_reference`（std_srvs/Trigger）
|
||||
请求本站节点在采集线程里、获取新画面后执行 SDK Sensor.reset()，不重启采集、不操作夹爪。
|
||||
成功后清空该侧的光流缓存和力历史；离线侧返回未重置，连接恢复后不自动补执行旧请求。
|
||||
超时明确报告结果未知，不自动重试。建议传感器无受力时操作。
|
||||
跟踪/对孔/旋入流程与底图重置互斥，避免执行中更换测量参考。
|
||||
|
||||
触觉区最新布局：每侧 X 力图、光流、Y 力图为三个等大的正方形画布，随可用宽高适配。
|
||||
切向力图纵轴固定 −10～10 N，显示数值刻度，超出范围的曲线裁剪；不再自动缩放。
|
||||
横轴固定滚动 60 秒（−60、−30、0s）。后端标定力历史容量扩至 2100 点，
|
||||
按实际时间截取最近一分钟，满足 30 Hz 采样；重置底图仍清空对应历史。
|
||||
|
||||
触觉标题采用 `left(30fps) 当前法向力 5N` 形式，fps 小字、不使用圆点分隔符，
|
||||
下方不再重复显示法向力。曲线末端的圆点标记也已去除。
|
||||
曲线横轴改为最近 1800 个采样点的序号（0/900/1800），纵轴仍固定 ±10 N；
|
||||
不再由墙钟时间推动横轴或剔除数据。没有新样本时曲线保持不动。
|
||||
新增轻量 `tactile_preview` 增量通道，携带 JPEG 与自上次序号之后的标定力样本，
|
||||
最高 30 Hz；Qt 绘图计时器直接消费该缓存，避免低频状态刷新导致曲线跳动。
|
||||
每次重置/刷新会变更参考版本，使客户端清空旧曲线；重复返回的采样序号不会重复追加。
|
||||
|
||||
### 远端相机预览
|
||||
|
||||
`config/site.yaml` 的 `remote_camera_preview` 显式选择远端相机。当前读取
|
||||
192.168.2.10（域 0）的 d405_left / d435 原始图像，通过本机有线地址
|
||||
192.168.2.50 发现，避免组播走 VPN 虚拟网卡。远端未发布 JPEG 预览时使用 raw。
|
||||
|
||||
只有 `station.cam_bridge` 子进程改用远端 ROS 域和网卡；机械臂后端、夹爪与
|
||||
传感器的环境不变。“刷新相机”重建只读预览订阅并等待画面，不启动/停止
|
||||
本机或远端物理相机驱动。首页远端订阅不再要求该相机由本机后端持有。
|
||||
远端模式仅用于显示，不把网络图像自动接入本机机械臂跟踪或标定运动。
|
||||
|
||||
关闭 `remote_camera_preview.enabled` 并重启工位即可恢复本站采集模式。
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user