P6 通止规装配：主流程节点（S00–S11 跑通）+ 入口脚本 + 真机/仿真验证
新增
----
- arm_application/gauge_assembly_node.py：主流程。每个阶段一个 _step_*，
返回值就是状态机的事件；纯逻辑（状态机/配置/限位/夹爪后端/自检）全部
在 gauge_*.py 里，节点只做"发指令 + 读反馈 + 报错"。
* S00 自检报告带"缺了会怎样"，必检拦住、选检告警；
* S01 回 home → 切工具系 → 逆时针预绕，**事后用 TF 实测绕工具 Z 转了多少度**
（转轴偏差 >5° 直接中止：以"度"为单位的动作全建立在这条假设上）；
* S03 手外粗定位：没有 eye-to-hand 标定文件时明确报"缺哪个文件、怎么生成"，
回 S04 兜底（不做假装成功的反投影）；
* S05 稳定判据用"到均值的最远距离"，不用相邻帧之差；
* S07 悬停处测基线 → 步进下压 → 两路切向任一突变即停，法向超安全上限抬起中止；
* S08 棘轮四步（慢转→松爪→快回转→夹紧），停止判据「切向 >阈值 且 法向突增」，
掉件走 E1 重放；S09 反向拧出（比拧入多一轮）；
* E1 重放等 /gauge/reload_ok 人工确认（超时按中止），E2 任何中止都先退安全高度。
- launch/gauge_assembly.launch.py：只做一件事 —— 发布手眼静态 TF（S05 的前提），
其余判据全交给 S00（判断标准只有一处）。
- scripts/run_gauge_assembly.sh：--arm/--sim/--dry-run/--holes/--backend。
- gauge_assembly 入口（setup.py）+ 站点参数 reload_timeout_s。
- test/test_gauge_step_events.py：**读节点源码 AST**，把每个 _step_* 可能返回的
事件逐个问状态机"合法吗" —— 节点与转移表是两份数据，漏登记要在 pytest 里炸，
而不是在真机上跑到那一步才炸。
顺手修掉的两个真问题
--------------------
1. 四份 launch 里的四元数换算**已经互相抄歪了**（退化分支的公式各不相同）。
统一收口到 handeye_lib.read_transform_yaml，并加回环测试（真实标定文件 +
180°/270° 等分支切换角度）；实测 4 个 launch 输出与库完全一致。
2. pipeline_node.AssemblyPipeline 支持 node_name 复用（本流程继承它的一阶段
跟踪/二阶段定位链，但在图上必须是另一个节点名）。
验证（都在本机真跑过）
----------------------
pytest（仿真环境） → 349 passed, 14 skipped（缺 onnx 的用例跳过）
干跑全流程 → S00→S01→…→S11→DONE，exit 0
仿真里不干跑（缺 /home 等） → S00 拦住，exit 1，未发任何运动指令
另：仿真环境没有 onnx（只有真机 conda 有），相关用例改为 skip —— 让测试结果
"红就是真红"。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
File diff suppressed because it is too large
Load Diff
@@ -67,8 +67,12 @@ class BootCheck:
|
||||
_ABSOLUTE_PROBES = frozenset({"service", "action", "topic", "tactile", "camera"})
|
||||
|
||||
|
||||
def _abs(name: str) -> str:
|
||||
"""把 ROS 名字规范成绝对名（``home`` → ``/home``）。"""
|
||||
def abs_name(name: str) -> str:
|
||||
"""把 ROS 名字规范成绝对名（``home`` → ``/home``）。
|
||||
|
||||
对外公开：调用方（节点）也要用同一套规范化去比较名字，否则会出现
|
||||
"清单里是 /foo、参数里是 foo，比较不相等"这类只在运行时暴露的错。
|
||||
"""
|
||||
n = (name or "").strip()
|
||||
if not n:
|
||||
return n
|
||||
@@ -116,7 +120,12 @@ def build_checks(
|
||||
probe="action",
|
||||
target=joint_action,
|
||||
why="没有它，S01/S06/S07/S10 的所有运动都发不出去",
|
||||
expect_type="arm_msgs/action/FollowJointTrajectory",
|
||||
# 类型必须是 **control_msgs**：本流程（和 stage2/stage3/pipeline 一样）
|
||||
# 直接把轨迹发给 ros2_control 的 JTC 服务端，不经过 arm_control 的
|
||||
# 轨迹适配器（那个用的是 arm_msgs/action/FollowJointTrajectory）。
|
||||
# 写成 arm_msgs 的会得到一个"永远探不到服务端"的自检 —— 假阴性，
|
||||
# 而且看起来像控制器没起来。
|
||||
expect_type="control_msgs/action/FollowJointTrajectory",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_home",
|
||||
@@ -134,20 +143,16 @@ def build_checks(
|
||||
why="/home 内部转发给它；它不在时回零会超时",
|
||||
expect_type="arm_msgs/srv/SetNamedPose",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_switch_tool",
|
||||
label="工具系切换服务",
|
||||
probe="service",
|
||||
target="/switch_tool",
|
||||
why="tool1 必须全程垂直向下且位姿锁定，切不过去后面全是歪的",
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
),
|
||||
BootCheck(
|
||||
key="arm_compliant",
|
||||
label="柔顺下压 action",
|
||||
probe="action",
|
||||
target="/compliant_descent",
|
||||
why="S07 的柔顺接触靠它；缺了只能用位置控制硬压",
|
||||
# 选检：S07 现在走节点内的"步进 + 力突变停"（与这个 action 内部
|
||||
# 同一套做法），不依赖它；有它时驱动侧原语更顺，属于加分项。
|
||||
# 标成必检会让仿真（arm_driver_flexiv 不参与）永远过不了自检。
|
||||
why="有它时 S07 用驱动侧柔顺原语；没有则用节点内步进（不影响正确性）",
|
||||
required=False,
|
||||
expect_type="arm_msgs/action/CompliantDescent",
|
||||
),
|
||||
BootCheck(
|
||||
@@ -223,6 +228,33 @@ def build_checks(
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
# 配了工具系就必须能切：S01 切不过去会直接 TIMEOUT 中止，
|
||||
# 所以在自检里按**必检**拦下（早报比跑到 S01 再报少一次上电动作）。
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="arm_switch_tool",
|
||||
label="工具系切换服务",
|
||||
probe="service",
|
||||
target="/switch_tool",
|
||||
why=f"site.yaml 指定了工具 {tool_name!r}，切不过去后面全是歪的",
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
else:
|
||||
# 没配工具系：S01 只会告警不会切，所以这条只算选检 ——
|
||||
# 真机没配工具系是**配置问题**（看板要求 tool1），但仿真里根本没有
|
||||
# 工具系切换服务，标必检会让仿真永远过不去。
|
||||
checks.append(
|
||||
BootCheck(
|
||||
key="arm_switch_tool",
|
||||
label="工具系切换服务",
|
||||
probe="service",
|
||||
target="/switch_tool",
|
||||
why="本机没配 robot.tool_name：真机应配 tool1 并确认这个服务在；仿真不需要",
|
||||
required=False,
|
||||
expect_type="arm_msgs/srv/SwitchTool",
|
||||
)
|
||||
)
|
||||
if weights_path:
|
||||
checks.append(
|
||||
BootCheck(
|
||||
@@ -239,7 +271,7 @@ def build_checks(
|
||||
return [
|
||||
c if c.probe not in _ABSOLUTE_PROBES
|
||||
else BootCheck(
|
||||
key=c.key, label=c.label, probe=c.probe, target=_abs(c.target),
|
||||
key=c.key, label=c.label, probe=c.probe, target=abs_name(c.target),
|
||||
why=c.why, required=c.required, expect_type=c.expect_type,
|
||||
)
|
||||
for c in checks
|
||||
|
||||
@@ -90,6 +90,7 @@ _TYPES: Dict[str, type] = {
|
||||
"unscrew_extra_cycles": int,
|
||||
"holes_total": int,
|
||||
"dry_run": bool,
|
||||
"reload_timeout_s": float,
|
||||
}
|
||||
|
||||
#: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。
|
||||
@@ -131,6 +132,7 @@ _UNITS: Dict[str, str] = {
|
||||
"unscrew_extra_cycles": "",
|
||||
"holes_total": "",
|
||||
"dry_run": "",
|
||||
"reload_timeout_s": "s",
|
||||
}
|
||||
|
||||
#: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类
|
||||
@@ -201,6 +203,9 @@ DEFAULTS: Dict[str, Any] = {
|
||||
# 全局
|
||||
"holes_total": 2,
|
||||
"dry_run": False,
|
||||
# E1（掉件）重放：等操作员放好并确认 /gauge/reload_ok 的最长时间。
|
||||
# 超时 = 中止（臂在 home，人不在旁边时"继续等"比"停"更危险）。
|
||||
"reload_timeout_s": 180.0,
|
||||
}
|
||||
|
||||
|
||||
@@ -245,6 +250,7 @@ class GaugeConfig:
|
||||
unscrew_extra_cycles: int
|
||||
holes_total: int
|
||||
dry_run: bool
|
||||
reload_timeout_s: float
|
||||
|
||||
# -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）-------------------
|
||||
@property
|
||||
|
||||
@@ -7,8 +7,10 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Deque, List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
@@ -32,6 +34,69 @@ class CameraIntrinsics:
|
||||
dist: np.ndarray
|
||||
|
||||
|
||||
def R_to_quat_xyzw(R: np.ndarray) -> Tuple[float, float, float, float]:
|
||||
"""3×3 旋转 → ``(x, y, z, w)`` 四元数（Shepperd 法，按最大分量选分支）。
|
||||
|
||||
为什么放在库里而不是各个 launch 里：这段换算**已经在两份 launch 里各抄了
|
||||
一遍**，而且抄错过（对角分支的公式被写歪，症状是手眼 TF 差了 30°，
|
||||
定位看起来"总能到附近"但永远差一点）。换算只有一份才可能被测试：
|
||||
``test_handeye_lib_roundtrip.py`` 拿真实标定文件验证
|
||||
``quat_xyzw_to_R(R_to_quat_xyzw(R)) == R``。
|
||||
"""
|
||||
R = np.asarray(R, dtype=float).reshape(3, 3)
|
||||
tr = float(R[0, 0] + R[1, 1] + R[2, 2])
|
||||
if tr > 0.0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
w = 0.25 * s
|
||||
x = (R[2, 1] - R[1, 2]) / s
|
||||
y = (R[0, 2] - R[2, 0]) / s
|
||||
z = (R[1, 0] - R[0, 1]) / s
|
||||
elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
|
||||
s = math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
|
||||
w = (R[2, 1] - R[1, 2]) / s
|
||||
x = 0.25 * s
|
||||
y = (R[0, 1] + R[1, 0]) / s
|
||||
z = (R[0, 2] + R[2, 0]) / s
|
||||
elif R[1, 1] > R[2, 2]:
|
||||
s = math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
|
||||
w = (R[0, 2] - R[2, 0]) / s
|
||||
x = (R[0, 1] + R[1, 0]) / s
|
||||
y = 0.25 * s
|
||||
z = (R[1, 2] + R[2, 1]) / s
|
||||
else:
|
||||
s = math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
|
||||
w = (R[1, 0] - R[0, 1]) / s
|
||||
x = (R[0, 2] + R[2, 0]) / s
|
||||
y = (R[1, 2] + R[2, 1]) / s
|
||||
z = 0.25 * s
|
||||
n = math.sqrt(x * x + y * y + z * z + w * w)
|
||||
if n <= 0.0:
|
||||
raise ValueError("旋转矩阵无法转成四元数（矩阵为零？）")
|
||||
return x / n, y / n, z / n, w / n
|
||||
|
||||
|
||||
def read_transform_yaml(path: str | Path) -> Tuple[np.ndarray, Tuple[float, float, float, float]]:
|
||||
"""读手眼标定产物 → ``(xyz_m, quat_xyzw)``。
|
||||
|
||||
文件格式见 :func:`format_transform_yaml`：``T_4x4`` 是 4 行 4 列的列表。
|
||||
读取时**必须校验形状**：手眼文件是现场人手拷来拷去的，形状不对时
|
||||
直接报错，比"取到一堆 nan 然后发了个全零 TF"好得多（后者会让定位
|
||||
永远差一个常量，而且看起来像相机标定不准）。
|
||||
"""
|
||||
import yaml # 局部导入：本模块其它函数不需要 yaml（标定计算本身不依赖）
|
||||
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
T = data.get("T_4x4")
|
||||
if T is None:
|
||||
raise ValueError(f"{path}: 缺少 T_4x4 字段")
|
||||
arr = np.asarray(T, dtype=float)
|
||||
if arr.shape != (4, 4):
|
||||
raise ValueError(f"{path}: T_4x4 应为 4x4，实际 {arr.shape}")
|
||||
xyz = np.array([arr[0, 3], arr[1, 3], arr[2, 3]], dtype=float)
|
||||
return xyz, R_to_quat_xyzw(arr[:3, :3])
|
||||
|
||||
|
||||
def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
|
||||
x, y, z, w = [float(v) for v in q]
|
||||
n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12
|
||||
|
||||
@@ -25,8 +25,12 @@ from arm_application.stage3_search_node import Stage3Search
|
||||
|
||||
|
||||
class AssemblyPipeline(Stage3Search):
|
||||
def __init__(self) -> None:
|
||||
super().__init__(node_name="assembly_pipeline")
|
||||
def __init__(self, node_name: str = "assembly_pipeline") -> None:
|
||||
# node_name 可覆盖：通止规装配（gauge_assembly_node）继承本类以复用
|
||||
# 一阶段跟踪的拉起/拆分与二阶段定位链，但它是**另一条流程**，
|
||||
# 在图上必须是另一个节点名（否则 ros2 node list 里两个同名的节点，
|
||||
# 出问题时连"是哪条流程在发指令"都分不清）。
|
||||
super().__init__(node_name=node_name)
|
||||
self.declare_parameter("skip_stage1", False)
|
||||
self.declare_parameter("skip_stage2", False)
|
||||
self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth
|
||||
|
||||
+157
@@ -0,0 +1,157 @@
|
||||
#!/usr/bin/env python3
|
||||
"""启动通止规装配主流程（看板 S00–S11）。
|
||||
|
||||
与 stage2/stage3 的 launch 差别：**这里不检查任何服务**。
|
||||
"所有服务是否就绪"是 S00 的职责（``gauge_boot`` + 节点里的探测），
|
||||
在 launch/shell 里再查一遍就会出现两处判据 —— 一处改了另一处没改，
|
||||
现场表现为"脚本说没问题、节点说没数据"。
|
||||
|
||||
唯一在这里做的是**发布手眼静态 TF**：腕部相机的 eye-in-hand 外参是主流程
|
||||
（S05 精拍定位）算坐标的前提，而它只能由标定产物提供（site.yaml →
|
||||
paths.handeye_file）。文件不存在时不发 TF，节点会在 S05 报"查不到 TF"。
|
||||
|
||||
用法（一般由 scripts/run_gauge_assembly.sh 调用）::
|
||||
|
||||
ros2 launch arm_application gauge_assembly.launch.py
|
||||
ros2 launch arm_application gauge_assembly.launch.py dry_run:=true holes_total:=1
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _ws_root() -> Path:
|
||||
"""工作区根目录：优先环境变量，其次向上找 config/site.yaml。"""
|
||||
for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
|
||||
v = os.environ.get(key, "").strip()
|
||||
if v:
|
||||
return Path(v)
|
||||
here = Path(__file__).resolve()
|
||||
for i in range(2, 8):
|
||||
p = here.parents[i]
|
||||
if (p / "config" / "site.yaml").is_file():
|
||||
return p
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
**不要在这里再写一遍四元数换算**：这段换算在别的 launch 里已各抄了一遍，
|
||||
抄错过的症状是"手眼 TF 差了 30°"（定位总能到附近，永远差一点）。
|
||||
实现只有一份在 ``arm_application.handeye_lib``，且被回环测试钉住。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
root = _ws_root()
|
||||
sys.path.insert(0, str(root / "scripts" / "lib"))
|
||||
from export_site_env import apply_active_arm, load_site, site_path # type: ignore
|
||||
|
||||
cfg = os.environ.get("SITE_CONFIG", "").strip()
|
||||
path = Path(cfg).expanduser() if cfg else site_path()
|
||||
if not path.is_file():
|
||||
path = root / "config" / "site.yaml"
|
||||
data = load_site(path)
|
||||
merged, robot = apply_active_arm(data)
|
||||
|
||||
ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
|
||||
handeye = str((merged.get("paths") or {}).get("handeye_file") or "").strip()
|
||||
if not handeye:
|
||||
handeye = str((robot.get("paths") or {}).get("handeye_file") or "").strip()
|
||||
cam_cfg = merged.get("camera") or {}
|
||||
cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
|
||||
cam_frame = f"{cam}_color_optical_frame"
|
||||
|
||||
def _flag(name: str) -> bool:
|
||||
return LaunchConfiguration(name).perform(context).strip().lower() in (
|
||||
"1", "true", "yes", "on",
|
||||
)
|
||||
|
||||
dry_run = _flag("dry_run")
|
||||
holes = LaunchConfiguration("holes_total").perform(context).strip()
|
||||
backend = LaunchConfiguration("gripper_backend").perform(context).strip() or "auto"
|
||||
|
||||
nodes = []
|
||||
frames = []
|
||||
if os.path.isfile(handeye):
|
||||
try:
|
||||
tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
frames.append(f"[gauge] 手眼文件读不了（{handeye}）：{exc}")
|
||||
else:
|
||||
frames.append(f"[gauge] 手眼 TF {ee_frame} ← {cam_frame}（{Path(handeye).name}）")
|
||||
nodes.append(
|
||||
Node(
|
||||
package="tf2_ros",
|
||||
executable="static_transform_publisher",
|
||||
name="gauge_handeye_static_tf",
|
||||
arguments=[
|
||||
"--x", str(tx), "--y", str(ty), "--z", str(tz),
|
||||
"--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
|
||||
"--frame-id", ee_frame,
|
||||
"--child-frame-id", cam_frame,
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
)
|
||||
else:
|
||||
frames.append(
|
||||
f"[gauge] 没有手眼文件（{handeye or 'paths.handeye_file 为空'}）"
|
||||
"—— 不发布静态 TF，S05 会在查 TF 时报错。先跑 scripts/run_handeye.sh"
|
||||
)
|
||||
|
||||
nodes.append(
|
||||
Node(
|
||||
package="arm_application",
|
||||
executable="gauge_assembly",
|
||||
name="gauge_assembly",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"robot_sn": str(
|
||||
robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""
|
||||
),
|
||||
"dry_run": dry_run,
|
||||
"holes_total": int(holes) if holes else -1,
|
||||
"gripper_backend": backend,
|
||||
}
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
return [
|
||||
LogInfo(msg=f"[gauge] site={path}"),
|
||||
LogInfo(msg=f"[gauge] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
|
||||
LogInfo(
|
||||
msg=f"[gauge] dry_run={dry_run} holes={holes or 'site'} backend={backend}"
|
||||
),
|
||||
*[LogInfo(msg=m) for m in frames],
|
||||
*nodes,
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("dry_run", default_value="false"),
|
||||
DeclareLaunchArgument("holes_total", default_value=""),
|
||||
DeclareLaunchArgument("gripper_backend", default_value="auto"),
|
||||
OpaqueFunction(function=_launch_setup),
|
||||
]
|
||||
)
|
||||
@@ -7,12 +7,10 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
from launch.actions import EmitEvent, RegisterEventHandler
|
||||
@@ -34,43 +32,21 @@ def _ws_root() -> Path:
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _quat_from_R(R):
|
||||
tr = float(R[0][0] + R[1][1] + R[2][2])
|
||||
if tr > 0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
w = 0.25 * s
|
||||
x = (R[2][1] - R[1][2]) / s
|
||||
y = (R[0][2] - R[2][0]) / s
|
||||
z = (R[1][0] - R[0][1]) / s
|
||||
elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
|
||||
s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
|
||||
w = (R[2][1] - R[1][2]) / s
|
||||
x = 0.25 * s
|
||||
y = (R[0][1] + R[1][0]) / s
|
||||
z = (R[0][2] - R[2][1]) / s
|
||||
elif R[1][1] > R[2][2]:
|
||||
s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
|
||||
w = (R[0][2] - R[2][1]) / s
|
||||
x = (R[0][1] + R[1][0]) / s
|
||||
y = 0.25 * s
|
||||
z = (R[1][2] - R[2][1]) / s
|
||||
else:
|
||||
s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
|
||||
w = (R[1][2] - R[0][1]) / s
|
||||
x = (R[0][2] - R[0][1]) / s
|
||||
y = (R[1][2] - R[2][0]) / s
|
||||
z = 0.25 * s
|
||||
return x, y, z, w
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f)
|
||||
T = data["T_4x4"]
|
||||
tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
|
||||
R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
|
||||
qx, qy, qz, qw = _quat_from_R(R)
|
||||
return tx, ty, tz, qx, qy, qz, qw
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
|
||||
并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
|
||||
这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
|
||||
手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _as_bool(s: str) -> bool:
|
||||
|
||||
@@ -7,12 +7,10 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
@@ -32,43 +30,21 @@ def _ws_root() -> Path:
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _quat_from_R(R):
|
||||
tr = float(R[0][0] + R[1][1] + R[2][2])
|
||||
if tr > 0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
w = 0.25 * s
|
||||
x = (R[2][1] - R[1][2]) / s
|
||||
y = (R[0][2] - R[2][0]) / s
|
||||
z = (R[1][0] - R[0][1]) / s
|
||||
elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
|
||||
s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
|
||||
w = (R[2][1] - R[1][2]) / s
|
||||
x = (R[0][1] + R[1][0]) / s
|
||||
y = (R[1][0] + R[0][1]) / s
|
||||
z = (R[0][2] - R[2][1]) / s
|
||||
elif R[1][1] > R[2][2]:
|
||||
s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
|
||||
w = (R[0][2] - R[2][1]) / s
|
||||
x = (R[0][1] + R[1][0]) / s
|
||||
y = (R[1][2] + R[2][1]) / s
|
||||
z = (R[1][2] - R[2][1]) / s
|
||||
else:
|
||||
s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
|
||||
w = (R[1][2] - R[0][1]) / s
|
||||
x = (R[0][2] - R[0][1]) / s
|
||||
y = (R[1][2] - R[2][0]) / s
|
||||
z = (R[1][2] - R[2][1]) / s
|
||||
return x, y, z, w
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f)
|
||||
T = data["T_4x4"]
|
||||
tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
|
||||
R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
|
||||
qx, qy, qz, qw = _quat_from_R(R)
|
||||
return tx, ty, tz, qx, qy, qz, qw
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
|
||||
并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
|
||||
这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
|
||||
手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
|
||||
@@ -8,12 +8,10 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
@@ -32,43 +30,21 @@ def _ws_root() -> Path:
|
||||
return Path.home() / "ros2"
|
||||
|
||||
|
||||
def _quat_from_R(R):
|
||||
tr = float(R[0][0] + R[1][1] + R[2][2])
|
||||
if tr > 0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
w = 0.25 * s
|
||||
x = (R[2][1] - R[1][2]) / s
|
||||
y = (R[0][2] - R[2][0]) / s
|
||||
z = (R[1][0] - R[0][1]) / s
|
||||
elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
|
||||
s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
|
||||
w = (R[2][1] - R[1][2]) / s
|
||||
x = (R[0][1] + R[1][0]) / s
|
||||
y = (R[1][0] + R[0][1]) / s
|
||||
z = (R[1][2] - R[2][1]) / s
|
||||
elif R[1][1] > R[2][2]:
|
||||
s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
|
||||
w = (R[0][2] - R[2][1]) / s
|
||||
x = (R[1][0] + R[0][1]) / s
|
||||
y = (R[1][2] + R[2][1]) / s
|
||||
z = (R[1][2] - R[2][1]) / s
|
||||
else:
|
||||
s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
|
||||
w = (R[1][2] - R[0][1]) / s
|
||||
x = (R[0][2] - R[0][1]) / s
|
||||
y = (R[1][2] + R[2][1]) / s
|
||||
z = (R[1][2] - R[2][1]) / s
|
||||
return x, y, z, w
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f)
|
||||
T = data["T_4x4"]
|
||||
tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
|
||||
R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
|
||||
qx, qy, qz, qw = _quat_from_R(R)
|
||||
return tx, ty, tz, qx, qy, qz, qw
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
|
||||
并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
|
||||
这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
|
||||
手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
|
||||
@@ -6,12 +6,10 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
|
||||
@@ -54,43 +52,21 @@ def _load_site():
|
||||
return merged, path, perception_overrides(data), servo_overrides(data), robot
|
||||
|
||||
|
||||
def _quat_from_R(R):
|
||||
tr = float(R[0][0] + R[1][1] + R[2][2])
|
||||
if tr > 0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
w = 0.25 * s
|
||||
x = (R[2][1] - R[1][2]) / s
|
||||
y = (R[0][2] - R[2][0]) / s
|
||||
z = (R[1][0] - R[0][1]) / s
|
||||
elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
|
||||
s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
|
||||
w = (R[2][1] - R[1][2]) / s
|
||||
x = 0.25 * s
|
||||
y = (R[0][1] + R[1][0]) / s
|
||||
z = (R[0][2] + R[2][0]) / s
|
||||
elif R[1][1] > R[2][2]:
|
||||
s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
|
||||
w = (R[0][2] - R[2][0]) / s
|
||||
x = (R[0][1] + R[1][0]) / s
|
||||
y = 0.25 * s
|
||||
z = (R[1][2] + R[2][1]) / s
|
||||
else:
|
||||
s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
|
||||
w = (R[1][0] - R[0][1]) / s
|
||||
x = (R[0][2] + R[2][0]) / s
|
||||
y = (R[1][2] + R[2][1]) / s
|
||||
z = 0.25 * s
|
||||
return x, y, z, w
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f)
|
||||
T = data["T_4x4"]
|
||||
tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
|
||||
R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
|
||||
qx, qy, qz, qw = _quat_from_R(R)
|
||||
return tx, ty, tz, qx, qy, qz, qw
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
实现只有一份，在 ``arm_application.handeye_lib``（``read_transform_yaml``），
|
||||
并由回环测试钉住：``quat → R`` 必须能还原文件里的 ``T_4x4``。
|
||||
这段换算曾经在 4 个 launch 文件里各抄了一遍，且抄错了 3 个分支 ——
|
||||
手眼姿态差几十度，定位"总能到附近、永远差一点"，看起来像相机标定不准。
|
||||
"""
|
||||
from arm_application.handeye_lib import read_transform_yaml
|
||||
|
||||
xyz, quat = read_transform_yaml(path)
|
||||
return (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
float(quat[0]), float(quat[1]), float(quat[2]), float(quat[3]),
|
||||
)
|
||||
|
||||
|
||||
def _launch_setup(context, *args, **kwargs):
|
||||
|
||||
@@ -78,6 +78,8 @@ setup(
|
||||
'stage3_search = arm_application.stage3_search_node:main',
|
||||
# 三段式装配流水线（跟踪 → 定位 → 找孔，单进程）
|
||||
'assembly_pipeline = arm_application.pipeline_node:main',
|
||||
# 通止规装配（看板 S00–S11，含自检/换孔/掉件重放）
|
||||
'gauge_assembly = arm_application.gauge_assembly_node:main',
|
||||
'tactile = arm_application.tactile_node:main',
|
||||
'tactile_cli = arm_application.tactile_cli:main',
|
||||
],
|
||||
|
||||
@@ -46,12 +46,13 @@ def _by_key(checks):
|
||||
# 清单内容
|
||||
# =============================================================================
|
||||
def test_core_checks_are_present_and_required():
|
||||
checks = _by_key(_build())
|
||||
"""核心判据依赖的东西必须是**必检**（配了工具系的那种机器）。"""
|
||||
checks = _by_key(_build(tool_name="tool1"))
|
||||
for key in (
|
||||
"arm_jtc",
|
||||
"arm_home",
|
||||
"arm_named_pose",
|
||||
"arm_switch_tool",
|
||||
"arm_compliant",
|
||||
"gripper_open",
|
||||
"gripper_close",
|
||||
"camera_color",
|
||||
@@ -63,6 +64,36 @@ def test_core_checks_are_present_and_required():
|
||||
assert checks[key].required, f"{key} 是核心判据依赖，必须是必检"
|
||||
|
||||
|
||||
def test_jtc_check_expects_the_type_this_flow_actually_calls():
|
||||
"""JTC 自检的类型必须是 control_msgs（本流程直接发给 ros2_control）。
|
||||
|
||||
写成 arm_msgs/action/FollowJointTrajectory（arm_control 适配器那一套）会
|
||||
让自检**永远探不到服务端**：报"控制器没起来"，而控制器明明好着。
|
||||
"""
|
||||
c = _by_key(_build())["arm_jtc"]
|
||||
assert c.expect_type == "control_msgs/action/FollowJointTrajectory"
|
||||
|
||||
|
||||
def test_switch_tool_required_only_when_a_tool_is_configured():
|
||||
"""配了工具系 → 切不过去 S01 会中止，所以按必检拦；没配 → 选检。
|
||||
|
||||
没配工具系的机器上（比如仿真）标必检，会让"仿真里根本不存在
|
||||
/switch_tool 服务"变成每次启动的一条红字，人很快就不看自检了。
|
||||
"""
|
||||
assert _by_key(_build(tool_name="tool1"))["arm_switch_tool"].required is True
|
||||
assert _by_key(_build(tool_name=""))["arm_switch_tool"].required is False
|
||||
|
||||
|
||||
def test_compliant_descent_is_optional():
|
||||
"""S07 不依赖柔顺 action（节点内步进 + 力突变停是同一套做法）。
|
||||
|
||||
标必检的后果：仿真里永远过不了自检，而 S07 其实跑得挺好。
|
||||
"""
|
||||
c = _by_key(_build())["arm_compliant"]
|
||||
assert c.required is False
|
||||
assert "S07" in c.why
|
||||
|
||||
|
||||
def test_tactile_must_not_be_downgraded_to_optional():
|
||||
"""**负向**：触觉是 S07/S08/E1 三个判据的唯一数据源。
|
||||
|
||||
@@ -175,9 +206,17 @@ def test_report_lists_why_so_the_operator_knows_urgency():
|
||||
|
||||
|
||||
def test_counts_summary():
|
||||
"""数量刻意写死：加/减一条自检项时必须来这里改一行并想清楚它属于哪一类。
|
||||
|
||||
当前（未配工具系、有手外相机、无权重）：
|
||||
必检 9 = arm_jtc, arm_home, arm_named_pose, gripper_open, gripper_close,
|
||||
camera_color, camera_info, tactile_status, gripper_status
|
||||
选检 3 = coarse_camera, arm_switch_tool, arm_compliant
|
||||
"""
|
||||
c = summary_counts(_results(coarse_camera=False))
|
||||
assert c["required_total"] == 11 and c["required_ok"] == 11
|
||||
assert c["optional_total"] == 1 and c["optional_ok"] == 0
|
||||
assert c["required_total"] == 9 and c["required_ok"] == 9
|
||||
# 选检里只有手外相机没起来 → 3 项里 2 项通过（工具系/柔顺下压按通过算）
|
||||
assert c["optional_total"] == 3 and c["optional_ok"] == 2
|
||||
|
||||
|
||||
def test_all_ready_says_so():
|
||||
|
||||
@@ -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,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"
|
||||
)
|
||||
@@ -39,6 +39,11 @@ from arm_application.model_labels import (
|
||||
|
||||
def _make_onnx(path, names_json=None, nc=None, metadata=None):
|
||||
"""造一个最小可读的 ONNX，用于测试 metadata 读取。"""
|
||||
# onnx 只在真机环境（ros_flexiv）里装了，仿真环境（ros_gz_sim）没有。
|
||||
# 缺它时**跳过**而不是失败：仿真环境里每次 pytest 都多十几条红字，
|
||||
# 很快就没有人看测试结果了（"狼来了"），而真正需要 onnx 的用例
|
||||
# 本来也没法在没有 onnx 的环境里验证。
|
||||
pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）")
|
||||
import onnx
|
||||
from onnx import TensorProto, helper
|
||||
|
||||
|
||||
@@ -45,6 +45,11 @@ REPO = Path(__file__).resolve().parents[3]
|
||||
|
||||
|
||||
def _make_onnx(path: Path, names_json: str | None = None) -> Path:
|
||||
# onnx 只在真机环境（ros_flexiv）里装了，仿真环境（ros_gz_sim）没有。
|
||||
# 缺它时**跳过**而不是失败：仿真环境里每次 pytest 都多十几条红字，
|
||||
# 很快就没有人看测试结果了（"狼来了"），而真正需要 onnx 的用例
|
||||
# 本来也没法在没有 onnx 的环境里验证。
|
||||
pytest.importorskip("onnx", reason="本环境没装 onnx（只有真机 conda 环境有）")
|
||||
import onnx
|
||||
from onnx import TensorProto, helper
|
||||
|
||||
|
||||
Reference in New Issue
Block a user