主界面一屏要同时看臂、夹爪、相机和触觉，按钮再被拉高就会叠到画面上。相机按原分辨率走回调取最新帧，场景一动不再等红外齐帧把彩色丢掉。模型和相机都留在工位窗口里，启动默认不再弹 RViz。
验收：test_listed_arms 与 test_camera_preview 共 20 项通过。D435 1280×720 发布约 29 帧/秒。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -213,6 +213,34 @@ WIRING_HINT = (
|
||||
)
|
||||
|
||||
|
||||
def open_failure_text(port: str, exc: BaseException) -> str:
|
||||
"""串口打不开时把「被占用」和「没应答」分开。
|
||||
|
||||
独占锁失败（errno 11）以前和真没上电写成同一句「需要 24 V DC」，
|
||||
界面上看起来像缺自动上电，实际是别的进程还握着这个口。
|
||||
"""
|
||||
text = str(exc).lower()
|
||||
errno = getattr(exc, "errno", None)
|
||||
busy = errno in (11, 16) or any(
|
||||
token in text
|
||||
for token in (
|
||||
"temporarily unavailable",
|
||||
"device or resource busy",
|
||||
"could not exclusively lock",
|
||||
"resource busy",
|
||||
)
|
||||
)
|
||||
if busy:
|
||||
return (
|
||||
f"无法独占打开 {port}：{exc}。"
|
||||
"串口已被别的进程占用（夹爪驱动或状态读取）。"
|
||||
)
|
||||
return (
|
||||
f"无法打开 {port}：{exc}。"
|
||||
"需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
|
||||
)
|
||||
|
||||
|
||||
class DhPgeaGripper:
|
||||
"""面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
|
||||
|
||||
@@ -257,10 +285,7 @@ class DhPgeaGripper:
|
||||
exclusive=True,
|
||||
)
|
||||
except Exception as exc:
|
||||
raise DhPgeaError(
|
||||
f"无法打开 {self.port}：{exc}。"
|
||||
"需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
|
||||
) from exc
|
||||
raise DhPgeaError(open_failure_text(self.port, exc)) from exc
|
||||
try:
|
||||
self._ser.dtr = True
|
||||
except Exception:
|
||||
@@ -562,6 +587,36 @@ def _scan_bus(port: str) -> int:
|
||||
return 1
|
||||
|
||||
|
||||
def ctrl_values(
|
||||
cmd: str,
|
||||
value: str = "",
|
||||
force: int = 50,
|
||||
speed: int = 50,
|
||||
) -> Optional[tuple[float, float, float, float]]:
|
||||
"""节点已经占着串口时，改发 ``/gripper/ctrl`` 的四个数。
|
||||
|
||||
顺序是初始化、位置千分比、力度、速度。``probe`` / ``status`` 只读，返回 None。
|
||||
"""
|
||||
name = str(cmd or "").strip().lower()
|
||||
if name in ("probe", "status"):
|
||||
return None
|
||||
if name == "init":
|
||||
return (1.0, 0.0, float(force), float(speed))
|
||||
if name == "open":
|
||||
pos = 1000
|
||||
elif name == "close":
|
||||
pos = 0
|
||||
elif name in ("pos", "move"):
|
||||
if str(value).strip() == "":
|
||||
raise ValueError("pos 需要 0–1000")
|
||||
pos = int(round(float(value)))
|
||||
if pos < 0 or pos > 1000:
|
||||
raise ValueError("位置只接受 0–1000")
|
||||
else:
|
||||
raise ValueError(f"节点在线时不支持 {name}")
|
||||
return (0.0, float(pos), float(force), float(speed))
|
||||
|
||||
|
||||
def _cli(argv: Optional[list[str]] = None) -> int:
|
||||
site = _site_defaults()
|
||||
ap = argparse.ArgumentParser(
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -2,9 +2,11 @@
|
||||
|
||||
只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
|
||||
|
||||
回调里先丢掉当前页没在看的流。彩色和深度都缩成宽不超过 480 的 RGB，
|
||||
界面直接贴这块小图。不再每帧把整幅图拷进 Python，也不在界面线程里解 JPEG。
|
||||
手眼页单独要一张更宽的 JPEG，只在那一页、那一路彩色上编码。
|
||||
回调里先丢掉当前页没在看的流。彩色按相机原分辨率收进来，不按时钟抽帧，
|
||||
也不在收图的线程里做颜色转换。另一条线只画最新的一张，所以帧率跟相机走，
|
||||
848 和 1280 都不会为了帧率被缩成 480。深度是伪彩色，仍然抽稀，
|
||||
而且没打开深度时不订阅，避免大图挤占彩色。
|
||||
手眼页要 JPEG 时也按原分辨率编码，只编正在看的那一路。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -15,12 +17,15 @@ import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
# 主界面同时看几路彩色。480 宽够看清，界面缩放的代价也小。
|
||||
_PREVIEW_WIDTH = 480
|
||||
# 手眼要在这张图上找棋盘，比预览宽一档，但仍然不把 1280 原图编码。
|
||||
_JPEG_WIDTH = 960
|
||||
# 主界面要看清运动和推理框，10Hz 太顿。0.05s 约 20 帧。
|
||||
_INTERVAL = 0.05
|
||||
# 彩色上限。D405 是 848×480，D435 是 1280×720，都不超过它，所以不会被缩小。
|
||||
_COLOR_WIDTH = 1280
|
||||
# 深度上色不是要看的画质。全幅每帧上色会把刷新占满，所以这一路仍抽稀。
|
||||
_DEPTH_WIDTH = 480
|
||||
# 手眼只在正在看的那一页编码，宽度跟彩色预览一样，不另缩一档。
|
||||
_JPEG_WIDTH = 1280
|
||||
# 只留给还在用 ``accept_frame`` 的调用。彩色实况不再用它抽帧：
|
||||
# 相机大约 30 帧，再按这个间隔丢一次，画面会掉到十几帧。
|
||||
_INTERVAL = 1.0 / 30.0
|
||||
_DET_HOLD_S = 1.5
|
||||
|
||||
_LOCK = threading.Lock()
|
||||
@@ -34,6 +39,16 @@ _NEXT: dict[str, float] = {}
|
||||
_DET: dict[str, tuple[float, list[float]]] = {}
|
||||
# 没画框的预览。检测一到就在这张上重画，不必等下一帧相机。
|
||||
_CLEAN: dict[str, dict] = {}
|
||||
# 每路最近一秒的到达时刻，用来算帧率。值是 (宽, 高, 帧/秒)，宽高是相机原图。
|
||||
_META: dict[str, tuple[int, int, float]] = {}
|
||||
_STAMPS: dict[str, list[float]] = {}
|
||||
# 每路最新原图。收图线程只往这里放，画图线程只取最新的一张。
|
||||
_RAW: dict[str, dict] = {}
|
||||
_SLOTS: dict[str, list] = {}
|
||||
_SLOT_AT: dict[str, int] = {}
|
||||
_PAINTED: dict[str, int] = {}
|
||||
_WAKE = threading.Event()
|
||||
_PAINT_THREAD: threading.Thread | None = None
|
||||
_STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
|
||||
|
||||
|
||||
@@ -63,13 +78,60 @@ def preview_targets(site: dict) -> list[dict]:
|
||||
return found
|
||||
|
||||
|
||||
def sim_preview_targets(site: dict) -> list[dict]:
|
||||
"""仿真腕部和仿真手外。画面进主界面，不另开 Gazebo 的相机窗。"""
|
||||
from arm_application.site_io import overhead_camera
|
||||
|
||||
found: list[dict] = []
|
||||
robots = ((site or {}).get("robot") or {}).get("robots") or []
|
||||
cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
|
||||
for robot in robots:
|
||||
if not isinstance(robot, dict):
|
||||
continue
|
||||
if str(robot.get("sn") or "").strip().lower() != "sim":
|
||||
continue
|
||||
cam = robot.get("camera") if isinstance(robot.get("camera"), dict) else {}
|
||||
name = str((cam or {}).get("name") or "").strip().strip("/")
|
||||
if not name:
|
||||
continue
|
||||
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"
|
||||
found.append({"id": name, "label": f"仿真左腕 {tag}", "role": "wrist", "source": "sim"})
|
||||
break
|
||||
overhead, _others = overhead_camera(site or {}, {"arm": "sim"})
|
||||
oname = str((overhead or {}).get("name") or "").strip().strip("/")
|
||||
if oname.startswith("sim"):
|
||||
tag = str((overhead or {}).get("model") or "D435").strip().upper() or "D435"
|
||||
found.append({"id": oname, "label": f"仿真手外 {tag}", "role": "overhead", "source": "sim"})
|
||||
return found
|
||||
|
||||
|
||||
def stream_targets(site: dict | None = None) -> list[dict]:
|
||||
"""真机和仿真都订上。主界面切换看哪一组，没在看的那组回调里直接丢掉。"""
|
||||
if site is None:
|
||||
from arm_application.operator_console import _site
|
||||
|
||||
site = _site()
|
||||
real = preview_targets(site)
|
||||
for item in real:
|
||||
item["source"] = "real"
|
||||
seen = {item["id"] for item in real}
|
||||
return real + [item for item in sim_preview_targets(site) if item["id"] not in seen]
|
||||
|
||||
|
||||
def targets() -> list[dict]:
|
||||
"""当前站点的主界面相机。规则在 ``preview_targets``。"""
|
||||
"""当前站点的主界面真机相机。规则在 ``preview_targets``。"""
|
||||
from arm_application.operator_console import _site
|
||||
|
||||
return preview_targets(_site())
|
||||
|
||||
|
||||
def sim_targets() -> list[dict]:
|
||||
from arm_application.operator_console import _site
|
||||
|
||||
return sim_preview_targets(_site())
|
||||
|
||||
|
||||
def state() -> dict:
|
||||
with _LOCK:
|
||||
return {
|
||||
@@ -91,10 +153,32 @@ def previews() -> dict[str, tuple[bytes, int, int]]:
|
||||
return dict(_PREVIEW)
|
||||
|
||||
|
||||
def preview_meta() -> dict[str, tuple[int, int, float]]:
|
||||
"""每路相机原图的宽、高，以及最近一秒的帧率。还没来帧就是空。"""
|
||||
with _LOCK:
|
||||
return dict(_META)
|
||||
|
||||
|
||||
def _mark_rate(key: str, width: int, height: int) -> None:
|
||||
"""记下这一帧的原图尺寸，并用最近一秒的到达间隔算帧率。"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
stamps = _STAMPS.setdefault(key, [])
|
||||
stamps.append(now)
|
||||
cut = now - 1.0
|
||||
while len(stamps) > 1 and stamps[0] < cut:
|
||||
stamps.pop(0)
|
||||
if len(stamps) >= 2 and stamps[-1] > stamps[0]:
|
||||
fps = (len(stamps) - 1) / (stamps[-1] - stamps[0])
|
||||
else:
|
||||
fps = 0.0
|
||||
_META[key] = (int(width), int(height), float(fps))
|
||||
|
||||
|
||||
def watch(preview: set[str], jpeg: set[str] | None = None) -> None:
|
||||
"""当前页要哪几路。没点名的流在回调里直接丢掉，不拷图像。
|
||||
|
||||
``preview`` 走 RGB 小图，``jpeg`` 走手眼用的 JPEG。两边都空着就只保持订阅。
|
||||
``preview`` 走原分辨率 RGB，``jpeg`` 走手眼用的 JPEG。两边都空着就只保持订阅。
|
||||
"""
|
||||
global _PREVIEW_WANT, _JPEG_WANT
|
||||
jpeg = set(jpeg or ())
|
||||
@@ -114,8 +198,112 @@ def wants(key: str) -> tuple[bool, bool]:
|
||||
return key in _PREVIEW_WANT, key in _JPEG_WANT
|
||||
|
||||
|
||||
def offer_color(key: str, image: np.ndarray, *, bgr: bool, role: str) -> None:
|
||||
"""收一帧彩色原图。没人看就丢掉。有人看就整幅留下，不按时钟抽帧。"""
|
||||
preview, jpeg = wants(key)
|
||||
if not preview and not jpeg:
|
||||
return
|
||||
if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
|
||||
return
|
||||
_offer(key, image, bgr=bgr, role=role, kind="color")
|
||||
|
||||
|
||||
def offer_depth(key: str, image: np.ndarray) -> None:
|
||||
"""收一帧深度原图。伪彩色仍在画图线程里做，不堵收图。"""
|
||||
preview, _jpeg = wants(key)
|
||||
if not preview:
|
||||
return
|
||||
if image.ndim != 2 or image.shape[0] < 1 or image.shape[1] < 1:
|
||||
return
|
||||
_offer(key, image, bgr=False, role="", kind="depth")
|
||||
|
||||
|
||||
def _offer(key: str, image: np.ndarray, *, bgr: bool, role: str, kind: str) -> None:
|
||||
height, width = int(image.shape[0]), int(image.shape[1])
|
||||
with _LOCK:
|
||||
slots = _SLOTS.setdefault(key, [None, None])
|
||||
index = _SLOT_AT.get(key, 0)
|
||||
slot = slots[index]
|
||||
if slot is None or slot.shape != image.shape or slot.dtype != image.dtype:
|
||||
slot = np.empty(image.shape, dtype=image.dtype)
|
||||
slots[index] = slot
|
||||
np.copyto(slot, image)
|
||||
seq = int(_RAW.get(key, {}).get("seq", 0)) + 1
|
||||
_RAW[key] = {
|
||||
"seq": seq,
|
||||
"frame": slot,
|
||||
"bgr": bool(bgr),
|
||||
"role": role,
|
||||
"kind": kind,
|
||||
"w": width,
|
||||
"h": height,
|
||||
}
|
||||
_SLOT_AT[key] = 1 - index
|
||||
_mark_rate(key, width, height)
|
||||
_WAKE.set()
|
||||
|
||||
|
||||
def publish_latest() -> None:
|
||||
"""把各路还没画的最新原图做成预览。中间帧直接跳过，避免越画越落后。"""
|
||||
with _LOCK:
|
||||
jobs = []
|
||||
for key, item in _RAW.items():
|
||||
if _PAINTED.get(key) == item["seq"]:
|
||||
continue
|
||||
jobs.append((
|
||||
key,
|
||||
int(item["seq"]),
|
||||
np.array(item["frame"], copy=True),
|
||||
bool(item["bgr"]),
|
||||
str(item["role"]),
|
||||
str(item["kind"]),
|
||||
int(item["w"]),
|
||||
int(item["h"]),
|
||||
))
|
||||
for key, seq, frame, bgr, role, kind, width, height in jobs:
|
||||
try:
|
||||
preview, jpeg = wants(key)
|
||||
if kind == "depth":
|
||||
if preview:
|
||||
colored = depth_preview(frame)
|
||||
_store(key, colored, preview=colored, jpeg=False)
|
||||
elif preview or jpeg:
|
||||
limit = _JPEG_WIDTH if jpeg else _COLOR_WIDTH
|
||||
rgb = rgb_preview(frame, bgr=bgr, limit=limit)
|
||||
shown = rgb
|
||||
if preview and int(rgb.shape[1]) > _COLOR_WIDTH:
|
||||
shown = rgb_preview(rgb, bgr=False, limit=_COLOR_WIDTH)
|
||||
if preview:
|
||||
_remember_clean(key, shown, role, width, height)
|
||||
shown = _overlay_detection(shown, role, width, height)
|
||||
_store(key, rgb, preview=shown if preview else None, jpeg=jpeg)
|
||||
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，下一帧继续
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
finally:
|
||||
with _LOCK:
|
||||
if _PAINTED.get(key, 0) < seq:
|
||||
_PAINTED[key] = seq
|
||||
|
||||
|
||||
def _paint_loop() -> None:
|
||||
while not _STOP.is_set():
|
||||
_WAKE.wait(0.005)
|
||||
_WAKE.clear()
|
||||
if _STOP.is_set():
|
||||
return
|
||||
try:
|
||||
publish_latest()
|
||||
except Exception as exc: # noqa: BLE001 —— 画图线程不能因为一帧坏掉就退出
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
|
||||
|
||||
def accept_frame(key: str) -> bool:
|
||||
"""这一帧要不要做进预览。没人看，或者距上一帧不到间隔，都返回 False。"""
|
||||
"""这一帧要不要做进预览。没人看，或者距上一帧不到间隔，都返回 False。
|
||||
|
||||
彩色实况不再走这里。固定间隔会把相机自己的帧率再砍掉一截。
|
||||
"""
|
||||
now = time.monotonic()
|
||||
with _LOCK:
|
||||
if key not in _PREVIEW_WANT and key not in _JPEG_WANT:
|
||||
@@ -253,7 +441,7 @@ def _stamp_role(role: str) -> None:
|
||||
_STATE["stamp"] = time.time()
|
||||
|
||||
|
||||
def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _PREVIEW_WIDTH) -> np.ndarray:
|
||||
def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _COLOR_WIDTH) -> np.ndarray:
|
||||
"""缩成宽不超过 ``limit`` 的 RGB。返回的数组自己持有内存，不跟调用方共用缓冲。"""
|
||||
import cv2
|
||||
|
||||
@@ -271,7 +459,7 @@ def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _PREVIEW_WIDTH) ->
|
||||
return view
|
||||
|
||||
|
||||
def depth_preview(depth: np.ndarray, limit: int = _PREVIEW_WIDTH) -> np.ndarray:
|
||||
def depth_preview(depth: np.ndarray, limit: int = _DEPTH_WIDTH) -> np.ndarray:
|
||||
"""uint16 深度画成 RGB。百分位只在抽稀后的样本上算，无效像素保持黑色。"""
|
||||
import cv2
|
||||
|
||||
@@ -374,6 +562,7 @@ def _loop(cams: list[dict]) -> None:
|
||||
self._det("overhead"),
|
||||
det_qos,
|
||||
)
|
||||
self._depth_subs: dict[str, object] = {}
|
||||
for cam in cams:
|
||||
name = cam["id"]
|
||||
role = str(cam.get("role") or "wrist")
|
||||
@@ -383,12 +572,26 @@ def _loop(cams: list[dict]) -> None:
|
||||
self._color(name, role),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/aligned_depth_to_color/image_raw",
|
||||
self._depth(name),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
|
||||
def sync_depth(self) -> None:
|
||||
"""深度没打开就不订。订上之后每一帧都会整幅拷进进程，彩色帧率会被拖下去。"""
|
||||
with _LOCK:
|
||||
want = {
|
||||
cam["id"]
|
||||
for cam in cams
|
||||
if f"{cam['id']}_depth" in _PREVIEW_WANT
|
||||
}
|
||||
for cam in cams:
|
||||
name = cam["id"]
|
||||
if name in want and name not in self._depth_subs:
|
||||
self._depth_subs[name] = self.create_subscription(
|
||||
Image,
|
||||
f"/{name}/aligned_depth_to_color/image_raw",
|
||||
self._depth(name),
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
elif name not in want and name in self._depth_subs:
|
||||
self.destroy_subscription(self._depth_subs.pop(name))
|
||||
|
||||
def _det(self, role: str):
|
||||
def _cb(msg: Float32MultiArray) -> None:
|
||||
@@ -399,24 +602,12 @@ def _loop(cams: list[dict]) -> None:
|
||||
key = f"{name}_color"
|
||||
|
||||
def _cb(msg: Image) -> None:
|
||||
# 先看这路有没有人要。30Hz 的原图不再整幅拷进 Python。
|
||||
# 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。
|
||||
try:
|
||||
if not accept_frame(key):
|
||||
return
|
||||
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)
|
||||
offer_color(key, view, bgr=(msg.encoding == "bgr8"), role=role)
|
||||
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
@@ -427,14 +618,10 @@ def _loop(cams: list[dict]) -> None:
|
||||
|
||||
def _cb(msg: Image) -> None:
|
||||
try:
|
||||
if not accept_frame(key):
|
||||
return
|
||||
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)
|
||||
offer_depth(key, view)
|
||||
except Exception as exc: # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
@@ -444,7 +631,8 @@ def _loop(cams: list[dict]) -> None:
|
||||
node = _Node()
|
||||
try:
|
||||
while not _STOP.is_set():
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
node.sync_depth()
|
||||
rclpy.spin_once(node, timeout_sec=0.01)
|
||||
except Exception as exc:
|
||||
with _LOCK:
|
||||
_STATE["error"] = str(exc)
|
||||
@@ -457,8 +645,8 @@ def _loop(cams: list[dict]) -> None:
|
||||
|
||||
|
||||
def start() -> str:
|
||||
global _THREAD
|
||||
cams = targets()
|
||||
global _THREAD, _PAINT_THREAD
|
||||
cams = stream_targets()
|
||||
if not cams:
|
||||
with _LOCK:
|
||||
_STATE["error"] = "site 里没有可预览的真机相机"
|
||||
@@ -470,14 +658,18 @@ def start() -> str:
|
||||
_STATE["on"] = True
|
||||
_STATE["error"] = ""
|
||||
_STOP.clear()
|
||||
_WAKE.clear()
|
||||
_THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view")
|
||||
_THREAD.start()
|
||||
_PAINT_THREAD = threading.Thread(target=_paint_loop, daemon=True, name="cam-paint")
|
||||
_PAINT_THREAD.start()
|
||||
names = "、".join(cam["label"] for cam in cams)
|
||||
return f"预览订了 {names} 的彩色和深度，不含左右红外。"
|
||||
return f"预览订了 {names} 的彩色原图，深度点开才订，不含左右红外。"
|
||||
|
||||
|
||||
def stop() -> None:
|
||||
_STOP.set()
|
||||
thread = _THREAD
|
||||
if thread is not None:
|
||||
thread.join(timeout=1.5)
|
||||
_WAKE.set()
|
||||
for thread in (_THREAD, _PAINT_THREAD):
|
||||
if thread is not None:
|
||||
thread.join(timeout=1.5)
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
#!/usr/bin/env python3
|
||||
"""三阶段串联：视觉跟踪锁定 → 孔定位 → 搜孔并轻轻按压（默认不拧入）。
|
||||
"""跟踪锁定 → 孔定位。搜孔轻压已取消，本节点不再下压搜孔。
|
||||
|
||||
流畅性（相对 bash 杀进程再起节点）：
|
||||
- 相机 / 触觉 / 手眼 TF 全程只起一次
|
||||
- 一阶段锁定后 SIGTERM 跟踪节点，立刻 pin 当前关节，不回零、不抬臂
|
||||
- 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完立刻下压搜孔
|
||||
- 找到孔位后力闭环轻压保持；拧入是后续阶段（--depth 才插入）
|
||||
- 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完就停，不再下压搜孔
|
||||
- 旋入不在这个节点里
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -125,7 +125,7 @@ class AssemblyPipeline(Stage3Search):
|
||||
super().__init__(node_name=node_name)
|
||||
self.declare_parameter("skip_stage1", False)
|
||||
self.declare_parameter("skip_stage2", False)
|
||||
self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth
|
||||
self.declare_parameter("skip_stage3", True) # 搜孔轻压已取消，写成 false 也不再下压
|
||||
self.declare_parameter("d435_move_only", False)
|
||||
self.declare_parameter("init_z_m", -1.0) # >0：启动后先到这个法兰高度
|
||||
self.declare_parameter("stage1_timeout_sec", 150.0)
|
||||
@@ -333,7 +333,9 @@ class AssemblyPipeline(Stage3Search):
|
||||
def run(self) -> int:
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
if not bool(self.get_parameter("skip_stage3").value):
|
||||
self.get_logger().info("搜孔轻压已取消，忽略要求重新打开它的参数")
|
||||
skip3 = True
|
||||
if self.d435_move_only:
|
||||
skip2 = True
|
||||
skip3 = True
|
||||
@@ -365,11 +367,7 @@ class AssemblyPipeline(Stage3Search):
|
||||
return code
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
|
||||
+ (
|
||||
" — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
|
||||
if skip3
|
||||
else " — 立刻下压搜孔并轻压（不拧入）"
|
||||
)
|
||||
" — 搜孔已取消，停在这里。旋入请另跑。"
|
||||
)
|
||||
else:
|
||||
self._state("skip_stage2")
|
||||
@@ -383,7 +381,7 @@ class AssemblyPipeline(Stage3Search):
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
"跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
|
||||
"搜孔已取消 —— 臂停在当前高度。"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
if final:
|
||||
|
||||
@@ -176,6 +176,10 @@ class RealsenseColorPub(TolerantNode):
|
||||
self._overlay: Optional[list] = None
|
||||
self._overlay_stamp = 0.0
|
||||
self._capture_started = False
|
||||
self._cap_lock = threading.Lock()
|
||||
self._cap_event = threading.Event()
|
||||
self._pending = None
|
||||
self._last_frame_mono = 0.0
|
||||
self._usb_desc = ''
|
||||
self._last_ok = None
|
||||
self._site_yaml = _site_yaml_path()
|
||||
@@ -309,23 +313,26 @@ class RealsenseColorPub(TolerantNode):
|
||||
self._info_pub.publish(info)
|
||||
|
||||
if depth_u16 is not None:
|
||||
dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
|
||||
dmsg = Image()
|
||||
dmsg.header.stamp = stamp
|
||||
dmsg.header.frame_id = self._frame_id
|
||||
dmsg.height = dh
|
||||
dmsg.width = dw
|
||||
dmsg.encoding = '16UC1'
|
||||
dmsg.is_bigendian = 0
|
||||
dmsg.step = dw * 2
|
||||
dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
|
||||
self._depth_pub.publish(dmsg)
|
||||
if not self._logged_depth:
|
||||
self._logged_depth = True
|
||||
self.get_logger().info(
|
||||
f'首个对齐深度 {dw}x{dh} → '
|
||||
f'/{self._name}/aligned_depth_to_color/image_raw'
|
||||
)
|
||||
self._publish_depth(depth_u16, stamp)
|
||||
|
||||
def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None:
|
||||
dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
|
||||
dmsg = Image()
|
||||
dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
|
||||
dmsg.header.frame_id = self._frame_id
|
||||
dmsg.height = dh
|
||||
dmsg.width = dw
|
||||
dmsg.encoding = '16UC1'
|
||||
dmsg.is_bigendian = 0
|
||||
dmsg.step = dw * 2
|
||||
dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
|
||||
self._depth_pub.publish(dmsg)
|
||||
if not self._logged_depth:
|
||||
self._logged_depth = True
|
||||
self.get_logger().info(
|
||||
f'首个对齐深度 {dw}x{dh} → '
|
||||
f'/{self._name}/aligned_depth_to_color/image_raw'
|
||||
)
|
||||
|
||||
def _start_pipeline(self, reconnect: bool = False) -> None:
|
||||
try:
|
||||
@@ -457,7 +464,9 @@ class RealsenseColorPub(TolerantNode):
|
||||
trial.enable_stream(
|
||||
rs.stream.infrared, 2, w, h, rs.format.y8, fps
|
||||
)
|
||||
profile = pipe.start(trial)
|
||||
# 回调里只留最新一帧。wait_for_frames 会等彩色、深度、红外齐套，
|
||||
# 场景一动红外对不齐，彩色帧就在这里被丢掉。
|
||||
profile = pipe.start(trial, self._on_sdk_frame)
|
||||
have_depth = with_depth
|
||||
have_infra = infra_mode != 'none'
|
||||
self._infra_mode = infra_mode
|
||||
@@ -508,6 +517,13 @@ class RealsenseColorPub(TolerantNode):
|
||||
|
||||
self._pipe = pipe
|
||||
self._profile = profile
|
||||
try:
|
||||
sensor = profile.get_device().first_color_sensor()
|
||||
if sensor.supports(rs.option.frames_queue_size):
|
||||
# 队列只留最新一张，避免越积越慢，动起来画面还停在旧帧。
|
||||
sensor.set_option(rs.option.frames_queue_size, 1)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
self._have_depth = have_depth
|
||||
self._have_infra = have_infra
|
||||
self._logged_depth = False
|
||||
@@ -609,7 +625,7 @@ class RealsenseColorPub(TolerantNode):
|
||||
|
||||
if not getattr(self, '_capture_started', False):
|
||||
self._capture_started = True
|
||||
self._thread = threading.Thread(target=self._capture_loop, daemon=True)
|
||||
self._thread = threading.Thread(target=self._publish_loop, daemon=True, name="rs-publish")
|
||||
self._thread.start()
|
||||
|
||||
def _enable_color_ae_fast(self, profile, rs) -> None:
|
||||
@@ -702,18 +718,60 @@ class RealsenseColorPub(TolerantNode):
|
||||
)
|
||||
return False
|
||||
|
||||
def _capture_loop(self) -> None:
|
||||
def _on_sdk_frame(self, frame) -> None:
|
||||
"""SDK 回调。只拷走彩色并握住这一组帧，立刻返回。
|
||||
|
||||
对齐、深度换算和 ROS 发布若做在这个回调里，下一帧来不及取，
|
||||
动起来时彩色帧号会跳。那些工作放到 ``_publish_loop``。
|
||||
"""
|
||||
try:
|
||||
frames = frame.as_frameset()
|
||||
color = frames.get_color_frame()
|
||||
if not color:
|
||||
return
|
||||
bgr = np.array(np.asanyarray(color.get_data()), copy=True)
|
||||
frames.keep()
|
||||
with self._cap_lock:
|
||||
self._pending = (bgr, frames)
|
||||
self._last_frame_mono = time.monotonic()
|
||||
self._cap_event.set()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
try:
|
||||
self.get_logger().warn(f'收帧：{exc}', throttle_duration_sec=2.0)
|
||||
except Exception: # noqa: BLE001
|
||||
pass
|
||||
|
||||
def _publish_loop(self) -> None:
|
||||
got = False
|
||||
misses = 0
|
||||
while self._running and rclpy.ok():
|
||||
if self._pipe is None:
|
||||
if not self._reconnect():
|
||||
time.sleep(2.0)
|
||||
continue
|
||||
if not self._cap_event.wait(0.2):
|
||||
silent = self._last_frame_mono and time.monotonic() - self._last_frame_mono > 2.0
|
||||
if silent:
|
||||
usb = getattr(self, '_usb_desc', '?')
|
||||
self.get_logger().error(
|
||||
f'相机超过 2 秒没有新帧（USB={usb}）。正在重连 —— '
|
||||
'请给 USB 线缆留点余量，别让机械臂把它拽掉。'
|
||||
)
|
||||
self._last_frame_mono = 0.0
|
||||
got = False
|
||||
self._reconnect()
|
||||
continue
|
||||
self._cap_event.clear()
|
||||
with self._cap_lock:
|
||||
item = self._pending
|
||||
self._pending = None
|
||||
if item is None:
|
||||
continue
|
||||
bgr, frames = item
|
||||
try:
|
||||
t_wait0 = time.monotonic()
|
||||
frames = self._pipe.wait_for_frames(timeout_ms=2000)
|
||||
self._t_wait = time.monotonic() - t_wait0
|
||||
t_pub = time.monotonic()
|
||||
# 彩色先发出去。深度对齐再慢，也不能把这一帧彩色拖掉。
|
||||
self._publish_frame(bgr, None)
|
||||
self._t_pub = time.monotonic() - t_pub
|
||||
ir_l = (
|
||||
frames.get_infrared_frame(1)
|
||||
if self._have_infra and self._infra_mode in ('both', 'left')
|
||||
@@ -724,44 +782,45 @@ class RealsenseColorPub(TolerantNode):
|
||||
if self._have_infra and self._infra_mode in ('both', 'right')
|
||||
else None
|
||||
)
|
||||
aligned = frames
|
||||
if self._align is not None:
|
||||
t_al = time.monotonic()
|
||||
frames = self._align.process(frames)
|
||||
aligned = self._align.process(frames)
|
||||
self._t_align = time.monotonic() - t_al
|
||||
depth = aligned.get_depth_frame()
|
||||
color = frames.get_color_frame()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
misses += 1
|
||||
text = str(exc)
|
||||
dropped = (
|
||||
'disconnect' in text.lower()
|
||||
or 'ioctl' in text.lower()
|
||||
or 'errno=5' in text.lower()
|
||||
)
|
||||
if dropped or misses >= 4:
|
||||
if dropped:
|
||||
usb = getattr(self, '_usb_desc', '?')
|
||||
self.get_logger().error(
|
||||
f'相机数据流中断（USB={usb}）：{text[:120]}。'
|
||||
'正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。'
|
||||
)
|
||||
got = False
|
||||
misses = 0
|
||||
self._last_frame_mono = 0.0
|
||||
self._reconnect()
|
||||
continue
|
||||
if misses == 1:
|
||||
self.get_logger().warn(f'wait_for_frames：{exc}')
|
||||
else:
|
||||
self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
|
||||
continue
|
||||
if not color:
|
||||
misses += 1
|
||||
continue
|
||||
misses = 0
|
||||
bgr = np.asanyarray(color.get_data())
|
||||
depth = frames.get_depth_frame()
|
||||
depth_u16 = np.asanyarray(depth.get_data()) if depth else None
|
||||
if depth_u16 is not None and self._depth_to_mm != 1.0:
|
||||
# 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
|
||||
depth_u16 = np.clip(
|
||||
depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
|
||||
).astype(np.uint16)
|
||||
if depth_u16 is not None:
|
||||
try:
|
||||
self._publish_depth(depth_u16)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
if not self._running:
|
||||
break
|
||||
self.get_logger().warn(f'发布深度失败：{exc}', throttle_duration_sec=2.0)
|
||||
self._frame_i += 1
|
||||
self._fps_meter(color, depth, depth_u16)
|
||||
# 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
|
||||
@@ -794,7 +853,6 @@ class RealsenseColorPub(TolerantNode):
|
||||
)
|
||||
try:
|
||||
t_pub = time.monotonic()
|
||||
self._publish_frame(bgr, depth_u16)
|
||||
if self._ir1_pub is not None and ir_l is not None:
|
||||
self._publish_ir(ir_l, self._ir1_pub)
|
||||
if self._ir2_pub is not None and ir_r is not None:
|
||||
|
||||
@@ -159,7 +159,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
[
|
||||
DeclareLaunchArgument("skip_stage1", default_value="false"),
|
||||
DeclareLaunchArgument("skip_stage2", default_value="false"),
|
||||
DeclareLaunchArgument("skip_stage3", default_value="false"),
|
||||
DeclareLaunchArgument("skip_stage3", default_value="true"),
|
||||
DeclareLaunchArgument("target_hole", default_value=""),
|
||||
DeclareLaunchArgument("insert_depth_mm", default_value=""),
|
||||
DeclareLaunchArgument("screw_pitch_mm", default_value=""),
|
||||
|
||||
@@ -51,8 +51,11 @@ def test_color_preview_is_rgb_and_not_wider_than_the_widget_budget() -> None:
|
||||
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
|
||||
assert fitted.shape == (720, 1280, 3)
|
||||
wrist = np.zeros((480, 848, 3), np.uint8)
|
||||
assert camera_view.rgb_preview(wrist, bgr=False).shape == (480, 848, 3)
|
||||
assert camera_view._COLOR_WIDTH >= 1280
|
||||
assert camera_view._INTERVAL <= 1.0 / 30.0 + 1e-9
|
||||
with pytest.raises(ValueError, match="彩色预览"):
|
||||
camera_view.rgb_preview(np.zeros((8, 8), np.uint8), bgr=True)
|
||||
|
||||
@@ -110,7 +113,7 @@ def test_frame_label_paints_rgb_without_set_pixmap() -> None:
|
||||
|
||||
def test_preview_interval_is_fast_enough_for_the_main_view() -> None:
|
||||
"""主界面要看运动和推理框。退回 0.10s 就是大约 10 帧，这里会红。"""
|
||||
assert camera_view._INTERVAL <= 0.05
|
||||
assert camera_view._INTERVAL <= 1.0 / 30.0 + 1e-9
|
||||
|
||||
|
||||
def test_detection_box_scales_onto_the_preview_and_empty_clears() -> None:
|
||||
@@ -157,6 +160,76 @@ def test_detection_is_drawn_on_the_camera_frame() -> None:
|
||||
camera_view._PREVIEW.pop("cam_color", None)
|
||||
|
||||
|
||||
def test_color_burst_keeps_native_resolution() -> None:
|
||||
"""两帧紧挨着来也不能抽掉，预览必须仍是相机原分辨率。"""
|
||||
camera_view._RAW.clear()
|
||||
camera_view._PREVIEW.clear()
|
||||
camera_view._META.clear()
|
||||
camera_view._STAMPS.clear()
|
||||
camera_view._PAINTED.clear()
|
||||
try:
|
||||
camera_view.watch({"cam_color"}, set())
|
||||
image = np.zeros((720, 1280, 3), np.uint8)
|
||||
camera_view.offer_color("cam_color", image, bgr=True, role="wrist")
|
||||
camera_view.offer_color("cam_color", image, bgr=True, role="wrist")
|
||||
camera_view.publish_latest()
|
||||
blob, width, height = camera_view.previews()["cam_color"]
|
||||
assert (height, width) == (720, 1280)
|
||||
assert len(blob) == 720 * 1280 * 3
|
||||
assert camera_view.preview_meta()["cam_color"][:2] == (1280, 720)
|
||||
finally:
|
||||
camera_view.watch(set(), set())
|
||||
camera_view._RAW.clear()
|
||||
camera_view._PREVIEW.clear()
|
||||
camera_view._META.clear()
|
||||
camera_view._STAMPS.clear()
|
||||
camera_view._PAINTED.clear()
|
||||
|
||||
|
||||
def test_rate_uses_the_last_second_and_keeps_the_source_size() -> None:
|
||||
camera_view._META.clear()
|
||||
camera_view._STAMPS.clear()
|
||||
try:
|
||||
camera_view._mark_rate("cam_color", 1280, 720)
|
||||
time.sleep(0.05)
|
||||
camera_view._mark_rate("cam_color", 1280, 720)
|
||||
width, height, fps = camera_view.preview_meta()["cam_color"]
|
||||
assert (width, height) == (1280, 720)
|
||||
assert fps > 1.0
|
||||
finally:
|
||||
camera_view._META.clear()
|
||||
camera_view._STAMPS.clear()
|
||||
|
||||
|
||||
def test_sim_cameras_are_a_separate_pair_for_the_same_view() -> None:
|
||||
"""仿真腕部和手外要能在主界面里看，但不能把真机那两路挤掉。"""
|
||||
site = {
|
||||
"robot": {
|
||||
"robots": [
|
||||
{"sn": "Rizon4-063989", "arm": "left", "camera": {"name": "d405_left"}},
|
||||
{"sn": "sim", "arm": "sim", "camera": {"name": "sim_d405"}},
|
||||
]
|
||||
},
|
||||
"cameras": {
|
||||
"d405_left": {"name": "d405_left", "model": "d405", "role": "left_wrist"},
|
||||
"sim_d405": {"name": "sim_d405", "model": "d405", "role": "sim_wrist", "arm": "sim"},
|
||||
"d435": {"name": "d435", "model": "d435", "role": "overhead_eye_to_hand"},
|
||||
"sim_d435": {
|
||||
"name": "sim_d435",
|
||||
"model": "d435",
|
||||
"role": "overhead_eye_to_hand",
|
||||
"arm": "sim",
|
||||
},
|
||||
},
|
||||
}
|
||||
real = camera_view.preview_targets(site)
|
||||
sim = camera_view.sim_preview_targets(site)
|
||||
both = camera_view.stream_targets(site)
|
||||
assert [cam["id"] for cam in real] == ["d405_left", "d435"]
|
||||
assert [cam["id"] for cam in sim] == ["sim_d405", "sim_d435"]
|
||||
assert [cam["id"] for cam in both] == ["d405_left", "d435", "sim_d405", "sim_d435"]
|
||||
|
||||
|
||||
def test_main_preview_is_left_wrist_and_shared_overhead_only() -> None:
|
||||
"""右臂腕部和仿真手外不进主界面。左腕和共用 D435 才进。"""
|
||||
site = {
|
||||
|
||||
@@ -14,6 +14,8 @@ from __future__ import annotations
|
||||
from pathlib import Path
|
||||
|
||||
from arm_application.dh_pgea import (
|
||||
ctrl_values,
|
||||
open_failure_text,
|
||||
REG_POSITION,
|
||||
S_GRIP_ARRIVED,
|
||||
S_GRIP_CAUGHT,
|
||||
@@ -94,6 +96,62 @@ def test_force_init_does_not_return_on_the_stale_finished_bit():
|
||||
assert wrote["n"] == 1
|
||||
|
||||
|
||||
def test_node_owned_port_uses_ctrl_topic_not_a_second_open() -> None:
|
||||
assert ctrl_values("open", force=30, speed=40) == (0.0, 1000.0, 30.0, 40.0)
|
||||
assert ctrl_values("close", force=20, speed=10) == (0.0, 0.0, 20.0, 10.0)
|
||||
assert ctrl_values("pos", "250", force=33, speed=25) == (0.0, 250.0, 33.0, 25.0)
|
||||
assert ctrl_values("init") == (1.0, 0.0, 50.0, 50.0)
|
||||
assert ctrl_values("status") is None
|
||||
try:
|
||||
ctrl_values("pos", "1200")
|
||||
except ValueError as exc:
|
||||
assert "0–1000" in str(exc)
|
||||
else:
|
||||
raise AssertionError("超范围位置应当失败")
|
||||
|
||||
|
||||
def test_exclusive_lock_is_not_called_a_power_failure():
|
||||
exc = OSError(11, "Could not exclusively lock port /dev/ttyUSB0: Resource temporarily unavailable")
|
||||
text = open_failure_text("/dev/ttyUSB0", exc)
|
||||
assert "占用" in text
|
||||
assert "24 V" not in text
|
||||
|
||||
|
||||
def test_a_real_open_failure_still_names_power_and_wiring():
|
||||
exc = FileNotFoundError(2, "No such file or directory")
|
||||
text = open_failure_text("/dev/ttyUSB9", exc)
|
||||
assert "24 V" in text
|
||||
assert "RS485" in text
|
||||
|
||||
|
||||
def test_parallel_cli_stops_before_start_when_the_port_is_missing(tmp_path):
|
||||
"""缺串口必须在拉起驱动之前失败，不能去开真口，也不能写回零。"""
|
||||
import os
|
||||
import subprocess
|
||||
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
site = (root / "config" / "site.yaml").read_text(encoding="utf-8")
|
||||
cfg = tmp_path / "site.yaml"
|
||||
cfg.write_text(site.replace("/dev/ttyUSB0", "/dev/ttyUSB-missing-test"), encoding="utf-8")
|
||||
env = os.environ.copy()
|
||||
env["SITE_CONFIG"] = str(cfg)
|
||||
proc = subprocess.run(
|
||||
["bash", str(root / "scripts" / "run_gripper.sh"), "--arm", "89", "--cli", "probe"],
|
||||
cwd=str(root),
|
||||
env=env,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
timeout=90,
|
||||
)
|
||||
out = (proc.stdout or "") + (proc.stderr or "")
|
||||
assert proc.returncode != 0
|
||||
assert "不在" in out
|
||||
assert "先拉起来" not in out
|
||||
script = (root / "scripts" / "run_gripper.sh").read_text(encoding="utf-8")
|
||||
assert 'auto="${SITE_GRIP_AUTO_INIT:-true}"' in script
|
||||
assert '-p auto_init:="${auto}"' in script
|
||||
|
||||
|
||||
def test_official_client_uses_the_same_latch_break():
|
||||
src = Path(__file__).resolve().parents[1] / "arm_application" / "dh_official.py"
|
||||
text = src.read_text(encoding="utf-8")
|
||||
|
||||
@@ -113,7 +113,8 @@ def test_main_page_holds_status_sim_cameras_tactile_and_actions():
|
||||
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)
|
||||
power = [button for button in main.findChildren(QPushButton) if "上电控制栈" in button.text()]
|
||||
assert power and not power[0].icon().isNull()
|
||||
assert win._cam_pic
|
||||
assert all(label.minimumHeight() <= 96 for label in win._cam_pic.values())
|
||||
from arm_application.operator_camera_view import targets
|
||||
|
||||
@@ -402,6 +402,27 @@ def test_cameras_skips_sim_arm(tmp_path: Path) -> None:
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def test_joint_angles_survive_status_packing() -> None:
|
||||
"""关节角曾经让 classify_state(**state) 抛 TypeError，JSON 出不来，界面不更新。"""
|
||||
import rdk_status
|
||||
from rdk_helpers import rdk_whitelist_lists
|
||||
|
||||
payload = rdk_status.pack_status({
|
||||
"connected": True,
|
||||
"operational": True,
|
||||
"op_status": "READY",
|
||||
"mode": "IDLE",
|
||||
"fault": False,
|
||||
"estop_released": True,
|
||||
"error": "",
|
||||
"q": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7],
|
||||
})
|
||||
assert payload["q"] == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
|
||||
assert payload["level"] == rdk_status.LEVEL_OK
|
||||
# 内核把 USB 网卡从 eth0 改名后，白名单必须仍是本机机器人网段地址。
|
||||
assert rdk_whitelist_lists("eth0", "192.168.2.10")[0] == ["192.168.2.10"]
|
||||
|
||||
|
||||
def test_network_action_appears_once_in_the_manual_list() -> None:
|
||||
"""check_network 与 rdk_status 给的"去查网线"必须是**同一串字**。
|
||||
|
||||
|
||||
@@ -15,6 +15,7 @@ site.yaml 有两层：顶层是站点配置，``robots[]`` 里每台臂可以覆
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Iterable, List, Mapping
|
||||
@@ -94,3 +95,55 @@ def test_sim_robot_carries_the_truth_backend_override(arm_section_keys):
|
||||
assert sim, "site.yaml 里没有 sim 机器人 —— 仿真流程会挑不到配置"
|
||||
src = (sim[0].get("perception") or {}).get("source")
|
||||
assert src == "truth", f"sim 的 perception.source 应为 truth，现在是 {src!r}"
|
||||
|
||||
|
||||
def _flatten_for(sn: str) -> dict:
|
||||
import export_site_env # type: ignore
|
||||
|
||||
previous = os.environ.get("ROBOT_SN")
|
||||
os.environ["ROBOT_SN"] = sn
|
||||
try:
|
||||
return export_site_env.flatten(export_site_env.load_site())
|
||||
finally:
|
||||
if previous is None:
|
||||
os.environ.pop("ROBOT_SN", None)
|
||||
else:
|
||||
os.environ["ROBOT_SN"] = previous
|
||||
|
||||
|
||||
def test_live_left_gripper_does_not_auto_home(arm_section_keys):
|
||||
"""左臂现场 auto_init 是 false。拉起驱动若把它弄成 true，会写 0xA5 回零。"""
|
||||
flat = _flatten_for("89")
|
||||
assert flat["SITE_GRIP_AUTO_INIT"] == "false"
|
||||
assert flat["SITE_GRIP_PORT"] == "/dev/ttyUSB0"
|
||||
|
||||
|
||||
def test_auto_init_override_is_not_ignored(arm_section_keys, tmp_path):
|
||||
"""把现场改成 true 必须真的变成 true。否则上面那条是 exporter 写死的假通过。"""
|
||||
import export_site_env # type: ignore
|
||||
|
||||
doc = yaml.safe_load((ROOT / "config" / "site.yaml").read_text(encoding="utf-8"))
|
||||
changed = False
|
||||
for robot in robot_entries(doc):
|
||||
if str(robot.get("sn") or "").endswith("063989"):
|
||||
robot["gripper"]["auto_init"] = True
|
||||
changed = True
|
||||
assert changed
|
||||
cfg = tmp_path / "site.yaml"
|
||||
cfg.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
previous_cfg = os.environ.get("SITE_CONFIG")
|
||||
previous_sn = os.environ.get("ROBOT_SN")
|
||||
os.environ["SITE_CONFIG"] = str(cfg)
|
||||
os.environ["ROBOT_SN"] = "89"
|
||||
try:
|
||||
flat = export_site_env.flatten(export_site_env.load_site())
|
||||
finally:
|
||||
if previous_cfg is None:
|
||||
os.environ.pop("SITE_CONFIG", None)
|
||||
else:
|
||||
os.environ["SITE_CONFIG"] = previous_cfg
|
||||
if previous_sn is None:
|
||||
os.environ.pop("ROBOT_SN", None)
|
||||
else:
|
||||
os.environ["ROBOT_SN"] = previous_sn
|
||||
assert flat["SITE_GRIP_AUTO_INIT"] == "true"
|
||||
|
||||
Reference in New Issue
Block a user