主界面要直接下发夹爪并看仿真，再开夹爪页会把相机和触觉挡住。开口、力度和旋转收到左栏，壳体、抱闸、手指改成对比色，黑指在深色底上会看不见。臂清单从 site.yaml 读，界面不再写死哪一台。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+38
-3
@@ -8,10 +8,45 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROOT}/scripts/env.sh"
|
||||
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
|
||||
if [[ -z "${DISPLAY:-}" && -S /tmp/.X11-unix/X1 ]]; then
|
||||
export DISPLAY=:1
|
||||
# 屏幕跟当前登录会话走。这台工控机是 :1，别的 Linux 桌面常常是 :0。
|
||||
if [[ -n "${DISPLAY:-}" ]]; then
|
||||
_disp="${DISPLAY#:}"
|
||||
_disp="${_disp%%.*}"
|
||||
if [[ ! -S "/tmp/.X11-unix/X${_disp}" ]]; then
|
||||
unset DISPLAY
|
||||
fi
|
||||
unset _disp
|
||||
fi
|
||||
if [[ -z "${XAUTHORITY:-}" && -r "/run/user/$(id -u)/gdm/Xauthority" ]]; then
|
||||
if [[ -z "${DISPLAY:-}" ]]; then
|
||||
_seat="$(who | awk -v u="$(id -un)" '$1==u && $2 ~ /^:/ {print $2; exit}')"
|
||||
_seat="${_seat%%.*}"
|
||||
_n="${_seat#:}"
|
||||
if [[ -n "${_n}" && -S "/tmp/.X11-unix/X${_n}" ]]; then
|
||||
export DISPLAY="${_seat}"
|
||||
else
|
||||
for _sock in /tmp/.X11-unix/X*; do
|
||||
if [[ -S "${_sock}" ]]; then
|
||||
export DISPLAY=":${_sock##*/X}"
|
||||
break
|
||||
fi
|
||||
done
|
||||
fi
|
||||
unset _seat _n _sock
|
||||
fi
|
||||
# 有真实屏幕时不要留在 offscreen，否则 OpenGL 窗口起不来，模型也不会画到显卡上。
|
||||
if [[ "${QT_QPA_PLATFORM:-}" == "offscreen" && -n "${DISPLAY:-}" ]]; then
|
||||
_disp="${DISPLAY#:}"
|
||||
_disp="${_disp%%.*}"
|
||||
if [[ -S "/tmp/.X11-unix/X${_disp}" ]]; then
|
||||
unset QT_QPA_PLATFORM
|
||||
fi
|
||||
unset _disp
|
||||
fi
|
||||
if [[ -z "${XAUTHORITY:-}" ]]; then
|
||||
if [[ -r "${HOME}/.Xauthority" ]]; then
|
||||
export XAUTHORITY="${HOME}/.Xauthority"
|
||||
elif [[ -r "/run/user/$(id -u)/gdm/Xauthority" ]]; then
|
||||
export XAUTHORITY="/run/user/$(id -u)/gdm/Xauthority"
|
||||
fi
|
||||
fi
|
||||
exec python3 -m arm_application.operator_app
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,339 @@
|
||||
"""用非夕给的 Rizon4 外壳网格摆出当前姿态。
|
||||
|
||||
网格和安装位姿来自 ``flexiv_description`` 的 ``visual_parameters.yaml``
|
||||
与 ``meshes/Rizon4/visual/*.obj``，不另画一套简图。面数按固定步长抽稀，
|
||||
只为了界面里能转得动，几何仍是厂家的。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from arm_application.operator_arm_state import link_frames
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
# 单个连杆原始面数大约 5000。再按 700 抽稀会在壳上留出三角洞，
|
||||
# 白壳和灰蓝关节分不出来。这里保留全部面。
|
||||
_FACES_PER_LINK = 8000
|
||||
_MODELS: dict[str, list[tuple[list[tuple[float, float, float]], tuple]]] | None = None
|
||||
|
||||
|
||||
def posed_triangles(q: list[float]) -> list[tuple]:
|
||||
"""返回基座系下的三角面 ``(a, b, c, 明暗, rgb)``。
|
||||
|
||||
``rgb`` 来自同目录 ``.mtl`` 的 ``Kd``。外壳是白的，关节壳体是灰蓝，
|
||||
这是厂家导出的两种材质，不是另涂的颜色。
|
||||
"""
|
||||
world, shade, rgb = _posed_arrays(q)
|
||||
return [
|
||||
(
|
||||
(float(world[i, 0, 0]), float(world[i, 0, 1]), float(world[i, 0, 2])),
|
||||
(float(world[i, 1, 0]), float(world[i, 1, 1]), float(world[i, 1, 2])),
|
||||
(float(world[i, 2, 0]), float(world[i, 2, 1]), float(world[i, 2, 2])),
|
||||
float(shade[i]),
|
||||
(float(rgb[i, 0]), float(rgb[i, 1]), float(rgb[i, 2])),
|
||||
)
|
||||
for i in range(shade.shape[0])
|
||||
]
|
||||
|
||||
|
||||
def posed_gl(q: list[float]) -> tuple[bytes, int, tuple[float, float, float], float]:
|
||||
"""给显卡的一份网格：交错 ``xyzrgb``、顶点数、中心、半径。
|
||||
|
||||
拖视角只改观察矩阵，不再把四万个面在界面线程里重投影。
|
||||
"""
|
||||
world, shade, rgb = _posed_arrays(q)
|
||||
light = (0.55 + 0.45 * shade)[:, None]
|
||||
colored = (rgb * light).astype(np.float32)
|
||||
pos = world.reshape(-1, 3).astype(np.float32)
|
||||
col = np.repeat(colored, 3, axis=0)
|
||||
center = pos.mean(axis=0)
|
||||
radius = float(np.linalg.norm(pos - center, axis=1).max())
|
||||
packed = np.concatenate([pos, col], axis=1).astype(np.float32, copy=False)
|
||||
return (
|
||||
packed.tobytes(),
|
||||
int(pos.shape[0]),
|
||||
(float(center[0]), float(center[1]), float(center[2])),
|
||||
max(radius, 1e-4),
|
||||
)
|
||||
|
||||
|
||||
def _posed_arrays(q: list[float]):
|
||||
"""每个连杆做一次矩阵乘法，不再对每个顶点走 Python 循环。"""
|
||||
frames = link_frames(q)
|
||||
worlds: list[np.ndarray] = []
|
||||
shades: list[np.ndarray] = []
|
||||
colors: list[np.ndarray] = []
|
||||
for link, parts in _models().items():
|
||||
frame = frames.get(link)
|
||||
if frame is None:
|
||||
continue
|
||||
rot = np.asarray(frame[0], dtype=np.float64)
|
||||
origin = np.asarray(frame[1], dtype=np.float64)
|
||||
for part in parts:
|
||||
local = np.einsum("ij,fvj->fvi", part["erot"], part["verts"]) + part["epos"]
|
||||
world = np.einsum("ij,fvj->fvi", rot, local) + origin
|
||||
ab = world[:, 1] - world[:, 0]
|
||||
ac = world[:, 2] - world[:, 0]
|
||||
normal = np.cross(ab, ac)
|
||||
norm = np.linalg.norm(normal, axis=1)
|
||||
norm = np.where(norm > 0.0, norm, 1.0)
|
||||
facing = np.abs(normal[:, 0] + normal[:, 1]) / (norm * math.sqrt(2.0))
|
||||
worlds.append(world)
|
||||
shades.append(0.28 + 0.72 * facing)
|
||||
colors.append(part["rgb"])
|
||||
if not worlds:
|
||||
empty = np.zeros((0, 3, 3), dtype=np.float64)
|
||||
return empty, np.zeros((0,), dtype=np.float64), np.zeros((0, 3), dtype=np.float64)
|
||||
return np.concatenate(worlds), np.concatenate(shades), np.concatenate(colors)
|
||||
|
||||
|
||||
def view_rotation(yaw: float, pitch: float):
|
||||
"""先绕竖直轴转，再俯仰。拖拽视角用这一份，不在界面里另写一套。"""
|
||||
return _mul(_rx(pitch), _rz(yaw))
|
||||
|
||||
|
||||
def view_scale(radius: float, width: int, height: int) -> tuple[float, float]:
|
||||
"""和着色器里的 ``u_sx`` / ``u_sy`` 同一套缩放。宽屏时横向再缩一档。"""
|
||||
fit = 0.85 / max(float(radius), 1e-4)
|
||||
width = max(int(width), 1)
|
||||
height = max(int(height), 1)
|
||||
if width >= height:
|
||||
return fit * height / width, fit
|
||||
return fit, fit * width / height
|
||||
|
||||
|
||||
def screen_point(
|
||||
point: tuple[float, float, float],
|
||||
yaw: float,
|
||||
pitch: float,
|
||||
center: tuple[float, float, float],
|
||||
radius: float,
|
||||
width: int,
|
||||
height: int,
|
||||
) -> tuple[float, float]:
|
||||
"""基座系的点投到控件像素。纵向和着色器里的 ``p.z`` 是同一套，+Z 朝画面上方。"""
|
||||
shifted = (
|
||||
float(point[0]) - float(center[0]),
|
||||
float(point[1]) - float(center[1]),
|
||||
float(point[2]) - float(center[2]),
|
||||
)
|
||||
viewed = _apply(view_rotation(yaw, pitch), shifted)
|
||||
sx, sy = view_scale(radius, width, height)
|
||||
ndc_x = viewed[0] * sx
|
||||
# 着色器写的是 ``p.z * u_sy``。OpenGL 的 y 朝上，这里再换成 Qt 的像素 y。
|
||||
ndc_y = viewed[2] * sy
|
||||
width = max(int(width), 1)
|
||||
height = max(int(height), 1)
|
||||
return (ndc_x * 0.5 + 0.5) * width, (1.0 - (ndc_y * 0.5 + 0.5)) * height
|
||||
|
||||
|
||||
def ground_grid(extent: float = 1.0, step: float = 0.25) -> list[tuple[tuple[float, float, float], tuple[float, float, float]]]:
|
||||
"""地平面格子，z = 0，单位米。以前这张格子在 RViz 里，现在画在工位仿真上。"""
|
||||
if step <= 0 or extent <= 0:
|
||||
raise ValueError("地平面格子的间距和范围都要大于 0")
|
||||
count = int(round(extent / step))
|
||||
lines = []
|
||||
for index in range(-count, count + 1):
|
||||
coord = index * step
|
||||
lines.append(((-extent, coord, 0.0), (extent, coord, 0.0)))
|
||||
lines.append(((coord, -extent, 0.0), (coord, extent, 0.0)))
|
||||
return lines
|
||||
|
||||
|
||||
def axis_tip(
|
||||
origin: tuple[float, float, float],
|
||||
rot: tuple,
|
||||
axis: int,
|
||||
length: float,
|
||||
) -> tuple[float, float, float]:
|
||||
"""坐标系某一轴的端点。``rot`` 把连杆坐标变到基座坐标，列向量就是三根轴。"""
|
||||
basis = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))[axis]
|
||||
direction = _apply(rot, basis)
|
||||
return (
|
||||
origin[0] + direction[0] * length,
|
||||
origin[1] + direction[1] * length,
|
||||
origin[2] + direction[2] * length,
|
||||
)
|
||||
|
||||
|
||||
def _place(rot, origin, erot, epos, vertex):
|
||||
local = _add(epos, _apply(erot, vertex))
|
||||
return _add(origin, _apply(rot, local))
|
||||
|
||||
|
||||
def _shade(a, b, c) -> float:
|
||||
ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
|
||||
vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
|
||||
nx = uy * vz - uz * vy
|
||||
ny = uz * vx - ux * vz
|
||||
nz = ux * vy - uy * vx
|
||||
norm = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
|
||||
facing = abs(nx + ny) / (norm * math.sqrt(2.0))
|
||||
return 0.28 + 0.72 * facing
|
||||
|
||||
|
||||
def _models() -> dict[str, list]:
|
||||
global _MODELS
|
||||
if _MODELS is not None:
|
||||
return _MODELS
|
||||
root = _description_root()
|
||||
visual = yaml.safe_load((root / "config/Rizon4/visual_parameters.yaml").read_text(encoding="utf-8"))
|
||||
params = (visual or {}).get("visual_parameters") or {}
|
||||
loaded: dict[str, list] = {}
|
||||
for link, spec in params.items():
|
||||
shell = ((spec or {}).get("visual") or {})
|
||||
parts = []
|
||||
for kind in ("shell", "ring"):
|
||||
item = shell.get(kind)
|
||||
if not isinstance(item, dict):
|
||||
continue
|
||||
mesh = item.get("mesh") or {}
|
||||
path = root / str(mesh.get("path") or "")
|
||||
if not path.is_file():
|
||||
continue
|
||||
scale = _scale(mesh.get("scale"))
|
||||
origin = item.get("origin") or {}
|
||||
erot = _rpy(
|
||||
float(origin.get("roll") or 0.0),
|
||||
float(origin.get("pitch") or 0.0),
|
||||
float(origin.get("yaw") or 0.0),
|
||||
)
|
||||
epos = (
|
||||
float(origin.get("x") or 0.0),
|
||||
float(origin.get("y") or 0.0),
|
||||
float(origin.get("z") or 0.0),
|
||||
)
|
||||
parts.append(_part_arrays(_load_obj(path, scale, _materials(path)), erot, epos))
|
||||
if parts:
|
||||
loaded[str(link)] = parts
|
||||
if "base_link" not in loaded or "link7" not in loaded:
|
||||
raise FileNotFoundError(f"Rizon4 外壳网格不完整：{root}")
|
||||
_MODELS = loaded
|
||||
return loaded
|
||||
|
||||
|
||||
def _part_arrays(faces: list[tuple], erot, epos) -> dict:
|
||||
"""把一个零件收成数组，姿态更新时整块相乘。"""
|
||||
if not faces:
|
||||
verts = np.zeros((0, 3, 3), dtype=np.float64)
|
||||
rgb = np.zeros((0, 3), dtype=np.float64)
|
||||
else:
|
||||
verts = np.asarray([[a, b, c] for a, b, c, _color in faces], dtype=np.float64)
|
||||
rgb = np.asarray([color for _a, _b, _c, color in faces], dtype=np.float64)
|
||||
return {
|
||||
"verts": verts,
|
||||
"rgb": rgb,
|
||||
"erot": np.asarray(erot, dtype=np.float64),
|
||||
"epos": np.asarray(epos, dtype=np.float64),
|
||||
}
|
||||
|
||||
|
||||
def _description_root() -> Path:
|
||||
root = ros2_root()
|
||||
candidates = (
|
||||
root / "src/flexiv_description",
|
||||
root / "install/flexiv_description/share/flexiv_description",
|
||||
)
|
||||
for item in candidates:
|
||||
if (item / "config/Rizon4/visual_parameters.yaml").is_file():
|
||||
return item
|
||||
raise FileNotFoundError("找不到 flexiv_description 的 Rizon4 外观")
|
||||
|
||||
|
||||
def _materials(path: Path) -> dict[str, tuple[float, float, float]]:
|
||||
mtl = path.with_suffix(".mtl")
|
||||
colors: dict[str, tuple[float, float, float]] = {}
|
||||
name = ""
|
||||
if not mtl.is_file():
|
||||
return colors
|
||||
for line in mtl.read_text(encoding="utf-8", errors="replace").splitlines():
|
||||
if line.startswith("newmtl "):
|
||||
name = line.split(None, 1)[1].strip()
|
||||
elif name and line.startswith("Kd "):
|
||||
parts = line.split()
|
||||
if len(parts) >= 4:
|
||||
colors[name] = (float(parts[1]), float(parts[2]), float(parts[3]))
|
||||
return colors
|
||||
|
||||
|
||||
def _load_obj(path: Path, scale: tuple[float, float, float], materials: dict) -> list[tuple]:
|
||||
fallback = (0.75, 0.76, 0.78)
|
||||
rgb = fallback
|
||||
vertices: list[tuple[float, float, float]] = []
|
||||
faces: list[tuple[int, int, int, tuple]] = []
|
||||
for line in path.read_text(encoding="utf-8", errors="replace").splitlines():
|
||||
if line.startswith("usemtl "):
|
||||
rgb = materials.get(line.split(None, 1)[1].strip(), fallback)
|
||||
elif line.startswith("v "):
|
||||
_v, x, y, z, *_rest = line.split()
|
||||
vertices.append((float(x) * scale[0], float(y) * scale[1], float(z) * scale[2]))
|
||||
elif line.startswith("f "):
|
||||
idx = []
|
||||
for token in line.split()[1:]:
|
||||
raw = token.split("/")[0]
|
||||
if not raw:
|
||||
continue
|
||||
number = int(raw)
|
||||
idx.append(number - 1 if number > 0 else number)
|
||||
for offset in range(1, len(idx) - 1):
|
||||
faces.append((idx[0], idx[offset], idx[offset + 1], rgb))
|
||||
if not faces or not vertices:
|
||||
return []
|
||||
step = max(1, len(faces) // _FACES_PER_LINK)
|
||||
chosen = faces[::step]
|
||||
out = []
|
||||
count = len(vertices)
|
||||
for ia, ib, ic, color in chosen:
|
||||
if not (0 <= ia < count and 0 <= ib < count and 0 <= ic < count):
|
||||
continue
|
||||
out.append((vertices[ia], vertices[ib], vertices[ic], color))
|
||||
return out
|
||||
|
||||
|
||||
def _scale(raw) -> tuple[float, float, float]:
|
||||
if raw is None:
|
||||
return (1.0, 1.0, 1.0)
|
||||
if isinstance(raw, (int, float)):
|
||||
value = float(raw)
|
||||
return (value, value, value)
|
||||
text = str(raw).replace(",", " ").split()
|
||||
nums = [float(item) for item in text[:3]]
|
||||
while len(nums) < 3:
|
||||
nums.append(nums[-1] if nums else 1.0)
|
||||
return (nums[0], nums[1], nums[2])
|
||||
|
||||
|
||||
def _rpy(roll: float, pitch: float, yaw: float):
|
||||
return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
|
||||
|
||||
|
||||
def _rx(angle: float):
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
|
||||
|
||||
|
||||
def _ry(angle: float):
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
|
||||
|
||||
|
||||
def _rz(angle: float):
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
|
||||
|
||||
|
||||
def _mul(a, b):
|
||||
return tuple(tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)) for i in range(3))
|
||||
|
||||
|
||||
def _apply(rot, vec):
|
||||
return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3))
|
||||
|
||||
|
||||
def _add(a, b):
|
||||
return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
|
||||
@@ -0,0 +1,595 @@
|
||||
"""工位界面用的机械臂状态：只读使能 / Remote，并用关节角画出姿态。
|
||||
|
||||
不会调用 Enable，也不会在控制栈或拖拽桥已经占着 RDK 时再连一次。
|
||||
``robot.mode()`` 看不出 Manual，必须看 ``operational_status``。
|
||||
示教器上的 Auto（Remote）没有远程切换接口，这里只负责把这句话摆在界面上。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import re
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
from arm_application.site_io import listed_arms, ros2_root
|
||||
|
||||
# 和 scripts/lib/rdk_status.py 的 MANUAL_HINT 同一件事，界面上始终可见。
|
||||
REMOTE_PROMPT = (
|
||||
"请在示教器上打开 Auto（Remote）。"
|
||||
"RDK 没有远程拨这个开关的接口，必须人到现场拨。"
|
||||
)
|
||||
|
||||
_AXES = (
|
||||
(0.0, 0.0, 1.0),
|
||||
(0.0, 1.0, 0.0),
|
||||
(0.0, 0.0, 1.0),
|
||||
(0.0, 1.0, 0.0),
|
||||
(0.0, 0.0, 1.0),
|
||||
(0.0, 1.0, 0.0),
|
||||
(0.0, 0.0, 1.0),
|
||||
)
|
||||
_JOINTS = tuple(f"joint{i}" for i in range(1, 8))
|
||||
_LOCK = threading.Lock()
|
||||
_CACHE: dict[str, dict] = {}
|
||||
_STOP = False
|
||||
_STARTED = False
|
||||
_HOLD = 0
|
||||
_HOLD_LOCK = threading.Lock()
|
||||
|
||||
|
||||
def _empty(arm: str) -> dict:
|
||||
return {
|
||||
"arm": arm,
|
||||
"level": "warn",
|
||||
"enabled": False,
|
||||
"summary": "还没读到这台臂的状态",
|
||||
"prompt": REMOTE_PROMPT,
|
||||
"prompt_hot": True,
|
||||
"mode": "",
|
||||
"op_status": "",
|
||||
"fault": None,
|
||||
"estop_released": None,
|
||||
"q": None,
|
||||
"points": [],
|
||||
"source": "",
|
||||
"stack_up": False,
|
||||
"stamp": 0.0,
|
||||
}
|
||||
|
||||
|
||||
def snapshot(arm: str) -> dict:
|
||||
with _LOCK:
|
||||
row = _CACHE.get(arm)
|
||||
return dict(row) if row else _empty(arm)
|
||||
|
||||
|
||||
def present(
|
||||
arm: str,
|
||||
*,
|
||||
stack_up: bool,
|
||||
bridge_up: bool,
|
||||
probe: dict | None,
|
||||
joints: list[float] | None,
|
||||
error: str = "",
|
||||
) -> dict:
|
||||
"""把一次读数收成界面直接能画的字典。纯函数，不连机器人。"""
|
||||
probe = probe or {}
|
||||
q = _seven(joints) or _seven(probe.get("q"))
|
||||
op_status = str(probe.get("op_status") or "")
|
||||
mode = str(probe.get("mode") or "")
|
||||
manual = "MANUAL" in op_status.upper()
|
||||
if stack_up and q is not None:
|
||||
enabled = True
|
||||
level = "ok"
|
||||
summary = "控制栈在发关节角，这台臂已经接上。"
|
||||
if probe.get("_shared_stack"):
|
||||
summary = "控制栈在发关节角，但看不出是 89 还是 91，两台显示的是同一份。"
|
||||
source = "控制栈 /joint_states"
|
||||
elif stack_up:
|
||||
enabled = False
|
||||
level = "fail"
|
||||
summary = "控制栈在跑，但没有关节数据。通常是还没使能，一直在等 operational。"
|
||||
source = "控制栈"
|
||||
elif bridge_up and not probe:
|
||||
enabled = False
|
||||
level = "warn"
|
||||
summary = "软拖拽桥占着 RDK，界面不再另开一条连接。"
|
||||
source = "拖拽桥"
|
||||
q = None
|
||||
elif probe.get("connected"):
|
||||
enabled = bool(probe.get("operational"))
|
||||
level = str(probe.get("level") or ("ok" if enabled else "fail"))
|
||||
summary = str(probe.get("summary") or ("已使能" if enabled else "未使能"))
|
||||
source = "RDK 直读（未使能，只查询）"
|
||||
else:
|
||||
enabled = False
|
||||
level = "fail" if error else "warn"
|
||||
summary = error or str(probe.get("summary") or probe.get("error") or "还没连上 RDK")
|
||||
source = "RDK 直读" if probe else ""
|
||||
hot = (not enabled) or manual
|
||||
return {
|
||||
"arm": arm,
|
||||
"level": level,
|
||||
"enabled": enabled,
|
||||
"summary": summary,
|
||||
"prompt": REMOTE_PROMPT,
|
||||
"prompt_hot": hot,
|
||||
"mode": mode,
|
||||
"op_status": op_status,
|
||||
"fault": probe.get("fault") if probe else None,
|
||||
"estop_released": probe.get("estop_released") if probe else None,
|
||||
"q": q,
|
||||
"points": fk_points(q) if q else fk_points([0.0] * 7),
|
||||
"pose_live": q is not None,
|
||||
"source": source,
|
||||
"stack_up": stack_up,
|
||||
"stamp": time.time(),
|
||||
}
|
||||
|
||||
|
||||
def link_frames(q: list[float]) -> dict[str, tuple]:
|
||||
"""各连杆坐标系，和 ``rizon_arm.xacro`` 的关节顺序一致。
|
||||
|
||||
返回 ``base_link`` 与 ``link1``…``link7``。每个值是 ``(旋转, 原点)``，
|
||||
旋转把连杆坐标变到基座坐标。厂家网格就画在这些坐标系里。
|
||||
"""
|
||||
kin = _kinematics()
|
||||
rot = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))
|
||||
pos = (0.0, 0.0, 0.0)
|
||||
frames: dict[str, tuple] = {"base_link": (rot, pos)}
|
||||
for index, angle in enumerate(q[:7]):
|
||||
spec = kin[_JOINTS[index]]
|
||||
pos = _add(pos, _apply(rot, (spec["x"], spec["y"], spec["z"])))
|
||||
rot = _mul(rot, _rpy(spec["roll"], spec["pitch"], spec["yaw"]))
|
||||
rot = _mul(rot, _axis_rot(_AXES[index], float(angle)))
|
||||
frames[f"link{index + 1}"] = (rot, pos)
|
||||
return frames
|
||||
|
||||
|
||||
def fk_points(q: list[float]) -> list[tuple[float, float, float]]:
|
||||
"""按 Rizon4 的 default_kinematics 做正运动学，返回基座到法兰的点（米）。
|
||||
|
||||
关节轴与 ``rizon_arm.xacro`` 一致：1/3/5/7 绕 z，2/4/6 绕 y。
|
||||
URDF 顺序是先平移（父坐标系），再 rpy，再绕关节轴转 q。
|
||||
"""
|
||||
frames = link_frames(q)
|
||||
points = [frames["base_link"][1]]
|
||||
points.extend(frames[f"link{index}"][1] for index in range(1, 8))
|
||||
_rot, pos = _flange_pose(frames)
|
||||
points.append(pos)
|
||||
return points
|
||||
|
||||
|
||||
def display_frames(q: list[float], workpiece: tuple | None = None) -> list[dict]:
|
||||
"""界面要画的坐标系，都在基座系里。
|
||||
|
||||
第一套是基座。第二套默认是法兰（旋转与 ``link7`` 相同，原点再加
|
||||
``link7_to_flange``）。传入工件系时，第二套改画在工件上，不再画法兰。
|
||||
"""
|
||||
frames = link_frames(q)
|
||||
base_rot, base_pos = frames["base_link"]
|
||||
shown = [{"name": "基座", "rot": base_rot, "pos": base_pos}]
|
||||
if workpiece is None:
|
||||
rot, pos = _flange_pose(frames)
|
||||
shown.append({"name": "法兰", "rot": rot, "pos": pos})
|
||||
return shown
|
||||
rot, pos = workpiece
|
||||
if not isinstance(rot, tuple) or len(rot) != 3 or not isinstance(pos, tuple) or len(pos) != 3:
|
||||
raise ValueError("工件坐标系要有旋转和原点")
|
||||
shown.append({"name": "工件", "rot": rot, "pos": pos})
|
||||
return shown
|
||||
|
||||
|
||||
def workpiece_frame(robot: dict | None) -> tuple | None:
|
||||
"""有完整工件系时返回 ``(旋转, 原点)``。没有就返回 None。
|
||||
|
||||
``workpiece_surface_z_m`` 只是上表面高度，没有原点的 XY，也没有朝向，
|
||||
不能当成坐标系。配了 ``gauge_assembly.workpiece_frame`` 却缺 ``xyz`` 或
|
||||
``rpy`` 时直接报错，不用单位阵把缺的朝向补上。
|
||||
``xyz`` 单位米，``rpy`` 单位弧度，都写在基座系，旋转顺序与连杆 ``rpy`` 相同。
|
||||
"""
|
||||
if not isinstance(robot, dict):
|
||||
return None
|
||||
gauge = robot.get("gauge_assembly")
|
||||
if not isinstance(gauge, dict) or "workpiece_frame" not in gauge:
|
||||
return None
|
||||
raw = gauge.get("workpiece_frame")
|
||||
if not isinstance(raw, dict):
|
||||
raise ValueError("工件坐标系要写成 xyz 和 rpy，各三个数")
|
||||
xyz = raw.get("xyz")
|
||||
rpy = raw.get("rpy")
|
||||
if not _three_floats(xyz) or not _three_floats(rpy):
|
||||
raise ValueError("工件坐标系要有 xyz 和 rpy，各三个数，单位是米和弧度，写在基座系里")
|
||||
return _rpy(float(rpy[0]), float(rpy[1]), float(rpy[2])), (
|
||||
float(xyz[0]), float(xyz[1]), float(xyz[2]),
|
||||
)
|
||||
|
||||
|
||||
def _flange_pose(frames: dict) -> tuple:
|
||||
"""法兰位姿。旋转就是 ``link7`` 的旋转，平移来自 ``link7_to_flange``。"""
|
||||
rot, pos = frames["link7"]
|
||||
flange = _kinematics()["link7_to_flange"]
|
||||
pos = _add(pos, _apply(rot, (float(flange["x"]), float(flange["y"]), float(flange["z"]))))
|
||||
return rot, pos
|
||||
|
||||
|
||||
def _three_floats(raw: Any) -> bool:
|
||||
if not isinstance(raw, (list, tuple)) or len(raw) != 3:
|
||||
return False
|
||||
return all(isinstance(item, (int, float)) and not isinstance(item, bool) for item in raw)
|
||||
|
||||
|
||||
def stack_running() -> bool:
|
||||
return _stack_arm() is not None or _alive("ros2_control_node")
|
||||
|
||||
|
||||
def control_holds_rdk() -> bool:
|
||||
"""有控制命令在用 RDK 时，状态轮询必须让开。"""
|
||||
with _HOLD_LOCK:
|
||||
return _HOLD > 0
|
||||
|
||||
|
||||
def release_for_control() -> None:
|
||||
"""停掉正在读状态的 RDK 子进程，把机器人交给控制命令。"""
|
||||
with _HOLD_LOCK:
|
||||
global _HOLD
|
||||
_HOLD += 1
|
||||
_stop_status_probes()
|
||||
deadline = time.time() + 3.0
|
||||
while time.time() < deadline and _status_probe_running():
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
def resume_after_control() -> None:
|
||||
with _HOLD_LOCK:
|
||||
global _HOLD
|
||||
_HOLD = max(0, _HOLD - 1)
|
||||
|
||||
|
||||
def hold_until_idle(pid: int, *, until_stack: bool = False) -> None:
|
||||
"""后台脚本还在时继续占着 RDK。进程退出，或控制栈已经起来，就放开。"""
|
||||
def _watch() -> None:
|
||||
while True:
|
||||
if until_stack and stack_running():
|
||||
break
|
||||
if not _pid_alive(pid):
|
||||
break
|
||||
time.sleep(0.3)
|
||||
resume_after_control()
|
||||
|
||||
threading.Thread(target=_watch, daemon=True, name="arm-rdk-hold").start()
|
||||
|
||||
|
||||
def motion_blocked(action: str, *, stack_up: bool, op_status: str) -> str:
|
||||
"""控制栈没起来时，动臂按钮给一句能执行的原因，而不是把脚本挂在那儿。"""
|
||||
needs_stack = {
|
||||
"pipeline", "stage_track", "stage_locate", "stage_screw",
|
||||
"reset_home", "gauge_run", "handeye_calibrate",
|
||||
}
|
||||
if action not in needs_stack or stack_up:
|
||||
return ""
|
||||
manual = "MANUAL" in str(op_status or "").upper()
|
||||
tail = "示教器现在是 Manual，要先拨到 Auto（Remote）。" if manual else ""
|
||||
return f"控制栈没起来，这个动作动不了臂。{tail}请先在操作页执行「上电控制栈」。"
|
||||
|
||||
|
||||
def start() -> None:
|
||||
global _STARTED
|
||||
with _LOCK:
|
||||
if _STARTED:
|
||||
return
|
||||
_STARTED = True
|
||||
threading.Thread(target=_loop, daemon=True, name="arm-state").start()
|
||||
|
||||
|
||||
def retain_pose(fresh: dict, previous: dict | None, *, now: float | None = None, hold_s: float = 15.0) -> dict:
|
||||
"""这一拍没读到关节角时，短时间沿用上一拍，避免模型被打回零位。
|
||||
|
||||
超过 ``hold_s`` 仍没有新读数就放开，界面回到“没连上”，不把过期姿态一直当成当前。
|
||||
"""
|
||||
if fresh.get("q") is not None or not previous or previous.get("q") is None:
|
||||
return fresh
|
||||
moment = time.time() if now is None else now
|
||||
if moment - float(previous.get("stamp") or 0.0) > hold_s:
|
||||
return fresh
|
||||
kept = dict(fresh)
|
||||
kept["q"] = list(previous["q"])
|
||||
kept["points"] = list(previous.get("points") or [])
|
||||
kept["pose_live"] = True
|
||||
note = "关节角沿用上次读数。"
|
||||
summary = str(kept.get("summary") or "")
|
||||
if note not in summary:
|
||||
kept["summary"] = f"{summary} {note}".strip()
|
||||
return kept
|
||||
|
||||
|
||||
def _loop() -> None:
|
||||
while not _STOP:
|
||||
if control_holds_rdk():
|
||||
time.sleep(0.2)
|
||||
continue
|
||||
try:
|
||||
arms = _arms()
|
||||
if not arms:
|
||||
time.sleep(2.0)
|
||||
continue
|
||||
stack_arm = _stack_arm()
|
||||
stack_up = stack_arm is not None or _alive("ros2_control_node")
|
||||
bridge_up = _alive("rdk_drag_bridge.py")
|
||||
joints = _read_joints() if stack_up else None
|
||||
if stack_up:
|
||||
targets = [row for row in arms if stack_arm is None or row["id"] == stack_arm]
|
||||
if not targets:
|
||||
targets = arms
|
||||
shared = stack_arm is None
|
||||
for row in targets:
|
||||
fresh = present(
|
||||
row["id"],
|
||||
stack_up=True,
|
||||
bridge_up=bridge_up,
|
||||
probe={"_shared_stack": True} if shared else None,
|
||||
joints=joints,
|
||||
)
|
||||
_store(retain_pose(fresh, snapshot(row["id"])))
|
||||
else:
|
||||
for row in arms:
|
||||
probe: dict | None = None
|
||||
error = ""
|
||||
if control_holds_rdk():
|
||||
break
|
||||
if not bridge_up:
|
||||
probe, error = _probe(row["sn"], row["iface"], row["pc_ip"])
|
||||
fresh = present(
|
||||
row["id"],
|
||||
stack_up=False,
|
||||
bridge_up=bridge_up,
|
||||
probe=probe,
|
||||
joints=None,
|
||||
error=error,
|
||||
)
|
||||
_store(retain_pose(fresh, snapshot(row["id"])))
|
||||
except Exception as exc: # noqa: BLE001 —— 读状态失败不能把界面线程打死
|
||||
for row in _arms():
|
||||
_store(present(
|
||||
row["id"], stack_up=False, bridge_up=False, probe=None, joints=None, error=str(exc),
|
||||
))
|
||||
time.sleep(4.0)
|
||||
|
||||
|
||||
def _stop_status_probes() -> None:
|
||||
try:
|
||||
subprocess.run(
|
||||
["pkill", "-f", "scripts/lib/rdk_status.py"],
|
||||
capture_output=True, text=True, timeout=2,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return
|
||||
|
||||
|
||||
def _status_probe_running() -> bool:
|
||||
return "rdk_status.py" in _pgrep("scripts/lib/rdk_status.py")
|
||||
|
||||
|
||||
def _pid_alive(pid: int) -> bool:
|
||||
if pid <= 0:
|
||||
return False
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def _store(row: dict) -> None:
|
||||
with _LOCK:
|
||||
_CACHE[str(row.get("arm") or "")] = row
|
||||
|
||||
|
||||
def _arms() -> list[dict]:
|
||||
path = ros2_root() / "config" / "site.yaml"
|
||||
if not path.is_file():
|
||||
return []
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
return [
|
||||
{"id": row["id"], "sn": row["sn"], "iface": row["iface"], "pc_ip": row["pc_ip"]}
|
||||
for row in listed_arms(data)
|
||||
]
|
||||
|
||||
|
||||
def _stack_arm() -> str | None:
|
||||
text = _pgrep("run_real.sh")
|
||||
ids = {row["id"] for row in _arms()}
|
||||
for match in re.finditer(r"--arm(?:=|\s+)(\S+)", text):
|
||||
token = match.group(1)
|
||||
if token in ids:
|
||||
return token
|
||||
return None
|
||||
|
||||
|
||||
def _alive(pattern: str) -> bool:
|
||||
return bool(_pgrep(pattern).strip())
|
||||
|
||||
|
||||
def _pgrep(pattern: str) -> str:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["pgrep", "-af", pattern],
|
||||
capture_output=True, text=True, timeout=2,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return ""
|
||||
lines = []
|
||||
for line in proc.stdout.splitlines():
|
||||
if "pgrep" in line:
|
||||
continue
|
||||
lines.append(line)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def _read_joints() -> list[float] | None:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["timeout", "4", "ros2", "topic", "echo", "/joint_states", "--once"],
|
||||
capture_output=True, text=True, timeout=6,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return None
|
||||
return _parse_joint_echo(proc.stdout)
|
||||
|
||||
|
||||
def _parse_joint_echo(text: str) -> list[float] | None:
|
||||
for chunk in text.split("---"):
|
||||
if "position:" not in chunk:
|
||||
continue
|
||||
try:
|
||||
data = yaml.safe_load(chunk)
|
||||
except yaml.YAMLError:
|
||||
continue
|
||||
if not isinstance(data, dict):
|
||||
continue
|
||||
names = list(data.get("name") or [])
|
||||
pos = list(data.get("position") or [])
|
||||
if names and len(names) == len(pos):
|
||||
by = {str(name): float(value) for name, value in zip(names, pos)}
|
||||
q = []
|
||||
for joint in _JOINTS:
|
||||
key = next((name for name in by if name == joint or name.endswith(joint)), None)
|
||||
if key is None:
|
||||
q = []
|
||||
break
|
||||
q.append(by[key])
|
||||
if len(q) == 7:
|
||||
return q
|
||||
if len(pos) >= 7:
|
||||
try:
|
||||
return [float(v) for v in pos[:7]]
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
return None
|
||||
|
||||
|
||||
def _probe(sn: str, iface: str, pc_ip: str = "") -> tuple[dict | None, str]:
|
||||
python = os.environ.get("RDK_PYTHON", "").strip() or str(Path.home() / "rdk_py310" / "bin" / "python")
|
||||
script = ros2_root() / "scripts" / "lib" / "rdk_status.py"
|
||||
if not Path(python).is_file() or not script.is_file():
|
||||
return None, "找不到 RDK 解释器或 rdk_status.py，无法读使能状态"
|
||||
cmd = _probe_cmd(python, script, sn, iface, pc_ip)
|
||||
try:
|
||||
proc = subprocess.run(cmd, capture_output=True, text=True, timeout=8)
|
||||
except subprocess.TimeoutExpired:
|
||||
return None, "读 RDK 超时。若控制栈没开，检查网线，并确认示教器已开 Remote。"
|
||||
except OSError as exc:
|
||||
return None, f"读 RDK 失败：{exc}"
|
||||
payload = _json_object(proc.stdout) or _json_object(proc.stderr)
|
||||
if not payload:
|
||||
tail = (proc.stdout + proc.stderr).strip()[-240:]
|
||||
return None, tail or "RDK 没有返回状态"
|
||||
return payload, ""
|
||||
|
||||
|
||||
def _probe_cmd(python: str, script: Path, sn: str, iface: str, pc_ip: str) -> list[str]:
|
||||
"""RDK 白名单要本机 IPv4。只传网卡名 ``eth0`` 时，内核改名叫 enx… 就永远连不上。"""
|
||||
cmd = [python, str(script), "--sn", sn, "--json"]
|
||||
if iface:
|
||||
cmd += ["--iface", iface]
|
||||
if pc_ip:
|
||||
cmd += ["--pc-ip", pc_ip]
|
||||
return cmd
|
||||
|
||||
|
||||
def _json_object(text: str) -> dict:
|
||||
decoder = json.JSONDecoder()
|
||||
for index, char in enumerate(text):
|
||||
if char != "{":
|
||||
continue
|
||||
try:
|
||||
data, _end = decoder.raw_decode(text[index:])
|
||||
except json.JSONDecodeError:
|
||||
continue
|
||||
if isinstance(data, dict) and "connected" in data:
|
||||
return data
|
||||
return {}
|
||||
|
||||
|
||||
_KIN: dict[str, dict[str, float]] | None = None
|
||||
|
||||
|
||||
def _kinematics() -> dict[str, dict[str, float]]:
|
||||
global _KIN
|
||||
if _KIN is not None:
|
||||
return _KIN
|
||||
root = ros2_root()
|
||||
candidates = (
|
||||
root / "src/flexiv_description/config/Rizon4/default_kinematics.yaml",
|
||||
root / "install/flexiv_description/share/flexiv_description/config/Rizon4/default_kinematics.yaml",
|
||||
)
|
||||
path = next((item for item in candidates if item.is_file()), None)
|
||||
if path is None:
|
||||
raise FileNotFoundError("找不到 Rizon4 default_kinematics.yaml")
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
kin = data.get("kinematics") or {}
|
||||
_KIN = kin
|
||||
return kin
|
||||
|
||||
|
||||
def _rpy(roll: float, pitch: float, yaw: float) -> tuple:
|
||||
return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
|
||||
|
||||
|
||||
def _rx(angle: float) -> tuple:
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
|
||||
|
||||
|
||||
def _ry(angle: float) -> tuple:
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
|
||||
|
||||
|
||||
def _rz(angle: float) -> tuple:
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
|
||||
|
||||
|
||||
def _axis_rot(axis: tuple[float, float, float], angle: float) -> tuple:
|
||||
x, y, z = axis
|
||||
if abs(z) > 0.5:
|
||||
return _rz(angle if z > 0 else -angle)
|
||||
if abs(y) > 0.5:
|
||||
return _ry(angle if y > 0 else -angle)
|
||||
return _rx(angle if x > 0 else -angle)
|
||||
|
||||
|
||||
def _mul(a: tuple, b: tuple) -> tuple:
|
||||
return tuple(
|
||||
tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
|
||||
for i in range(3)
|
||||
)
|
||||
|
||||
|
||||
def _apply(rot: tuple, vec: tuple[float, float, float]) -> tuple[float, float, float]:
|
||||
return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3)) # type: ignore[return-value]
|
||||
|
||||
|
||||
def _add(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float, float]:
|
||||
return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
|
||||
|
||||
|
||||
def _seven(raw: Any) -> list[float] | None:
|
||||
if not isinstance(raw, list) or len(raw) < 7:
|
||||
return None
|
||||
try:
|
||||
q = [float(v) for v in raw[:7]]
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if any(v != v or abs(v) > 12.6 for v in q):
|
||||
return None
|
||||
return q
|
||||
@@ -1,6 +1,10 @@
|
||||
"""工位程序里的相机预览。
|
||||
|
||||
只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
|
||||
|
||||
回调里先丢掉当前页没在看的流。彩色和深度都缩成宽不超过 480 的 RGB，
|
||||
界面直接贴这块小图。不再每帧把整幅图拷进 Python，也不在界面线程里解 JPEG。
|
||||
手眼页单独要一张更宽的 JPEG，只在那一页、那一路彩色上编码。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -11,33 +15,61 @@ import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
# 主界面同时看几路彩色。480 宽够看清，界面缩放的代价也小。
|
||||
_PREVIEW_WIDTH = 480
|
||||
# 手眼要在这张图上找棋盘，比预览宽一档，但仍然不把 1280 原图编码。
|
||||
_JPEG_WIDTH = 960
|
||||
# 主界面要看清运动和推理框，10Hz 太顿。0.05s 约 20 帧。
|
||||
_INTERVAL = 0.05
|
||||
_DET_HOLD_S = 1.5
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_STOP = threading.Event()
|
||||
_THREAD: threading.Thread | None = None
|
||||
_JPEG: dict[str, bytes] = {}
|
||||
_PREVIEW: dict[str, tuple[bytes, int, int]] = {}
|
||||
_PREVIEW_WANT: set[str] = set()
|
||||
_JPEG_WANT: set[str] = set()
|
||||
_NEXT: dict[str, float] = {}
|
||||
_DET: dict[str, tuple[float, list[float]]] = {}
|
||||
# 没画框的预览。检测一到就在这张上重画，不必等下一帧相机。
|
||||
_CLEAN: dict[str, dict] = {}
|
||||
_STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
|
||||
|
||||
|
||||
def targets() -> list[dict]:
|
||||
"""真机腕部相机和手外相机。仿真相机不进预览。"""
|
||||
from arm_application.operator_console import _arms, _site
|
||||
from arm_application.site_io import overhead_camera
|
||||
def preview_targets(site: dict) -> list[dict]:
|
||||
"""主界面只要左臂腕部相机和共用的手外相机。
|
||||
|
||||
右臂腕部不进这一屏。仿真相机名字以 ``sim`` 开头，也不进。
|
||||
推理框的角色仍是 ``wrist`` / ``overhead``，跟话题对得上。
|
||||
"""
|
||||
from arm_application.site_io import listed_arms, overhead_camera
|
||||
|
||||
found: list[dict] = []
|
||||
seen: set[str] = set()
|
||||
for arm in _arms():
|
||||
name = str(arm.get("camera") or "").strip().strip("/")
|
||||
if not name or name.startswith("sim") or name in seen:
|
||||
continue
|
||||
left = next((row for row in listed_arms(site) if row.get("side") == "left"), None)
|
||||
name = str((left or {}).get("camera") or "").strip().strip("/")
|
||||
if name and not name.startswith("sim"):
|
||||
cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
|
||||
cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {}
|
||||
tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405"
|
||||
seen.add(name)
|
||||
found.append({"id": name, "label": f"{arm.get('label') or name} 腕部"})
|
||||
cam, _others = overhead_camera(_site(), None)
|
||||
name = str((cam or {}).get("name") or "").strip().strip("/")
|
||||
if name and not name.startswith("sim") and name not in seen:
|
||||
found.append({"id": name, "label": str((cam or {}).get("label") or "手外相机")})
|
||||
found.append({"id": name, "label": f"左腕 {tag}", "role": "wrist"})
|
||||
cam, _others = overhead_camera(site or {}, None)
|
||||
oname = str((cam or {}).get("name") or "").strip().strip("/")
|
||||
if oname and not oname.startswith("sim") and oname not in seen:
|
||||
tag = str((cam or {}).get("model") or "D435").strip().upper() or "D435"
|
||||
found.append({"id": oname, "label": f"手外 {tag}", "role": "overhead"})
|
||||
return found
|
||||
|
||||
|
||||
def targets() -> list[dict]:
|
||||
"""当前站点的主界面相机。规则在 ``preview_targets``。"""
|
||||
from arm_application.operator_console import _site
|
||||
|
||||
return preview_targets(_site())
|
||||
|
||||
|
||||
def state() -> dict:
|
||||
with _LOCK:
|
||||
return {
|
||||
@@ -53,85 +85,302 @@ def frame(name: str, kind: str) -> bytes | None:
|
||||
return _JPEG.get(f"{name}_{kind}")
|
||||
|
||||
|
||||
def previews() -> dict[str, tuple[bytes, int, int]]:
|
||||
"""当前预览。值是 ``(rgb888, 宽, 高)``，同一帧反复取到的是同一块字节。"""
|
||||
with _LOCK:
|
||||
return dict(_PREVIEW)
|
||||
|
||||
|
||||
def watch(preview: set[str], jpeg: set[str] | None = None) -> None:
|
||||
"""当前页要哪几路。没点名的流在回调里直接丢掉，不拷图像。
|
||||
|
||||
``preview`` 走 RGB 小图，``jpeg`` 走手眼用的 JPEG。两边都空着就只保持订阅。
|
||||
"""
|
||||
global _PREVIEW_WANT, _JPEG_WANT
|
||||
jpeg = set(jpeg or ())
|
||||
preview = set(preview)
|
||||
with _LOCK:
|
||||
if preview == _PREVIEW_WANT and jpeg == _JPEG_WANT:
|
||||
return
|
||||
_PREVIEW_WANT = preview
|
||||
_JPEG_WANT = jpeg
|
||||
for key in preview | jpeg:
|
||||
_NEXT.pop(key, None)
|
||||
|
||||
|
||||
def wants(key: str) -> tuple[bool, bool]:
|
||||
"""返回 ``(要预览, 要 JPEG)``。"""
|
||||
with _LOCK:
|
||||
return key in _PREVIEW_WANT, key in _JPEG_WANT
|
||||
|
||||
|
||||
def accept_frame(key: str) -> bool:
|
||||
"""这一帧要不要做进预览。没人看，或者距上一帧不到间隔，都返回 False。"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
if key not in _PREVIEW_WANT and key not in _JPEG_WANT:
|
||||
return False
|
||||
if now < _NEXT.get(key, 0.0):
|
||||
return False
|
||||
_NEXT[key] = now + _INTERVAL
|
||||
return True
|
||||
|
||||
|
||||
def _close_external_viewers() -> None:
|
||||
"""关掉独立的四窗格查看器。它会把左右红外挂出来，本程序不再用它。"""
|
||||
subprocess.run(["pkill", "-f", "lib/arm_application/realsense_view"], check=False)
|
||||
subprocess.run(["pkill", "-f", "arm_application.realsense_view"], check=False)
|
||||
|
||||
|
||||
def _jpeg(img: np.ndarray) -> bytes | None:
|
||||
def paint_detection(
|
||||
rgb: np.ndarray,
|
||||
xyxy: list[float] | tuple,
|
||||
src_width: int,
|
||||
src_height: int,
|
||||
) -> np.ndarray:
|
||||
"""把推理结果画到相机预览上：绿框、画面中心、跟踪红点、置信度和距离。
|
||||
|
||||
``xyxy`` 前四项是原图像素的框。够长时第 5 项是置信度，第 7、8 项是跟踪点，
|
||||
第 9 项是距离（米）。空框原样返回。
|
||||
"""
|
||||
import cv2
|
||||
|
||||
if img.size == 0:
|
||||
if rgb.ndim != 3 or src_width <= 0 or src_height <= 0 or len(xyxy) < 4:
|
||||
return rgb
|
||||
scale_x = rgb.shape[1] / float(src_width)
|
||||
scale_y = rgb.shape[0] / float(src_height)
|
||||
x1 = int(round(float(xyxy[0]) * scale_x))
|
||||
y1 = int(round(float(xyxy[1]) * scale_y))
|
||||
x2 = int(round(float(xyxy[2]) * scale_x))
|
||||
y2 = int(round(float(xyxy[3]) * scale_y))
|
||||
height, width = rgb.shape[:2]
|
||||
x1 = max(0, min(width - 1, x1))
|
||||
x2 = max(0, min(width - 1, x2))
|
||||
y1 = max(0, min(height - 1, y1))
|
||||
y2 = max(0, min(height - 1, y2))
|
||||
if x2 <= x1 or y2 <= y1:
|
||||
return rgb
|
||||
out = np.array(rgb, copy=True)
|
||||
thick = max(2, width // 180)
|
||||
cv2.rectangle(out, (x1, y1), (x2, y2), (80, 220, 120), thick)
|
||||
cv2.drawMarker(
|
||||
out,
|
||||
(width // 2, height // 2),
|
||||
(255, 220, 60),
|
||||
cv2.MARKER_CROSS,
|
||||
max(8, width // 24),
|
||||
1,
|
||||
)
|
||||
if len(xyxy) >= 8:
|
||||
px = int(round(float(xyxy[6]) * scale_x))
|
||||
py = int(round(float(xyxy[7]) * scale_y))
|
||||
px = max(0, min(width - 1, px))
|
||||
py = max(0, min(height - 1, py))
|
||||
cv2.circle(out, (px, py), max(3, width // 80), (255, 48, 48), -1)
|
||||
if len(xyxy) >= 5:
|
||||
conf = float(xyxy[4])
|
||||
dist = f" z={float(xyxy[8]):.3f}m" if len(xyxy) >= 9 else ""
|
||||
cv2.putText(
|
||||
out,
|
||||
f"{conf:.2f}{dist}",
|
||||
(x1, max(14, y1 - 6)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.45,
|
||||
(80, 220, 120),
|
||||
1,
|
||||
cv2.LINE_AA,
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def inference_status() -> str:
|
||||
"""腕部和手外最近一次推理框。超过保持时间就当没有。"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
items = dict(_DET)
|
||||
parts = []
|
||||
for role, title in (("wrist", "腕部"), ("overhead", "手外")):
|
||||
item = items.get(role)
|
||||
if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 5:
|
||||
parts.append(f"{title} 没有框")
|
||||
continue
|
||||
parts.append(f"{title} 置信 {float(item[1][4]):.2f}")
|
||||
return "推理 " + " ".join(parts)
|
||||
|
||||
|
||||
def _remember_detection(role: str, data: list[float]) -> None:
|
||||
with _LOCK:
|
||||
if len(data) < 4:
|
||||
_DET.pop(role, None)
|
||||
else:
|
||||
_DET[role] = (time.monotonic(), data)
|
||||
_stamp_role(role)
|
||||
|
||||
|
||||
def _remember_clean(key: str, rgb: np.ndarray, role: str, src_w: int, src_h: int) -> None:
|
||||
with _LOCK:
|
||||
_CLEAN[key] = {
|
||||
"rgb": np.array(rgb, copy=True),
|
||||
"role": role,
|
||||
"src_w": int(src_w),
|
||||
"src_h": int(src_h),
|
||||
}
|
||||
|
||||
|
||||
def _overlay_detection(rgb: np.ndarray, role: str, src_width: int, src_height: int) -> np.ndarray:
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
item = _DET.get(role)
|
||||
if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 4:
|
||||
return rgb
|
||||
return paint_detection(rgb, item[1], src_width, src_height)
|
||||
|
||||
|
||||
def _stamp_role(role: str) -> None:
|
||||
"""检测更新后，把框画回这一路相机已经在显示的画面上。"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
frames = [(key, dict(meta)) for key, meta in _CLEAN.items() if meta.get("role") == role]
|
||||
item = _DET.get(role)
|
||||
fresh = item is not None and now - item[0] <= _DET_HOLD_S and len(item[1]) >= 4
|
||||
for key, meta in frames:
|
||||
shown = meta["rgb"]
|
||||
if fresh:
|
||||
shown = paint_detection(shown, item[1], meta["src_w"], meta["src_h"])
|
||||
with _LOCK:
|
||||
_PREVIEW[key] = (np.ascontiguousarray(shown).tobytes(), int(shown.shape[1]), int(shown.shape[0]))
|
||||
_STATE["frames"][key] = True
|
||||
_STATE["stamp"] = time.time()
|
||||
|
||||
|
||||
def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _PREVIEW_WIDTH) -> np.ndarray:
|
||||
"""缩成宽不超过 ``limit`` 的 RGB。返回的数组自己持有内存，不跟调用方共用缓冲。"""
|
||||
import cv2
|
||||
|
||||
if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
|
||||
raise ValueError("彩色预览要的是高×宽×3")
|
||||
view = image
|
||||
width = int(view.shape[1])
|
||||
if width > limit:
|
||||
height = max(1, int(round(int(view.shape[0]) * limit / float(width))))
|
||||
view = cv2.resize(view, (limit, height), interpolation=cv2.INTER_AREA)
|
||||
if bgr:
|
||||
return cv2.cvtColor(view, cv2.COLOR_BGR2RGB)
|
||||
if not view.flags["OWNDATA"] or not view.flags["C_CONTIGUOUS"]:
|
||||
return np.ascontiguousarray(view)
|
||||
return view
|
||||
|
||||
|
||||
def depth_preview(depth: np.ndarray, limit: int = _PREVIEW_WIDTH) -> np.ndarray:
|
||||
"""uint16 深度画成 RGB。百分位只在抽稀后的样本上算，无效像素保持黑色。"""
|
||||
import cv2
|
||||
|
||||
if depth.ndim != 2 or depth.shape[0] < 1 or depth.shape[1] < 1:
|
||||
raise ValueError("深度预览要的是高×宽")
|
||||
step = max(1, int(depth.shape[1]) // limit)
|
||||
small = depth[::step, ::step]
|
||||
valid = small > 0
|
||||
gray = np.zeros(small.shape, np.uint8)
|
||||
if bool(valid.any()):
|
||||
sample = small[valid]
|
||||
if sample.size > 8000:
|
||||
sample = sample[:: max(1, sample.size // 8000)]
|
||||
lo, hi = np.percentile(sample.astype(np.float32, copy=False), [5, 95])
|
||||
hi = max(float(hi), float(lo) + 1.0)
|
||||
norm = np.clip((small.astype(np.float32) - float(lo)) / (hi - float(lo)), 0.0, 1.0)
|
||||
gray = (norm * 255.0).astype(np.uint8)
|
||||
gray[~valid] = 0
|
||||
colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
|
||||
colored[~valid] = 0
|
||||
return rgb_preview(colored, bgr=True, limit=limit)
|
||||
|
||||
|
||||
def _jpeg_bgr(rgb: np.ndarray) -> bytes | None:
|
||||
import cv2
|
||||
|
||||
if rgb.size == 0:
|
||||
return None
|
||||
view = img
|
||||
if view.shape[1] > 640:
|
||||
scale = 640.0 / float(view.shape[1])
|
||||
view = cv2.resize(view, (640, max(1, int(view.shape[0] * scale))))
|
||||
ok, buf = cv2.imencode(".jpg", view, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
|
||||
ok, buf = cv2.imencode(
|
||||
".jpg",
|
||||
cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR),
|
||||
[int(cv2.IMWRITE_JPEG_QUALITY), 70],
|
||||
)
|
||||
if not ok:
|
||||
return None
|
||||
return buf.tobytes()
|
||||
|
||||
|
||||
def _depth_bgr(depth: np.ndarray) -> np.ndarray:
|
||||
import cv2
|
||||
|
||||
vis = np.zeros((depth.shape[0], depth.shape[1], 3), np.uint8)
|
||||
valid = depth > 0
|
||||
if not bool(valid.any()):
|
||||
return vis
|
||||
values = depth.astype(np.float32)
|
||||
lo, hi = np.percentile(values[valid], [5, 95])
|
||||
hi = max(float(hi), float(lo) + 1.0)
|
||||
gray = (np.clip((values - lo) / (hi - lo), 0.0, 1.0) * 255.0).astype(np.uint8)
|
||||
gray[~valid] = 0
|
||||
colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
|
||||
colored[~valid] = 0
|
||||
return colored
|
||||
def _as_array(data, dtype, count: int) -> np.ndarray:
|
||||
"""优先直接看消息缓冲。缓冲不支持时才拷一份。"""
|
||||
try:
|
||||
arr = np.frombuffer(data, dtype=dtype, count=count)
|
||||
except (TypeError, BufferError, ValueError):
|
||||
arr = np.frombuffer(bytes(data), dtype=dtype, count=count)
|
||||
if arr.size < count:
|
||||
raise ValueError("图像字节数不够")
|
||||
return arr
|
||||
|
||||
|
||||
def _color_array(msg) -> np.ndarray | None:
|
||||
def _color_view(msg) -> np.ndarray | None:
|
||||
if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
|
||||
return None
|
||||
arr = np.frombuffer(bytes(msg.data), dtype=np.uint8)
|
||||
need = int(msg.height) * int(msg.width) * 3
|
||||
if arr.size < need:
|
||||
return None
|
||||
arr = arr[:need].reshape(int(msg.height), int(msg.width), 3)
|
||||
if msg.encoding == "rgb8":
|
||||
arr = arr[:, :, ::-1]
|
||||
return np.ascontiguousarray(arr)
|
||||
arr = _as_array(msg.data, np.uint8, need)
|
||||
return arr.reshape(int(msg.height), int(msg.width), 3)
|
||||
|
||||
|
||||
def _depth_array(msg) -> np.ndarray | None:
|
||||
def _depth_view(msg) -> np.ndarray | None:
|
||||
if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
|
||||
return None
|
||||
arr = np.frombuffer(bytes(msg.data), dtype=np.uint16)
|
||||
need = int(msg.height) * int(msg.width)
|
||||
if arr.size < need:
|
||||
return None
|
||||
return np.ascontiguousarray(arr[:need].reshape(int(msg.height), int(msg.width)))
|
||||
arr = _as_array(msg.data, np.uint16, need)
|
||||
return arr.reshape(int(msg.height), int(msg.width))
|
||||
|
||||
|
||||
def _store(key: str, rgb: np.ndarray, *, preview: np.ndarray | None, jpeg: bool) -> None:
|
||||
encoded = _jpeg_bgr(rgb) if jpeg else None
|
||||
with _LOCK:
|
||||
if preview is not None:
|
||||
_PREVIEW[key] = (preview.tobytes(), int(preview.shape[1]), int(preview.shape[0]))
|
||||
if encoded:
|
||||
_JPEG[key] = encoded
|
||||
if preview is not None or encoded:
|
||||
_STATE["frames"][key] = True
|
||||
_STATE["stamp"] = time.time()
|
||||
_STATE["error"] = ""
|
||||
|
||||
|
||||
def _loop(cams: list[dict]) -> None:
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
latest: dict[str, np.ndarray] = {}
|
||||
lock = threading.Lock()
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
class _Node(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("operator_camera_view")
|
||||
det_qos = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
)
|
||||
self.create_subscription(
|
||||
Float32MultiArray, "workpiece/detection", self._det("wrist"), det_qos
|
||||
)
|
||||
self.create_subscription(
|
||||
Float32MultiArray,
|
||||
"workpiece/detection_overhead",
|
||||
self._det("overhead"),
|
||||
det_qos,
|
||||
)
|
||||
for cam in cams:
|
||||
name = cam["id"]
|
||||
role = str(cam.get("role") or "wrist")
|
||||
self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/color/image_raw",
|
||||
self._color(name),
|
||||
self._color(name, role),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
self.create_subscription(
|
||||
@@ -141,53 +390,61 @@ def _loop(cams: list[dict]) -> None:
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
|
||||
def _color(self, name: str):
|
||||
def _det(self, role: str):
|
||||
def _cb(msg: Float32MultiArray) -> None:
|
||||
_remember_detection(role, [float(v) for v in msg.data])
|
||||
return _cb
|
||||
|
||||
def _color(self, name: str, role: str):
|
||||
key = f"{name}_color"
|
||||
|
||||
def _cb(msg: Image) -> None:
|
||||
arr = _color_array(msg)
|
||||
if arr is None:
|
||||
# 先看这路有没有人要。30Hz 的原图不再整幅拷进 Python。
|
||||
try:
|
||||
if not accept_frame(key):
|
||||
return
|
||||
with lock:
|
||||
latest[f"{name}_color"] = arr
|
||||
view = _color_view(msg)
|
||||
if view is None:
|
||||
return
|
||||
preview, jpeg = wants(key)
|
||||
limit = _JPEG_WIDTH if jpeg else _PREVIEW_WIDTH
|
||||
rgb = rgb_preview(view, bgr=(msg.encoding == "bgr8"), limit=limit)
|
||||
shown = rgb
|
||||
if preview and int(rgb.shape[1]) > _PREVIEW_WIDTH:
|
||||
shown = rgb_preview(rgb, bgr=False, limit=_PREVIEW_WIDTH)
|
||||
if preview:
|
||||
src_h, src_w = view.shape[:2]
|
||||
_remember_clean(key, shown, role, int(src_w), int(src_h))
|
||||
shown = _overlay_detection(shown, role, int(src_w), int(src_h))
|
||||
_store(key, rgb, preview=shown if preview else None, jpeg=jpeg)
|
||||
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
return _cb
|
||||
|
||||
def _depth(self, name: str):
|
||||
key = f"{name}_depth"
|
||||
|
||||
def _cb(msg: Image) -> None:
|
||||
arr = _depth_array(msg)
|
||||
if arr is None:
|
||||
try:
|
||||
if not accept_frame(key):
|
||||
return
|
||||
with lock:
|
||||
latest[f"{name}_depth"] = arr
|
||||
view = _depth_view(msg)
|
||||
if view is None:
|
||||
return
|
||||
preview, _jpeg_wanted = wants(key)
|
||||
colored = depth_preview(view)
|
||||
_store(key, colored, preview=colored if preview else None, jpeg=False)
|
||||
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
return _cb
|
||||
|
||||
rclpy.init()
|
||||
node = _Node()
|
||||
next_encode = 0.0
|
||||
try:
|
||||
while not _STOP.is_set():
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
now = time.monotonic()
|
||||
if now < next_encode:
|
||||
continue
|
||||
next_encode = now + 0.12
|
||||
with lock:
|
||||
snap = dict(latest)
|
||||
latest.clear()
|
||||
frames = {}
|
||||
blobs = {}
|
||||
for key, arr in snap.items():
|
||||
name, _, kind = key.rpartition("_")
|
||||
img = _depth_bgr(arr) if kind == "depth" else arr
|
||||
blob = _jpeg(img)
|
||||
if not blob:
|
||||
continue
|
||||
blobs[key] = blob
|
||||
frames[key] = True
|
||||
if blobs:
|
||||
with _LOCK:
|
||||
_JPEG.update(blobs)
|
||||
_STATE["frames"].update(frames)
|
||||
_STATE["stamp"] = time.time()
|
||||
_STATE["error"] = ""
|
||||
except Exception as exc:
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
|
||||
@@ -23,7 +23,7 @@ from urllib.parse import urlparse
|
||||
import yaml
|
||||
|
||||
from arm_application import operator_tactile_view as tactile_view
|
||||
from arm_application.site_io import eye_to_hand_result_name, overhead_camera
|
||||
from arm_application.site_io import eye_to_hand_result_name, listed_arms, overhead_camera, usb_expectations
|
||||
|
||||
HOST = "127.0.0.1"
|
||||
PORT = 8766
|
||||
@@ -76,29 +76,26 @@ def _site() -> dict:
|
||||
|
||||
|
||||
def _arms() -> list[dict]:
|
||||
robots = (_site().get("robot") or {}).get("robots") or []
|
||||
out = []
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "")
|
||||
arm_no = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
|
||||
if not arm_no:
|
||||
continue
|
||||
out.append({
|
||||
"id": arm_no,
|
||||
"label": str(robot.get("label") or f"臂{arm_no}"),
|
||||
"sn": sn,
|
||||
"tactile": bool(robot.get("tactile")),
|
||||
"camera": str((robot.get("camera") or {}).get("name") or ""),
|
||||
"gripper": str((robot.get("gripper") or {}).get("model") or ""),
|
||||
})
|
||||
return out or [
|
||||
{"id": "89", "label": "左臂89", "sn": "", "tactile": True, "camera": "d405_left", "gripper": ""},
|
||||
{"id": "91", "label": "右臂91", "sn": "", "tactile": False, "camera": "d405_right", "gripper": ""},
|
||||
"""真机臂来自 site.yaml。没有清单就空着，不补本机的 89/91。"""
|
||||
return [
|
||||
{
|
||||
"id": row["id"],
|
||||
"label": row["label"],
|
||||
"sn": row["sn"],
|
||||
"tactile": row["tactile"],
|
||||
"camera": row["camera"],
|
||||
"gripper": row["gripper"],
|
||||
"rotary": row["rotary"],
|
||||
}
|
||||
for row in listed_arms(_site())
|
||||
]
|
||||
|
||||
|
||||
def _default_arm() -> str:
|
||||
rows = _arms()
|
||||
return str(rows[0]["id"]) if rows else ""
|
||||
|
||||
|
||||
def _usb() -> list[dict]:
|
||||
found = {"d405": [], "d435": [], "orisys": []}
|
||||
want = {
|
||||
@@ -119,10 +116,11 @@ def _usb() -> list[dict]:
|
||||
continue
|
||||
speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?"
|
||||
found[key].append({"port": dev.name, "speed": speed})
|
||||
expect = usb_expectations(_site())
|
||||
return [
|
||||
{"id": "d405", "label": "腕部 D405", "expect": 2, "items": found["d405"]},
|
||||
{"id": "d435", "label": "手外 D435", "expect": 1, "items": found["d435"]},
|
||||
{"id": "orisys", "label": "视触觉", "expect": 2, "items": found["orisys"]},
|
||||
{"id": "d405", "label": "腕部 D405", "expect": expect["d405"], "items": found["d405"]},
|
||||
{"id": "d435", "label": "手外 D435", "expect": expect["d435"], "items": found["d435"]},
|
||||
{"id": "orisys", "label": "视触觉", "expect": expect["orisys"], "items": found["orisys"]},
|
||||
]
|
||||
|
||||
|
||||
@@ -214,13 +212,16 @@ def _poll_tactile() -> None:
|
||||
|
||||
|
||||
def _robot(arm: str) -> dict:
|
||||
from arm_application.site_io import arm_short_id
|
||||
|
||||
robots = (_site().get("robot") or {}).get("robots") or []
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "")
|
||||
short = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
|
||||
if short == arm:
|
||||
if sn.lower() == "sim":
|
||||
continue
|
||||
if arm_short_id(sn) == arm:
|
||||
return robot
|
||||
return {}
|
||||
|
||||
@@ -305,12 +306,30 @@ def _handeye(arm: str) -> dict:
|
||||
or shared.get("handeye_waypoints_eye_to_hand")
|
||||
or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml")
|
||||
))
|
||||
wrist = _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {})
|
||||
wrist_name = ""
|
||||
if isinstance(robot.get("camera"), dict):
|
||||
wrist_name = str(robot["camera"].get("name") or "").strip().strip("/")
|
||||
wrist["preview_camera"] = wrist_name
|
||||
wrist["preview_label"] = "腕部相机"
|
||||
outside = _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {})
|
||||
outside["preview_camera"] = cam_name.strip().strip("/")
|
||||
outside["preview_label"] = "手外相机"
|
||||
return {
|
||||
"eye_in_hand": _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {}),
|
||||
"eye_to_hand": _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {}),
|
||||
"eye_in_hand": wrist,
|
||||
"eye_to_hand": outside,
|
||||
}
|
||||
|
||||
|
||||
def _by_arm(reader) -> dict:
|
||||
return {row["id"]: reader(row["id"]) for row in _arms()}
|
||||
|
||||
|
||||
def _gripper_live() -> dict:
|
||||
from arm_application.operator_gripper_state import snapshot as grip_snapshot
|
||||
return _by_arm(grip_snapshot)
|
||||
|
||||
|
||||
def status() -> dict:
|
||||
with _LOCK:
|
||||
tactile = {
|
||||
@@ -322,20 +341,23 @@ def status() -> dict:
|
||||
"left": list(_HIST["left"]),
|
||||
"right": list(_HIST["right"]),
|
||||
}
|
||||
from arm_application.operator_arm_state import snapshot as arm_snapshot
|
||||
return {
|
||||
"arms": _arms(),
|
||||
"arm_live": _by_arm(arm_snapshot),
|
||||
"gripper_live": _gripper_live(),
|
||||
"usb": _usb(),
|
||||
"services": _services(),
|
||||
"grippers": _grippers(),
|
||||
"tactile": tactile,
|
||||
"history": history,
|
||||
"view": tactile_view.state(),
|
||||
"handeye": {"89": _handeye("89"), "91": _handeye("91")},
|
||||
"handeye": _by_arm(_handeye),
|
||||
"now": time.time(),
|
||||
}
|
||||
|
||||
|
||||
# 会动真机的动作必须 confirm。argv 里的 {arm} 只接受 89 或 91。
|
||||
# 会动真机的动作必须 confirm。臂号必须是 site.yaml 里某台真机的末两位。
|
||||
_ACTIONS = {
|
||||
"real_up": {
|
||||
"title": "上电控制栈",
|
||||
@@ -377,21 +399,21 @@ _ACTIONS = {
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "probe"],
|
||||
"moves": False,
|
||||
"blurb": "读夹爪是否在线，不动手指。",
|
||||
"blurb": "读夹爪是否在线，不动手指。驱动没在跑时先拉起，不回零。",
|
||||
},
|
||||
"gripper_open": {
|
||||
"title": "夹爪张开",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "open"],
|
||||
"moves": True,
|
||||
"blurb": "张开当前臂夹爪。",
|
||||
"blurb": "张开当前臂夹爪。驱动没在跑时先拉起，不回零。",
|
||||
},
|
||||
"gripper_close": {
|
||||
"title": "夹爪闭合",
|
||||
"script": "run_gripper.sh",
|
||||
"args": ["--arm", "{arm}", "--cli", "close"],
|
||||
"moves": True,
|
||||
"blurb": "闭合当前臂夹爪。",
|
||||
"blurb": "闭合当前臂夹爪。驱动没在跑时先拉起，不回零。",
|
||||
},
|
||||
"gripper_rotate": {
|
||||
"title": "右爪转 90°",
|
||||
@@ -401,11 +423,11 @@ _ACTIONS = {
|
||||
"blurb": "右臂旋转爪转到 90°。左臂平行爪没有这一轴。",
|
||||
},
|
||||
"pipeline": {
|
||||
"title": "整体三段",
|
||||
"title": "跟踪并对孔",
|
||||
"script": "run_all.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"moves": True,
|
||||
"blurb": "跟踪、对孔、搜孔轻压一次跑完。会动臂。",
|
||||
"blurb": "跟踪和对孔一次跑完。搜孔轻压已取消，这一步不含旋入。会动臂。",
|
||||
},
|
||||
"stage_track": {
|
||||
"title": "只测跟踪",
|
||||
@@ -421,12 +443,12 @@ _ACTIONS = {
|
||||
"moves": True,
|
||||
"blurb": "只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。",
|
||||
},
|
||||
"stage_search": {
|
||||
"title": "只测搜孔",
|
||||
"stage_screw": {
|
||||
"title": "只测旋转装配",
|
||||
"script": "run_stage3_search.sh",
|
||||
"args": ["--arm", "{arm}"],
|
||||
"args": ["--arm", "{arm}", "--screw"],
|
||||
"moves": True,
|
||||
"blurb": "只跑三阶段：触面、搜孔、轻压。会动臂。臂应已停在孔上方。",
|
||||
"blurb": "按现场配置的拧入深度和螺距把螺纹规旋进去。会动臂。臂应已在孔口。",
|
||||
},
|
||||
"reset_home": {
|
||||
"title": "复位到 home",
|
||||
@@ -462,9 +484,9 @@ _ACTIONS = {
|
||||
"handeye_teach": {
|
||||
"title": "示教采点",
|
||||
"script": "run_handeye.sh",
|
||||
"args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "12"],
|
||||
"args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "{points}"],
|
||||
"moves": True,
|
||||
"blurb": "软拖拽示教，目标 12 个姿态。要先停控制栈，会动臂。",
|
||||
"blurb": "软拖拽示教。点数和是否接着采由手眼页上的控件决定。要先停控制栈，会动臂。",
|
||||
},
|
||||
"handeye_merge": {
|
||||
"title": "合并示教点",
|
||||
@@ -514,7 +536,39 @@ def _stop_tactile() -> str:
|
||||
return "已发送停止视触觉节点"
|
||||
|
||||
|
||||
def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand") -> dict:
|
||||
def render_action_args(
|
||||
template: list[str], arm: str, mode: str, extras: dict | None = None,
|
||||
) -> list[str]:
|
||||
"""把动作模板里的臂号、模式、点数换成这次界面上的值。"""
|
||||
extra = extras or {}
|
||||
needs_points = any("{points}" in str(token) for token in template)
|
||||
points_text = ""
|
||||
if needs_points:
|
||||
try:
|
||||
points = int(extra.get("points", 12))
|
||||
except (TypeError, ValueError):
|
||||
raise ValueError("目标点数不是整数") from None
|
||||
if points < 4 or points > 30:
|
||||
raise ValueError("目标点数只接受 4–30")
|
||||
points_text = str(points)
|
||||
repl = {"{arm}": arm, "{mode}": mode, "{points}": points_text}
|
||||
out: list[str] = []
|
||||
for token in template:
|
||||
text = str(token)
|
||||
for key, value in repl.items():
|
||||
text = text.replace(key, value)
|
||||
out.append(text)
|
||||
if extra.get("append"):
|
||||
if "teach" not in out:
|
||||
raise ValueError("只有示教采点能接着采")
|
||||
out.append("--append")
|
||||
return out
|
||||
|
||||
|
||||
def run_action(
|
||||
action: str, arm: str, confirm: bool, mode: str = "eye_in_hand",
|
||||
extras: dict | None = None,
|
||||
) -> dict:
|
||||
if action == "tactile_down":
|
||||
tactile_view.stop()
|
||||
return {"ok": True, "message": _stop_tactile()}
|
||||
@@ -528,8 +582,8 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
|
||||
spec = _ACTIONS.get(action)
|
||||
if spec is None:
|
||||
return {"ok": False, "message": "没有这个动作"}
|
||||
if arm not in ("89", "91"):
|
||||
return {"ok": False, "message": "臂号只接受 89 或 91"}
|
||||
if arm not in {row["id"] for row in _arms()}:
|
||||
return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
|
||||
if mode not in ("eye_in_hand", "eye_to_hand"):
|
||||
return {"ok": False, "message": "手眼模式只接受 eye_in_hand 或 eye_to_hand"}
|
||||
if spec["moves"] and not confirm:
|
||||
@@ -537,10 +591,39 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
|
||||
script = ROOT / "scripts" / spec["script"]
|
||||
if not script.is_file():
|
||||
return {"ok": False, "message": f"找不到 {script}"}
|
||||
args = [a.replace("{arm}", arm).replace("{mode}", mode) for a in spec["args"]]
|
||||
from arm_application.operator_arm_state import (
|
||||
hold_until_idle,
|
||||
motion_blocked,
|
||||
release_for_control,
|
||||
resume_after_control,
|
||||
snapshot as arm_snapshot,
|
||||
stack_running,
|
||||
)
|
||||
blocked = motion_blocked(
|
||||
action,
|
||||
stack_up=stack_running(),
|
||||
op_status=str((arm_snapshot(arm) or {}).get("op_status") or ""),
|
||||
)
|
||||
if blocked:
|
||||
return {"ok": False, "message": blocked}
|
||||
uses_rdk = action in {
|
||||
"real_up", "handeye_teach", "pipeline", "stage_track", "stage_locate",
|
||||
"stage_screw", "reset_home", "gauge_run", "handeye_calibrate",
|
||||
}
|
||||
if uses_rdk:
|
||||
release_for_control()
|
||||
from arm_application.operator_gripper_state import hold_serial
|
||||
grip_port = action in {
|
||||
"gripper_probe", "gripper_open", "gripper_close", "gripper_rotate", "gripper_up",
|
||||
}
|
||||
try:
|
||||
args = render_action_args(list(spec["args"]), arm, mode, extras)
|
||||
except ValueError as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
LOG_DIR.mkdir(parents=True, exist_ok=True)
|
||||
log_path = LOG_DIR / f"{action}_{arm}.log"
|
||||
shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
|
||||
try:
|
||||
if spec.get("sync"):
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
@@ -570,7 +653,15 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
|
||||
start_new_session=True,
|
||||
)
|
||||
_PROCS[action] = proc.pid
|
||||
if grip_port:
|
||||
hold_serial(proc.pid)
|
||||
if uses_rdk:
|
||||
hold_until_idle(proc.pid, until_stack=(action == "real_up"))
|
||||
uses_rdk = False
|
||||
return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
|
||||
finally:
|
||||
if uses_rdk:
|
||||
resume_after_control()
|
||||
|
||||
|
||||
def _bounded(name: str, raw, low: float, high: float) -> float:
|
||||
@@ -585,12 +676,12 @@ def _bounded(name: str, raw, low: float, high: float) -> float:
|
||||
|
||||
def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
|
||||
"""按界面上的位置、力度、速度下发夹爪。右臂另有角度、圈数。"""
|
||||
if arm not in ("89", "91"):
|
||||
return {"ok": False, "message": "臂号只接受 89 或 91"}
|
||||
if arm not in {row["id"] for row in _arms()}:
|
||||
return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
|
||||
if kind not in ("pos", "rotate", "rotate_by", "revolutions"):
|
||||
return {"ok": False, "message": "没有这种夹爪调节"}
|
||||
if kind != "pos" and arm != "91":
|
||||
return {"ok": False, "message": "左臂平行爪没有旋转轴"}
|
||||
if kind != "pos" and not any(row["id"] == arm and row.get("rotary") for row in _arms()):
|
||||
return {"ok": False, "message": "这台夹爪没有旋转轴"}
|
||||
try:
|
||||
if kind == "pos":
|
||||
pos = int(round(_bounded("位置", numbers.get("pos"), 0, 1000)))
|
||||
@@ -615,7 +706,17 @@ def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
|
||||
summary = f"转 {turns:.3f} 圈 力矩 {force}% 速度 {speed}%"
|
||||
except ValueError as exc:
|
||||
return {"ok": False, "message": str(exc)}
|
||||
from arm_application.operator_gripper_state import pause_serial, resume_serial
|
||||
|
||||
script = ROOT / "scripts" / "run_gripper.sh"
|
||||
pause_serial()
|
||||
try:
|
||||
return _run_gripper_argv(script, arm, force, speed, cli, summary)
|
||||
finally:
|
||||
resume_serial()
|
||||
|
||||
|
||||
def _run_gripper_argv(script, arm: str, force: int, speed: int, cli: list[str], summary: str) -> dict:
|
||||
argv = [
|
||||
"bash", str(script),
|
||||
"--arm", arm,
|
||||
@@ -632,11 +733,20 @@ def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
|
||||
ok = proc.returncode == 0
|
||||
return {
|
||||
"ok": ok,
|
||||
"message": summary if ok else f"夹爪退出码 {proc.returncode}",
|
||||
"message": summary if ok else _grip_failure_message(proc.returncode, detail),
|
||||
"detail": (summary + "\n" + shown + "\n" + detail)[-4000:],
|
||||
}
|
||||
|
||||
|
||||
def _grip_failure_message(code: int, detail: str) -> str:
|
||||
"""退出码旁边带上脚本打出来的原因，别只剩一个数字。"""
|
||||
for line in reversed(detail.splitlines()):
|
||||
text = line.strip()
|
||||
if text.startswith("ERROR:") or text.startswith("无法"):
|
||||
return text
|
||||
return f"夹爪退出码 {code}"
|
||||
|
||||
|
||||
class Handler(BaseHTTPRequestHandler):
|
||||
def log_message(self, fmt: str, *args) -> None:
|
||||
return
|
||||
@@ -700,12 +810,12 @@ class Handler(BaseHTTPRequestHandler):
|
||||
return
|
||||
if path == "/api/tactile/view":
|
||||
action = "tactile_view_on" if body.get("on") else "tactile_view_off"
|
||||
result = run_action(action, "89", False)
|
||||
result = run_action(action, _default_arm(), False)
|
||||
self._json(200 if result.get("ok") else 400, result)
|
||||
return
|
||||
result = run_action(
|
||||
str(body.get("action") or ""),
|
||||
str(body.get("arm") or "89"),
|
||||
str(body.get("arm") or _default_arm()),
|
||||
bool(body.get("confirm")),
|
||||
str(body.get("mode") or "eye_in_hand"),
|
||||
)
|
||||
@@ -715,6 +825,10 @@ class Handler(BaseHTTPRequestHandler):
|
||||
def main() -> None:
|
||||
import atexit
|
||||
atexit.register(tactile_view.stop)
|
||||
from arm_application.operator_arm_state import start as start_arm_state
|
||||
from arm_application.operator_gripper_state import start as start_gripper_state
|
||||
start_arm_state()
|
||||
start_gripper_state()
|
||||
threading.Thread(target=_poll_tactile, daemon=True).start()
|
||||
server = ThreadingHTTPServer((HOST, PORT), Handler)
|
||||
print(f"Orisys 工位界面 http://{HOST}:{PORT}", flush=True)
|
||||
|
||||
@@ -0,0 +1,138 @@
|
||||
"""大寰夹爪的一比一外形。
|
||||
|
||||
左臂按下载包 ``PGEA-3D-Model-and-2D-Drawing-4`` 里的
|
||||
``PGEA-100-26-W-F`` 尺寸图。图签就是这个型号，总长 138、宽 55、厚 29，
|
||||
安装端抱闸约 20，主体 78，手指伸出约 40，闭合缝 1.2。
|
||||
现场装的是 ``PGEA-100-20-W``，这包里没有 100-20 的图，开口仍用
|
||||
``site.yaml`` 的 20 mm，不用图上的 26。
|
||||
|
||||
同包另一张完整的带抱闸图是 ``PGEA-100-40-W-F``（手指更长，张开约 40.8）。
|
||||
右臂是 ``RGI-100-30``，这包里没有旋转爪的图，仍按手册外形。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
# 米。来源见模块说明。长指向指尖，宽是开合方向，厚是图上的 29。
|
||||
PGEA_SIZE = (0.138, 0.055, 0.029)
|
||||
PGEA_STROKE_MM = 20.0
|
||||
PGEA_CLOSED_GAP_M = 0.0012
|
||||
PGEA_FINGER_M = 0.040
|
||||
RGI_SIZE = (0.158, 0.0755, 0.047)
|
||||
RGI_STROKE_MM = 30.0
|
||||
|
||||
# 深色底上要能分开：银壳体、蓝抱闸、橙手指、绿导轨。
|
||||
# 手指以前是 0.10 的黑，乘上未连接时的 0.55 就沉进背景里。
|
||||
_BODY = (0.86, 0.88, 0.91)
|
||||
_BRAKE = (0.16, 0.42, 0.90)
|
||||
_FINGER = (0.95, 0.46, 0.10)
|
||||
_RAIL = (0.18, 0.78, 0.28)
|
||||
|
||||
|
||||
def triangles(
|
||||
rotary: bool,
|
||||
permille: float | None,
|
||||
rotation_deg: float | None,
|
||||
stroke_mm: float | None,
|
||||
) -> tuple[list[tuple], str]:
|
||||
"""返回 ``(三角面, 说明)``。三角面是 ``(a, b, c, 明暗占位, rgb)``，明暗由界面再乘。"""
|
||||
if rotary:
|
||||
length, width, thick = RGI_SIZE
|
||||
stroke = (float(stroke_mm) if stroke_mm else RGI_STROKE_MM) / 1000.0
|
||||
note = (
|
||||
f"RGI-100-30 外形 {length*1000:.0f}×{width*1000:.1f}×{thick*1000:.0f} mm"
|
||||
f" 行程 {stroke*1000:.0f} mm。这包图纸里没有旋转爪，外形仍按手册。"
|
||||
)
|
||||
body_end = 0.118
|
||||
brake = None
|
||||
else:
|
||||
length, width, thick = PGEA_SIZE
|
||||
stroke = (float(stroke_mm) if stroke_mm else PGEA_STROKE_MM) / 1000.0
|
||||
note = (
|
||||
f"PGEA-100-26-W-F 尺寸图 外形 {length*1000:.0f}×{width*1000:.0f}×{thick*1000:.0f} mm"
|
||||
f" 现场行程 {stroke*1000:.0f} mm。包里没有 100-20 的图，开口不用图上的 26。"
|
||||
)
|
||||
body_end = length - PGEA_FINGER_M
|
||||
brake = (0.0, 0.020)
|
||||
gap = stroke * (0.5 if permille is None else max(0.0, min(1.0, float(permille) / 1000.0)))
|
||||
if not rotary:
|
||||
gap += PGEA_CLOSED_GAP_M
|
||||
parts = _solid(length, width, thick, body_end, gap, brake, rotary)
|
||||
if rotary and rotation_deg:
|
||||
parts = _spin_fingers(parts, math.radians(float(rotation_deg)), body_end)
|
||||
out = []
|
||||
for a, b, c, rgb in parts:
|
||||
out.append((a, b, c, _face_shade(a, b, c), rgb))
|
||||
if permille is None:
|
||||
note = "没有读数。下面是半开，不是当前开口。 " + note
|
||||
return out, note
|
||||
|
||||
|
||||
def _solid(length, width, thick, body_end, gap, brake, rotary: bool):
|
||||
hx, hy = thick / 2.0, width / 2.0
|
||||
faces = []
|
||||
z0 = 0.0
|
||||
if brake is not None:
|
||||
z0 = brake[1]
|
||||
faces.extend(_box(0.0, brake[1], -hy, hy, -hx, hx, _BRAKE))
|
||||
faces.extend(_box(z0, body_end, -hy, hy, -hx, hx, _BODY))
|
||||
if rotary:
|
||||
finger_t = min(0.010, hy * 0.4)
|
||||
finger_h = thick * 0.72
|
||||
else:
|
||||
# 尺寸图端视图：指厚 6.5，指高 16，都落在 55×29 的外形里面。
|
||||
finger_t = 0.0065
|
||||
finger_h = 0.016
|
||||
rail = 0.003
|
||||
faces.extend(_box(z0, body_end, -hy * 0.62, -hy * 0.62 + rail, hx - rail, hx, _RAIL))
|
||||
faces.extend(_box(z0, body_end, hy * 0.62 - rail, hy * 0.62, hx - rail, hx, _RAIL))
|
||||
half = gap / 2.0
|
||||
faces.extend(_box(body_end, length, -(half + finger_t), -half, -finger_h / 2, finger_h / 2, _FINGER))
|
||||
faces.extend(_box(body_end, length, half, half + finger_t, -finger_h / 2, finger_h / 2, _FINGER))
|
||||
return faces
|
||||
|
||||
|
||||
def _spin_fingers(parts, angle: float, body_end: float):
|
||||
c, s = math.cos(angle), math.sin(angle)
|
||||
spun = []
|
||||
for a, b, c0, rgb in parts:
|
||||
if a[0] >= body_end - 1e-6 and b[0] >= body_end - 1e-6 and c0[0] >= body_end - 1e-6:
|
||||
a, b, c0 = (_rot_z(a, c, s), _rot_z(b, c, s), _rot_z(c0, c, s))
|
||||
spun.append((a, b, c0, rgb))
|
||||
return spun
|
||||
|
||||
|
||||
def _face_shade(a, b, c) -> float:
|
||||
"""顶面亮、底面暗。零件颜色已经分开，明暗再把面分出来。"""
|
||||
ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
|
||||
vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
|
||||
nx = uy * vz - uz * vy
|
||||
ny = uz * vx - ux * vz
|
||||
nz = ux * vy - uy * vx
|
||||
length = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
|
||||
dot = (nx * 0.15 + ny * -0.25 + nz * 0.95) / length
|
||||
return 0.42 + 0.58 * max(0.0, min(1.0, dot))
|
||||
|
||||
|
||||
def _rot_z(point, c, s):
|
||||
x, y, z = point
|
||||
return (x, y * c - z * s, y * s + z * c)
|
||||
|
||||
|
||||
def _box(x0, x1, y0, y1, z0, z1, rgb) -> list[tuple]:
|
||||
"""轴向盒子。x 指向指尖，y 是开合方向，z 是厚度。"""
|
||||
p = (
|
||||
(x0, y0, z0), (x1, y0, z0), (x1, y1, z0), (x0, y1, z0),
|
||||
(x0, y0, z1), (x1, y0, z1), (x1, y1, z1), (x0, y1, z1),
|
||||
)
|
||||
quads = (
|
||||
(0, 1, 2, 3), (4, 7, 6, 5),
|
||||
(0, 4, 5, 1), (3, 2, 6, 7),
|
||||
(0, 3, 7, 4), (1, 5, 6, 2),
|
||||
)
|
||||
out = []
|
||||
for a, b, c, d in quads:
|
||||
out.append((p[a], p[b], p[c], rgb))
|
||||
out.append((p[a], p[c], p[d], rgb))
|
||||
return out
|
||||
@@ -0,0 +1,436 @@
|
||||
"""工位界面用的夹爪状态：只读开口和转角，并交给界面画出来。
|
||||
|
||||
夹爪节点已经占着串口时，只订它发布的话题，不再自己打开串口。
|
||||
没有节点时才短连一次。连上之后只读寄存器，不写初始化，避免手指回零把工件松开。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
from arm_application.site_io import listed_arms, ros2_root
|
||||
|
||||
_GRIP = {0: "运动中", 1: "已到位", 2: "已夹住", 3: "已掉落"}
|
||||
_ROT = {0: "转动中", 1: "已到位", 2: "受阻", 3: "曾经受阻"}
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
_CACHE: dict[str, dict] = {}
|
||||
_STARTED = False
|
||||
_PORT_HOLD = 0
|
||||
_PORT_BUSY = 0
|
||||
_PORT_LOCK = threading.Lock()
|
||||
|
||||
|
||||
def snapshot(arm: str) -> dict:
|
||||
with _LOCK:
|
||||
row = _CACHE.get(arm)
|
||||
return dict(row) if row else _empty(arm)
|
||||
|
||||
|
||||
def _empty(arm: str) -> dict:
|
||||
row = next((item for item in _arms() if item["id"] == arm), None)
|
||||
return present(
|
||||
arm,
|
||||
rotary=bool(row and row["rotary"]),
|
||||
stroke_mm=float(row["stroke_mm"]) if row else 20.0,
|
||||
reading=None,
|
||||
error="还没读到夹爪",
|
||||
)
|
||||
|
||||
|
||||
def present(
|
||||
arm: str,
|
||||
*,
|
||||
rotary: bool,
|
||||
stroke_mm: float,
|
||||
reading: dict | None,
|
||||
error: str = "",
|
||||
) -> dict:
|
||||
"""把一次读数收成界面能画的字典。不打开串口。"""
|
||||
stroke = float(stroke_mm) if stroke_mm and stroke_mm > 0 else 20.0
|
||||
row = reading or {}
|
||||
permille = _unit(row.get("permille"), 0, 1000)
|
||||
connected = bool(row.get("connected")) and permille is not None
|
||||
opening = None if permille is None else permille / 1000.0 * stroke
|
||||
rotation = _finite(row.get("rotation_deg")) if rotary else 0.0
|
||||
grip_state = row.get("grip_state")
|
||||
rot_state = row.get("rotation_state")
|
||||
if not connected:
|
||||
summary = error or str(row.get("error_msg") or "夹爪没连上")
|
||||
else:
|
||||
bits = [_GRIP.get(int(grip_state), "状态未知") if _finite(grip_state) is not None else "状态未知"]
|
||||
bits.append(f"开口 {opening:.1f} mm" if opening is not None else "")
|
||||
if rotary and rotation is not None:
|
||||
bits.append(f"转角 {rotation:.1f}°")
|
||||
if _finite(rot_state) is not None:
|
||||
bits.append(_ROT.get(int(rot_state), ""))
|
||||
summary = " ".join(bit for bit in bits if bit)
|
||||
return {
|
||||
"arm": arm,
|
||||
"rotary": rotary,
|
||||
"connected": connected,
|
||||
"initialized": bool(row.get("initialized")) if connected else False,
|
||||
"permille": permille,
|
||||
"opening_mm": opening,
|
||||
"stroke_mm": stroke,
|
||||
"rotation_deg": rotation if rotary else None,
|
||||
"force": _finite(row.get("force")),
|
||||
"speed": _finite(row.get("speed")),
|
||||
"grip_state": int(grip_state) if _finite(grip_state) is not None else None,
|
||||
"grip_text": _GRIP.get(int(grip_state), "—") if _finite(grip_state) is not None else "—",
|
||||
"rotation_text": _ROT.get(int(rot_state), "—") if rotary and _finite(rot_state) is not None else "—",
|
||||
"moving": bool(row.get("moving")) if connected else False,
|
||||
"gripped": bool(row.get("gripped")) or (_finite(grip_state) == 2) if connected else False,
|
||||
"summary": summary,
|
||||
"error": "" if connected else (error or str(row.get("error_msg") or "")),
|
||||
"source": str(row.get("source") or ""),
|
||||
"pose_live": connected,
|
||||
"stamp": time.time(),
|
||||
}
|
||||
|
||||
|
||||
def retain_reading(
|
||||
fresh: dict,
|
||||
previous: dict | None,
|
||||
*,
|
||||
now: float | None = None,
|
||||
hold_s: float = 15.0,
|
||||
) -> dict:
|
||||
"""这一拍没读到开口时，短时间沿用上一拍。
|
||||
|
||||
和机械臂同一件事：话题 echo 偶发超时会把刚读到的开口清成“没连上”，
|
||||
模型和顶栏就停在那里。超过 ``hold_s`` 仍没有新读数才放开。
|
||||
"""
|
||||
if fresh.get("connected") or not previous or not previous.get("connected"):
|
||||
return fresh
|
||||
moment = time.time() if now is None else now
|
||||
if moment - float(previous.get("stamp") or 0.0) > hold_s:
|
||||
return fresh
|
||||
kept = dict(fresh)
|
||||
for key in (
|
||||
"connected", "initialized", "permille", "opening_mm", "stroke_mm",
|
||||
"rotation_deg", "force", "speed", "grip_state", "grip_text",
|
||||
"rotation_text", "moving", "gripped", "pose_live", "rotary", "source",
|
||||
):
|
||||
if key in previous:
|
||||
kept[key] = previous[key]
|
||||
kept["summary"] = str(previous.get("summary") or fresh.get("summary") or "")
|
||||
note = "沿用上次读数。"
|
||||
if note not in kept["summary"]:
|
||||
kept["summary"] = f"{kept['summary']} {note}".strip()
|
||||
return kept
|
||||
|
||||
|
||||
def pause_serial() -> None:
|
||||
"""控制命令要自己开串口，或把驱动拉起来。先拦住状态线程，等它把口放开。
|
||||
|
||||
一次短读要过好几个寄存器，1.5 秒经常还没结束。提前返回会让新驱动
|
||||
去抢还被占着的口，然后以「需要 24 V」失败。
|
||||
"""
|
||||
global _PORT_HOLD
|
||||
with _PORT_LOCK:
|
||||
_PORT_HOLD += 1
|
||||
deadline = time.time() + 6.0
|
||||
while time.time() < deadline:
|
||||
with _PORT_LOCK:
|
||||
busy = _PORT_BUSY
|
||||
if not busy:
|
||||
return
|
||||
time.sleep(0.05)
|
||||
|
||||
|
||||
def hold_serial(pid: int) -> None:
|
||||
"""后台夹爪脚本还活着时，状态线程不要去开串口。"""
|
||||
pause_serial()
|
||||
|
||||
def _watch() -> None:
|
||||
while pid > 0:
|
||||
try:
|
||||
os.kill(pid, 0)
|
||||
except OSError:
|
||||
break
|
||||
time.sleep(0.2)
|
||||
resume_serial()
|
||||
|
||||
threading.Thread(target=_watch, daemon=True, name="gripper-port-hold").start()
|
||||
|
||||
|
||||
def resume_serial() -> None:
|
||||
global _PORT_HOLD
|
||||
with _PORT_LOCK:
|
||||
_PORT_HOLD = max(0, _PORT_HOLD - 1)
|
||||
|
||||
|
||||
def serial_paused() -> bool:
|
||||
with _PORT_LOCK:
|
||||
return _PORT_HOLD > 0
|
||||
|
||||
|
||||
def start() -> None:
|
||||
global _STARTED
|
||||
with _LOCK:
|
||||
if _STARTED:
|
||||
return
|
||||
_STARTED = True
|
||||
threading.Thread(target=_loop, daemon=True, name="gripper-state").start()
|
||||
|
||||
|
||||
def _loop() -> None:
|
||||
while True:
|
||||
try:
|
||||
arms = _arms()
|
||||
nodes = _nodes()
|
||||
for arm in arms:
|
||||
reading, error = _read_one(arm, nodes)
|
||||
fresh = present(
|
||||
arm["id"],
|
||||
rotary=arm["rotary"],
|
||||
stroke_mm=arm["stroke_mm"],
|
||||
reading=reading,
|
||||
error=error,
|
||||
)
|
||||
_store(retain_reading(fresh, snapshot(arm["id"])))
|
||||
except Exception as exc: # noqa: BLE001 —— 读夹爪失败不能拖死界面
|
||||
for row in _arms():
|
||||
_store(present(
|
||||
row["id"], rotary=bool(row.get("rotary")), stroke_mm=float(row.get("stroke_mm") or 20.0),
|
||||
reading=None, error=str(exc),
|
||||
))
|
||||
time.sleep(2.5)
|
||||
|
||||
|
||||
def _store(row: dict) -> None:
|
||||
with _LOCK:
|
||||
_CACHE[str(row.get("arm") or "")] = row
|
||||
|
||||
|
||||
def _arms() -> list[dict]:
|
||||
path = ros2_root() / "config" / "site.yaml"
|
||||
if not path.is_file():
|
||||
return []
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
shared = data.get("gripper") if isinstance(data.get("gripper"), dict) else {}
|
||||
baud = int(shared.get("baud") or 115200)
|
||||
slave = int(shared.get("slave_id") or 1)
|
||||
rts = str(shared.get("rs485_rts") or "")
|
||||
out = []
|
||||
for row in listed_arms(data):
|
||||
item = dict(row)
|
||||
item["baud"] = baud
|
||||
item["slave"] = slave
|
||||
item["rts"] = rts
|
||||
out.append(item)
|
||||
return out
|
||||
|
||||
|
||||
def _nodes() -> dict[str, bool]:
|
||||
text = _pgrep("dh_pgea_node|dual_gripper")
|
||||
return {
|
||||
"pgea": "dh_pgea_node" in text or "lib/arm_application/dh_pgea" in text,
|
||||
"dual": "dual_gripper" in text,
|
||||
}
|
||||
|
||||
|
||||
def _read_one(arm: dict, nodes: dict[str, bool]) -> tuple[dict | None, str]:
|
||||
if nodes.get("dual") and arm.get("side") in ("left", "right"):
|
||||
topic = f"/dual_gripper_driver/{arm['side']}/status"
|
||||
parsed = _parse_status_echo(_echo(topic))
|
||||
if parsed is not None:
|
||||
parsed["source"] = f"话题 {topic}"
|
||||
return parsed, ""
|
||||
return None, "夹爪节点在跑，但这侧话题没有数据"
|
||||
if not arm.get("rotary") and nodes.get("pgea"):
|
||||
parsed = _parse_states_echo(_echo("/gripper/states"))
|
||||
if parsed is not None:
|
||||
parsed["source"] = "话题 /gripper/states"
|
||||
return parsed, ""
|
||||
return None, "平行爪节点在跑，但 /gripper/states 没有数据"
|
||||
if not arm.get("port"):
|
||||
return None, "site.yaml 里这台夹爪没有串口"
|
||||
if serial_paused():
|
||||
return None, "控制命令正在用串口"
|
||||
return _read_serial(arm)
|
||||
|
||||
|
||||
def _read_serial(arm: dict) -> tuple[dict | None, str]:
|
||||
"""短连串口只读。auto_initialize 必须关掉，否则旋转爪一连上就会找零。"""
|
||||
global _PORT_BUSY
|
||||
if serial_paused():
|
||||
return None, "控制命令正在用串口"
|
||||
with _PORT_LOCK:
|
||||
_PORT_BUSY += 1
|
||||
try:
|
||||
return _read_serial_open(arm)
|
||||
finally:
|
||||
with _PORT_LOCK:
|
||||
_PORT_BUSY = max(0, _PORT_BUSY - 1)
|
||||
|
||||
|
||||
def _read_serial_open(arm: dict) -> tuple[dict | None, str]:
|
||||
try:
|
||||
if arm["rotary"]:
|
||||
from arm_application.dual_gripper import RotaryGripper, RotaryGripperConfig
|
||||
grip = RotaryGripper(RotaryGripperConfig(
|
||||
port=arm["port"],
|
||||
slave_id=int(arm["slave"]),
|
||||
baudrate=int(arm["baud"]),
|
||||
timeout=0.25,
|
||||
auto_initialize=False,
|
||||
))
|
||||
grip.connect()
|
||||
try:
|
||||
status = grip.get_status()
|
||||
finally:
|
||||
grip.disconnect()
|
||||
if not status.connected:
|
||||
return None, status.error_msg or "旋转爪没有应答"
|
||||
return {
|
||||
"connected": True,
|
||||
"initialized": bool(status.initialized),
|
||||
"permille": float(status.finger_position_per_mille),
|
||||
"rotation_deg": float(status.rotation_deg),
|
||||
"force": float(status.force),
|
||||
"speed": float(status.velocity),
|
||||
"grip_state": int(status.gripper_state),
|
||||
"rotation_state": int(status.rotation_state),
|
||||
"moving": bool(status.is_moving),
|
||||
"gripped": bool(status.is_gripped),
|
||||
"error_msg": status.error_msg or "",
|
||||
"source": f"串口 {arm['port']} 只读",
|
||||
}, ""
|
||||
from arm_application.dh_pgea import DhPgeaError, DhPgeaGripper
|
||||
grip = DhPgeaGripper(
|
||||
port=arm["port"],
|
||||
baudrate=int(arm["baud"]),
|
||||
slave_id=int(arm["slave"]),
|
||||
stroke_mm=float(arm["stroke_mm"]),
|
||||
rs485_rts=str(arm["rts"]),
|
||||
)
|
||||
grip.open()
|
||||
try:
|
||||
state = grip.get_run_states(retries=1)
|
||||
finally:
|
||||
grip.close()
|
||||
return {
|
||||
"connected": True,
|
||||
"initialized": bool(state.initialized),
|
||||
"permille": float(state.position),
|
||||
"rotation_deg": 0.0,
|
||||
"force": float(state.target_force),
|
||||
"speed": None,
|
||||
"grip_state": int(state.grip_state),
|
||||
"rotation_state": None,
|
||||
"moving": int(state.grip_state) == 0,
|
||||
"gripped": int(state.grip_state) == 2,
|
||||
"error_msg": "",
|
||||
"source": f"串口 {arm['port']} 只读",
|
||||
}, ""
|
||||
except Exception as exc: # noqa: BLE001 —— 串口被节点占着、没上电，都要变成一句人话
|
||||
text = str(exc).strip() or exc.__class__.__name__
|
||||
if "DhPgeaError" in exc.__class__.__name__ or "Permission" in text or "busy" in text.lower():
|
||||
return None, text
|
||||
return None, text
|
||||
|
||||
|
||||
def _echo(topic: str) -> str:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["timeout", "3", "ros2", "topic", "echo", topic, "--once"],
|
||||
capture_output=True, text=True, timeout=5,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return ""
|
||||
return proc.stdout
|
||||
|
||||
|
||||
def _parse_status_echo(text: str) -> dict | None:
|
||||
for chunk in text.split("---"):
|
||||
if "finger_position_per_mille:" not in chunk and "connected:" not in chunk:
|
||||
continue
|
||||
try:
|
||||
data = yaml.safe_load(chunk)
|
||||
except yaml.YAMLError:
|
||||
continue
|
||||
if not isinstance(data, dict) or "connected" not in data:
|
||||
continue
|
||||
return {
|
||||
"connected": bool(data.get("connected")),
|
||||
"initialized": bool(data.get("initialized")),
|
||||
"permille": data.get("finger_position_per_mille"),
|
||||
"rotation_deg": data.get("rotation_deg"),
|
||||
"force": data.get("force"),
|
||||
"speed": data.get("velocity"),
|
||||
"grip_state": data.get("gripper_state"),
|
||||
"rotation_state": data.get("rotation_state"),
|
||||
"moving": bool(data.get("is_moving")),
|
||||
"gripped": bool(data.get("is_gripped")),
|
||||
"error_msg": str(data.get("error_msg") or ""),
|
||||
}
|
||||
return None
|
||||
|
||||
|
||||
def _parse_states_echo(text: str) -> dict | None:
|
||||
for chunk in text.split("---"):
|
||||
if "data:" not in chunk:
|
||||
continue
|
||||
try:
|
||||
data = yaml.safe_load(chunk)
|
||||
except yaml.YAMLError:
|
||||
continue
|
||||
if not isinstance(data, dict):
|
||||
continue
|
||||
raw = data.get("data")
|
||||
if not isinstance(raw, list) or len(raw) < 5:
|
||||
continue
|
||||
try:
|
||||
init, grip, pos, _target, force = (float(v) for v in raw[:5])
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
return {
|
||||
"connected": True,
|
||||
"initialized": init >= 0.5,
|
||||
"permille": pos,
|
||||
"rotation_deg": 0.0,
|
||||
"force": force,
|
||||
"speed": None,
|
||||
"grip_state": int(grip),
|
||||
"rotation_state": None,
|
||||
"moving": int(grip) == 0,
|
||||
"gripped": int(grip) == 2,
|
||||
"error_msg": "",
|
||||
}
|
||||
return None
|
||||
|
||||
|
||||
def _pgrep(pattern: str) -> str:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
["pgrep", "-af", pattern],
|
||||
capture_output=True, text=True, timeout=2,
|
||||
)
|
||||
except (OSError, subprocess.TimeoutExpired):
|
||||
return ""
|
||||
return "\n".join(line for line in proc.stdout.splitlines() if "pgrep" not in line)
|
||||
|
||||
|
||||
def _finite(raw: Any) -> float | None:
|
||||
try:
|
||||
value = float(raw)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if value != value:
|
||||
return None
|
||||
return value
|
||||
|
||||
|
||||
def _unit(raw: Any, low: float, high: float) -> float | None:
|
||||
value = _finite(raw)
|
||||
if value is None:
|
||||
return None
|
||||
return max(low, min(high, value))
|
||||
@@ -0,0 +1,91 @@
|
||||
"""手眼页的棋盘预览。只在已有彩色画面上画角点，不打开相机，也不动臂。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_application.handeye_lib import BoardConfig, detect_chessboard
|
||||
|
||||
|
||||
def coverage_fraction(corners: np.ndarray, width: int, height: int) -> float:
|
||||
"""角点外接框占画面的比例。宽或高为 0 时返回 0。"""
|
||||
if width <= 0 or height <= 0:
|
||||
return 0.0
|
||||
pts = np.asarray(corners, dtype=float).reshape(-1, 2)
|
||||
if pts.size == 0:
|
||||
return 0.0
|
||||
area = float(pts[:, 0].max() - pts[:, 0].min()) * float(pts[:, 1].max() - pts[:, 1].min())
|
||||
return area / float(width * height)
|
||||
|
||||
|
||||
def coverage_words(fraction: float) -> str:
|
||||
"""30%–70% 是示教时棋盘该占的画面。再小对不准，再大姿态拉不开。"""
|
||||
if fraction < 0.30:
|
||||
return "偏小，再靠近一点"
|
||||
if fraction > 0.70:
|
||||
return "偏大，再离远一点"
|
||||
return "大小合适"
|
||||
|
||||
|
||||
def annotate_board(jpeg: bytes, cols: int, rows: int) -> tuple[bytes, str]:
|
||||
"""返回（可能画了角点的 JPEG，一行中文状态）。检测失败时 JPEG 原样返回。"""
|
||||
import cv2
|
||||
|
||||
if cols <= 0 or rows <= 0:
|
||||
return jpeg, "配置里没有这块板的内角点"
|
||||
buf = np.frombuffer(jpeg, dtype=np.uint8)
|
||||
image = cv2.imdecode(buf, cv2.IMREAD_COLOR)
|
||||
if image is None:
|
||||
return jpeg, "这一帧解不出来"
|
||||
found = detect_chessboard(image, BoardConfig(int(cols), int(rows), 1.0))
|
||||
if found is None:
|
||||
return jpeg, "画面里没有这块板"
|
||||
corners, pattern = found
|
||||
drawn = image.copy()
|
||||
cv2.drawChessboardCorners(drawn, pattern, corners, True)
|
||||
fraction = coverage_fraction(corners, drawn.shape[1], drawn.shape[0])
|
||||
ok, out = cv2.imencode(".jpg", drawn, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
|
||||
blob = out.tobytes() if ok else jpeg
|
||||
percent = int(round(fraction * 100))
|
||||
return blob, f"棋盘已找到，约占画面 {percent}%。{coverage_words(fraction)}"
|
||||
|
||||
|
||||
class BoardWatch:
|
||||
"""后台画角点。界面线程只提交最新一帧，检测再慢也不卡住窗口。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._lock = threading.Lock()
|
||||
self._pending: tuple | None = None
|
||||
self._result = {"token": 0, "jpeg": b"", "text": ""}
|
||||
self._stop = threading.Event()
|
||||
self._thread = threading.Thread(target=self._loop, daemon=True, name="handeye-board")
|
||||
self._thread.start()
|
||||
|
||||
def submit(self, token: int, jpeg: bytes, cols: int, rows: int) -> None:
|
||||
with self._lock:
|
||||
if self._result["token"] == token and not self._pending:
|
||||
return
|
||||
self._pending = (token, jpeg, int(cols), int(rows))
|
||||
|
||||
def latest(self) -> tuple[bytes, str, int]:
|
||||
with self._lock:
|
||||
return self._result["jpeg"], self._result["text"], int(self._result["token"])
|
||||
|
||||
def _loop(self) -> None:
|
||||
while not self._stop.is_set():
|
||||
with self._lock:
|
||||
job = self._pending
|
||||
self._pending = None
|
||||
if job is None:
|
||||
time.sleep(0.05)
|
||||
continue
|
||||
token, jpeg, cols, rows = job
|
||||
try:
|
||||
blob, text = annotate_board(jpeg, cols, rows)
|
||||
except Exception as exc: # noqa: BLE001 —— 检测失败不能拖死预览
|
||||
blob, text = jpeg, f"棋盘检测失败：{exc}"
|
||||
with self._lock:
|
||||
self._result = {"token": token, "jpeg": blob, "text": text}
|
||||
@@ -21,9 +21,11 @@ _SN = ""
|
||||
|
||||
def _file(arm: str) -> Path:
|
||||
from arm_application.operator_console import ROOT
|
||||
from arm_application.site_io import listed_arms, load_site_dict
|
||||
|
||||
if arm not in ("89", "91"):
|
||||
raise ValueError("臂号只接受 89 或 91")
|
||||
known = {row["id"] for row in listed_arms(load_site_dict())}
|
||||
if arm not in known:
|
||||
raise ValueError(f"臂号 {arm} 不在 site.yaml 里")
|
||||
return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml"
|
||||
|
||||
|
||||
@@ -36,9 +38,14 @@ def _bridge_for(sn: str):
|
||||
return _BRIDGE
|
||||
close()
|
||||
from arm_application.handeye_teach import RdkBridge
|
||||
from arm_application.operator_arm_state import release_for_control, resume_after_control
|
||||
|
||||
release_for_control()
|
||||
try:
|
||||
_BRIDGE = RdkBridge(sn)
|
||||
_SN = sn
|
||||
finally:
|
||||
resume_after_control()
|
||||
return _BRIDGE
|
||||
|
||||
|
||||
|
||||
@@ -94,6 +94,80 @@ def site_path() -> Path:
|
||||
return ros2_root() / "config" / "site.yaml"
|
||||
|
||||
|
||||
def arm_short_id(sn: str) -> str:
|
||||
"""序列号末两位就是臂号（``Rizon4-063989`` → ``89``）。
|
||||
|
||||
和 ``scripts/lib/export_site_env.arm_short_id`` 同一条规则。换一台臂
|
||||
只改 ``site.yaml`` 的 SN，不要在界面里再写一张本机对照表。
|
||||
"""
|
||||
tail = str(sn or "").strip().rsplit("-", 1)[-1]
|
||||
return tail[-2:] if tail.isdigit() and len(tail) >= 2 else ""
|
||||
|
||||
|
||||
def listed_arms(data: dict[str, Any] | None) -> list[dict[str, Any]]:
|
||||
"""``robots[]`` 里的真机臂。``sn: sim`` 不进控制。
|
||||
|
||||
旋转爪只看夹爪型号里有没有 ``RGI``。右臂也可以是平行爪，不能靠臂号猜。
|
||||
"""
|
||||
robots = ((data or {}).get("robot") or {}).get("robots") or []
|
||||
out: list[dict[str, Any]] = []
|
||||
seen: set[str] = set()
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
sn = str(robot.get("sn") or "").strip()
|
||||
if not sn or sn.lower() == "sim":
|
||||
continue
|
||||
arm_no = arm_short_id(sn)
|
||||
if not arm_no:
|
||||
continue
|
||||
if arm_no in seen:
|
||||
raise ValueError(f"两台臂的序列号末两位都是 {arm_no}，--arm 分不清")
|
||||
seen.add(arm_no)
|
||||
side = str(robot.get("arm") or "").strip().lower()
|
||||
grip = robot.get("gripper") if isinstance(robot.get("gripper"), dict) else {}
|
||||
model = str(grip.get("model") or "")
|
||||
shared = (data or {}).get("gripper") if isinstance((data or {}).get("gripper"), dict) else {}
|
||||
stroke = grip.get("stroke_mm")
|
||||
if stroke is None:
|
||||
stroke = shared.get("stroke_mm") or 20.0
|
||||
out.append({
|
||||
"id": arm_no,
|
||||
"label": str(robot.get("label") or "").strip() or f"臂{arm_no}",
|
||||
"sn": sn,
|
||||
"side": side if side in ("left", "right") else "",
|
||||
"tactile": bool(robot.get("tactile")),
|
||||
"camera": str((robot.get("camera") or {}).get("name") or "") if isinstance(robot.get("camera"), dict) else "",
|
||||
"gripper": model,
|
||||
"rotary": "RGI" in model.upper(),
|
||||
"iface": str(robot.get("iface") or ""),
|
||||
"pc_ip": str(robot.get("pc_ip") or ""),
|
||||
"port": str(grip.get("port") or ""),
|
||||
"stroke_mm": float(stroke),
|
||||
})
|
||||
return out
|
||||
|
||||
|
||||
def usb_expectations(data: dict[str, Any] | None) -> dict[str, int]:
|
||||
"""设备页上「应有几台」跟 ``site.yaml`` 走，不写死本机的 2+1+2。"""
|
||||
cameras = (data or {}).get("cameras") or {}
|
||||
d405 = 0
|
||||
d435 = 0
|
||||
if isinstance(cameras, dict):
|
||||
for key, val in cameras.items():
|
||||
model = str((val or {}).get("model") or "") if isinstance(val, dict) else ""
|
||||
token = f"{key} {model}".lower()
|
||||
if "d405" in token:
|
||||
d405 += 1
|
||||
elif "d435" in token:
|
||||
d435 += 1
|
||||
tactile = (data or {}).get("tactile") or {}
|
||||
orisys = 0
|
||||
if isinstance(tactile, dict):
|
||||
orisys = sum(1 for key in ("left_video", "right_video") if tactile.get(key) is not None)
|
||||
return {"d405": d405, "d435": d435, "orisys": orisys}
|
||||
|
||||
|
||||
def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
|
||||
p = Path(path).expanduser() if path else site_path()
|
||||
if not p.is_file():
|
||||
|
||||
@@ -0,0 +1,200 @@
|
||||
"""相机预览必须在抽稀后的小图上做，并且贴图不能走 setPixmap。
|
||||
|
||||
以前每一帧都在整幅深度上算百分位再编码 JPEG，界面线程再解码，
|
||||
六路一起刷的时候整页布局跟着重算。这里的时间预算按那个旧路径会红。
|
||||
"""
|
||||
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application import operator_camera_view as camera_view
|
||||
|
||||
|
||||
def test_unwatched_frames_are_dropped_and_the_first_watched_frame_is_taken() -> None:
|
||||
try:
|
||||
camera_view.watch(set(), set())
|
||||
assert camera_view.wants("cam_color") == (False, False)
|
||||
assert not camera_view.accept_frame("cam_color")
|
||||
camera_view.watch({"cam_color"}, set())
|
||||
assert camera_view.wants("cam_color") == (True, False)
|
||||
assert camera_view.accept_frame("cam_color")
|
||||
assert not camera_view.accept_frame("cam_color")
|
||||
camera_view.watch(set(), {"cam_color"})
|
||||
assert camera_view.wants("cam_color") == (False, True)
|
||||
assert camera_view.accept_frame("cam_color")
|
||||
finally:
|
||||
camera_view.watch(set(), set())
|
||||
|
||||
|
||||
def test_depth_preview_stays_small_and_keeps_invalid_pixels_black() -> None:
|
||||
blank = np.zeros((48, 64), np.uint16)
|
||||
black = camera_view.depth_preview(blank)
|
||||
assert black.shape == (48, 64, 3)
|
||||
assert int(black.sum()) == 0
|
||||
|
||||
wide = np.full((90, 1280), 400, np.uint16)
|
||||
colored = camera_view.depth_preview(wide)
|
||||
assert colored.shape[1] == 480
|
||||
assert colored.shape[0] < 90
|
||||
assert colored.shape[2] == 3
|
||||
assert int(colored.sum()) > 0
|
||||
|
||||
|
||||
def test_color_preview_is_rgb_and_not_wider_than_the_widget_budget() -> None:
|
||||
image = np.zeros((20, 30, 3), np.uint8)
|
||||
image[5, 5] = (0, 0, 255)
|
||||
out = camera_view.rgb_preview(image, bgr=True)
|
||||
assert tuple(int(v) for v in out[5, 5]) == (255, 0, 0)
|
||||
|
||||
wide = np.zeros((720, 1280, 3), np.uint8)
|
||||
wide[:, :, 0] = 10
|
||||
fitted = camera_view.rgb_preview(wide, bgr=True)
|
||||
assert fitted.shape[1] == 480
|
||||
assert fitted.shape[2] == 3
|
||||
with pytest.raises(ValueError, match="彩色预览"):
|
||||
camera_view.rgb_preview(np.zeros((8, 8), np.uint8), bgr=True)
|
||||
|
||||
|
||||
def test_preview_build_stays_inside_the_frame_interval() -> None:
|
||||
"""三路 720p 彩色加三路深度。旧路径光深度就要大约 60ms，100ms 的刷新会被占满。"""
|
||||
color = np.random.randint(0, 255, (720, 1280, 3), np.uint8)
|
||||
depth = np.random.randint(1, 3000, (720, 1280), np.uint16)
|
||||
|
||||
def once() -> None:
|
||||
for _cam in range(3):
|
||||
camera_view.rgb_preview(color, bgr=True)
|
||||
camera_view.depth_preview(depth)
|
||||
|
||||
once()
|
||||
samples = []
|
||||
for _step in range(5):
|
||||
started = time.perf_counter()
|
||||
once()
|
||||
samples.append(time.perf_counter() - started)
|
||||
samples.sort()
|
||||
assert samples[len(samples) // 2] < 0.045
|
||||
|
||||
|
||||
def test_frame_label_paints_rgb_without_set_pixmap() -> None:
|
||||
"""setPixmap 会把父布局重算一遍。预览框的 pixmap() 必须一直是空的。"""
|
||||
import os
|
||||
|
||||
pytest.importorskip("PySide6")
|
||||
os.environ.setdefault("QT_QPA_PLATFORM", "offscreen")
|
||||
from PySide6.QtWidgets import QApplication
|
||||
|
||||
from arm_application.operator_app import FrameLabel
|
||||
|
||||
_app = QApplication.instance() or QApplication([])
|
||||
label = FrameLabel("没有画面")
|
||||
label.resize(500, 320)
|
||||
label.show()
|
||||
rgb = np.zeros((270, 480, 3), np.uint8)
|
||||
rgb[:, :, 0] = 220
|
||||
blob = rgb.tobytes()
|
||||
label.show_rgb(blob, 480, 270)
|
||||
assert label.pixmap() is None or label.pixmap().isNull()
|
||||
assert not label._scaled.isNull()
|
||||
again = []
|
||||
for _step in range(5):
|
||||
fresh = bytes(blob)
|
||||
started = time.perf_counter()
|
||||
label.show_rgb(fresh, 480, 270)
|
||||
again.append(time.perf_counter() - started)
|
||||
again.sort()
|
||||
assert again[len(again) // 2] < 0.02
|
||||
label.close()
|
||||
|
||||
|
||||
def test_preview_interval_is_fast_enough_for_the_main_view() -> None:
|
||||
"""主界面要看运动和推理框。退回 0.10s 就是大约 10 帧，这里会红。"""
|
||||
assert camera_view._INTERVAL <= 0.05
|
||||
|
||||
|
||||
def test_detection_box_scales_onto_the_preview_and_empty_clears() -> None:
|
||||
rgb = np.zeros((50, 100, 3), np.uint8)
|
||||
boxed = camera_view.paint_detection(rgb, (0, 0, 50, 25), 100, 50)
|
||||
assert int(rgb.sum()) == 0
|
||||
assert int(boxed[0, 10, 1]) == 220
|
||||
assert int(boxed[40, 90].sum()) == 0
|
||||
same = camera_view.paint_detection(rgb, (), 100, 50)
|
||||
assert int(same.sum()) == 0
|
||||
|
||||
camera_view._DET.clear()
|
||||
try:
|
||||
assert "没有框" in camera_view.inference_status()
|
||||
camera_view._remember_detection("wrist", [0, 0, 10, 10, 0.86, 0, 0, 0, 0])
|
||||
text = camera_view.inference_status()
|
||||
assert "腕部 置信 0.86" in text
|
||||
assert "手外 没有框" in text
|
||||
camera_view._remember_detection("wrist", [])
|
||||
assert "腕部 没有框" in camera_view.inference_status()
|
||||
finally:
|
||||
camera_view._DET.clear()
|
||||
camera_view._CLEAN.clear()
|
||||
|
||||
|
||||
def test_detection_is_drawn_on_the_camera_frame() -> None:
|
||||
"""框、跟踪点要落在相机预览上，不能只写在画面外面的一行字里。"""
|
||||
camera_view._DET.clear()
|
||||
camera_view._CLEAN.clear()
|
||||
camera_view._PREVIEW.pop("cam_color", None)
|
||||
try:
|
||||
rgb = np.zeros((40, 80, 3), np.uint8)
|
||||
camera_view._remember_clean("cam_color", rgb, "wrist", 160, 80)
|
||||
camera_view._remember_detection(
|
||||
"wrist", [20, 10, 60, 40, 0.91, 1, 40, 24, 0.2]
|
||||
)
|
||||
blob, width, height = camera_view.previews()["cam_color"]
|
||||
frame = np.frombuffer(blob, np.uint8).reshape(height, width, 3)
|
||||
assert int(frame[12, 20, 0]) == 255
|
||||
assert int(frame[12, 20, 1]) == 48
|
||||
finally:
|
||||
camera_view._DET.clear()
|
||||
camera_view._CLEAN.clear()
|
||||
camera_view._PREVIEW.pop("cam_color", None)
|
||||
|
||||
|
||||
def test_main_preview_is_left_wrist_and_shared_overhead_only() -> None:
|
||||
"""右臂腕部和仿真手外不进主界面。左腕和共用 D435 才进。"""
|
||||
site = {
|
||||
"robot": {
|
||||
"robots": [
|
||||
{
|
||||
"sn": "Rizon4-063989",
|
||||
"arm": "left",
|
||||
"label": "左",
|
||||
"camera": {"name": "d405_left"},
|
||||
},
|
||||
{
|
||||
"sn": "Rizon4-063991",
|
||||
"arm": "right",
|
||||
"label": "右",
|
||||
"camera": {"name": "d405_right"},
|
||||
},
|
||||
{"sn": "sim", "arm": "sim", "camera": {"name": "sim_d405"}},
|
||||
]
|
||||
},
|
||||
"cameras": {
|
||||
"d405_left": {"name": "d405_left", "model": "d405", "role": "left_wrist"},
|
||||
"d405_right": {"name": "d405_right", "model": "d405", "role": "right_wrist"},
|
||||
"sim_d435": {
|
||||
"name": "sim_d435",
|
||||
"model": "d435",
|
||||
"role": "overhead_eye_to_hand",
|
||||
"arm": "sim",
|
||||
},
|
||||
"d435": {
|
||||
"name": "d435",
|
||||
"model": "d435",
|
||||
"role": "overhead_eye_to_hand",
|
||||
},
|
||||
},
|
||||
}
|
||||
cams = camera_view.preview_targets(site)
|
||||
assert [cam["id"] for cam in cams] == ["d405_left", "d435"]
|
||||
assert [cam["role"] for cam in cams] == ["wrist", "overhead"]
|
||||
assert cams[0]["label"] == "左腕 D405"
|
||||
assert cams[1]["label"] == "手外 D435"
|
||||
@@ -0,0 +1,81 @@
|
||||
"""手眼页的棋盘预览和采点参数。不打开相机，也不动臂。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.operator_console import render_action_args
|
||||
from arm_application.operator_handeye_board import (
|
||||
annotate_board,
|
||||
coverage_fraction,
|
||||
coverage_words,
|
||||
)
|
||||
|
||||
|
||||
def test_coverage_words_follow_the_thirty_to_seventy_band():
|
||||
assert "偏小" in coverage_words(0.29)
|
||||
assert coverage_words(0.30) == "大小合适"
|
||||
assert coverage_words(0.70) == "大小合适"
|
||||
assert "偏大" in coverage_words(0.71)
|
||||
|
||||
|
||||
def test_coverage_fraction_is_the_corner_box():
|
||||
corners = np.array([[10.0, 10.0], [30.0, 10.0], [30.0, 40.0], [10.0, 40.0]])
|
||||
assert coverage_fraction(corners, 100, 100) == pytest.approx(0.06)
|
||||
assert coverage_fraction(corners, 0, 100) == 0.0
|
||||
|
||||
|
||||
def test_blank_frame_is_not_called_a_board():
|
||||
import cv2
|
||||
|
||||
image = np.full((80, 120, 3), 255, np.uint8)
|
||||
ok, buf = cv2.imencode(".jpg", image)
|
||||
assert ok
|
||||
blob, text = annotate_board(buf.tobytes(), 5, 4)
|
||||
assert blob
|
||||
assert "没有这块板" in text
|
||||
|
||||
|
||||
def test_drawn_board_is_found_and_keeps_a_jpeg():
|
||||
import cv2
|
||||
|
||||
cols, rows, cell, border = 5, 4, 30, 20
|
||||
width = (cols + 1) * cell + 2 * border
|
||||
height = (rows + 1) * cell + 2 * border
|
||||
image = np.full((height, width), 255, np.uint8)
|
||||
for y in range(rows + 1):
|
||||
for x in range(cols + 1):
|
||||
if (x + y) % 2 == 0:
|
||||
image[
|
||||
border + y * cell:border + (y + 1) * cell,
|
||||
border + x * cell:border + (x + 1) * cell,
|
||||
] = 0
|
||||
color = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
|
||||
ok, buf = cv2.imencode(".jpg", color)
|
||||
assert ok
|
||||
blob, text = annotate_board(buf.tobytes(), cols, rows)
|
||||
assert "棋盘已找到" in text
|
||||
assert cv2.imdecode(np.frombuffer(blob, np.uint8), cv2.IMREAD_COLOR) is not None
|
||||
|
||||
|
||||
def test_missing_board_size_does_not_pretend_to_search():
|
||||
blob, text = annotate_board(b"not-a-jpeg", 0, 0)
|
||||
assert blob == b"not-a-jpeg"
|
||||
assert "内角点" in text
|
||||
|
||||
|
||||
def test_teach_args_take_the_point_count_and_append():
|
||||
template = ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "{points}"]
|
||||
assert render_action_args(template, "01", "eye_to_hand", {"points": 8}) == [
|
||||
"--arm", "01", "teach", "--mode", "eye_to_hand", "--points", "8",
|
||||
]
|
||||
appended = render_action_args(template, "01", "eye_in_hand", {"points": 12, "append": True})
|
||||
assert appended[-1] == "--append"
|
||||
with pytest.raises(ValueError, match="4–30"):
|
||||
render_action_args(template, "01", "eye_in_hand", {"points": 2})
|
||||
|
||||
|
||||
def test_append_is_refused_on_a_command_that_is_not_teach():
|
||||
with pytest.raises(ValueError, match="示教采点"):
|
||||
render_action_args(["--arm", "{arm}", "calibrate"], "01", "eye_in_hand", {"append": True})
|
||||
@@ -0,0 +1,167 @@
|
||||
"""换一台 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
|
||||
|
||||
|
||||
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)
|
||||
assert any(text.startswith("⏻") for text in texts)
|
||||
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()
|
||||
|
||||
|
||||
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,270 @@
|
||||
"""厂家 Rizon4 网格能按关节角摆起来，而且零位时法兰比底座高。"""
|
||||
|
||||
import time
|
||||
|
||||
from arm_application.operator_arm_mesh import posed_gl, posed_triangles
|
||||
|
||||
|
||||
def test_vendor_mesh_stands_at_zero() -> None:
|
||||
triangles = posed_triangles([0.0] * 7)
|
||||
assert len(triangles) > 1000
|
||||
zs = [p[2] for tri in triangles for p in tri[:3]]
|
||||
assert min(zs) < 0.05
|
||||
assert max(zs) > 0.9
|
||||
assert all(v == v for tri in triangles for p in tri[:3] for v in p)
|
||||
|
||||
|
||||
def test_vendor_materials_are_not_flat_gray() -> None:
|
||||
"""白壳和灰蓝关节来自两份 Kd。抽稀之后两种都还在，否则界面又会变成一片灰。"""
|
||||
colors = {tuple(round(c, 2) for c in tri[4]) for tri in posed_triangles([0.0] * 7)}
|
||||
assert len(colors) >= 2
|
||||
assert max(max(rgb) for rgb in colors) > 0.9
|
||||
assert min(min(rgb) for rgb in colors) < 0.6
|
||||
|
||||
|
||||
def test_joint2_moves_the_shell() -> None:
|
||||
upright = posed_triangles([0.0] * 7)
|
||||
bent = posed_triangles([0.0, 1.2, 0.0, 0.0, 0.0, 0.0, 0.0])
|
||||
z_up = max(p[2] for tri in upright for p in tri[:3])
|
||||
z_bent = max(p[2] for tri in bent for p in tri[:3])
|
||||
assert abs(z_up - z_bent) > 0.05
|
||||
|
||||
|
||||
def test_numpy_pose_matches_the_single_placement() -> None:
|
||||
"""连杆旋转不是单位阵时，数组乘法和逐点公式必须重合。差一个下标整条臂会散开。"""
|
||||
from arm_application.operator_arm_mesh import _models, _place, _posed_arrays
|
||||
from arm_application.operator_arm_state import link_frames
|
||||
|
||||
joints = [0.2, -0.8, 0.4, 1.1, -0.3, 0.6, 0.2]
|
||||
frames = link_frames(joints)
|
||||
world, _shade, _rgb = _posed_arrays(joints)
|
||||
offset = 0
|
||||
checked = 0
|
||||
for link, parts in _models().items():
|
||||
frame = frames.get(link)
|
||||
if frame is None:
|
||||
continue
|
||||
rot, origin = frame
|
||||
for part in parts:
|
||||
count = part["verts"].shape[0]
|
||||
if count == 0:
|
||||
continue
|
||||
sample = world[offset + count // 2, 1]
|
||||
vertex = tuple(float(v) for v in part["verts"][count // 2, 1])
|
||||
erot = tuple(tuple(float(v) for v in row) for row in part["erot"])
|
||||
epos = tuple(float(v) for v in part["epos"])
|
||||
placed = _place(rot, origin, erot, epos, vertex)
|
||||
assert max(abs(placed[axis] - float(sample[axis])) for axis in range(3)) < 1e-6
|
||||
offset += count
|
||||
checked += 1
|
||||
assert checked >= 8
|
||||
assert offset == world.shape[0]
|
||||
|
||||
|
||||
def test_pose_update_stays_under_the_ui_budget() -> None:
|
||||
"""界面刷新是 800ms。姿态计算若回到逐顶点的 Python 循环（约 0.3s），这里会红。"""
|
||||
posed_gl([0.0] * 7)
|
||||
samples = []
|
||||
for index in range(5):
|
||||
joints = [0.0, 0.2 * index, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
started = time.perf_counter()
|
||||
blob, count, _center, radius = posed_gl(joints)
|
||||
samples.append(time.perf_counter() - started)
|
||||
assert count > 1000
|
||||
assert len(blob) == count * 24
|
||||
assert radius > 0.2
|
||||
samples.sort()
|
||||
assert samples[len(samples) // 2] < 0.08
|
||||
|
||||
|
||||
def test_gpu_drag_stays_under_the_frame_budget() -> None:
|
||||
"""拖视角只改观察矩阵。以前每帧在 CPU 上重投影要 150ms，这里必须远低于这个数，并且真的画出了外壳。"""
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
pytest.importorskip("PySide6")
|
||||
display = os.environ.get("DISPLAY", "")
|
||||
if os.environ.get("QT_QPA_PLATFORM") == "offscreen" or not display.startswith(":"):
|
||||
pytest.skip("没有可用的显示，OpenGL 控件画不出来")
|
||||
if not Path(f"/tmp/.X11-unix/X{display[1:]}").exists():
|
||||
pytest.skip("显示套接字不在")
|
||||
from PySide6.QtWidgets import QApplication
|
||||
|
||||
from arm_application.operator_app import ArmSketch
|
||||
|
||||
app = QApplication.instance() or QApplication([])
|
||||
view = ArmSketch()
|
||||
view.resize(800, 600)
|
||||
view.show()
|
||||
view.set_pose([0.2, -0.6, 0.3, 1.0, 0.0, 0.4, 0.0], True)
|
||||
frame = view.grabFramebuffer()
|
||||
assert _lit_pixels(frame) > 2000
|
||||
samples = []
|
||||
for _step in range(12):
|
||||
view._yaw += 0.2
|
||||
started = time.perf_counter()
|
||||
frame = view.grabFramebuffer()
|
||||
samples.append(time.perf_counter() - started)
|
||||
view.close()
|
||||
samples.sort()
|
||||
assert _lit_pixels(frame) > 2000
|
||||
assert samples[len(samples) // 2] < 0.02
|
||||
|
||||
|
||||
def test_base_and_flange_frames_follow_the_kinematics() -> None:
|
||||
"""基座在原点，法兰和正运动学的最后一个点是同一个点，旋转不另写一套。"""
|
||||
from arm_application.operator_arm_state import display_frames, fk_points, link_frames
|
||||
|
||||
joints = [0.2, -0.5, 0.1, 0.8, -0.2, 0.4, 0.1]
|
||||
frames = display_frames(joints)
|
||||
assert [item["name"] for item in frames] == ["基座", "法兰"]
|
||||
assert frames[0]["pos"] == (0.0, 0.0, 0.0)
|
||||
tip = fk_points(joints)[-1]
|
||||
assert max(abs(frames[1]["pos"][axis] - tip[axis]) for axis in range(3)) < 1e-9
|
||||
assert frames[1]["rot"] == link_frames(joints)["link7"][0]
|
||||
|
||||
|
||||
def test_zero_pose_flange_stands_above_the_base() -> None:
|
||||
from arm_application.operator_arm_state import display_frames
|
||||
|
||||
frames = display_frames([0.0] * 7)
|
||||
assert frames[1]["pos"][2] > frames[0]["pos"][2] + 0.5
|
||||
|
||||
|
||||
def test_surface_height_is_not_a_workpiece_frame() -> None:
|
||||
"""上表面只有一个 z。把它画成坐标系会编出原点和朝向。"""
|
||||
from arm_application.operator_arm_state import workpiece_frame
|
||||
from arm_application.site_io import load_site_dict
|
||||
|
||||
assert workpiece_frame({"gauge_assembly": {"workpiece_surface_z_m": 0.022}}) is None
|
||||
heights = []
|
||||
for robot in (load_site_dict().get("robot") or {}).get("robots") or []:
|
||||
gauge = robot.get("gauge_assembly") or {}
|
||||
if not isinstance(gauge, dict):
|
||||
continue
|
||||
if "workpiece_frame" in gauge:
|
||||
raise AssertionError("现场多了工件坐标系，要改成断言法兰切到工件上")
|
||||
height = gauge.get("workpiece_surface_z_m")
|
||||
if isinstance(height, (int, float)) and not isinstance(height, bool) and float(height) != 0.0:
|
||||
heights.append(float(height))
|
||||
assert workpiece_frame(robot) is None
|
||||
assert heights
|
||||
|
||||
|
||||
def test_a_real_workpiece_frame_replaces_the_flange() -> None:
|
||||
import pytest
|
||||
|
||||
from arm_application.operator_arm_state import display_frames, workpiece_frame
|
||||
|
||||
robot = {
|
||||
"gauge_assembly": {
|
||||
"workpiece_surface_z_m": 0.022,
|
||||
"workpiece_frame": {"xyz": [0.3, -0.1, 0.022], "rpy": [0.0, 0.0, 1.2]},
|
||||
}
|
||||
}
|
||||
workpiece = workpiece_frame(robot)
|
||||
assert workpiece is not None
|
||||
frames = display_frames([0.2, -0.4, 0.0, 0.6, 0.0, 0.2, 0.0], workpiece)
|
||||
assert [item["name"] for item in frames] == ["基座", "工件"]
|
||||
assert frames[1]["pos"] == (0.3, -0.1, 0.022)
|
||||
flange = display_frames([0.2, -0.4, 0.0, 0.6, 0.0, 0.2, 0.0])[1]["pos"]
|
||||
assert frames[1]["pos"] != flange
|
||||
with pytest.raises(ValueError, match="工件坐标系"):
|
||||
workpiece_frame({"gauge_assembly": {"workpiece_frame": {"xyz": [0.0, 0.0, 0.02]}}})
|
||||
|
||||
|
||||
def test_ground_grid_lies_on_the_floor() -> None:
|
||||
"""RViz 的地面格子改画在仿真里。线必须在 z=0，间距写坏时要报错。"""
|
||||
import pytest
|
||||
|
||||
from arm_application.operator_arm_mesh import ground_grid
|
||||
|
||||
lines = ground_grid()
|
||||
assert len(lines) == 18
|
||||
assert all(start[2] == 0.0 and end[2] == 0.0 for start, end in lines)
|
||||
assert ((-1.0, 0.0, 0.0), (1.0, 0.0, 0.0)) in lines
|
||||
assert ((0.0, -1.0, 0.0), (0.0, 1.0, 0.0)) in lines
|
||||
with pytest.raises(ValueError, match="地平面格子"):
|
||||
ground_grid(step=0)
|
||||
|
||||
|
||||
def test_launch_defaults_do_not_open_rviz() -> None:
|
||||
"""入口脚本和 MoveIt 启动默认不再拉起 RViz。改回 true 这里会红。"""
|
||||
from pathlib import Path
|
||||
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
assert "use_rviz:=false" in (root / "scripts/run_real.sh").read_text(encoding="utf-8")
|
||||
assert "use_rviz:=false" in (root / "scripts/run_sim.sh").read_text(encoding="utf-8")
|
||||
move_group = (root / "src/arm_moveit_config/launch/move_group.launch.py").read_text(encoding="utf-8")
|
||||
assert "return [move_group, rviz]" not in move_group
|
||||
assert "default_value='false'" in move_group
|
||||
|
||||
|
||||
def test_screen_projection_matches_the_shader() -> None:
|
||||
"""坐标系和网格用同一套投影。+Z 在画面上方，基座落在地平面上。改回 ``-p.z`` 会红。"""
|
||||
from arm_application.operator_app import _MESH_VERT
|
||||
from arm_application.operator_arm_mesh import axis_tip, screen_point
|
||||
|
||||
assert "p.z * u_sy" in _MESH_VERT
|
||||
assert "-p.z * u_sy" not in _MESH_VERT
|
||||
origin_x, origin_y = screen_point((0.0, 0.0, 0.0), 0.0, 0.0, (0.0, 0.0, 0.0), 1.0, 800, 600)
|
||||
_zx, z_y = screen_point((0.0, 0.0, 0.2), 0.0, 0.0, (0.0, 0.0, 0.0), 1.0, 800, 600)
|
||||
x_x, _xy = screen_point((0.2, 0.0, 0.0), 0.0, 0.0, (0.0, 0.0, 0.0), 1.0, 800, 600)
|
||||
assert abs(origin_x - 400) < 1.0
|
||||
assert abs(origin_y - 300) < 1.0
|
||||
assert z_y < origin_y - 5.0
|
||||
assert x_x > origin_x + 5.0
|
||||
ident = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))
|
||||
assert axis_tip((0.0, 0.0, 0.0), ident, 2, 0.15) == (0.0, 0.0, 0.15)
|
||||
|
||||
|
||||
def test_double_click_resets_the_view_and_leaves_the_joints() -> None:
|
||||
"""双击只把观察角放回初始值。实时关节继续画，不能被改成零位。"""
|
||||
import os
|
||||
|
||||
import pytest
|
||||
|
||||
pytest.importorskip("PySide6")
|
||||
os.environ.setdefault("QT_QPA_PLATFORM", "offscreen")
|
||||
from PySide6.QtCore import QEvent, QPointF, Qt
|
||||
from PySide6.QtGui import QMouseEvent
|
||||
from PySide6.QtWidgets import QApplication
|
||||
|
||||
from arm_application.operator_app import _VIEW_PITCH, _VIEW_YAW, ArmSketch
|
||||
|
||||
_app = QApplication.instance() or QApplication([])
|
||||
view = ArmSketch()
|
||||
bent = [0.4, -0.8, 0.2, 1.0, 0.1, 0.3, -0.2]
|
||||
view.set_pose(bent, True)
|
||||
assert view._frames[1]["name"] == "法兰"
|
||||
view._yaw = 1.7
|
||||
view._pitch = 0.4
|
||||
event = QMouseEvent(
|
||||
QEvent.Type.MouseButtonDblClick,
|
||||
QPointF(30, 30),
|
||||
Qt.MouseButton.LeftButton,
|
||||
Qt.MouseButton.LeftButton,
|
||||
Qt.KeyboardModifier.NoModifier,
|
||||
)
|
||||
view.mouseDoubleClickEvent(event)
|
||||
assert view._yaw == _VIEW_YAW
|
||||
assert view._pitch == _VIEW_PITCH
|
||||
assert view._pose_key[0] == tuple(bent)
|
||||
assert "复位" in view._caption()
|
||||
view.mouseDoubleClickEvent(event)
|
||||
assert view._yaw == _VIEW_YAW
|
||||
assert view._pitch == _VIEW_PITCH
|
||||
assert view._pose_key[0][1] == -0.8
|
||||
view.close()
|
||||
|
||||
|
||||
def _lit_pixels(image) -> int:
|
||||
count = 0
|
||||
for y in range(0, image.height(), 3):
|
||||
for x in range(0, image.width(), 3):
|
||||
color = image.pixelColor(x, y)
|
||||
if abs(color.red() - 12) + abs(color.green() - 14) + abs(color.blue() - 18) > 24:
|
||||
count += 1
|
||||
return count
|
||||
@@ -0,0 +1,218 @@
|
||||
"""机械臂状态页：使能结论和零位外形不连真机也能红。"""
|
||||
|
||||
from arm_application.operator_arm_state import (
|
||||
REMOTE_PROMPT,
|
||||
_parse_joint_echo,
|
||||
_probe_cmd,
|
||||
control_holds_rdk,
|
||||
fk_points,
|
||||
motion_blocked,
|
||||
present,
|
||||
release_for_control,
|
||||
resume_after_control,
|
||||
retain_pose,
|
||||
)
|
||||
|
||||
|
||||
def test_manual_mode_asks_for_remote_and_is_not_enabled() -> None:
|
||||
row = present(
|
||||
"89",
|
||||
stack_up=False,
|
||||
bridge_up=False,
|
||||
probe={
|
||||
"connected": True,
|
||||
"operational": False,
|
||||
"op_status": "OperationalStatus.IN_MANUAL_MODE",
|
||||
"mode": "Mode.IDLE",
|
||||
"level": "fail",
|
||||
"summary": "示教器在 Manual",
|
||||
"fault": False,
|
||||
"estop_released": True,
|
||||
},
|
||||
joints=None,
|
||||
)
|
||||
assert row["enabled"] is False
|
||||
assert row["prompt_hot"] is True
|
||||
assert "Remote" in row["prompt"]
|
||||
assert row["prompt"] == REMOTE_PROMPT
|
||||
assert row["pose_live"] is False
|
||||
|
||||
|
||||
def test_operational_is_enabled_but_remote_line_stays() -> None:
|
||||
row = present(
|
||||
"89",
|
||||
stack_up=False,
|
||||
bridge_up=False,
|
||||
probe={
|
||||
"connected": True,
|
||||
"operational": True,
|
||||
"op_status": "OperationalStatus.READY",
|
||||
"mode": "Mode.IDLE",
|
||||
"level": "ok",
|
||||
"summary": "已使能",
|
||||
"fault": False,
|
||||
"estop_released": True,
|
||||
"q": [0.0] * 7,
|
||||
},
|
||||
joints=None,
|
||||
)
|
||||
assert row["enabled"] is True
|
||||
assert row["prompt_hot"] is False
|
||||
assert "Remote" in row["prompt"]
|
||||
assert row["pose_live"] is True
|
||||
assert len(row["points"]) == 9
|
||||
|
||||
|
||||
def test_stack_without_joints_tells_you_to_enable() -> None:
|
||||
row = present("91", stack_up=True, bridge_up=False, probe=None, joints=None)
|
||||
assert row["enabled"] is False
|
||||
assert row["prompt_hot"] is True
|
||||
assert "operational" in row["summary"]
|
||||
|
||||
|
||||
def test_zero_pose_stands_up() -> None:
|
||||
points = fk_points([0.0] * 7)
|
||||
assert len(points) == 9
|
||||
assert points[-1][2] > 0.8
|
||||
assert all(len(p) == 3 and all(v == v for v in p) for p in points)
|
||||
|
||||
|
||||
def test_failed_poll_keeps_joints_briefly_then_drops_them() -> None:
|
||||
previous = present(
|
||||
"89",
|
||||
stack_up=False,
|
||||
bridge_up=False,
|
||||
probe={"connected": True, "operational": True, "q": [0.2] * 7, "level": "ok", "summary": "已使能"},
|
||||
joints=None,
|
||||
)
|
||||
missed = present("89", stack_up=False, bridge_up=False, probe=None, joints=None, error="读 RDK 超时")
|
||||
held = retain_pose(missed, previous, now=previous["stamp"] + 1.0)
|
||||
assert held["q"] == [0.2] * 7
|
||||
assert held["pose_live"] is True
|
||||
assert "沿用上次读数" in held["summary"]
|
||||
dropped = retain_pose(missed, previous, now=previous["stamp"] + 30.0)
|
||||
assert dropped["q"] is None
|
||||
assert dropped["pose_live"] is False
|
||||
|
||||
|
||||
def test_control_hold_keeps_status_probe_off_until_released() -> None:
|
||||
assert control_holds_rdk() is False
|
||||
release_for_control()
|
||||
release_for_control()
|
||||
assert control_holds_rdk() is True
|
||||
resume_after_control()
|
||||
assert control_holds_rdk() is True
|
||||
resume_after_control()
|
||||
assert control_holds_rdk() is False
|
||||
|
||||
|
||||
def test_arm_motion_says_why_when_the_stack_is_down() -> None:
|
||||
text = motion_blocked(
|
||||
"reset_home",
|
||||
stack_up=False,
|
||||
op_status="OperationalStatus.IN_MANUAL_MODE",
|
||||
)
|
||||
assert "Manual" in text
|
||||
assert "上电控制栈" in text
|
||||
assert motion_blocked("reset_home", stack_up=True, op_status="IN_MANUAL_MODE") == ""
|
||||
assert motion_blocked("gripper_open", stack_up=False, op_status="IN_MANUAL_MODE") == ""
|
||||
screw = motion_blocked("stage_screw", stack_up=False, op_status="IN_MANUAL_MODE")
|
||||
assert "上电控制栈" in screw
|
||||
assert motion_blocked("stage_screw", stack_up=True, op_status="IN_MANUAL_MODE") == ""
|
||||
|
||||
|
||||
def test_probe_command_passes_pc_ip_not_only_iface_name() -> None:
|
||||
cmd = _probe_cmd("/tmp/python", "/tmp/rdk_status.py", "Rizon4-063989", "eth0", "192.168.2.10")
|
||||
assert "--iface" in cmd and "eth0" in cmd
|
||||
assert cmd[cmd.index("--pc-ip") + 1] == "192.168.2.10"
|
||||
|
||||
|
||||
def test_joint_echo_picks_named_joints() -> None:
|
||||
text = """
|
||||
header:
|
||||
stamp: 0
|
||||
name:
|
||||
- joint1
|
||||
- joint2
|
||||
- finger
|
||||
- joint3
|
||||
- joint4
|
||||
- joint5
|
||||
- joint6
|
||||
- joint7
|
||||
position:
|
||||
- 0.1
|
||||
- 0.2
|
||||
- 9.9
|
||||
- 0.3
|
||||
- 0.4
|
||||
- 0.5
|
||||
- 0.6
|
||||
- 0.7
|
||||
---
|
||||
"""
|
||||
q = _parse_joint_echo(text)
|
||||
assert q == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
|
||||
|
||||
|
||||
def test_screw_stage_uses_site_pitch_and_refuses_zero(tmp_path) -> None:
|
||||
"""旋转装配的深度和螺距只从 site 读。写成 0 时脚本必须失败，不能当成搜孔轻压混过去。"""
|
||||
import os
|
||||
import subprocess
|
||||
|
||||
from arm_application.operator_console import catalog
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
action = next(item for item in catalog()["actions"] if item["id"] == "stage_screw")
|
||||
assert action["moves"] is True
|
||||
assert action["args"][-1] == "--screw"
|
||||
root = ros2_root()
|
||||
site = (root / "config/site.yaml").read_text(encoding="utf-8")
|
||||
assert "screw_stage_depth_mm: 6.0" in site
|
||||
assert "screw_stage_pitch_mm: 1.0" in site
|
||||
broken = tmp_path / "site.yaml"
|
||||
broken.write_text(site.replace("screw_stage_pitch_mm: 1.0", "screw_stage_pitch_mm: 0.0"), encoding="utf-8")
|
||||
env = os.environ.copy()
|
||||
env["SITE_CONFIG"] = str(broken)
|
||||
proc = subprocess.run(
|
||||
["bash", str(root / "scripts/run_stage3_search.sh"), "--arm", "89", "--screw"],
|
||||
cwd=root,
|
||||
env=env,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=90,
|
||||
check=False,
|
||||
)
|
||||
assert proc.returncode != 0
|
||||
assert "旋不进去" in proc.stderr
|
||||
|
||||
|
||||
def test_search_stage_is_refused() -> None:
|
||||
"""搜孔轻压不能从界面动作、单独脚本或整体流程里再打开。"""
|
||||
import subprocess
|
||||
|
||||
from arm_application.operator_console import catalog
|
||||
from arm_application.site_io import ros2_root
|
||||
|
||||
assert "stage_search" not in {item["id"] for item in catalog()["actions"]}
|
||||
root = ros2_root()
|
||||
script = subprocess.run(
|
||||
["bash", str(root / "scripts/run_stage3_search.sh"), "--arm", "89"],
|
||||
cwd=root,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=90,
|
||||
check=False,
|
||||
)
|
||||
assert script.returncode != 0
|
||||
assert "搜孔轻压已取消" in script.stderr
|
||||
overall = subprocess.run(
|
||||
["bash", str(root / "scripts/run_all.sh"), "--arm", "89", "--depth", "6"],
|
||||
cwd=root,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=30,
|
||||
check=False,
|
||||
)
|
||||
assert overall.returncode != 0
|
||||
assert "搜孔轻压已取消" in overall.stderr
|
||||
@@ -0,0 +1,61 @@
|
||||
"""平行爪外壳按 PGEA-100-26-W-F 尺寸图，开口仍按现场 20 mm，不拿图上的 26 mm。"""
|
||||
|
||||
from arm_application.operator_gripper_mesh import PGEA_SIZE, RGI_SIZE, triangles
|
||||
|
||||
|
||||
def _extent(tris):
|
||||
pts = [p for tri in tris for p in tri[:3]]
|
||||
return tuple(max(p[i] for p in pts) - min(p[i] for p in pts) for i in range(3))
|
||||
|
||||
|
||||
def _inner_gap(tris, front: float) -> float:
|
||||
ys = [p[1] for tri in tris for p in tri[:3] if p[0] > front]
|
||||
return min(y for y in ys if y >= 0.0) - max(y for y in ys if y <= 0.0)
|
||||
|
||||
|
||||
def test_pgea_envelope_is_the_braked_catalog_body() -> None:
|
||||
tris, note = triangles(False, 0, 0, 20)
|
||||
ext = _extent(tris)
|
||||
assert abs(ext[0] - PGEA_SIZE[0]) < 1e-9
|
||||
assert abs(ext[1] - PGEA_SIZE[1]) < 1e-9
|
||||
assert abs(ext[2] - PGEA_SIZE[2]) < 1e-9
|
||||
assert "138" in note and "20" in note
|
||||
|
||||
|
||||
def test_pgea_full_open_uses_site_stroke_not_26mm() -> None:
|
||||
shut, _ = triangles(False, 0, None, 20)
|
||||
opened, _ = triangles(False, 1000, None, 20)
|
||||
assert abs(_inner_gap(opened, 0.12) - _inner_gap(shut, 0.12) - 0.020) < 1e-9
|
||||
|
||||
|
||||
def test_parts_stay_distinguishable_on_a_dark_view() -> None:
|
||||
"""壳体、抱闸、手指、导轨必须是对比色。黑手指在深色底上会消失。"""
|
||||
from arm_application.operator_gripper_mesh import _BODY, _BRAKE, _FINGER, _RAIL
|
||||
|
||||
colors = (_BODY, _BRAKE, _FINGER, _RAIL)
|
||||
for index, left in enumerate(colors):
|
||||
for right in colors[index + 1 :]:
|
||||
assert max(abs(left[channel] - right[channel]) for channel in range(3)) >= 0.35
|
||||
assert min(_BODY) > 0.75
|
||||
assert _FINGER[0] > 0.85 and _FINGER[0] > _FINGER[2] + 0.4
|
||||
assert _BRAKE[2] > 0.75 and _BRAKE[2] > _BRAKE[0] + 0.4
|
||||
tris, _note = triangles(False, 500, None, 20)
|
||||
used = {tri[4] for tri in tris}
|
||||
assert {_BODY, _BRAKE, _FINGER, _RAIL} <= used
|
||||
shades = {tri[3] for tri in tris}
|
||||
assert max(shades) - min(shades) > 0.2
|
||||
|
||||
|
||||
def test_rgi_envelope_and_spin() -> None:
|
||||
shut, note = triangles(True, 0, 0, 30)
|
||||
ext = _extent(shut)
|
||||
assert abs(ext[0] - RGI_SIZE[0]) < 1e-9
|
||||
assert abs(ext[1] - RGI_SIZE[1]) < 1e-9
|
||||
assert abs(ext[2] - RGI_SIZE[2]) < 1e-9
|
||||
assert "158" in note
|
||||
spun, _ = triangles(True, 0, 90, 30)
|
||||
|
||||
def front_z(tris):
|
||||
return max(abs(p[2]) for tri in tris for p in tri[:3] if p[0] > 0.13)
|
||||
|
||||
assert abs(front_z(shut) - front_z(spun)) > 0.005
|
||||
@@ -0,0 +1,128 @@
|
||||
"""夹爪状态页：开口和转角不连串口也能红。"""
|
||||
|
||||
from arm_application.operator_gripper_state import (
|
||||
_parse_states_echo,
|
||||
_parse_status_echo,
|
||||
pause_serial,
|
||||
present,
|
||||
resume_serial,
|
||||
retain_reading,
|
||||
serial_paused,
|
||||
)
|
||||
|
||||
|
||||
def test_closed_jaw_is_zero_opening() -> None:
|
||||
row = present(
|
||||
"89",
|
||||
rotary=False,
|
||||
stroke_mm=20.0,
|
||||
reading={"connected": True, "permille": 0, "grip_state": 1, "initialized": True},
|
||||
)
|
||||
assert row["connected"] is True
|
||||
assert row["opening_mm"] == 0.0
|
||||
assert row["pose_live"] is True
|
||||
assert row["grip_text"] == "已到位"
|
||||
|
||||
|
||||
def test_open_jaw_uses_stroke() -> None:
|
||||
row = present(
|
||||
"89",
|
||||
rotary=False,
|
||||
stroke_mm=20.0,
|
||||
reading={"connected": True, "permille": 1000, "grip_state": 1, "force": 20},
|
||||
)
|
||||
assert row["opening_mm"] == 20.0
|
||||
assert row["force"] == 20.0
|
||||
|
||||
|
||||
def test_rotary_keeps_angle() -> None:
|
||||
row = present(
|
||||
"91",
|
||||
rotary=True,
|
||||
stroke_mm=30.0,
|
||||
reading={
|
||||
"connected": True,
|
||||
"permille": 500,
|
||||
"rotation_deg": 90,
|
||||
"rotation_state": 1,
|
||||
"grip_state": 2,
|
||||
},
|
||||
)
|
||||
assert row["opening_mm"] == 15.0
|
||||
assert row["rotation_deg"] == 90.0
|
||||
assert row["rotation_text"] == "已到位"
|
||||
assert row["gripped"] is True
|
||||
|
||||
|
||||
def test_missing_read_is_not_a_live_pose() -> None:
|
||||
row = present("89", rotary=False, stroke_mm=20.0, reading=None, error="串口没有应答")
|
||||
assert row["connected"] is False
|
||||
assert row["pose_live"] is False
|
||||
assert row["opening_mm"] is None
|
||||
assert "应答" in row["summary"]
|
||||
|
||||
|
||||
def test_missed_echo_keeps_opening_briefly_then_drops_it() -> None:
|
||||
previous = present(
|
||||
"89",
|
||||
rotary=False,
|
||||
stroke_mm=20.0,
|
||||
reading={"connected": True, "permille": 939, "grip_state": 1, "initialized": True},
|
||||
)
|
||||
missed = present("89", rotary=False, stroke_mm=20.0, reading=None, error="话题没有数据")
|
||||
held = retain_reading(missed, previous, now=previous["stamp"] + 1.0)
|
||||
assert held["connected"] is True
|
||||
assert held["permille"] == 939
|
||||
assert held["pose_live"] is True
|
||||
assert "沿用上次读数" in held["summary"]
|
||||
dropped = retain_reading(missed, previous, now=previous["stamp"] + 30.0)
|
||||
assert dropped["connected"] is False
|
||||
assert dropped["permille"] is None
|
||||
|
||||
|
||||
def test_gripper_failure_message_keeps_the_script_reason() -> None:
|
||||
from arm_application.operator_console import _grip_failure_message
|
||||
|
||||
detail = "准备\nERROR: /dev/ttyUSB0 不在。USB–RS485 没插上，夹爪驱动起不来。\n"
|
||||
assert _grip_failure_message(1, detail).startswith("ERROR:")
|
||||
assert "不在" in _grip_failure_message(1, detail)
|
||||
assert _grip_failure_message(1, "没有原因") == "夹爪退出码 1"
|
||||
|
||||
|
||||
def test_serial_pause_blocks_a_second_open() -> None:
|
||||
assert serial_paused() is False
|
||||
pause_serial()
|
||||
assert serial_paused() is True
|
||||
resume_serial()
|
||||
assert serial_paused() is False
|
||||
|
||||
|
||||
def test_parallel_topic_array() -> None:
|
||||
text = "data:\n- 1.0\n- 2.0\n- 250.0\n- 250.0\n- 40.0\n---\n"
|
||||
parsed = _parse_states_echo(text)
|
||||
assert parsed is not None
|
||||
assert parsed["permille"] == 250.0
|
||||
assert parsed["gripped"] is True
|
||||
assert parsed["initialized"] is True
|
||||
|
||||
|
||||
def test_rotary_topic_yaml() -> None:
|
||||
text = """
|
||||
connected: true
|
||||
initialized: true
|
||||
is_moving: false
|
||||
is_gripped: false
|
||||
finger_position_per_mille: 800.0
|
||||
rotation_deg: -15.5
|
||||
gripper_state: 1
|
||||
rotation_state: 1
|
||||
force: 33.0
|
||||
velocity: 25.0
|
||||
error_msg: ''
|
||||
---
|
||||
"""
|
||||
parsed = _parse_status_echo(text)
|
||||
assert parsed is not None
|
||||
assert parsed["permille"] == 800.0
|
||||
assert parsed["rotation_deg"] == -15.5
|
||||
assert parsed["speed"] == 25.0
|
||||
Reference in New Issue
Block a user