diff --git a/scripts/run_console.sh b/scripts/run_console.sh index fbfde37..b1bea9c 100755 --- a/scripts/run_console.sh +++ b/scripts/run_console.sh @@ -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 - export XAUTHORITY="/run/user/$(id -u)/gdm/Xauthority" +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 diff --git a/src/arm_application/arm_application/operator_app.py b/src/arm_application/arm_application/operator_app.py index 2c38a27..34d6e32 100644 --- a/src/arm_application/arm_application/operator_app.py +++ b/src/arm_application/arm_application/operator_app.py @@ -11,20 +11,36 @@ import sys import threading from pathlib import Path -from PySide6.QtCore import Qt, QSize, QThread, QTimer, Signal -from PySide6.QtGui import QColor, QFont, QPainter, QPen, QPixmap +from PySide6.QtCore import QPoint, QRect, QSize, Qt, QThread, QTimer, Signal +from PySide6.QtGui import ( + QBrush, + QColor, + QFont, + QImage, + QMatrix3x3, + QPainter, + QPen, + QPixmap, + QPolygon, + QSurfaceFormat, + QVector3D, +) +from PySide6.QtOpenGL import QOpenGLBuffer, QOpenGLShader, QOpenGLShaderProgram, QOpenGLVertexArrayObject +from PySide6.QtOpenGLWidgets import QOpenGLWidget from PySide6.QtWidgets import ( QApplication, + QCheckBox, QDoubleSpinBox, + QSpinBox, QFrame, QGridLayout, QHBoxLayout, QLabel, + QLayout, QLineEdit, QListWidget, QListWidgetItem, QMainWindow, - QMessageBox, QPlainTextEdit, QPushButton, QScrollArea, @@ -35,6 +51,7 @@ from PySide6.QtWidgets import ( QWidget, ) +from arm_application.site_io import arm_short_id, listed_arms, load_site_dict from arm_application.operator_console import ( _poll_tactile, catalog, @@ -42,8 +59,13 @@ from arm_application.operator_console import ( run_gripper_tune, status, ) +from arm_application import operator_arm_mesh as arm_mesh +from arm_application import operator_gripper_mesh as gripper_mesh +from arm_application import operator_arm_state as arm_state +from arm_application import operator_gripper_state as grip_state from arm_application import operator_teach as teach from arm_application import operator_camera_view as camera_view +from arm_application.operator_handeye_board import BoardWatch from arm_application import operator_tactile_view as tactile_view _LOGO = Path(__file__).resolve().parent / "operator_console_logo.png" @@ -83,14 +105,16 @@ class ValueTune(QWidget): decimals: int = 0, slider_low: float | None = None, slider_high: float | None = None, + compact: bool = False, ) -> None: super().__init__() self._scale = 10 ** decimals self._guard = False row = QHBoxLayout(self) row.setContentsMargins(0, 0, 0, 0) + row.setSpacing(4) label = QLabel(title) - label.setFixedWidth(88) + label.setFixedWidth(52 if compact else 88) self.slider = QSlider(Qt.Orientation.Horizontal) slo = low if slider_low is None else slider_low shi = high if slider_high is None else slider_high @@ -105,8 +129,10 @@ class ValueTune(QWidget): self.spin.valueChanged.connect(self._from_spin) row.addWidget(label) row.addWidget(self.slider, 1) - self.spin.setFixedWidth(120) + self.spin.setFixedWidth(72 if compact else 120) row.addWidget(self.spin) + if compact: + self.setMaximumHeight(28) def _clamp_slider(self, value: float) -> int: raw = int(round(value * self._scale)) @@ -133,42 +159,123 @@ class ValueTune(QWidget): self.spin.setValue(value) -class FrameLabel(QLabel): - """预览框。尺寸不跟图像走，否则每贴一帧标签就更大，画面会慢慢放大。""" +class _PictureRow(QWidget): + """并排画面的高度跟格子宽度走，按 16:9 把画面铺满。 - def __init__(self, text: str) -> None: + 彩色是 848×480 和 1280×720，都接近 16:9。格子若被纵向拉高， + 画面上下会留一条黑边，那块空白本该给触觉。 + """ + + def __init__(self, columns: int) -> None: + super().__init__() + self._columns = max(1, columns) + self._lock = False + policy = QSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Preferred) + policy.setHeightForWidth(True) + self.setSizePolicy(policy) + self.setMinimumHeight(96) + + def hasHeightForWidth(self) -> bool: # noqa: N802 + return True + + def heightForWidth(self, width: int) -> int: # noqa: N802 + gap = 4 * max(0, self._columns - 1) + cell = max(1, (max(1, width) - gap) // self._columns) + return 16 + int(round(cell * 9 / 16)) + + def resizeEvent(self, event) -> None: # noqa: N802 + super().resizeEvent(event) + if self._lock or self.width() < 40: + return + want = self.heightForWidth(self.width()) + if abs(self.minimumHeight() - want) <= 1 and abs(self.maximumHeight() - want) <= 1: + return + self._lock = True + self.setMinimumHeight(want) + self.setMaximumHeight(want) + self._lock = False + + +class FrameLabel(QLabel): + """预览框。尺寸不跟图像走，否则每贴一帧标签就更大，画面会慢慢放大。 + + 画面自己画。不调用 ``setPixmap``：那一下会 ``updateGeometry``， + 六路画面每帧都把整页布局重算一遍，窗口就卡。 + """ + + def __init__(self, text: str, *, compact: bool = False) -> None: super().__init__(text) + self._compact = compact self.setAlignment(Qt.AlignmentFlag.AlignCenter) - self.setMinimumSize(320, 200) + self.setMinimumSize(96, 64) if compact else self.setMinimumSize(320, 200) self.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) self.setObjectName("shot") self._source = QPixmap() + self._scaled = QPixmap() self._token = 0 self._drawn = QSize() self._busy = False def sizeHint(self) -> QSize: - return QSize(480, 270) + return QSize(200, 112) if self._compact else QSize(480, 270) def minimumSizeHint(self) -> QSize: - return QSize(320, 200) + return QSize(96, 64) if self._compact else QSize(320, 200) + + def setText(self, text: str) -> None: # noqa: N802 + self._source = QPixmap() + self._scaled = QPixmap() + self._token = 0 + self._drawn = QSize() + super().setText(text) def show_jpeg(self, blob: bytes) -> None: token = id(blob) - if token != self._token: - pix = QPixmap() - if not pix.loadFromData(blob, "JPG"): - return - self._source = pix - self._token = token - self._drawn = QSize() + if token == self._token and not self._source.isNull(): + self._draw(False) + return + pix = QPixmap() + if not pix.loadFromData(blob, "JPG"): + return + self._source = pix + self._token = token + self._drawn = QSize() self._draw(False) - def resizeEvent(self, event) -> None: + def show_rgb(self, blob: bytes, width: int, height: int) -> None: + """贴一块已经缩好的 RGB。不再在界面线程里解 JPEG。""" + token = id(blob) + if token == self._token and not self._source.isNull(): + self._draw(False) + return + if width < 2 or height < 2 or len(blob) < width * height * 3: + return + image = QImage(blob, width, height, width * 3, QImage.Format.Format_RGB888).copy() + if image.isNull(): + return + self._source = QPixmap.fromImage(image) + self._token = token + self._drawn = QSize() + self._draw(False) + + def resizeEvent(self, event) -> None: # noqa: N802 super().resizeEvent(event) self._drawn = QSize() self._draw(True) + def paintEvent(self, event) -> None: # noqa: N802 + if self._scaled.isNull(): + super().paintEvent(event) + return + painter = QPainter(self) + painter.fillRect(self.rect(), QColor("#0c0e12")) + pix = self._scaled + painter.drawPixmap( + (self.width() - pix.width()) // 2, + (self.height() - pix.height()) // 2, + pix, + ) + def _draw(self, smooth: bool) -> None: if self._busy or self._source.isNull(): return @@ -182,13 +289,15 @@ class FrameLabel(QLabel): if smooth else Qt.TransformationMode.FastTransformation ) - self.setPixmap(self._source.scaled( + self._scaled = self._source.scaled( size, Qt.AspectRatioMode.KeepAspectRatio, mode, - )) - self.setText("") + ) self._drawn = QSize(size) + if self.text(): + super().setText("") + self.update() finally: self._busy = False @@ -279,12 +388,497 @@ def _btn(text: str, danger: bool = False) -> QPushButton: return button +def _fit_text(label: QLabel) -> QLabel: + """长状态文字不能把窗口撑得比屏幕宽，否则标题栏拖不动。""" + label.setMinimumWidth(0) + label.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Preferred) + return label + + +# 操作键上的图案。用字符，不依赖这台机器有没有图标主题。 +_NODE_MARKS = { + "real_up": "⏻", + "camera_d405": "◉", + "camera_d435": "◎", + "tactile_up": "▦", + "gripper_up": "▣", + "gripper_probe": "⌕", + "gripper_open": "◁", + "gripper_close": "▷", + "gripper_rotate": "↻", + "pipeline": "▶", + "stage_track": "⌖", + "stage_locate": "⊕", + "stage_screw": "⟳", + "reset_home": "⌂", + "gauge_dry": "◻", + "gauge_run": "■", +} + + +# Qt 的 QOpenGLFunctions 只带函数，不带这些枚举。数值是 OpenGL 规范里的固定值。 +_GL_TRIANGLES = 0x0004 +_GL_DEPTH_BUFFER_BIT = 0x00000100 +_GL_CULL_FACE = 0x0B44 +_GL_DEPTH_TEST = 0x0B71 +_GL_FLOAT = 0x1406 +_GL_COLOR_BUFFER_BIT = 0x00004000 +_AXIS_X = QColor("#e24b4b") +_AXIS_Y = QColor("#3dbe6a") +_AXIS_Z = QColor("#4c86d4") +# 打开界面时的观察角。双击只回到这里，不改关节。 +_VIEW_YAW = 0.9 +_VIEW_PITCH = -0.35 + + +_MESH_VERT = """ +#version 330 core +layout(location = 0) in vec3 a_pos; +layout(location = 1) in vec3 a_color; +uniform mat3 u_rot; +uniform vec3 u_center; +uniform float u_sx; +uniform float u_sy; +uniform float u_radius; +out vec3 v_color; +void main() { + vec3 p = u_rot * (a_pos - u_center); + gl_Position = vec4(p.x * u_sx, p.z * u_sy, p.y / (u_radius * 2.0), 1.0); + v_color = a_color; +} +""" + +_MESH_FRAG = """ +#version 330 core +in vec3 v_color; +uniform float u_tint; +out vec4 frag_color; +void main() { + frag_color = vec4(v_color * u_tint, 1.0); +} +""" + + +class MeshView(QOpenGLWidget): + """三角网格。左键拖拽绕模型转视角，三角面交给显卡画。""" + + def __init__(self, min_w: int, min_h: int) -> None: + super().__init__() + fmt = QSurfaceFormat() + fmt.setRenderableType(QSurfaceFormat.RenderableType.OpenGL) + fmt.setVersion(3, 3) + fmt.setProfile(QSurfaceFormat.OpenGLContextProfile.CoreProfile) + fmt.setSamples(4) + fmt.setDepthBufferSize(24) + self.setFormat(fmt) + self._tris: list = [] + self._note = "" + self._live = False + self._yaw = _VIEW_YAW + self._pitch = _VIEW_PITCH + self._press: QPoint | None = None + self._cache_key: tuple | None = None + self._batch: list[tuple[QColor, QPolygon]] = [] + self._gen = 0 + self._sig: tuple | None = None + self._gl_blob = b"" + self._gl_count = 0 + self._gl_center = (0.0, 0.0, 0.0) + self._gl_radius = 1.0 + self._gl_dirty = False + self._gl_program: QOpenGLShaderProgram | None = None + self._gl_buffer: QOpenGLBuffer | None = None + self._gl_vao: QOpenGLVertexArrayObject | None = None + self._frames: list = [] + self._thin_caption = False + self.setMinimumSize(min_w, min_h) + self.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) + self.setCursor(Qt.CursorShape.OpenHandCursor) + + def set_mesh(self, triangles: list, note: str, live: bool) -> None: + live = bool(live) + sig = (len(triangles), note, live) + if triangles: + sig = sig + (triangles[0][0], triangles[-1][0]) + if sig == self._sig: + return + self._sig = sig + self._tris = triangles + self._note = note + self._live = live + self._gen += 1 + self._gl_count = 0 + self.update() + + def set_gl_mesh( + self, + blob: bytes, + count: int, + center: tuple[float, float, float], + radius: float, + note: str, + live: bool, + ) -> None: + """大网格走显卡顶点缓冲。拖视角不再在 CPU 上重算每个三角。""" + self._gl_blob = blob + self._gl_count = count + self._gl_center = center + self._gl_radius = radius + self._gl_dirty = True + self._tris = [] + self._batch = [] + self._note = note + self._live = bool(live) + self._sig = None + self.update() + + def initializeGL(self) -> None: # noqa: N802 + program = QOpenGLShaderProgram(self) + ok = program.addShaderFromSourceCode(QOpenGLShader.ShaderTypeBit.Vertex, _MESH_VERT) + ok = program.addShaderFromSourceCode(QOpenGLShader.ShaderTypeBit.Fragment, _MESH_FRAG) and ok + program.bindAttributeLocation("a_pos", 0) + program.bindAttributeLocation("a_color", 1) + ok = program.link() and ok + if not ok: + self._note = "显卡着色器没链上，网格画不出来。" + self._gl_program = None + return + self._gl_program = program + self._gl_loc = { + name: program.uniformLocation(name.encode()) + for name in ("u_rot", "u_center", "u_sx", "u_sy", "u_radius", "u_tint") + } + buffer = QOpenGLBuffer(QOpenGLBuffer.Type.VertexBuffer) + buffer.create() + self._gl_buffer = buffer + vao = QOpenGLVertexArrayObject(self) + vao.create() + self._gl_vao = vao + + def mousePressEvent(self, event) -> None: # noqa: N802 + if event.button() == Qt.MouseButton.LeftButton: + self._press = event.position().toPoint() + self.setCursor(Qt.CursorShape.ClosedHandCursor) + event.accept() + return + super().mousePressEvent(event) + + def mouseMoveEvent(self, event) -> None: # noqa: N802 + if self._press is None: + super().mouseMoveEvent(event) + return + now = event.position().toPoint() + self._yaw += (now.x() - self._press.x()) * 0.012 + self._pitch = max(-1.35, min(1.35, self._pitch + (now.y() - self._press.y()) * 0.010)) + self._press = now + self.update() + event.accept() + + def mouseReleaseEvent(self, event) -> None: # noqa: N802 + self._press = None + self.setCursor(Qt.CursorShape.OpenHandCursor) + self.update() + event.accept() + + def paintGL(self) -> None: # noqa: N802 + if self._gl_count and self._gl_program is not None and self._gl_buffer is not None: + self._paint_gpu() + return + # 夹爪面少，仍用画笔。机械臂不走这里。 + key = (self.width(), self.height(), round(self._yaw, 3), round(self._pitch, 3), self._gen) + if key != self._cache_key: + self._batch = self._project() + self._cache_key = key + painter = QPainter(self) + painter.setRenderHint(QPainter.RenderHint.Antialiasing) + painter.fillRect(self.rect(), QColor("#0c0e12")) + if not self._batch and not self._tris: + painter.setPen(QColor("#8b929d")) + painter.drawText(self.rect(), int(Qt.AlignmentFlag.AlignCenter), self._note or "没有网格") + painter.end() + return + painter.setPen(Qt.PenStyle.NoPen) + for color, poly in self._batch: + painter.setBrush(QBrush(color)) + painter.drawPolygon(poly) + self._paint_caption(painter) + painter.end() + + def _caption(self) -> str: + return "左键拖拽改视角。 " + self._note + + def _paint_gpu(self) -> None: + """一次画出全部三角。视角只改 uniform，不把面片搬回 CPU。""" + painter = QPainter(self) + painter.beginNativePainting() + functions = self.context().functions() + functions.glClearColor(12 / 255, 14 / 255, 18 / 255, 1.0) + functions.glClear(_GL_COLOR_BUFFER_BIT | _GL_DEPTH_BUFFER_BIT) + functions.glEnable(_GL_DEPTH_TEST) + functions.glDisable(_GL_CULL_FACE) + program = self._gl_program + buffer = self._gl_buffer + vao = self._gl_vao + assert program is not None and buffer is not None and vao is not None + vao.bind() + program.bind() + buffer.bind() + if self._gl_dirty: + buffer.allocate(self._gl_blob, len(self._gl_blob)) + self._gl_dirty = False + program.setAttributeBuffer(0, _GL_FLOAT, 0, 3, 24) + program.setAttributeBuffer(1, _GL_FLOAT, 12, 3, 24) + program.enableAttributeArray(0) + program.enableAttributeArray(1) + rot = arm_mesh.view_rotation(self._yaw, self._pitch) + # QMatrix3x3 按行读入。GLSL 再按列使用，和 view_rotation 的行向量乘法一致。 + flat = [rot[row][col] for row in range(3) for col in range(3)] + loc = self._gl_loc + program.setUniformValue(loc["u_rot"], QMatrix3x3(flat)) + cx, cy, cz = self._gl_center + program.setUniformValue(loc["u_center"], QVector3D(cx, cy, cz)) + width = max(self.width(), 1) + height = max(self.height(), 1) + sx, sy = arm_mesh.view_scale(self._gl_radius, width, height) + program.setUniformValue1f(loc["u_sx"], sx) + program.setUniformValue1f(loc["u_sy"], sy) + program.setUniformValue1f(loc["u_radius"], self._gl_radius) + program.setUniformValue1f(loc["u_tint"], 1.0 if self._live else 0.55) + functions.glDrawArrays(_GL_TRIANGLES, 0, self._gl_count) + buffer.release() + program.release() + vao.release() + painter.endNativePainting() + self._paint_ground(painter) + self._paint_frames(painter) + self._paint_caption(painter) + painter.end() + + def _caption_band(self) -> int: + if self._thin_caption: + return 16 + return 14 if self.height() < 160 else 60 + + def _paint_caption(self, painter: QPainter) -> None: + band = self._caption_band() + if band <= 16: + rect = QRect(6, self.height() - 16, self.width() - 12, 14) + else: + rect = QRect(12, self.height() - 72, self.width() - 24, 60) + painter.setPen(QColor("#8b929d")) + painter.drawText( + rect, + int(Qt.AlignmentFlag.AlignLeft | Qt.AlignmentFlag.AlignBottom | Qt.TextFlag.TextWordWrap), + self._caption(), + ) + + def _paint_ground(self, painter: QPainter) -> None: + """地平面格子。只在机械臂仿真上画，线数固定，不跟模型一起重算。""" + if not self._frames or self.width() < 8 or self.height() < 8: + return + pen = QPen(QColor("#3a4150")) + pen.setWidth(1) + painter.setPen(pen) + width = self.width() + height = self.height() + for start, end in arm_mesh.ground_grid(): + x0, y0 = arm_mesh.screen_point( + start, self._yaw, self._pitch, self._gl_center, self._gl_radius, width, height, + ) + x1, y1 = arm_mesh.screen_point( + end, self._yaw, self._pitch, self._gl_center, self._gl_radius, width, height, + ) + painter.drawLine(int(x0), int(y0), int(x1), int(y1)) + + def _paint_frames(self, painter: QPainter) -> None: + """坐标系画在地面格子上面。轴长 0.15 m，和模型同一套米制。""" + if not self._frames or self.width() < 8 or self.height() < 8: + return + length = 0.15 + width = self.width() + height = self.height() + for frame in self._frames: + origin = frame["pos"] + rot = frame["rot"] + ox, oy = arm_mesh.screen_point( + origin, self._yaw, self._pitch, self._gl_center, self._gl_radius, width, height, + ) + for axis, color in enumerate((_AXIS_X, _AXIS_Y, _AXIS_Z)): + tip = arm_mesh.axis_tip(origin, rot, axis, length) + tx, ty = arm_mesh.screen_point( + tip, self._yaw, self._pitch, self._gl_center, self._gl_radius, width, height, + ) + pen = QPen(color) + pen.setWidth(3) + painter.setPen(pen) + painter.drawLine(int(ox), int(oy), int(tx), int(ty)) + painter.setPen(QColor("#e7ebf0")) + painter.drawText(int(ox) + 8, int(oy) - 8, frame["name"]) + + def _project(self) -> list[tuple[QColor, QPolygon]]: + triangles = self._tris + if not triangles or self.width() < 8 or self.height() < 8: + return [] + rot = arm_mesh.view_rotation(self._yaw, self._pitch) + projected = [] + for item in triangles: + a, b, c = item[0], item[1], item[2] + shade = float(item[3]) + rgb = item[4] if len(item) > 4 else (0.75, 0.76, 0.78) + pts = [arm_mesh._apply(rot, p) for p in (a, b, c)] + screen = [(p[0], -p[2]) for p in pts] + depth = pts[0][1] + pts[1][1] + pts[2][1] + projected.append((depth, shade, rgb, screen)) + projected.sort(key=lambda row: row[0], reverse=True) + xs = [p[0] for _d, _s, _c, pts in projected for p in pts] + ys = [p[1] for _d, _s, _c, pts in projected for p in pts] + span = max(max(xs) - min(xs), max(ys) - min(ys), 1e-4) + band = self._caption_band() + scale = min(self.width() - 20, max(8, self.height() - band - 8)) / span + cx = (min(xs) + max(xs)) / 2 + cy = (min(ys) + max(ys)) / 2 + origin_y = (self.height() - band) / 2 + tint = 1.0 if self._live else 0.55 + batch = [] + for _depth, shade, rgb, pts in projected: + light = (0.55 + 0.45 * max(0.0, min(1.0, shade))) * tint + color = QColor( + int(max(0, min(255, rgb[0] * 255 * light))), + int(max(0, min(255, rgb[1] * 255 * light))), + int(max(0, min(255, rgb[2] * 255 * light))), + ) + poly = QPolygon([ + QPoint( + int(self.width() / 2 + (point[0] - cx) * scale), + int(origin_y + (point[1] - cy) * scale), + ) + for point in pts + ]) + batch.append((color, poly)) + return batch + + +class ArmSketch(MeshView): + """用非夕 Rizon4 的外壳网格画当前姿态。没有关节角时摆在零位，并标明。 + + 双击把观察角放回打开界面时的角度。关节仍是实时的，真机不动。 + """ + + def __init__(self) -> None: + super().__init__(240, 180) + self._workpiece = None + self._pose_key = None + + def _caption(self) -> str: + return "左键拖拽改视角。双击把视角复位。 " + self._note + + def mouseDoubleClickEvent(self, event) -> None: # noqa: N802 + if event.button() != Qt.MouseButton.LeftButton: + return + self._yaw = _VIEW_YAW + self._pitch = _VIEW_PITCH + self._press = None + self.update() + event.accept() + + def set_pose(self, q: list | None, live: bool, workpiece: tuple | None = None) -> None: + self._workpiece = workpiece + if isinstance(q, list) and len(q) >= 7: + joints = [round(float(v), 3) for v in q[:7]] + draw_live = bool(live) + else: + joints = [0.0] * 7 + draw_live = False + try: + pose_key = (tuple(joints), draw_live, _workpiece_key(workpiece)) + if pose_key == self._pose_key: + return + self._pose_key = pose_key + blob, count, center, radius = arm_mesh.posed_gl(joints) + self._frames = arm_state.display_frames(joints, workpiece) + except Exception as exc: # noqa: BLE001 —— 网格缺失时把原因画出来，不退回另一套模型 + self._pose_key = None + self._frames = [] + self.set_mesh([], f"厂家网格读失败：{exc}", False) + return + names = "、".join(item["name"] for item in self._frames) + if not draw_live: + note = f"非夕 Rizon4 外壳，零位。还没有关节角，这不是当前姿态。坐标系：{names}。" + else: + note = f"非夕 Rizon4 外壳，按实时关节角摆放。白壳和灰蓝关节是厂家材质。坐标系：{names}。" + self.set_gl_mesh(blob, count, center, radius, note, draw_live) + + +def _workpiece_key(workpiece: tuple | None) -> tuple | None: + if workpiece is None: + return None + rot, pos = workpiece + return ( + tuple(round(float(v), 5) for row in rot for v in row), + tuple(round(float(v), 5) for v in pos), + ) + + +def _workpieces_by_arm(site: dict) -> dict[str, tuple | None]: + """每台真机臂的工件系。没配完整坐标系就是 None，法兰继续画在法兰上。""" + found: dict[str, tuple | None] = {} + robots = ((site or {}).get("robot") or {}).get("robots") or [] + for robot in robots: + if not isinstance(robot, dict): + continue + sn = str(robot.get("sn") or "") + if sn.lower() == "sim": + continue + arm_no = arm_short_id(sn) + if arm_no: + found[arm_no] = arm_state.workpiece_frame(robot) + return found + + +class GripperSketch(MeshView): + """按尺寸图画大寰夹爪。开口和旋转跟寄存器走。""" + + def __init__(self, min_w: int = 360, min_h: int = 280) -> None: + super().__init__(min_w, min_h) + self._thin_caption = True + + def _caption(self) -> str: + return "拖拽改视角" + + def set_pose( + self, + permille: float | None, + rotation_deg: float | None, + rotary: bool, + live: bool, + stroke_mm: float | None = None, + ) -> None: + if isinstance(permille, (int, float)): + permille = round(float(permille)) + if isinstance(rotation_deg, (int, float)): + rotation_deg = round(float(rotation_deg), 1) + pose_key = (bool(rotary), permille, rotation_deg, bool(live), stroke_mm) + if pose_key == getattr(self, "_pose_key", None): + return + self._pose_key = pose_key + triangles, note = gripper_mesh.triangles(bool(rotary), permille, rotation_deg, stroke_mm) + if live and permille is not None: + note = f"开口 {float(permille):.0f}‰ " + note + if rotary and isinstance(rotation_deg, (int, float)): + note = f"转角 {float(rotation_deg):.1f}° " + note + self.set_mesh(triangles, note, bool(live) and permille is not None) + + class StationApp(QMainWindow): def __init__(self) -> None: super().__init__() self.setWindowTitle("ORISYS 通止规工位") self.resize(1440, 900) - self._arm = "89" + site = load_site_dict() + self._catalog = listed_arms(site) + self._workpieces = _workpieces_by_arm(site) + self._board = BoardWatch() + self._arm = self._catalog[0]["id"] if self._catalog else "" self._mode = "eye_in_hand" self._snap: dict = {} self._job: _Job | None = None @@ -297,7 +891,8 @@ class StationApp(QMainWindow): self._timer.start(800) self._images = QTimer(self) self._images.timeout.connect(self._paint_images) - self._images.start(200) + # 和相机预览同一档：大约 20 帧，推理框才跟得上画面。 + self._images.start(50) self._stack.currentChanged.connect(lambda _index: self._paint_images()) self._refresh() @@ -307,93 +902,95 @@ class StationApp(QMainWindow): layout = QHBoxLayout(root) layout.setContentsMargins(0, 0, 0, 0) layout.setSpacing(0) + # 子控件的最小宽度不许写进窗口。否则一串长状态会把窗口撑过屏幕，最大化后拖不动。 + layout.setSizeConstraint(QLayout.SizeConstraint.SetNoConstraint) layout.addWidget(self._sidebar()) right = QVBoxLayout() right.setContentsMargins(0, 0, 0, 0) right.setSpacing(0) right.addWidget(self._header()) self._stack = QStackedWidget() - for key, _label in ( - ("overview", "总览"), - ("tactile", "视触觉"), - ("handeye", "手眼"), - ("camera", "相机"), - ("gripper", "夹爪"), - ("control", "控制"), - ("teach", "示教"), - ("assembly", "装配"), - ("operate", "操作"), - ): - page = getattr(self, f"_page_{key}")() + handeye = self._page_handeye() + self._he_out.setFixedHeight(72) + built = ( + ("main", self._page_overview()), + ("handeye", self._with_back(handeye, "手眼")), + ("teach", self._with_back(self._scroll(self._page_teach()), "示教")), + ) + for key, page in built: self._pages[key] = page self._stack.addWidget(page) right.addWidget(self._stack, 1) self._detail = QPlainTextEdit() self._detail.setReadOnly(True) - self._detail.setFixedHeight(120) + self._detail.setFixedHeight(56) self._detail.setPlaceholderText("命令输出显示在这里") right.addWidget(self._detail) holder = QWidget() holder.setLayout(right) layout.addWidget(holder, 1) - self._show_page("overview") + self._show_page("main") + self.setMinimumSize(1100, 680) + + def _with_back(self, inner: QWidget, title: str) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + lay.setContentsMargins(12, 8, 12, 8) + bar = QHBoxLayout() + back = _btn("← 返回主界面") + back.clicked.connect(lambda: self._show_page("main")) + bar.addWidget(back) + bar.addWidget(_title(title)) + bar.addStretch(1) + lay.addLayout(bar) + lay.addWidget(inner, 1) + return page def _sidebar(self) -> QWidget: bar = QWidget() bar.setObjectName("side") - bar.setFixedWidth(220) + bar.setFixedWidth(112) lay = QVBoxLayout(bar) + lay.setContentsMargins(8, 8, 8, 8) logo = QLabel() if _LOGO.is_file(): pix = QPixmap(str(_LOGO)) - logo.setPixmap(pix.scaledToWidth(180, Qt.TransformationMode.SmoothTransformation)) + logo.setPixmap(pix.scaledToWidth(88, Qt.TransformationMode.SmoothTransformation)) logo.setAlignment(Qt.AlignmentFlag.AlignCenter) lay.addWidget(logo) - station = QLabel("通止规工位") - station.setObjectName("sideNote") - station.setAlignment(Qt.AlignmentFlag.AlignCenter) - lay.addWidget(station) items = ( - ("overview", "总览", "设备与流程"), - ("tactile", "视触觉", "切向、热成像、曲线"), - ("handeye", "手眼", "眼在手上 / 手外"), - ("camera", "相机", "D405 与 D435"), - ("gripper", "夹爪", "位置、力度、速度"), - ("control", "控制", "分步、整体、复位"), - ("teach", "示教", "软拖拽记点"), - ("assembly", "装配", "S00 到 S11"), - ("operate", "操作", "启动与确认"), + ("main", "⌂ 主界面"), + ("handeye", "✦ 手眼"), + ("teach", "✎ 示教"), ) - for key, name, hint in items: - button = QPushButton(f"{name}\n{hint}") + for key, name in items: + button = QPushButton(name) button.setObjectName("nav") button.clicked.connect(lambda _checked=False, k=key: self._show_page(k)) self._nav.append((key, button)) lay.addWidget(button) lay.addStretch(1) - note = QLabel("读的是本机 USB、进程和话题。动臂、张合夹爪都要再确认一次。") - note.setWordWrap(True) - note.setObjectName("sideNote") - lay.addWidget(note) return bar def _header(self) -> QWidget: bar = QWidget() bar.setObjectName("head") - bar.setFixedHeight(64) + bar.setFixedHeight(52) lay = QHBoxLayout(bar) - self._arm_line = QLabel("左臂 89") + first = next((row for row in self._catalog if row["id"] == self._arm), None) + self._arm_line = QLabel(first["label"] if first else "没有机械臂") self._arm_line.setObjectName("armLine") lay.addWidget(self._arm_line) lay.addStretch(1) self._chips = QLabel("") self._chips.setObjectName("chips") - lay.addWidget(self._chips) + _fit_text(self._chips) + lay.addWidget(self._chips, 1) self._arm_buttons: dict[str, QPushButton] = {} - for arm, text in (("89", "左臂89"), ("91", "右臂91")): - button = _btn(text) - button.clicked.connect(lambda _checked=False, a=arm: self._set_arm(a)) - self._arm_buttons[arm] = button + for row in self._catalog: + button = _btn(row["label"]) + button.clicked.connect(lambda _checked=False, a=row["id"]: self._set_arm(a)) + self._arm_buttons[row["id"]] = button lay.addWidget(button) self._clock = QLabel("--:--:--") self._clock.setObjectName("clock") @@ -408,53 +1005,414 @@ class StationApp(QMainWindow): return area def _page_overview(self) -> QWidget: + """主界面。状态、仿真、相机、触觉和操作键都在这一屏，不放滚动区。""" + page = QWidget() + root = QHBoxLayout(page) + root.setContentsMargins(8, 6, 8, 6) + root.setSpacing(8) + root.addWidget(self._main_arm_column(), 2) + root.addWidget(self._main_work_column(), 5) + return page + + def _main_arm_column(self) -> QWidget: page = QWidget() lay = QVBoxLayout(page) - self._lead = QLabel("") + lay.setContentsMargins(0, 0, 0, 0) + lay.setSpacing(4) + self._lead = _fit_text(QLabel("")) self._lead.setObjectName("lead") - self._lead.setWordWrap(True) + self._lead.setMaximumHeight(20) lay.addWidget(self._lead) - grid = QGridLayout() + self._arm_banner = _fit_text(QLabel(arm_state.REMOTE_PROMPT)) + self._arm_banner.setObjectName("warn") + self._arm_banner.setMaximumHeight(20) + lay.addWidget(self._arm_banner) + usb = QHBoxLayout() + usb.setSpacing(4) self._usb_labels: list[tuple[QLabel, QLabel, QLabel]] = [] - for index, name in enumerate(("腕部 D405", "手外 D435", "视触觉", "夹爪串口")): + for name in ("腕部", "手外", "触觉", "串口"): card = _card() + card.setMaximumHeight(52) box = QVBoxLayout(card) + box.setContentsMargins(6, 2, 6, 2) + box.setSpacing(0) box.addWidget(_title(name)) count = QLabel("—") - count.setObjectName("stat") flag = QLabel("") - detail = QLabel("") + detail = _fit_text(QLabel("")) detail.setObjectName("muted") - detail.setWordWrap(True) - box.addWidget(count) - box.addWidget(flag) + detail.setMaximumHeight(16) + line = QHBoxLayout() + line.addWidget(count) + line.addWidget(flag) + line.addStretch(1) + box.addLayout(line) box.addWidget(detail) - grid.addWidget(card, index // 2, index % 2) + usb.addWidget(card) self._usb_labels.append((count, flag, detail)) + lay.addLayout(usb) + self._arm_lead = _fit_text(QLabel("只读机械臂。Remote 只能在示教器上开。")) + self._arm_lead.setObjectName("muted") + self._arm_lead.setMaximumHeight(18) + lay.addWidget(self._arm_lead) + self._arm_prompt = _fit_text(QLabel(arm_state.REMOTE_PROMPT)) + self._arm_prompt.setObjectName("warn") + self._arm_prompt.setWordWrap(True) + self._arm_prompt.setMaximumHeight(36) + lay.addWidget(self._arm_prompt) + self._arm_sketch = ArmSketch() + lay.addWidget(self._arm_sketch, 1) + cards = QHBoxLayout() + cards.setSpacing(4) + self._arm_cards: dict[str, QLabel] = {} + for key, title in ( + ("enable", "使能"), + ("mode", "模式"), + ("fault", "故障 / 急停"), + ("source", "数据"), + ): + card = _card() + card.setMaximumHeight(58) + box = QVBoxLayout(card) + box.setContentsMargins(6, 2, 6, 2) + box.setSpacing(0) + box.addWidget(_title(title)) + label = QLabel("—") + label.setObjectName("muted") + label.setWordWrap(True) + box.addWidget(label) + self._arm_cards[key] = label + cards.addWidget(card) + lay.addLayout(cards) + self._arm_summary = _fit_text(QLabel("")) + self._arm_summary.setObjectName("muted") + self._arm_summary.setMaximumHeight(18) + lay.addWidget(self._arm_summary) + self._arm_joints = _fit_text(QLabel("")) + self._arm_joints.setObjectName("muted") + self._arm_joints.setWordWrap(True) + self._arm_joints.setMaximumHeight(34) + lay.addWidget(self._arm_joints) + lay.addWidget(self._grip_panel()) + return page + + def _grip_panel(self) -> QWidget: + """主界面上的夹爪仿真和下发。放在臂这一列，不跟相机、触觉抢高度。""" + panel = _card() + lay = QVBoxLayout(panel) + lay.setContentsMargins(6, 4, 6, 4) + lay.setSpacing(4) + head = QHBoxLayout() + head.addWidget(_title("夹爪")) + self._grip_summary = _fit_text(QLabel("还没读到夹爪")) + self._grip_summary.setObjectName("warn") + head.addWidget(self._grip_summary, 1) + lay.addLayout(head) + self._grip_lead = _fit_text(QLabel("")) + self._grip_lead.setObjectName("muted") + self._grip_lead.setWordWrap(True) + self._grip_lead.setMaximumHeight(32) + lay.addWidget(self._grip_lead) + body = QHBoxLayout() + body.setSpacing(6) + self._grip_sketch = GripperSketch(120, 96) + self._grip_sketch.setFixedHeight(168) + self._grip_sketch.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Fixed) + body.addWidget(self._grip_sketch, 2) + ctrl = QVBoxLayout() + ctrl.setSpacing(2) + cards = QHBoxLayout() + cards.setSpacing(4) + self._grip_cards: dict[str, QLabel] = {} + for key, title in ( + ("link", "连接"), + ("opening", "开口"), + ("hold", "夹持"), + ("spin", "旋转"), + ): + card = _card() + card.setMaximumHeight(46) + box = QVBoxLayout(card) + box.setContentsMargins(4, 2, 4, 2) + box.setSpacing(0) + box.addWidget(_title(title)) + label = _fit_text(QLabel("—")) + label.setObjectName("muted") + box.addWidget(label) + self._grip_cards[key] = label + cards.addWidget(card) + ctrl.addLayout(cards) + self._pos = ValueTune("位置", 0, 1000, 500, compact=True) + self._force = ValueTune("力度%", 20, 100, 33, compact=True) + self._speed = ValueTune("速度%", 1, 100, 25, compact=True) + ctrl.addWidget(self._pos) + ctrl.addWidget(self._force) + ctrl.addWidget(self._speed) + row = QHBoxLayout() + row.setSpacing(4) + apply_btn = _btn("按此夹持", True) + open_btn = _btn("张开", True) + close_btn = _btn("闭合", True) + apply_btn.clicked.connect(lambda: self._grip_move("pos")) + open_btn.clicked.connect(lambda: self._grip_preset(1000)) + close_btn.clicked.connect(lambda: self._grip_preset(0)) + for button in (apply_btn, open_btn, close_btn): + button.setMinimumWidth(0) + button.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Fixed) + row.addWidget(button) + ctrl.addLayout(row) + body.addLayout(ctrl, 3) + lay.addLayout(body) + self._grip_draw = _fit_text(QLabel("")) + self._grip_draw.setObjectName("muted") + self._grip_draw.setMaximumHeight(16) + lay.addWidget(self._grip_draw) + + self._rotary = _card() + rot = QVBoxLayout(self._rotary) + rot.setContentsMargins(6, 4, 6, 4) + rot.setSpacing(2) + rot.addWidget(_title("旋转")) + self._angle = ValueTune( + "绝对角", -32767, 32767, 0, step=0.1, decimals=1, + slider_low=-3600, slider_high=3600, compact=True, + ) + self._delta = ValueTune( + "相对角", -32767, 32767, 15, step=0.1, decimals=1, + slider_low=-720, slider_high=720, compact=True, + ) + self._turns = ValueTune( + "圈数", -50, 50, 0.25, step=0.01, decimals=2, + slider_low=-10, slider_high=10, compact=True, + ) + self._rot_speed = ValueTune("转速%", 1, 100, 25, compact=True) + self._torque = ValueTune("力矩%", 20, 100, 33, compact=True) + for widget in (self._angle, self._delta, self._turns, self._rot_speed, self._torque): + rot.addWidget(widget) + rot_row = QHBoxLayout() + rot_row.setSpacing(4) + for title, kind in ( + ("转到绝对角", "rotate"), + ("相对转动", "rotate_by"), + ("按圈数转", "revolutions"), + ("旋转回零", "rotate_home"), + ): + button = _btn(title, True) + button.setMinimumWidth(0) + button.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Fixed) + button.clicked.connect(lambda _checked=False, k=kind: self._grip_move(k)) + rot_row.addWidget(button) + rot.addLayout(rot_row) + self._rotary.setVisible(False) + lay.addWidget(self._rotary) + return panel + + def _main_work_column(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + lay.setContentsMargins(0, 0, 0, 0) + lay.setSpacing(4) + self._infer_line = _fit_text(QLabel("推理 腕部 没有框 手外 没有框")) + self._infer_line.setObjectName("ok") + self._infer_line.setMaximumHeight(20) + lay.addWidget(self._infer_line) + self._cam_note = _fit_text(QLabel("")) + self._cam_note.setObjectName("muted") + self._cam_note.setMaximumHeight(18) + lay.addWidget(self._cam_note) + cam_bar = QHBoxLayout() + cam_bar.addWidget(_title("相机")) + depth = _btn("◎ 深度") + depth.clicked.connect(self._toggle_depth) + self._depth_btn = depth + self._depth_on = False + cam_bar.addWidget(depth) + for action in ("camera_d405", "camera_d435"): + cam_bar.addWidget(self._node_button(action)) + cam_bar.addStretch(1) + lay.addLayout(cam_bar) + self._cam_pic: dict[str, FrameLabel] = {} + cams = camera_view.targets() + colors = QHBoxLayout() + colors.setSpacing(4) + colors.setContentsMargins(0, 0, 0, 0) + depths = QHBoxLayout() + depths.setSpacing(4) + depths.setContentsMargins(0, 0, 0, 0) + for cam in cams: + col = QVBoxLayout() + col.setSpacing(0) + cap = QLabel(cam["label"]) + cap.setObjectName("muted") + cap.setMaximumHeight(16) + shot = FrameLabel("没有画面", compact=True) + self._cam_pic[f"{cam['id']}_color"] = shot + col.addWidget(cap) + col.addWidget(shot, 1) + colors.addLayout(col) + dcol = QVBoxLayout() + dcol.setSpacing(0) + dcap = QLabel(f"{cam['label']} 深度") + dcap.setObjectName("muted") + dcap.setMaximumHeight(16) + dshot = FrameLabel("没有深度", compact=True) + self._cam_pic[f"{cam['id']}_depth"] = dshot + dcol.addWidget(dcap) + dcol.addWidget(dshot, 1) + depths.addLayout(dcol) + color_box = _PictureRow(len(cams) or 1) + color_box.setLayout(colors) + self._color_box = color_box + lay.addWidget(color_box, 0) + self._depth_box = _PictureRow(len(cams) or 1) + self._depth_box.setLayout(depths) + self._depth_box.setVisible(False) + lay.addWidget(self._depth_box, 0) + tac_bar = QHBoxLayout() + tac_bar.addWidget(_title("视触觉")) + see = _btn("▶ 开始成像") + hide = _btn("■ 停止成像") + see.clicked.connect(lambda: self._run("tactile_view_on", False)) + hide.clicked.connect(lambda: self._run("tactile_view_off", False)) + tac_bar.addWidget(see) + tac_bar.addWidget(hide) + tac_bar.addWidget(self._node_button("tactile_up")) + tac_bar.addStretch(1) + lay.addLayout(tac_bar) + self._view_note = _fit_text(QLabel("")) + self._view_note.setObjectName("muted") + self._view_note.setMaximumHeight(18) + lay.addWidget(self._view_note) + self._charts: dict[str, ForceChart] = {} + self._pic: dict[str, FrameLabel] = {} self._mini = [QLabel("左指 —"), QLabel("右指 —")] - for index, label in enumerate(self._mini): - label.setObjectName("muted") - label.setWordWrap(True) - card = _card() - box = QVBoxLayout(card) - box.addWidget(_title("左指" if index == 0 else "右指")) - box.addWidget(label) - grid.addWidget(card, index, 2) - lay.addLayout(grid) - flow = QHBoxLayout() + pics = QHBoxLayout() + pics.setSpacing(4) + for index, (side, name) in enumerate((("left", "左指"), ("right", "右指"))): + for kind, caption in (("shear", "切向力"), ("thermal", "热成像")): + col = QVBoxLayout() + col.setSpacing(0) + cap = QLabel(f"{name} {caption}") + cap.setObjectName("muted") + cap.setMaximumHeight(16) + shot = FrameLabel("没有画面", compact=True) + self._pic[f"{side}_{kind}"] = shot + col.addWidget(cap) + col.addWidget(shot, 1) + pics.addLayout(col) + self._mini[index].setObjectName("muted") + self._mini[index].setMaximumHeight(16) + tac_box = QWidget() + tac_box.setLayout(pics) + tac_box.setMinimumHeight(200) + tac_box.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) + self._tac_box = tac_box + lay.addWidget(tac_box, 1) + forces = QHBoxLayout() + for label in self._mini: + forces.addWidget(label) + lay.addLayout(forces) self._flow_labels: list[QLabel] = [] - for name in ("跟踪", "对孔", "搜孔轻压"): + flow = QHBoxLayout() + for name in ("跟踪", "对孔", "旋转装配"): + label = _fit_text(QLabel(name)) + label.setObjectName("muted") + label.setMaximumHeight(18) + self._flow_labels.append(label) + flow.addWidget(label, 1) + lay.addLayout(flow) + stages = " ".join(f"{sid} {name}" for sid, name in catalog()["stages"]) + stage_line = _fit_text(QLabel(stages)) + stage_line.setObjectName("muted") + stage_line.setWordWrap(True) + stage_line.setMaximumHeight(32) + lay.addWidget(stage_line) + self._op_lead = _fit_text(QLabel("")) + self._op_lead.setObjectName("lead") + self._op_lead.setMaximumHeight(18) + lay.addWidget(self._op_lead) + grid = QGridLayout() + grid.setSpacing(4) + groups = ( + ("设备", ("real_up", "gripper_up")), + ("夹爪", ("gripper_probe", "gripper_open", "gripper_close", "gripper_rotate")), + ("作业", ( + "pipeline", "stage_track", "stage_locate", "stage_screw", + "reset_home", "gauge_dry", "gauge_run", + )), + ) + index = 0 + for _heading, actions in groups: + for action in actions: + grid.addWidget(self._node_button(action), index // 4, index % 4) + index += 1 + lay.addLayout(grid) + return page + + def _node_button(self, action: str) -> QPushButton: + spec = self._spec(action) or {} + mark = _NODE_MARKS.get(action, "•") + button = QPushButton(f"{mark} {spec.get('title') or action}") + button.setObjectName("danger" if spec.get("moves") else "node") + button.setMinimumWidth(0) + button.setSizePolicy(QSizePolicy.Policy.Ignored, QSizePolicy.Policy.Fixed) + button.clicked.connect(lambda _checked=False, a=action: self._ask(a)) + return button + + def _toggle_depth(self) -> None: + self._depth_on = not self._depth_on + self._depth_box.setVisible(self._depth_on) + self._depth_btn.setText("◉ 收起深度" if self._depth_on else "◎ 深度") + self._sync_camera_watch() + + def _page_arm(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + self._arm_lead = QLabel( + "只读机械臂状态，不会在这里自动使能。Remote 只能在示教器上开。" + ) + self._arm_lead.setObjectName("lead") + self._arm_lead.setWordWrap(True) + lay.addWidget(self._arm_lead) + self._arm_prompt = QLabel(arm_state.REMOTE_PROMPT) + self._arm_prompt.setObjectName("warn") + self._arm_prompt.setWordWrap(True) + lay.addWidget(self._arm_prompt) + grid = QGridLayout() + self._arm_cards: dict[str, QLabel] = {} + for index, (key, title) in enumerate(( + ("enable", "使能"), + ("mode", "模式"), + ("fault", "故障 / 急停"), + ("source", "数据从哪来"), + )): card = _card() box = QVBoxLayout(card) - box.addWidget(_title(name)) - label = QLabel("") - label.setObjectName("muted") + box.addWidget(_title(title)) + label = QLabel("—") + label.setObjectName("stat" if key == "enable" else "muted") label.setWordWrap(True) box.addWidget(label) - self._flow_labels.append(label) - flow.addWidget(card) - lay.addLayout(flow) - lay.addStretch(1) + self._arm_cards[key] = label + grid.addWidget(card, 0, index) + lay.addLayout(grid) + body = QHBoxLayout() + self._arm_sketch = ArmSketch() + body.addWidget(self._arm_sketch, 1) + side = QVBoxLayout() + self._arm_summary = QLabel("") + self._arm_summary.setWordWrap(True) + self._arm_summary.setObjectName("muted") + side.addWidget(self._arm_summary) + self._arm_joints = QLabel("") + self._arm_joints.setWordWrap(True) + self._arm_joints.setObjectName("muted") + side.addWidget(self._arm_joints) + side.addStretch(1) + holder = QWidget() + holder.setLayout(side) + body.addWidget(holder, 1) + lay.addLayout(body, 1) return page def _page_tactile(self) -> QWidget: @@ -523,9 +1481,10 @@ class StationApp(QMainWindow): page = QWidget() lay = QVBoxLayout(page) lead = QLabel( - "眼在手上用腕部 D405 和 A5 板，结果是相机到法兰。" - "眼在手外用顶置 D435 和 A4 板，结果是相机到基座。两份文件不能混用。" - "示教和求解会动臂。" + "眼在手上用腕部相机和 A5 板，结果是相机到法兰。" + "眼在手外用手外相机和 A4 板，结果是相机到基座。两份文件不能混用。" + "先在这一页看画面，棋盘约占 30%–70% 再采点。示教时相机会被采点程序独占，" + "求解前要重新拉起这路相机。示教和求解会动臂。" ) lead.setWordWrap(True) lead.setObjectName("lead") @@ -539,13 +1498,46 @@ class StationApp(QMainWindow): row.addWidget(button) row.addStretch(1) lay.addLayout(row) + body = QHBoxLayout() + preview = _card() + preview_box = QVBoxLayout(preview) + preview_box.addWidget(_title("当前相机")) + self._he_shot = FrameLabel("预览没开") + self._he_shot.setMinimumHeight(280) + preview_box.addWidget(self._he_shot, 1) + self._he_board = QLabel("还没有画面") + self._he_board.setWordWrap(True) + self._he_board.setObjectName("muted") + preview_box.addWidget(self._he_board) + body.addWidget(preview, 1) + info = _card() + info_box = QVBoxLayout(info) self._he_body = QLabel("") self._he_body.setWordWrap(True) self._he_body.setTextInteractionFlags(Qt.TextInteractionFlag.TextSelectableByMouse) - card = _card() - box = QVBoxLayout(card) - box.addWidget(self._he_body) - lay.addWidget(card) + info_box.addWidget(self._he_body) + info_box.addStretch(1) + body.addWidget(info, 1) + lay.addLayout(body, 1) + tools = QHBoxLayout() + open_cam = _btn("拉起这路相机") + open_cam.clicked.connect(self._handeye_camera) + see = _btn("看画面") + see.clicked.connect(self._handeye_preview_on) + hide = _btn("关掉画面") + hide.clicked.connect(self._handeye_preview_off) + tools.addWidget(open_cam) + tools.addWidget(see) + tools.addWidget(hide) + tools.addWidget(QLabel("目标点数")) + self._he_points = QSpinBox() + self._he_points.setRange(4, 30) + self._he_points.setValue(12) + tools.addWidget(self._he_points) + self._he_append = QCheckBox("接着上次的点") + tools.addWidget(self._he_append) + tools.addStretch(1) + lay.addLayout(tools) actions = QHBoxLayout() for action, danger in ( ("handeye_show", False), @@ -555,14 +1547,18 @@ class StationApp(QMainWindow): ): spec = self._spec(action) button = _btn(spec["title"] if spec else action, danger) - button.clicked.connect(lambda _checked=False, a=action: self._ask(a)) + if action == "handeye_teach": + button.clicked.connect(self._handeye_teach) + else: + button.clicked.connect(lambda _checked=False, a=action: self._ask(a)) actions.addWidget(button) actions.addStretch(1) lay.addLayout(actions) self._he_out = QPlainTextEdit() self._he_out.setReadOnly(True) - self._he_out.setPlaceholderText("查看配置后，脚本输出显示在这里。") - lay.addWidget(self._he_out, 1) + self._he_out.setPlaceholderText("查看配置、合并和求解的输出显示在这里。") + self._he_out.setFixedHeight(140) + lay.addWidget(self._he_out) return page def _page_camera(self) -> QWidget: @@ -603,94 +1599,19 @@ class StationApp(QMainWindow): lay.addWidget(card, 1) return self._scroll(page) - def _page_gripper(self) -> QWidget: - page = QWidget() - lay = QVBoxLayout(page) - self._grip_lead = QLabel("") - self._grip_lead.setWordWrap(True) - self._grip_lead.setObjectName("lead") - lay.addWidget(self._grip_lead) - tools = QHBoxLayout() - for action in ("gripper_probe", "gripper_up"): - spec = self._spec(action) - button = _btn(spec["title"] if spec else action, False) - button.clicked.connect(lambda _checked=False, a=action: self._ask(a)) - tools.addWidget(button) - tools.addStretch(1) - lay.addLayout(tools) - - hold = _card() - box = QVBoxLayout(hold) - box.addWidget(_title("夹持")) - note = QLabel("位置 0 是合拢，1000 是张开，单位是千分比。力度最低 20%。滑条和数字框是同一个值。") - note.setObjectName("muted") - note.setWordWrap(True) - box.addWidget(note) - self._pos = ValueTune("位置", 0, 1000, 500) - self._force = ValueTune("力度 %", 20, 100, 33) - self._speed = ValueTune("速度 %", 1, 100, 25) - box.addWidget(self._pos) - box.addWidget(self._force) - box.addWidget(self._speed) - row = QHBoxLayout() - apply_btn = _btn("按此夹持", True) - open_btn = _btn("张开", True) - close_btn = _btn("闭合", True) - apply_btn.clicked.connect(lambda: self._grip_move("pos")) - open_btn.clicked.connect(lambda: self._grip_preset(1000)) - close_btn.clicked.connect(lambda: self._grip_preset(0)) - row.addWidget(apply_btn) - row.addWidget(open_btn) - row.addWidget(close_btn) - row.addStretch(1) - box.addLayout(row) - lay.addWidget(hold) - - self._rotary = _card() - rot = QVBoxLayout(self._rotary) - rot.addWidget(_title("旋转")) - rot_note = QLabel( - "绝对角和相对角可以打到 ±32767°。滑条只覆盖常用段，数字框是完整范围。" - "圈数可以到小数。力矩和旋转速度跟上面的夹持力度、速度是分开的。" - ) - rot_note.setObjectName("muted") - rot_note.setWordWrap(True) - rot.addWidget(rot_note) - self._angle = ValueTune("绝对角 °", -32767, 32767, 0, step=0.1, decimals=1, slider_low=-3600, slider_high=3600) - self._delta = ValueTune("相对角 °", -32767, 32767, 15, step=0.1, decimals=1, slider_low=-720, slider_high=720) - self._turns = ValueTune("圈数", -50, 50, 0.25, step=0.01, decimals=2, slider_low=-10, slider_high=10) - self._rot_speed = ValueTune("转速 %", 1, 100, 25) - self._torque = ValueTune("力矩 %", 20, 100, 33) - for widget in (self._angle, self._delta, self._turns, self._rot_speed, self._torque): - rot.addWidget(widget) - rot_row = QHBoxLayout() - for title, kind in ( - ("转到绝对角", "rotate"), - ("相对转动", "rotate_by"), - ("按圈数转", "revolutions"), - ("旋转回零", "rotate_home"), - ): - button = _btn(title, True) - button.clicked.connect(lambda _checked=False, k=kind: self._grip_move(k)) - rot_row.addWidget(button) - rot_row.addStretch(1) - rot.addLayout(rot_row) - lay.addWidget(self._rotary) - lay.addStretch(1) - return self._scroll(page) - def _page_control(self) -> QWidget: page = QWidget() lay = QVBoxLayout(page) lead = QLabel( - "分步是单独拉起跟踪、对孔或搜孔。整体是三段一次跑完。" - "复位先停跟踪，再回到命名位姿 home，前提是控制栈已经起来。都会动臂。" + "分步是单独拉起跟踪、对孔或旋转装配。搜孔轻压已取消，整体只跑跟踪和对孔。" + "旋转装配按现场配置里的拧入深度和螺距走。复位先停跟踪，再回到命名位姿 home，" + "前提是控制栈已经起来。都会动臂。" ) lead.setWordWrap(True) lead.setObjectName("lead") lay.addWidget(lead) groups = ( - ("分步测试", ("stage_track", "stage_locate", "stage_search")), + ("分步测试", ("stage_track", "stage_locate", "stage_screw")), ("整体测试", ("pipeline",)), ("复位", ("reset_home",)), ) @@ -708,8 +1629,7 @@ class StationApp(QMainWindow): button.clicked.connect(lambda _checked=False, a=action: self._ask(a)) box.addWidget(button) lay.addWidget(card) - lay.addStretch(1) - return self._scroll(page) + return page def _page_teach(self) -> QWidget: page = QWidget() @@ -760,8 +1680,7 @@ class StationApp(QMainWindow): back.addStretch(1) list_box.addLayout(back) lay.addWidget(listed) - lay.addStretch(1) - return self._scroll(page) + return page def _page_assembly(self) -> QWidget: page = QWidget() @@ -789,9 +1708,21 @@ class StationApp(QMainWindow): row.addWidget(real) row.addStretch(1) lay.addLayout(row) - lay.addStretch(1) return page + def _page_job(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + for title, builder in ( + ("示教", self._page_teach), + ("装配", self._page_assembly), + ("控制", self._page_control), + ("操作", self._page_operate), + ): + lay.addWidget(_title(title)) + lay.addWidget(builder()) + return self._scroll(page) + def _page_operate(self) -> QWidget: page = QWidget() lay = QVBoxLayout(page) @@ -802,7 +1733,7 @@ class StationApp(QMainWindow): skip = { "gripper_open", "gripper_close", "gripper_rotate", "gripper_probe", "handeye_show", "handeye_teach", "handeye_merge", "handeye_calibrate", - "pipeline", "stage_track", "stage_locate", "stage_search", "reset_home", + "pipeline", "stage_track", "stage_locate", "stage_screw", "reset_home", } shown = [spec for spec in catalog()["actions"] if spec["id"] not in skip] for index, spec in enumerate(shown): @@ -813,13 +1744,12 @@ class StationApp(QMainWindow): blurb.setWordWrap(True) blurb.setObjectName("muted") box.addWidget(blurb) - button = _btn("确认后执行" if spec.get("moves") else "执行", bool(spec.get("moves"))) + button = _btn("执行", bool(spec.get("moves"))) button.clicked.connect(lambda _checked=False, a=spec["id"]: self._ask(a)) box.addWidget(button) grid.addWidget(card, index // 3, index % 3) lay.addLayout(grid) - lay.addStretch(1) - return self._scroll(page) + return page def _show_page(self, key: str) -> None: self._stack.setCurrentWidget(self._pages[key]) @@ -827,6 +1757,22 @@ class StationApp(QMainWindow): button.setProperty("active", name == key) button.style().unpolish(button) button.style().polish(button) + self._sync_camera_watch() + + def _sync_camera_watch(self) -> None: + """只有正在看的那一页才处理图像。别的页上相机回调直接丢掉。""" + page = self._stack.currentWidget() + if page is self._pages.get("main"): + keys = {key for key in self._cam_pic if key.endswith("_color")} + if self._depth_on: + keys |= {key for key in self._cam_pic if key.endswith("_depth")} + camera_view.watch(keys, set()) + elif page is self._pages.get("handeye"): + name = str(self._handeye_card().get("preview_camera") or "") + keys = {f"{name}_color"} if name else set() + camera_view.watch(set(), keys) + else: + camera_view.watch(set(), set()) def _set_arm(self, arm: str) -> None: self._arm = arm @@ -840,35 +1786,6 @@ class StationApp(QMainWindow): def _spec(self, action: str) -> dict | None: return next((item for item in catalog()["actions"] if item["id"] == action), None) - def _cmd(self, action: str) -> str: - spec = self._spec(action) - if not spec: - return action - args = " ".join(part.replace("{arm}", self._arm).replace("{mode}", self._mode) for part in spec.get("args") or []) - script = { - "real_up": "run_real.sh", - "camera_d405": "run_camera_d405.sh", - "camera_d435": "run_camera_d435.sh", - "tactile_up": "run_tactile.sh", - "gripper_up": "run_gripper.sh", - "gripper_probe": "run_gripper.sh", - "gripper_open": "run_gripper.sh", - "gripper_close": "run_gripper.sh", - "gripper_rotate": "run_gripper.sh", - "pipeline": "run_all.sh", - "stage_track": "run_visual_servo_rt.sh", - "stage_locate": "run_stage2_locate.sh", - "stage_search": "run_stage3_search.sh", - "reset_home": "go_home.sh", - "gauge_dry": "run_gauge_assembly.sh", - "gauge_run": "run_gauge_assembly.sh", - "handeye_show": "run_handeye.sh", - "handeye_teach": "run_handeye.sh", - "handeye_merge": "run_handeye.sh", - "handeye_calibrate": "run_handeye.sh", - }.get(action, "") - return f"bash scripts/{script} {args}".strip() - def _grip_preset(self, pos: int) -> None: self._pos.set_value(pos) self._grip_move("pos") @@ -885,18 +1802,9 @@ class StationApp(QMainWindow): "torque": self._torque.value(), } send_kind = "rotate" if kind == "rotate_home" else kind - if send_kind != "pos" and self._arm != "91": - self.statusBar().showMessage("旋转只对右臂", 4000) - return - if send_kind == "pos": - text = f"位置 {numbers['pos']:.0f} 力度 {numbers['force']:.0f}% 速度 {numbers['speed']:.0f}%" - elif send_kind == "rotate": - text = f"绝对角 {numbers['angle']:.1f}° 力矩 {numbers['torque']:.0f}% 转速 {numbers['rot_speed']:.0f}%" - elif send_kind == "rotate_by": - text = f"相对 {numbers['delta']:.1f}° 力矩 {numbers['torque']:.0f}% 转速 {numbers['rot_speed']:.0f}%" - else: - text = f"{numbers['revolutions']:.2f} 圈 力矩 {numbers['torque']:.0f}% 转速 {numbers['rot_speed']:.0f}%" - if not self._confirm("夹爪动作", f"臂 {self._arm}。{text}\n夹爪会动。", ""): + current = next((row for row in self._catalog if row["id"] == self._arm), None) + if send_kind != "pos" and not (current and current.get("rotary")): + self.statusBar().showMessage("这台夹爪没有旋转轴", 4000) return captured = dict(numbers) arm = self._arm @@ -913,8 +1821,6 @@ class StationApp(QMainWindow): if not sn: self.statusBar().showMessage("这台臂没有序列号", 4000) return - if not self._confirm("开启软拖拽", f"臂 {self._arm}。会占用 RDK，控制栈必须先停掉。臂可以被手推动。", ""): - return self._start(lambda: teach.drag_on(sn)) def _teach_off(self) -> None: @@ -949,12 +1855,6 @@ class StationApp(QMainWindow): if not sn or not name: self.statusBar().showMessage("先选臂和点位", 3000) return - if not self._confirm( - "回到示教点", - f"臂 {self._arm} 回到 {name}。关节速度 {speed:.2f} rad/s。会先关拖拽再动臂。", - "", - ): - return arm = self._arm self._start(lambda: teach.goto(sn, arm, name, speed)) @@ -963,24 +1863,33 @@ class StationApp(QMainWindow): current = next((arm for arm in arms if arm.get("id") == self._arm), None) return str((current or {}).get("sn") or "") - def _confirm(self, title: str, text: str, command: str) -> bool: - box = QMessageBox(self) - box.setWindowTitle(title) - box.setText(text) - if command: - box.setInformativeText(command) - box.setStandardButtons(QMessageBox.StandardButton.Yes | QMessageBox.StandardButton.No) - box.button(QMessageBox.StandardButton.Yes).setText("确认执行") - box.button(QMessageBox.StandardButton.No).setText("取消") - return box.exec() == QMessageBox.StandardButton.Yes + def _handeye_card(self) -> dict: + return ((self._snap.get("handeye") or {}).get(self._arm) or {}).get(self._mode) or {} + + def _handeye_camera(self) -> None: + action = "camera_d405" if self._mode == "eye_in_hand" else "camera_d435" + self._ask(action) + self.statusBar().showMessage(camera_view.start(), 4000) + + def _handeye_preview_on(self) -> None: + self.statusBar().showMessage(camera_view.start(), 4000) + + def _handeye_preview_off(self) -> None: + camera_view.stop() + self._he_shot.setText("预览没开") + self._he_board.setText("画面已关") + + def _handeye_teach(self) -> None: + spec = self._spec("handeye_teach") + confirm = bool(spec and spec.get("moves")) + arm, mode = self._arm, self._mode + extras = {"points": self._he_points.value(), "append": self._he_append.isChecked()} + self._start(lambda: run_action("handeye_teach", arm, confirm, mode, extras)) def _ask(self, action: str) -> None: + """点下去就下发。动臂仍带 confirm，后台才肯执行，界面不再弹窗。""" spec = self._spec(action) - confirm = False - if spec and spec.get("moves"): - if not self._confirm(spec["title"], f"{spec['blurb']}\n当前臂 {self._arm}。", self._cmd(action)): - return - confirm = True + confirm = bool(spec and spec.get("moves")) arm, mode = self._arm, self._mode self._start(lambda: run_action(action, arm, confirm, mode)) @@ -1017,16 +1926,25 @@ class StationApp(QMainWindow): button.style().unpolish(button) button.style().polish(button) chips = [] + live = (self._snap.get("arm_live") or {}).get(self._arm) or {} + chips.append("已使能" if live.get("enabled") else "未使能") + grip = (self._snap.get("gripper_live") or {}).get(self._arm) or {} + if grip.get("connected") and grip.get("permille") is not None: + chips.append(f"夹爪 {grip['permille']:.0f}‰") + else: + chips.append("夹爪未连接") for service in self._snap.get("services") or []: chips.append(f"{service['label']} {'在跑' if service['up'] else '未开'}") self._chips.setText(" ".join(chips)) self._fill_overview(current) + self._fill_arm(live) self._fill_tactile() self._fill_handeye() self._fill_camera() - self._fill_gripper() + self._sync_camera_watch() + self._fill_gripper(grip) self._fill_teach() - self._op_lead.setText(f"当前臂 {self._arm}。红色按钮会动设备，先确认再执行。") + self._op_lead.setText(f"当前臂 {self._arm}。红色按钮会动设备，点下去就执行。") def _fill_overview(self, current: dict | None) -> None: if current: @@ -1035,6 +1953,12 @@ class StationApp(QMainWindow): f"{current.get('label')} · {current.get('sn') or ''} · 相机 {current.get('camera') or '—'} · " f"夹爪 {current.get('gripper') or '—'} · {tactile}" ) + live = (self._snap.get("arm_live") or {}).get(self._arm) or {} + enable_word = "已使能" if live.get("enabled") else "未使能" + self._arm_banner.setText(f"{enable_word} {live.get('summary') or ''}".strip()) + self._arm_banner.setObjectName("ok" if live.get("enabled") and not live.get("prompt_hot") else "warn") + self._arm_banner.style().unpolish(self._arm_banner) + self._arm_banner.style().polish(self._arm_banner) usb = list(self._snap.get("usb") or []) ports = self._snap.get("grippers") or [] usb.append({"id": "tty", "items": [{"port": name, "speed": ""} for name in ports], "expect": 1}) @@ -1047,7 +1971,7 @@ class StationApp(QMainWindow): count.setText(f"{len(items)} / {dev.get('expect')}") ok = len(items) >= int(dev.get("expect") or 0) flag.setText("齐" if ok else ("缺一" if items else "缺失")) - detail.setText("\n".join( + detail.setText(" ".join( f"{item['port']} · {item['speed']} Mbps" if item.get("speed") else item["port"] for item in items ) or "未检测到") @@ -1059,11 +1983,52 @@ class StationApp(QMainWindow): texts = ( "D405 看住工件，锁住悬停", "拍照找孔，移到孔上方", - "力闭环入孔，再轻压", + "按现场配置的深度和螺距旋进去", ) state = "进程在跑，具体阶段看终端日志。" if pipe and pipe.get("up") else "流程未启动。" for label, text in zip(self._flow_labels, texts): - label.setText(f"{text}\n{state}") + label.setText(f"{text} {state}") + + def _fill_arm(self, live: dict) -> None: + enabled = bool(live.get("enabled")) + self._arm_cards["enable"].setText("已使能" if enabled else "未使能") + mode = live.get("mode") or "—" + op_status = live.get("op_status") or "还没读到" + self._arm_cards["mode"].setText(f"mode {mode}\nop_status {op_status}") + fault = live.get("fault") + estop = live.get("estop_released") + fault_text = "有故障" if fault else ("无故障" if fault is False else "故障未知") + if estop is False: + estop_text = "急停没松开" + elif estop is True: + estop_text = "急停已松开" + else: + estop_text = "急停未知" + self._arm_cards["fault"].setText(f"{fault_text}\n{estop_text}") + self._arm_cards["source"].setText(live.get("source") or "还没有") + self._arm_prompt.setText(live.get("prompt") or arm_state.REMOTE_PROMPT) + self._arm_prompt.setObjectName("warn" if live.get("prompt_hot", True) else "ok") + self._arm_prompt.style().unpolish(self._arm_prompt) + self._arm_prompt.style().polish(self._arm_prompt) + self._arm_summary.setText(live.get("summary") or "") + q = live.get("q") + points = live.get("points") or [] + if isinstance(q, list) and len(q) >= 7: + degrees = " ".join( + f"J{i + 1} {v * 180.0 / 3.141592653589793:.1f}°" for i, v in enumerate(q[:7]) + ) + else: + degrees = "关节角还没有。图上是零位，用来看外形，不是这台臂现在的姿势。" + tip = "" + if points: + x, y, z = points[-1] + tip = f"\n法兰约 x {x * 1000:.0f} y {y * 1000:.0f} z {z * 1000:.0f} mm" + self._arm_joints.setText(degrees + tip) + self._arm_sketch.set_pose( + q if isinstance(q, list) else None, + bool(live.get("pose_live")), + self._workpieces.get(self._arm), + ) def _force_line(self, row: dict) -> str: def num(key: str) -> str: @@ -1084,7 +2049,7 @@ class StationApp(QMainWindow): def _paint_images(self) -> None: current = self._stack.currentWidget() - if current is self._pages.get("tactile"): + if current is self._pages.get("main"): view = tactile_view.state() frames = view.get("frames") or {} if view.get("on") else {} for side in ("left", "right"): @@ -1095,15 +2060,20 @@ class StationApp(QMainWindow): blob = tactile_view.frame(side, kind) if blob: label.show_jpeg(blob) - if current is self._pages.get("camera"): - frames = camera_view.state().get("frames") or {} + shots = camera_view.previews() for key, label in self._cam_pic.items(): - if not frames.get(key): + if key.endswith("_depth") and not self._depth_on: continue - name, _, kind = key.rpartition("_") - blob = camera_view.frame(name, kind) - if blob: - label.show_jpeg(blob) + item = shots.get(key) + if not item: + continue + blob, width, height = item + label.show_rgb(blob, width, height) + infer = camera_view.inference_status() + if infer != self._infer_line.text(): + self._infer_line.setText(infer) + if current is self._pages.get("handeye"): + self._paint_handeye() def _fill_handeye(self) -> None: for mode, button in self._mode_buttons.items(): @@ -1119,17 +2089,47 @@ class StationApp(QMainWindow): mm = "—" name = "眼在手上" if self._mode == "eye_in_hand" else "眼在手外" poses = "没有位姿文件" if card.get("poses") is None else f"{card.get('poses')} 个" + target = self._he_points.value() + preview = card.get("preview_label") or "相机" + cam_id = card.get("preview_camera") or "—" self._he_body.setText( f"{name} {'有结果' if card.get('exists') else '还没有结果文件'}\n" - f"{card.get('meaning') or ''}\n" + f"{card.get('meaning') or '还没有求解结果'}\n" + f"预览 {preview} {cam_id}\n" f"文件 {card.get('file') or '—'}\n" f"视角 {card.get('views') or '—'} 时间 {card.get('utc') or '—'}\n" - f"相机 {card.get('camera') or '—'} 工具 {card.get('tool') or '—'}\n" + f"结果里的相机 {card.get('camera') or '—'} 工具 {card.get('tool') or '—'}\n" f"平移 X / Y / Z {mm}\n" - f"示教点 {poses}\n" + f"已有示教点 {poses} 这次目标 {target} 个\n" f"板 {board.get('cols') or '—'}×{board.get('rows') or '—'} " - f"方格 {board.get('square_m') if board.get('square_m') is not None else '—'} m" + f"方格 {board.get('square_m') if board.get('square_m') is not None else '—'} m\n" + "顺序：看画面 → 停控制栈后采点 → 合并 → 再开控制栈和相机 → 求解" ) + if not camera_view.state().get("on"): + self._he_board.setText("预览没开。点「看画面」。相机节点没起来时再点「拉起这路相机」。") + + def _paint_handeye(self) -> None: + if not camera_view.state().get("on"): + return + card = self._handeye_card() + name = str(card.get("preview_camera") or "") + blob = camera_view.frame(name, "color") if name else None + if not blob: + if not camera_view.state().get("on"): + self._he_board.setText("预览没开。点「看画面」。相机节点没起来时再点「拉起这路相机」。") + else: + self._he_board.setText(f"还没有 {card.get('preview_label') or '这路相机'} 的彩色帧") + return + cols = int((card.get("board") or {}).get("cols") or 0) + rows = int((card.get("board") or {}).get("rows") or 0) + self._board.submit(id(blob), blob, cols, rows) + drawn, text, token = self._board.latest() + if drawn and token == id(blob): + self._he_shot.show_jpeg(drawn) + self._he_board.setText(text) + else: + self._he_shot.show_jpeg(blob) + self._he_board.setText(text or "正在找棋盘") def _fill_camera(self) -> None: view = camera_view.state() @@ -1140,19 +2140,64 @@ class StationApp(QMainWindow): bits.append(f"{name} {len(dev.get('items') or [])} 台") err = view.get("error") or "" ready = "、".join(key for key, ok in (view.get("frames") or {}).items() if ok) or "还没有帧" - self._cam_note.setText(f"{' '.join(bits)}。画面：{ready}。{err}") + text = f"{' '.join(bits)}。画面：{ready}。{err}" + if text != self._cam_note.text(): + self._cam_note.setText(text) - def _fill_gripper(self) -> None: + def _fill_gripper(self, live: dict) -> None: ports = " ".join(self._snap.get("grippers") or []) or "没有 ttyUSB" - self._rotary.setVisible(self._arm == "91") - if self._arm == "91": - self._grip_lead.setText( - f"当前是右臂旋转爪。串口 {ports}。位置、力度、速度以及转角都会按数字下发。夹爪节点占着串口时，命令行会失败。" - ) + current = next((row for row in self._catalog if row["id"] == self._arm), None) + rotary = bool(current and current.get("rotary")) + self._rotary.setVisible(rotary) + kind = "旋转爪" if rotary else "平行爪" + self._grip_lead.setText( + f"{kind} {ports}。滑条是要下发的命令，0 合拢、1000 张开。读状态不初始化、不张合。" + ) + connected = bool(live.get("connected")) + self._grip_summary.setText(live.get("summary") or "还没读到夹爪") + self._grip_summary.setObjectName("ok" if connected else "warn") + self._grip_summary.style().unpolish(self._grip_summary) + self._grip_summary.style().polish(self._grip_summary) + link = "已连接" if connected else "未连接" + if connected: + link += " · 已初始化" if live.get("initialized") else " · 未初始化" + if live.get("source"): + link += f" · {live['source']}" + err = live.get("error") or "" + if err and not connected: + link += f" · {err}" + self._grip_cards["link"].setText(link) + opening = live.get("opening_mm") + permille = live.get("permille") + if opening is None or permille is None: + self._grip_cards["opening"].setText("—") else: - self._grip_lead.setText( - f"当前是左臂平行爪。串口 {ports}。位置 0–1000，力度、速度按滑条下发。探测只读状态。" + self._grip_cards["opening"].setText( + f"{opening:.1f}/{float(live.get('stroke_mm') or 0):.0f} mm {permille:.0f}‰" ) + force = live.get("force") + speed = live.get("speed") + force_text = f"{force:.0f}%" if isinstance(force, (int, float)) else "—" + speed_text = f"{speed:.0f}%" if isinstance(speed, (int, float)) else "—" + self._grip_cards["hold"].setText( + f"{live.get('grip_text') or '—'} {force_text}/{speed_text}" + ) + if live.get("rotary"): + angle = live.get("rotation_deg") + angle_text = f"{angle:.1f}°" if isinstance(angle, (int, float)) else "—" + self._grip_cards["spin"].setText(f"{angle_text} {live.get('rotation_text') or '—'}") + else: + self._grip_cards["spin"].setText("平行爪") + self._grip_sketch.set_pose( + live.get("permille"), + live.get("rotation_deg"), + bool(live.get("rotary")), + bool(live.get("pose_live")), + live.get("stroke_mm"), + ) + draw = getattr(self, "_grip_draw", None) + if draw is not None: + draw.setText(self._grip_sketch._note) def _fill_teach(self) -> None: listed = teach.list_points(self._arm) @@ -1177,12 +2222,22 @@ def main() -> None: atexit.register(camera_view.stop) atexit.register(teach.close) threading.Thread(target=_poll_tactile, daemon=True, name="tactile-poll").start() + arm_state.start() + grip_state.start() camera_view.start() + QApplication.setAttribute(Qt.ApplicationAttribute.AA_ShareOpenGLContexts) + QApplication.setAttribute(Qt.ApplicationAttribute.AA_UseDesktopOpenGL) app = QApplication(sys.argv) app.setFont(QFont("Noto Sans SC", 11)) app.setStyleSheet(_STYLE) window = StationApp() - window.showMaximized() + screen = app.primaryScreen() + if screen is not None: + avail = screen.availableGeometry() + window.resize(min(1600, avail.width()), min(960, avail.height())) + # 不用最大化。桌面如果还记着上次的最大化，晚一拍取消，标题栏才能拖。 + window.showNormal() + QTimer.singleShot(300, window.showNormal) sys.exit(app.exec()) @@ -1192,6 +2247,8 @@ QWidget#side, QWidget#head { background: #0b0c0e; } QWidget#side { border-right: 1px solid #2a2e36; } QWidget#head { border-bottom: 1px solid #2a2e36; } QLabel#sideNote, QLabel#lead, QLabel#muted, QLabel#chips, QLabel#clock { color: #8b929d; } +QLabel#warn { color: #f0c674; } +QLabel#ok { color: #8fce8f; } QLabel#armLine { font-size: 16px; } QLabel#cardTitle { color: #9aa3b2; letter-spacing: 1px; } QLabel#stat { font-size: 28px; } @@ -1201,7 +2258,8 @@ QPushButton { background: #171a20; color: #f3f4f6; border: 1px solid #2a2e36; border-radius: 8px; padding: 8px 12px; text-align: left; } -QPushButton#nav { text-align: left; background: transparent; border: 1px solid transparent; } +QPushButton#nav, QPushButton#node { text-align: left; background: transparent; border: 1px solid transparent; } +QPushButton#node { background: #171a20; border-color: #2a2e36; padding: 6px 8px; } QPushButton#nav[active="true"], QPushButton[active="true"] { background: #12315d; border-color: #1a4580; } QPushButton#danger { background: #6e2a2a; border-color: #8a3a3a; } QPlainTextEdit, QLineEdit, QListWidget, QDoubleSpinBox { @@ -1212,7 +2270,6 @@ QSlider::handle:horizontal { width: 16px; margin: -6px 0; background: #4c86d4; border-radius: 8px; } QScrollArea { background: transparent; } -QMessageBox { background: #12141a; } """ diff --git a/src/arm_application/arm_application/operator_arm_mesh.py b/src/arm_application/arm_application/operator_arm_mesh.py new file mode 100644 index 0000000..20a7634 --- /dev/null +++ b/src/arm_application/arm_application/operator_arm_mesh.py @@ -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]) diff --git a/src/arm_application/arm_application/operator_arm_state.py b/src/arm_application/arm_application/operator_arm_state.py new file mode 100644 index 0000000..d91bd37 --- /dev/null +++ b/src/arm_application/arm_application/operator_arm_state.py @@ -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 diff --git a/src/arm_application/arm_application/operator_camera_view.py b/src/arm_application/arm_application/operator_camera_view.py index da4744e..a602ee0 100644 --- a/src/arm_application/arm_application/operator_camera_view.py +++ b/src/arm_application/arm_application/operator_camera_view.py @@ -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: - return - with lock: - latest[f"{name}_color"] = arr + # 先看这路有没有人要。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) + 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: - return - with lock: - latest[f"{name}_depth"] = arr + 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) + 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) diff --git a/src/arm_application/arm_application/operator_console.py b/src/arm_application/arm_application/operator_console.py index 7aed4c1..d7879ef 100644 --- a/src/arm_application/arm_application/operator_console.py +++ b/src/arm_application/arm_application/operator_console.py @@ -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,40 +591,77 @@ 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]) - if spec.get("sync"): - try: - proc = subprocess.run( - ["bash", str(script), *args], - cwd=str(ROOT), - capture_output=True, - text=True, - timeout=float(spec.get("timeout") or 40), - ) - except subprocess.TimeoutExpired: - return {"ok": False, "message": "命令超时还没返回", "detail": shown} - detail = ((proc.stdout or "") + (proc.stderr or "")).strip() - log_path.write_text(detail) - ok = proc.returncode == 0 - return { - "ok": ok, - "message": shown if ok else f"退出码 {proc.returncode}", - "detail": detail[-4000:], - "log": str(log_path), - } - log_f = log_path.open("ab") - proc = subprocess.Popen( - ["bash", str(script), *args], - cwd=str(ROOT), - stdout=log_f, - stderr=subprocess.STDOUT, - start_new_session=True, - ) - _PROCS[action] = proc.pid - return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)} + try: + if spec.get("sync"): + try: + proc = subprocess.run( + ["bash", str(script), *args], + cwd=str(ROOT), + capture_output=True, + text=True, + timeout=float(spec.get("timeout") or 40), + ) + except subprocess.TimeoutExpired: + return {"ok": False, "message": "命令超时还没返回", "detail": shown} + detail = ((proc.stdout or "") + (proc.stderr or "")).strip() + log_path.write_text(detail) + ok = proc.returncode == 0 + return { + "ok": ok, + "message": shown if ok else f"退出码 {proc.returncode}", + "detail": detail[-4000:], + "log": str(log_path), + } + log_f = log_path.open("ab") + proc = subprocess.Popen( + ["bash", str(script), *args], + cwd=str(ROOT), + stdout=log_f, + stderr=subprocess.STDOUT, + 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) diff --git a/src/arm_application/arm_application/operator_gripper_mesh.py b/src/arm_application/arm_application/operator_gripper_mesh.py new file mode 100644 index 0000000..523abf5 --- /dev/null +++ b/src/arm_application/arm_application/operator_gripper_mesh.py @@ -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 diff --git a/src/arm_application/arm_application/operator_gripper_state.py b/src/arm_application/arm_application/operator_gripper_state.py new file mode 100644 index 0000000..b31d263 --- /dev/null +++ b/src/arm_application/arm_application/operator_gripper_state.py @@ -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)) diff --git a/src/arm_application/arm_application/operator_handeye_board.py b/src/arm_application/arm_application/operator_handeye_board.py new file mode 100644 index 0000000..d7cb7f5 --- /dev/null +++ b/src/arm_application/arm_application/operator_handeye_board.py @@ -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} diff --git a/src/arm_application/arm_application/operator_teach.py b/src/arm_application/arm_application/operator_teach.py index 6761ad8..d1e2a4f 100644 --- a/src/arm_application/arm_application/operator_teach.py +++ b/src/arm_application/arm_application/operator_teach.py @@ -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 - _BRIDGE = RdkBridge(sn) - _SN = sn + release_for_control() + try: + _BRIDGE = RdkBridge(sn) + _SN = sn + finally: + resume_after_control() return _BRIDGE diff --git a/src/arm_application/arm_application/site_io.py b/src/arm_application/arm_application/site_io.py index 8ddc507..c16cb57 100644 --- a/src/arm_application/arm_application/site_io.py +++ b/src/arm_application/arm_application/site_io.py @@ -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(): diff --git a/src/arm_application/test/test_camera_preview.py b/src/arm_application/test/test_camera_preview.py new file mode 100644 index 0000000..b8c8d7b --- /dev/null +++ b/src/arm_application/test/test_camera_preview.py @@ -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" diff --git a/src/arm_application/test/test_handeye_preview.py b/src/arm_application/test/test_handeye_preview.py new file mode 100644 index 0000000..2842c05 --- /dev/null +++ b/src/arm_application/test/test_handeye_preview.py @@ -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}) diff --git a/src/arm_application/test/test_listed_arms.py b/src/arm_application/test/test_listed_arms.py new file mode 100644 index 0000000..fe237c1 --- /dev/null +++ b/src/arm_application/test/test_listed_arms.py @@ -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 diff --git a/src/arm_application/test/test_operator_arm_mesh.py b/src/arm_application/test/test_operator_arm_mesh.py new file mode 100644 index 0000000..8751ac3 --- /dev/null +++ b/src/arm_application/test/test_operator_arm_mesh.py @@ -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 diff --git a/src/arm_application/test/test_operator_arm_state.py b/src/arm_application/test/test_operator_arm_state.py new file mode 100644 index 0000000..f2ec137 --- /dev/null +++ b/src/arm_application/test/test_operator_arm_state.py @@ -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 diff --git a/src/arm_application/test/test_operator_gripper_mesh.py b/src/arm_application/test/test_operator_gripper_mesh.py new file mode 100644 index 0000000..faf4686 --- /dev/null +++ b/src/arm_application/test/test_operator_gripper_mesh.py @@ -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 diff --git a/src/arm_application/test/test_operator_gripper_state.py b/src/arm_application/test/test_operator_gripper_state.py new file mode 100644 index 0000000..63b87b8 --- /dev/null +++ b/src/arm_application/test/test_operator_gripper_state.py @@ -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