diff --git a/scripts/rdk_drag_bridge.py b/scripts/rdk_drag_bridge.py index bde0e4e..af19238 100755 --- a/scripts/rdk_drag_bridge.py +++ b/scripts/rdk_drag_bridge.py @@ -33,6 +33,7 @@ RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。 {"cmd":"drag_on","mode":"soft"} # soft | floating {"cmd":"drag_off"} {"cmd":"joints"} + {"cmd":"move_joints","q":[0,0,0,0,0,0,0],"speed":0.15} {"cmd":"quit"} 每个命令的应答都是单行 JSON，含 ``ok`` 字段；失败时另有 ``error``。 @@ -169,6 +170,69 @@ def _ensure_operational(robot) -> Optional[str]: return ensure_operational(robot) +def _restore_tracking_impedance(robot, flexivrdk) -> None: + """把关节阻抗从软拖拽拉回名义值，否则回放点位时臂跟不住指令。""" + try: + info = robot.info() + groups = dict(getattr(info, 'single_arm_groups', None) or {}) + except Exception as exc: + log(f'读名义阻抗失败: {exc}') + groups = {} + if groups: + for group in groups: + try: + nominal = [float(v) for v in list(info.K_q_nom[group])] + robot.SetJointImpedance(group, nominal) + log(f'恢复阻抗 {group} K={nominal}') + except Exception as exc: + log(f'恢复阻抗 {group}: {exc}') + return + try: + robot.SetJointImpedance([350.0] * 7, [7.0] * 7) + except Exception as exc: + log(f'恢复阻抗: {exc}') + + +def _send_joint_target(robot, flexivrdk, target: List[float], grouped: Optional[bool]) -> bool: + """下发 7 关节目标。返回是否走了分组接口。""" + if grouped is True: + _send_grouped_joint_target(robot, flexivrdk, target) + return True + try: + n = len(target) + robot.SendJointPosition(target, [0.0] * n, [1.5] * n, [2.0] * n) + return False + except Exception as exc: + if grouped is False: + raise + log(f'扁平关节指令失败，改分组: {exc}') + _send_grouped_joint_target(robot, flexivrdk, target) + return True + + +def _send_grouped_joint_target(robot, flexivrdk, target: List[float]) -> None: + info = robot.info() + groups = dict(getattr(info, 'single_arm_groups', None) or {}) + if not groups: + raise RuntimeError('没有分组，扁平关节指令也失败了') + states = robot.states() + cmds = {} + placed = False + for group in groups: + try: + current = [float(v) for v in list(states[group].q)] + except Exception: + current = list(target) + use = target if (not placed and len(current) == len(target)) else current + if use is target: + placed = True + n = len(use) + cmds[group] = flexivrdk.NrtJointPositionCmd(use, [0.0] * n, [1.5] * n, [2.0] * n) + if not placed: + raise RuntimeError('没有一组关节数和示教点一致，拒绝下发') + robot.SendJointPosition(cmds) + + def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool: """切到 IDLE，同时避免把 stdin 循环永久阻塞住。""" done = threading.Event() @@ -423,6 +487,65 @@ def main() -> int: info['tool'] = '' reply(info) + elif cmd == 'move_joints': + if robot is None: + reply({'ok': False, 'error': '尚未连接'}) + continue + raw = msg.get('q') + if not isinstance(raw, list) or len(raw) < 7: + reply({'ok': False, 'error': '需要 7 个关节角（弧度）'}) + continue + try: + target = [float(v) for v in raw[:7]] + except (TypeError, ValueError): + reply({'ok': False, 'error': '关节角不是数字'}) + continue + if any(v != v or abs(v) > 12.6 for v in target): + reply({'ok': False, 'error': '关节角不是有限值，或超出约两圈'}) + continue + try: + speed = float(msg.get('speed', 0.15)) + except (TypeError, ValueError): + speed = 0.15 + speed = max(0.05, min(0.35, speed)) + if soft is not None: + soft.stop() + soft = None + drag_on = False + err = _ensure_operational(robot) + if err: + reply({'ok': False, 'error': err}) + continue + try: + with rlock: + robot.SwitchMode(flexivrdk.Mode.NRT_JOINT_IMPEDANCE) + current = [float(v) for v in list(robot.states().q)[:7]] + _restore_tracking_impedance(robot, flexivrdk) + max_delta = max(abs(a - b) for a, b in zip(current, target)) + step_s = 0.05 + steps = max(1, int(max_delta / (speed * step_s))) + if steps * step_s > 150.0: + reply({ + 'ok': False, + 'error': f'路程太长（{max_delta:.2f} rad），请先拖近一点再回放', + }) + continue + grouped: Optional[bool] = None + for index in range(1, steps + 1): + alpha = index / steps + sample = [a + (b - a) * alpha for a, b in zip(current, target)] + with rlock: + grouped = _send_joint_target(robot, flexivrdk, sample, grouped) + time.sleep(step_s) + for _hold in range(8): + with rlock: + grouped = _send_joint_target(robot, flexivrdk, target, grouped) + time.sleep(step_s) + cached_q = list(target) + reply({'ok': True, 'q': target, 'from': current, 'steps': steps, 'speed': speed}) + except Exception as exc: + reply({'ok': False, 'error': f'回点失败: {exc}'}) + elif cmd == 'quit': if soft is not None: soft.stop() diff --git a/scripts/run_console.sh b/scripts/run_console.sh new file mode 100755 index 0000000..fbfde37 --- /dev/null +++ b/scripts/run_console.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash +# Orisys 通止规工位桌面程序（窗口，不是网页）。 +# +# bash ~/ros2/scripts/run_console.sh + +set -eo pipefail +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 +fi +if [[ -z "${XAUTHORITY:-}" && -r "/run/user/$(id -u)/gdm/Xauthority" ]]; then + export XAUTHORITY="/run/user/$(id -u)/gdm/Xauthority" +fi +exec python3 -m arm_application.operator_app diff --git a/scripts/run_gripper.sh b/scripts/run_gripper.sh index 62f7e30..3b53d7a 100755 --- a/scripts/run_gripper.sh +++ b/scripts/run_gripper.sh @@ -12,7 +12,9 @@ # bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open # bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close # bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90 +# bash ~/ros2/scripts/run_gripper.sh --arm 91 --force 40 --speed 20 --cli rotate_by -15 # bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2 +# --force / --speed 写在 --cli 前面，覆盖 site.yaml 里的力度和速度。 # bash ~/ros2/scripts/run_gripper.sh --arm 89 # ROS 节点 PGEA # bash ~/ros2/scripts/run_gripper.sh --arm 91 # ROS 节点 RGI # bash ~/ros2/scripts/run_dual_gripper.sh # 两个 ROS 节点都起 @@ -21,6 +23,8 @@ set -eo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" CLI=() DUAL=0 +GRIP_FORCE="" +GRIP_SPEED="" # shellcheck disable=SC1091 source "${ROOT}/scripts/lib/parse_arm_args.sh" @@ -29,6 +33,8 @@ set -- "${PARSE_ARM_REST[@]}" while [[ $# -gt 0 ]]; do case "$1" in + --force) GRIP_FORCE="${2:?}"; shift 2 ;; + --speed) GRIP_SPEED="${2:?}"; shift 2 ;; --cli) shift; CLI=("$@"); break ;; --dual) DUAL=1; shift ;; -h|--help) sed -n '2,24p' "$0"; exit 0 ;; @@ -70,11 +76,17 @@ if [[ ${#CLI[@]} -gt 0 ]]; then ARGS=(--arm right --test rotate) if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi ;; + rotate_by) + ARGS=(--arm right --test rotate_by) + if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi + ;; revolutions) ARGS=(--arm right --test rotate) if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi ;; esac + if [[ -n "${GRIP_FORCE}" ]]; then ARGS+=(--force "${GRIP_FORCE}"); fi + if [[ -n "${GRIP_SPEED}" ]]; then ARGS+=(--speed "${GRIP_SPEED}"); fi echo "=== DH RGI SN=${SITE_GRIP_SN:-?} ${SITE_GRIP_PORT} arm=${SITE_ROBOT_SHORT_ID:-?} ===" exec python3 -m arm_application.dual_gripper_cli "${ARGS[@]}" fi @@ -84,7 +96,7 @@ if [[ ${#CLI[@]} -gt 0 ]]; then echo "=== DH ${MODEL} SN=${SITE_GRIP_SN:-?} ${PORT} @${BAUD} slave=${SLAVE} ===" exec python3 "${ROOT}/src/arm_application/arm_application/dh_pgea.py" \ --port "${PORT}" --baud "${BAUD}" --slave "${SLAVE}" \ - --force "${SITE_GRIP_FORCE:-50}" --speed "${SITE_GRIP_SPEED:-50}" \ + --force "${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}" --speed "${GRIP_SPEED:-${SITE_GRIP_SPEED:-50}}" \ --rts "${SITE_GRIP_RS485_RTS:-}" \ "${CLI[@]}" fi diff --git a/src/arm_application/arm_application/handeye_teach.py b/src/arm_application/arm_application/handeye_teach.py index a78b0fc..e2f5b43 100644 --- a/src/arm_application/arm_application/handeye_teach.py +++ b/src/arm_application/arm_application/handeye_teach.py @@ -220,6 +220,13 @@ class RdkBridge: return self._last_q return self._last_q + def move_joints(self, joints: List[float], speed: float = 0.15) -> dict: + """按关节阻抗把臂缓慢送到已记录的 7 个关节角。speed 单位 rad/s。""" + return self._ask( + {'cmd': 'move_joints', 'q': [float(v) for v in joints[:7]], 'speed': float(speed)}, + timeout=180.0, + ) + def close(self) -> None: try: self._ask({'cmd': 'quit'}, timeout=3.0) diff --git a/src/arm_application/arm_application/operator_app.py b/src/arm_application/arm_application/operator_app.py new file mode 100644 index 0000000..2c38a27 --- /dev/null +++ b/src/arm_application/arm_application/operator_app.py @@ -0,0 +1,1220 @@ +"""Orisys 通止规工位桌面程序。 + +不是网页。运行： + + bash ~/ros2/scripts/run_console.sh +""" + +from __future__ import annotations + +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.QtWidgets import ( + QApplication, + QDoubleSpinBox, + QFrame, + QGridLayout, + QHBoxLayout, + QLabel, + QLineEdit, + QListWidget, + QListWidgetItem, + QMainWindow, + QMessageBox, + QPlainTextEdit, + QPushButton, + QScrollArea, + QSizePolicy, + QSlider, + QStackedWidget, + QVBoxLayout, + QWidget, +) + +from arm_application.operator_console import ( + _poll_tactile, + catalog, + run_action, + run_gripper_tune, + status, +) +from arm_application import operator_teach as teach +from arm_application import operator_camera_view as camera_view +from arm_application import operator_tactile_view as tactile_view + +_LOGO = Path(__file__).resolve().parent / "operator_console_logo.png" +_SERIES = ( + ("fnormal", QColor("#e8e8e8"), "法向"), + ("fshearx", QColor("#4c86d4"), "切向 X"), + ("fsheary", QColor("#d7a15a"), "切向 Y"), +) + + +class _Job(QThread): + finished_ok = Signal(dict) + + def __init__(self, fn) -> None: + super().__init__() + self._fn = fn + + def run(self) -> None: + try: + result = self._fn() + except Exception as exc: + result = {"ok": False, "message": str(exc)} + self.finished_ok.emit(result) + + +class ValueTune(QWidget): + """一条滑条，旁边是可以改的数字。滑条可以比数字框窄，用来细调常用范围。""" + + def __init__( + self, + title: str, + low: float, + high: float, + value: float, + *, + step: float = 1.0, + decimals: int = 0, + slider_low: float | None = None, + slider_high: float | None = None, + ) -> None: + super().__init__() + self._scale = 10 ** decimals + self._guard = False + row = QHBoxLayout(self) + row.setContentsMargins(0, 0, 0, 0) + label = QLabel(title) + label.setFixedWidth(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 + self.slider.setRange(int(round(slo * self._scale)), int(round(shi * self._scale))) + self.spin = QDoubleSpinBox() + self.spin.setRange(low, high) + self.spin.setDecimals(decimals) + self.spin.setSingleStep(step) + self.spin.setValue(value) + self.slider.setValue(self._clamp_slider(value)) + self.slider.valueChanged.connect(self._from_slider) + self.spin.valueChanged.connect(self._from_spin) + row.addWidget(label) + row.addWidget(self.slider, 1) + self.spin.setFixedWidth(120) + row.addWidget(self.spin) + + def _clamp_slider(self, value: float) -> int: + raw = int(round(value * self._scale)) + return max(self.slider.minimum(), min(self.slider.maximum(), raw)) + + def _from_slider(self, raw: int) -> None: + if self._guard: + return + self._guard = True + self.spin.setValue(raw / self._scale) + self._guard = False + + def _from_spin(self, value: float) -> None: + if self._guard: + return + self._guard = True + self.slider.setValue(self._clamp_slider(value)) + self._guard = False + + def value(self) -> float: + return float(self.spin.value()) + + def set_value(self, value: float) -> None: + self.spin.setValue(value) + + +class FrameLabel(QLabel): + """预览框。尺寸不跟图像走，否则每贴一帧标签就更大，画面会慢慢放大。""" + + def __init__(self, text: str) -> None: + super().__init__(text) + self.setAlignment(Qt.AlignmentFlag.AlignCenter) + self.setMinimumSize(320, 200) + self.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) + self.setObjectName("shot") + self._source = QPixmap() + self._token = 0 + self._drawn = QSize() + self._busy = False + + def sizeHint(self) -> QSize: + return QSize(480, 270) + + def minimumSizeHint(self) -> QSize: + return QSize(320, 200) + + 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() + self._draw(False) + + def resizeEvent(self, event) -> None: + super().resizeEvent(event) + self._drawn = QSize() + self._draw(True) + + def _draw(self, smooth: bool) -> None: + if self._busy or self._source.isNull(): + return + size = self.contentsRect().size() + if size.width() < 8 or size.height() < 8 or size == self._drawn: + return + self._busy = True + try: + mode = ( + Qt.TransformationMode.SmoothTransformation + if smooth + else Qt.TransformationMode.FastTransformation + ) + self.setPixmap(self._source.scaled( + size, + Qt.AspectRatioMode.KeepAspectRatio, + mode, + )) + self.setText("") + self._drawn = QSize(size) + finally: + self._busy = False + + +class ForceChart(QWidget): + """法向、切向 X、切向 Y 共用一条时间轴。""" + + def __init__(self) -> None: + super().__init__() + self._samples: list[dict] = [] + self.setMinimumHeight(210) + self.setSizePolicy(QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding) + + def set_samples(self, samples: list[dict] | None) -> None: + self._samples = list(samples or []) + self.update() + + def paintEvent(self, _event) -> None: + painter = QPainter(self) + painter.setRenderHint(QPainter.RenderHint.Antialiasing) + w, h = self.width(), self.height() + painter.fillRect(self.rect(), QColor("#0c0e12")) + rows = [ + p for p in self._samples + if any(isinstance(p.get(k), (int, float)) for k, _, _ in _SERIES) + ] + if len(rows) < 2: + painter.setPen(QColor("#8b929d")) + painter.drawText(16, 28, "还没有足够的采样") + painter.end() + return + lo, hi = float("inf"), float("-inf") + for point in rows: + for key, _, _ in _SERIES: + value = point.get(key) + if isinstance(value, (int, float)): + lo = min(lo, float(value)) + hi = max(hi, float(value)) + if not hi > lo: + hi = lo + 1.0 + pad = (hi - lo) * 0.08 + lo -= pad + hi += pad + t0 = float(rows[0]["t"]) + t1 = float(rows[-1]["t"]) + span = max(t1 - t0, 1e-3) + left, right, top, bottom = 48, 12, 14, 22 + zero_y = top + (hi - 0.0) / (hi - lo) * (h - top - bottom) + if top < zero_y < h - bottom: + painter.setPen(QPen(QColor("#2a2e36"), 1)) + painter.drawLine(left, int(zero_y), w - right, int(zero_y)) + for key, color, _name in _SERIES: + painter.setPen(QPen(color, 1.6)) + started = False + last = (0, 0) + for point in rows: + value = point.get(key) + if not isinstance(value, (int, float)): + continue + x = left + (float(point["t"]) - t0) / span * (w - left - right) + y = top + (hi - float(value)) / (hi - lo) * (h - top - bottom) + if started: + painter.drawLine(int(last[0]), int(last[1]), int(x), int(y)) + started = True + last = (x, y) + painter.setPen(QColor("#8b929d")) + painter.drawText(4, top + 12, f"{hi:.0f}") + painter.drawText(4, h - bottom, f"{lo:.0f}") + painter.drawText(w - 52, h - 6, f"{span:.0f} 秒") + painter.end() + + +def _card() -> QFrame: + frame = QFrame() + frame.setObjectName("card") + return frame + + +def _title(text: str) -> QLabel: + label = QLabel(text) + label.setObjectName("cardTitle") + return label + + +def _btn(text: str, danger: bool = False) -> QPushButton: + button = QPushButton(text) + button.setObjectName("danger" if danger else "action") + return button + + +class StationApp(QMainWindow): + def __init__(self) -> None: + super().__init__() + self.setWindowTitle("ORISYS 通止规工位") + self.resize(1440, 900) + self._arm = "89" + self._mode = "eye_in_hand" + self._snap: dict = {} + self._job: _Job | None = None + self._teach_sig: tuple | None = None + self._pages: dict[str, QWidget] = {} + self._nav: list[tuple[str, QPushButton]] = [] + self._build() + self._timer = QTimer(self) + self._timer.timeout.connect(self._refresh) + self._timer.start(800) + self._images = QTimer(self) + self._images.timeout.connect(self._paint_images) + self._images.start(200) + self._stack.currentChanged.connect(lambda _index: self._paint_images()) + self._refresh() + + def _build(self) -> None: + root = QWidget() + self.setCentralWidget(root) + layout = QHBoxLayout(root) + layout.setContentsMargins(0, 0, 0, 0) + layout.setSpacing(0) + 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}")() + 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.setPlaceholderText("命令输出显示在这里") + right.addWidget(self._detail) + holder = QWidget() + holder.setLayout(right) + layout.addWidget(holder, 1) + self._show_page("overview") + + def _sidebar(self) -> QWidget: + bar = QWidget() + bar.setObjectName("side") + bar.setFixedWidth(220) + lay = QVBoxLayout(bar) + logo = QLabel() + if _LOGO.is_file(): + pix = QPixmap(str(_LOGO)) + logo.setPixmap(pix.scaledToWidth(180, 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", "操作", "启动与确认"), + ) + for key, name, hint in items: + button = QPushButton(f"{name}\n{hint}") + 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) + lay = QHBoxLayout(bar) + self._arm_line = QLabel("左臂 89") + self._arm_line.setObjectName("armLine") + lay.addWidget(self._arm_line) + lay.addStretch(1) + self._chips = QLabel("") + self._chips.setObjectName("chips") + lay.addWidget(self._chips) + 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 + lay.addWidget(button) + self._clock = QLabel("--:--:--") + self._clock.setObjectName("clock") + lay.addWidget(self._clock) + return bar + + def _scroll(self, inner: QWidget) -> QScrollArea: + area = QScrollArea() + area.setWidgetResizable(True) + area.setFrameShape(QFrame.Shape.NoFrame) + area.setWidget(inner) + return area + + def _page_overview(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + self._lead = QLabel("") + self._lead.setObjectName("lead") + self._lead.setWordWrap(True) + lay.addWidget(self._lead) + grid = QGridLayout() + self._usb_labels: list[tuple[QLabel, QLabel, QLabel]] = [] + for index, name in enumerate(("腕部 D405", "手外 D435", "视触觉", "夹爪串口")): + card = _card() + box = QVBoxLayout(card) + box.addWidget(_title(name)) + count = QLabel("—") + count.setObjectName("stat") + flag = QLabel("") + detail = QLabel("") + detail.setObjectName("muted") + detail.setWordWrap(True) + box.addWidget(count) + box.addWidget(flag) + box.addWidget(detail) + grid.addWidget(card, index // 2, index % 2) + self._usb_labels.append((count, flag, detail)) + 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() + self._flow_labels: list[QLabel] = [] + for name in ("跟踪", "对孔", "搜孔轻压"): + card = _card() + box = QVBoxLayout(card) + box.addWidget(_title(name)) + label = QLabel("") + label.setObjectName("muted") + label.setWordWrap(True) + box.addWidget(label) + self._flow_labels.append(label) + flow.addWidget(card) + lay.addLayout(flow) + lay.addStretch(1) + return page + + def _page_tactile(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + lead = QLabel( + "切向力是 SDK 的剪切场箭头，热成像是接触散度的伪彩。" + "曲线是法向、切向 X、切向 Y，同一条时间轴。单位是形变读数，不是牛顿。" + ) + lead.setObjectName("lead") + lead.setWordWrap(True) + lay.addWidget(lead) + row = QHBoxLayout() + start = _btn("开始成像") + stop = _btn("停止成像") + node = _btn("启动三力节点") + kill = _btn("停止触觉节点") + start.clicked.connect(lambda: self._run("tactile_view_on", False)) + stop.clicked.connect(lambda: self._run("tactile_view_off", False)) + node.clicked.connect(lambda: self._run("tactile_up", False)) + kill.clicked.connect(lambda: self._run("tactile_down", False)) + for button in (start, stop, node, kill): + row.addWidget(button) + row.addStretch(1) + lay.addLayout(row) + self._view_note = QLabel("") + self._view_note.setObjectName("muted") + self._view_note.setWordWrap(True) + lay.addWidget(self._view_note) + pics = QHBoxLayout() + self._pic: dict[str, QLabel] = {} + for side, name in (("left", "左指"), ("right", "右指")): + card = _card() + box = QVBoxLayout(card) + box.addWidget(_title(name)) + pair = QHBoxLayout() + for kind, caption in (("shear", "切向力"), ("thermal", "热成像")): + col = QVBoxLayout() + cap = QLabel(caption) + cap.setObjectName("muted") + shot = FrameLabel("没有画面") + self._pic[f"{side}_{kind}"] = shot + col.addWidget(cap) + col.addWidget(shot, 1) + pair.addLayout(col) + box.addLayout(pair) + pics.addWidget(card) + lay.addLayout(pics) + charts = QHBoxLayout() + self._charts: dict[str, ForceChart] = {} + for side, name in (("left", "左指"), ("right", "右指")): + card = _card() + box = QVBoxLayout(card) + box.addWidget(_title(name)) + legend = QLabel("法向 切向 X 切向 Y") + legend.setObjectName("muted") + box.addWidget(legend) + chart = ForceChart() + self._charts[side] = chart + box.addWidget(chart, 1) + charts.addWidget(card) + lay.addLayout(charts, 1) + return page + + def _page_handeye(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + lead = QLabel( + "眼在手上用腕部 D405 和 A5 板，结果是相机到法兰。" + "眼在手外用顶置 D435 和 A4 板，结果是相机到基座。两份文件不能混用。" + "示教和求解会动臂。" + ) + lead.setWordWrap(True) + lead.setObjectName("lead") + lay.addWidget(lead) + row = QHBoxLayout() + self._mode_buttons: dict[str, QPushButton] = {} + for mode, text in (("eye_in_hand", "眼在手上"), ("eye_to_hand", "眼在手外")): + button = _btn(text) + button.clicked.connect(lambda _checked=False, m=mode: self._set_mode(m)) + self._mode_buttons[mode] = button + row.addWidget(button) + row.addStretch(1) + lay.addLayout(row) + 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) + actions = QHBoxLayout() + for action, danger in ( + ("handeye_show", False), + ("handeye_teach", True), + ("handeye_merge", False), + ("handeye_calibrate", True), + ): + spec = self._spec(action) + button = _btn(spec["title"] if spec else action, danger) + 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) + return page + + def _page_camera(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + lead = QLabel("彩色和深度都画在这一页。左右红外不显示，也不再另开预览窗。视触觉的切向力和热成像在「视触觉」页。") + lead.setObjectName("lead") + lead.setWordWrap(True) + lay.addWidget(lead) + self._cam_note = QLabel("") + self._cam_note.setObjectName("muted") + self._cam_note.setWordWrap(True) + lay.addWidget(self._cam_note) + row = QHBoxLayout() + for action in ("camera_d405", "camera_d435"): + spec = self._spec(action) + button = _btn(spec["title"] if spec else action) + button.clicked.connect(lambda _checked=False, a=action: self._ask(a)) + row.addWidget(button) + row.addStretch(1) + lay.addLayout(row) + self._cam_pic: dict[str, QLabel] = {} + for cam in camera_view.targets(): + card = _card() + box = QVBoxLayout(card) + box.addWidget(_title(cam["label"])) + pair = QHBoxLayout() + for kind, caption in (("color", "彩色"), ("depth", "深度")): + col = QVBoxLayout() + cap = QLabel(caption) + cap.setObjectName("muted") + shot = FrameLabel("没有画面") + self._cam_pic[f"{cam['id']}_{kind}"] = shot + col.addWidget(cap) + col.addWidget(shot, 1) + pair.addLayout(col) + box.addLayout(pair) + 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，前提是控制栈已经起来。都会动臂。" + ) + lead.setWordWrap(True) + lead.setObjectName("lead") + lay.addWidget(lead) + groups = ( + ("分步测试", ("stage_track", "stage_locate", "stage_search")), + ("整体测试", ("pipeline",)), + ("复位", ("reset_home",)), + ) + for heading, actions in groups: + card = _card() + box = QVBoxLayout(card) + box.addWidget(_title(heading)) + for action in actions: + spec = self._spec(action) or {} + blurb = QLabel(spec.get("blurb") or "") + blurb.setWordWrap(True) + blurb.setObjectName("muted") + box.addWidget(blurb) + button = _btn(spec.get("title") or action, True) + button.clicked.connect(lambda _checked=False, a=action: self._ask(a)) + box.addWidget(button) + lay.addWidget(card) + lay.addStretch(1) + return self._scroll(page) + + def _page_teach(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + lead = QLabel( + "软拖拽占用 RDK，要先停掉控制栈。记录的是当时的 7 个关节角，单位弧度。" + "回到点位会先关掉拖拽，再按下面的关节速度走过去。和 home 复位不是同一条路。" + ) + lead.setWordWrap(True) + lead.setObjectName("lead") + lay.addWidget(lead) + card = _card() + box = QVBoxLayout(card) + box.addWidget(_title("拖拽与记点")) + name_row = QHBoxLayout() + name_row.addWidget(QLabel("点名")) + self._teach_name = QLineEdit("p1") + name_row.addWidget(self._teach_name, 1) + box.addLayout(name_row) + self._teach_speed = ValueTune("速度 rad/s", 0.05, 0.35, 0.15, step=0.01, decimals=2) + box.addWidget(self._teach_speed) + row = QHBoxLayout() + on_btn = _btn("开启软拖拽", True) + off_btn = _btn("关闭拖拽") + rec_btn = _btn("记录当前点") + on_btn.clicked.connect(self._teach_on) + off_btn.clicked.connect(self._teach_off) + rec_btn.clicked.connect(self._teach_record) + row.addWidget(on_btn) + row.addWidget(off_btn) + row.addWidget(rec_btn) + row.addStretch(1) + box.addLayout(row) + lay.addWidget(card) + listed = _card() + list_box = QVBoxLayout(listed) + list_box.addWidget(_title("已记录的点")) + self._teach_list = QListWidget() + self._teach_list.setMinimumHeight(180) + list_box.addWidget(self._teach_list) + back = QHBoxLayout() + go_btn = _btn("回到选中点", True) + del_btn = _btn("删除选中点") + go_btn.clicked.connect(self._teach_goto) + del_btn.clicked.connect(self._teach_delete) + back.addWidget(go_btn) + back.addWidget(del_btn) + back.addStretch(1) + list_box.addLayout(back) + lay.addWidget(listed) + lay.addStretch(1) + return self._scroll(page) + + def _page_assembly(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + lead = QLabel("顺序与状态机一致。干跑只打印计划。真跑会动臂、会拧，这台机器上还没跑过完整真机装配。") + lead.setWordWrap(True) + lead.setObjectName("lead") + lay.addWidget(lead) + grid = QGridLayout() + for index, (sid, name) in enumerate(catalog()["stages"]): + card = _card() + box = QVBoxLayout(card) + head = QLabel(sid) + head.setObjectName("cardTitle") + box.addWidget(head) + box.addWidget(QLabel(name)) + grid.addWidget(card, index // 6, index % 6) + lay.addLayout(grid) + row = QHBoxLayout() + dry = _btn("干跑") + real = _btn("真跑", True) + dry.clicked.connect(lambda: self._ask("gauge_dry")) + real.clicked.connect(lambda: self._ask("gauge_run")) + row.addWidget(dry) + row.addWidget(real) + row.addStretch(1) + lay.addLayout(row) + lay.addStretch(1) + return page + + def _page_operate(self) -> QWidget: + page = QWidget() + lay = QVBoxLayout(page) + self._op_lead = QLabel("") + self._op_lead.setObjectName("lead") + lay.addWidget(self._op_lead) + grid = QGridLayout() + 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", + } + shown = [spec for spec in catalog()["actions"] if spec["id"] not in skip] + for index, spec in enumerate(shown): + card = _card() + box = QVBoxLayout(card) + box.addWidget(_title(spec["title"])) + blurb = QLabel(spec["blurb"]) + blurb.setWordWrap(True) + blurb.setObjectName("muted") + box.addWidget(blurb) + button = _btn("确认后执行" if spec.get("moves") else "执行", 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) + + def _show_page(self, key: str) -> None: + self._stack.setCurrentWidget(self._pages[key]) + for name, button in self._nav: + button.setProperty("active", name == key) + button.style().unpolish(button) + button.style().polish(button) + + def _set_arm(self, arm: str) -> None: + self._arm = arm + self._teach_sig = None + self._refresh() + + def _set_mode(self, mode: str) -> None: + self._mode = mode + self._refresh() + + 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") + + def _grip_move(self, kind: str) -> None: + numbers = { + "pos": self._pos.value(), + "force": self._force.value(), + "speed": self._speed.value(), + "angle": 0.0 if kind == "rotate_home" else self._angle.value(), + "delta": self._delta.value(), + "revolutions": self._turns.value(), + "rot_speed": self._rot_speed.value(), + "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夹爪会动。", ""): + return + captured = dict(numbers) + arm = self._arm + self._start(lambda: run_gripper_tune(arm, send_kind, captured)) + + def _selected_point(self) -> str: + item = self._teach_list.currentItem() + if item is None: + return "" + return str(item.data(Qt.ItemDataRole.UserRole) or "") + + def _teach_on(self) -> None: + sn = self._sn() + 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: + sn = self._sn() + if not sn: + self.statusBar().showMessage("这台臂没有序列号", 4000) + return + self._start(lambda: teach.drag_off(sn)) + + def _teach_record(self) -> None: + sn = self._sn() + name = self._teach_name.text().strip() + if not sn: + self.statusBar().showMessage("这台臂没有序列号", 4000) + return + arm = self._arm + self._start(lambda: teach.record(sn, arm, name)) + + def _teach_delete(self) -> None: + name = self._selected_point() + if not name: + self.statusBar().showMessage("先选一个点", 3000) + return + arm = self._arm + self._teach_sig = None + self._start(lambda: teach.delete_point(arm, name)) + + def _teach_goto(self) -> None: + sn = self._sn() + name = self._selected_point() + speed = self._teach_speed.value() + 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)) + + def _sn(self) -> str: + arms = (self._snap or {}).get("arms") or [] + 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 _ask(self, action: str) -> None: + 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 + arm, mode = self._arm, self._mode + self._start(lambda: run_action(action, arm, confirm, mode)) + + def _start(self, fn) -> None: + if self._job is not None and self._job.isRunning(): + self.statusBar().showMessage("上一个命令还没返回", 3000) + return + self._job = _Job(fn) + self._job.finished_ok.connect(self._ran) + self._job.start() + + def _ran(self, result: dict) -> None: + text = result.get("message") or ("已发出" if result.get("ok") else "失败") + self.statusBar().showMessage(text, 8000) + detail = result.get("detail") or text + self._detail.setPlainText(detail) + if getattr(self, "_he_out", None) is not None and result.get("detail"): + self._he_out.setPlainText(result["detail"]) + self._teach_sig = None + + def _refresh(self) -> None: + try: + self._snap = status() + except Exception as exc: + self.statusBar().showMessage(f"状态刷新失败：{exc}", 4000) + return + from datetime import datetime + self._clock.setText(datetime.now().strftime("%H:%M:%S")) + arms = self._snap.get("arms") or [] + current = next((arm for arm in arms if arm["id"] == self._arm), None) + self._arm_line.setText(current["label"] if current else f"臂 {self._arm}") + for arm, button in self._arm_buttons.items(): + button.setProperty("active", arm == self._arm) + button.style().unpolish(button) + button.style().polish(button) + chips = [] + 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_tactile() + self._fill_handeye() + self._fill_camera() + self._fill_gripper() + self._fill_teach() + self._op_lead.setText(f"当前臂 {self._arm}。红色按钮会动设备，先确认再执行。") + + def _fill_overview(self, current: dict | None) -> None: + if current: + tactile = "本臂有视触觉" if current.get("tactile") else "本臂无视触觉" + self._lead.setText( + f"{current.get('label')} · {current.get('sn') or ''} · 相机 {current.get('camera') or '—'} · " + f"夹爪 {current.get('gripper') or '—'} · {tactile}" + ) + 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}) + for (count, flag, detail), dev in zip(self._usb_labels, usb): + items = dev.get("items") or [] + if dev.get("id") == "tty": + count.setText(str(len(items))) + ok = len(items) > 0 + else: + 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( + f"{item['port']} · {item['speed']} Mbps" if item.get("speed") else item["port"] + for item in items + ) or "未检测到") + tactile = self._snap.get("tactile") or {} + for label, side, name in ((self._mini[0], "left", "左指"), (self._mini[1], "right", "右指")): + row = tactile.get(side) or {} + label.setText(self._force_line(row)) + pipe = next((s for s in self._snap.get("services") or [] if s["id"] == "pipeline"), None) + 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}") + + def _force_line(self, row: dict) -> str: + def num(key: str) -> str: + value = row.get(key) + return f"{value:.1f}" if isinstance(value, (int, float)) else "—" + return f"法向 {num('fnormal')} 切向 X {num('fshearx')} 切向 Y {num('fsheary')} {num('fps')} fps" + + def _fill_tactile(self) -> None: + view = self._snap.get("view") or {} + self._view_note.setText( + view.get("error") + or view.get("note") + or ("成像已开，等第一帧。" if view.get("on") else "成像没开。点开始成像后这里出切向力和热成像。") + ) + history = self._snap.get("history") or {} + for side, chart in self._charts.items(): + chart.set_samples(history.get(side)) + + def _paint_images(self) -> None: + current = self._stack.currentWidget() + if current is self._pages.get("tactile"): + view = tactile_view.state() + frames = view.get("frames") or {} if view.get("on") else {} + for side in ("left", "right"): + for kind in ("shear", "thermal"): + label = self._pic.get(f"{side}_{kind}") + if label is None or not frames.get(f"{side}_{kind}"): + continue + 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 {} + for key, label in self._cam_pic.items(): + if not frames.get(key): + continue + name, _, kind = key.rpartition("_") + blob = camera_view.frame(name, kind) + if blob: + label.show_jpeg(blob) + + def _fill_handeye(self) -> None: + for mode, button in self._mode_buttons.items(): + button.setProperty("active", mode == self._mode) + button.style().unpolish(button) + button.style().polish(button) + card = ((self._snap.get("handeye") or {}).get(self._arm) or {}).get(self._mode) or {} + board = card.get("board") or {} + trans = card.get("translation_m") + if isinstance(trans, list) and len(trans) >= 3: + mm = " ".join(f"{float(v) * 1000:.1f} mm" for v in trans[:3]) + else: + mm = "—" + name = "眼在手上" if self._mode == "eye_in_hand" else "眼在手外" + poses = "没有位姿文件" if card.get("poses") is None else f"{card.get('poses')} 个" + self._he_body.setText( + f"{name} {'有结果' if card.get('exists') else '还没有结果文件'}\n" + f"{card.get('meaning') or ''}\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"平移 X / Y / Z {mm}\n" + f"示教点 {poses}\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" + ) + + def _fill_camera(self) -> None: + view = camera_view.state() + usb = {item["id"]: item for item in self._snap.get("usb") or []} + bits = [] + for key, name in (("d405", "腕部"), ("d435", "手外")): + dev = usb.get(key) or {"items": []} + 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}") + + def _fill_gripper(self) -> 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}。位置、力度、速度以及转角都会按数字下发。夹爪节点占着串口时，命令行会失败。" + ) + else: + self._grip_lead.setText( + f"当前是左臂平行爪。串口 {ports}。位置 0–1000，力度、速度按滑条下发。探测只读状态。" + ) + + def _fill_teach(self) -> None: + listed = teach.list_points(self._arm) + if not listed.get("ok"): + self.statusBar().showMessage(listed.get("message") or "示教文件读失败", 4000) + return + points = listed.get("points") or [] + signature = tuple((row["name"], row["utc"]) for row in points) + if signature == self._teach_sig: + return + self._teach_sig = signature + self._teach_list.clear() + for row in points: + item = QListWidgetItem(f"{row['name']} {row['utc']}") + item.setData(Qt.ItemDataRole.UserRole, row["name"]) + self._teach_list.addItem(item) + + +def main() -> None: + import atexit + atexit.register(tactile_view.stop) + atexit.register(camera_view.stop) + atexit.register(teach.close) + threading.Thread(target=_poll_tactile, daemon=True, name="tactile-poll").start() + camera_view.start() + app = QApplication(sys.argv) + app.setFont(QFont("Noto Sans SC", 11)) + app.setStyleSheet(_STYLE) + window = StationApp() + window.showMaximized() + sys.exit(app.exec()) + + +_STYLE = """ +QMainWindow, QWidget { background: #070708; color: #f3f4f6; } +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#armLine { font-size: 16px; } +QLabel#cardTitle { color: #9aa3b2; letter-spacing: 1px; } +QLabel#stat { font-size: 28px; } +QLabel#shot { background: #0c0e12; border: 1px solid #243044; border-radius: 8px; color: #8b929d; } +QFrame#card { background: #101216; border: 1px solid #2a2e36; border-radius: 12px; } +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[active="true"], QPushButton[active="true"] { background: #12315d; border-color: #1a4580; } +QPushButton#danger { background: #6e2a2a; border-color: #8a3a3a; } +QPlainTextEdit, QLineEdit, QListWidget, QDoubleSpinBox { + background: #0c0e12; color: #f3f4f6; border: 1px solid #2a2e36; border-radius: 8px; padding: 4px 8px; +} +QSlider::groove:horizontal { height: 6px; background: #2a2e36; border-radius: 3px; } +QSlider::handle:horizontal { + width: 16px; margin: -6px 0; background: #4c86d4; border-radius: 8px; +} +QScrollArea { background: transparent; } +QMessageBox { background: #12141a; } +""" + + +if __name__ == "__main__": + main() diff --git a/src/arm_application/arm_application/operator_camera_view.py b/src/arm_application/arm_application/operator_camera_view.py new file mode 100644 index 0000000..da4744e --- /dev/null +++ b/src/arm_application/arm_application/operator_camera_view.py @@ -0,0 +1,226 @@ +"""工位程序里的相机预览。 + +只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。 +""" + +from __future__ import annotations + +import subprocess +import threading +import time + +import numpy as np + +_LOCK = threading.Lock() +_STOP = threading.Event() +_THREAD: threading.Thread | None = None +_JPEG: dict[str, bytes] = {} +_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 + + 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 + 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 "手外相机")}) + return found + + +def state() -> dict: + with _LOCK: + return { + "on": _STATE["on"], + "error": _STATE["error"], + "stamp": _STATE["stamp"], + "frames": dict(_STATE["frames"]), + } + + +def frame(name: str, kind: str) -> bytes | None: + with _LOCK: + return _JPEG.get(f"{name}_{kind}") + + +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: + import cv2 + + if img.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]) + 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 _color_array(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) + + +def _depth_array(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))) + + +def _loop(cams: list[dict]) -> None: + import rclpy + from rclpy.node import Node + from rclpy.qos import qos_profile_sensor_data + from sensor_msgs.msg import Image + + latest: dict[str, np.ndarray] = {} + lock = threading.Lock() + + class _Node(Node): + def __init__(self) -> None: + super().__init__("operator_camera_view") + for cam in cams: + name = cam["id"] + self.create_subscription( + Image, + f"/{name}/color/image_raw", + self._color(name), + qos_profile_sensor_data, + ) + self.create_subscription( + Image, + f"/{name}/aligned_depth_to_color/image_raw", + self._depth(name), + qos_profile_sensor_data, + ) + + def _color(self, name: str): + def _cb(msg: Image) -> None: + arr = _color_array(msg) + if arr is None: + return + with lock: + latest[f"{name}_color"] = arr + return _cb + + def _depth(self, name: str): + def _cb(msg: Image) -> None: + arr = _depth_array(msg) + if arr is None: + return + with lock: + latest[f"{name}_depth"] = arr + 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) + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + with _LOCK: + _STATE["on"] = False + + +def start() -> str: + global _THREAD + cams = targets() + if not cams: + with _LOCK: + _STATE["error"] = "site 里没有可预览的真机相机" + return _STATE["error"] + _close_external_viewers() + with _LOCK: + if _STATE["on"]: + return "相机预览已经在程序里" + _STATE["on"] = True + _STATE["error"] = "" + _STOP.clear() + _THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view") + _THREAD.start() + names = "、".join(cam["label"] for cam in cams) + return f"预览订了 {names} 的彩色和深度，不含左右红外。" + + +def stop() -> None: + _STOP.set() + thread = _THREAD + if thread is not None: + thread.join(timeout=1.5) diff --git a/src/arm_application/arm_application/operator_console.html b/src/arm_application/arm_application/operator_console.html new file mode 100644 index 0000000..c2fd581 --- /dev/null +++ b/src/arm_application/arm_application/operator_console.html @@ -0,0 +1,858 @@ + + + + + +ORISYS 通止规工位 + + + +
+ ORISYS 叠动科技 +
+ 通止规工位 + 左臂 89 +
+
+
+
+
--:--:--
+
+ +
+
+
+

现场

+

+
+
+
+
+
+
+
+ +
+
+

视触觉

+

切向力是 SDK 的剪切场箭头，热成像是接触散度的伪彩。下面两条曲线是法向、切向 X、切向 Y，同一时间轴。单位是形变读数，不是牛顿。

+
+
+ + + + +
+

+
+
+
+

左指

+
法向切向 X切向 Y
+ +
+
+

右指

+
法向切向 X切向 Y
+ +
+
+

+
+ +
+
+

手眼标定

+

眼在手上用腕部 D405 和 A5 板，结果是相机到法兰。眼在手外用顶置 D435 和 A4 板，结果是相机到基座。两份文件不能混用。示教和求解会动臂。

+
+
+ + +
+
+
+ + + + +
+ 
查看配置后，脚本输出显示在这里。
+
+ +
+
+

相机

+

腕部 D405 是 848×480 @ 30，手外 D435 是 1280×720 @ 30。预览开在桌面窗口，这一页只报链路，不造画面。

+
+
+
+ + +
+
+ +
+
+

夹爪

+

+
+
+
+ +
+
+

通止规装配

+

顺序与状态机一致。干跑只打印计划。真跑会动臂、会拧，而且这台机器上还没跑过完整真机装配。

+
+
+
+ + +
+
+ +
+
+

操作

+

+
+
+
+
+ + +

确认

+

+ +
+ + +
+ +
+ + + + diff --git a/src/arm_application/arm_application/operator_console.py b/src/arm_application/arm_application/operator_console.py new file mode 100644 index 0000000..7aed4c1 --- /dev/null +++ b/src/arm_application/arm_application/operator_console.py @@ -0,0 +1,725 @@ +#!/usr/bin/env python3 +"""Orisys 工位界面。 + +只做两件事：把本机真实状态摊开，以及按白名单启动仓库里已有的脚本。 +会动臂的动作必须带 ``confirm=true``，避免页面上误点一下就上电。 + + bash ~/ros2/scripts/run_console.sh +""" + +from __future__ import annotations + +import json +import os +import re +import subprocess +import threading +import time +from collections import deque +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +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 + +HOST = "127.0.0.1" +PORT = 8766 +_ROOT = Path(__file__).resolve() +for _p in _ROOT.parents: + if (_p / "scripts" / "run_real.sh").is_file(): + ROOT = _p + break +else: + ROOT = Path.home() / "ros2" + +LOG_DIR = Path("/tmp/orisys_console") +ASSETS = Path(__file__).resolve().parent +_LOCK = threading.Lock() +_TACTILE = {"left": {}, "right": {}, "stamp": 0.0} +_HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)} +_PROCS: dict[str, int] = {} + + +def _push_force(side: str, sample: dict) -> None: + if side not in _HIST: + return + point = { + "t": float(sample.get("t") or time.time()), + "fnormal": sample.get("fnormal"), + "fshearx": sample.get("fshearx"), + "fsheary": sample.get("fsheary"), + } + if not any(isinstance(point[k], (int, float)) for k in ("fnormal", "fshearx", "fsheary")): + return + with _LOCK: + _HIST[side].append(point) + _TACTILE[side] = { + "fnormal": point["fnormal"], + "fshearx": point["fshearx"], + "fsheary": point["fsheary"], + "connected": True, + } + + +tactile_view.set_sample_hook(_push_force) + + +def _site() -> dict: + path = ROOT / "config" / "site.yaml" + if not path.is_file(): + return {} + data = yaml.safe_load(path.read_text()) or {} + return data if isinstance(data, dict) else {} + + +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": ""}, + ] + + +def _usb() -> list[dict]: + found = {"d405": [], "d435": [], "orisys": []} + want = { + ("8086", "0b5b"): "d405", + ("8086", "0b07"): "d435", + ("1bcf", "2281"): "orisys", + } + base = Path("/sys/bus/usb/devices") + if not base.is_dir(): + return [] + for dev in base.iterdir(): + vid = dev / "idVendor" + pid = dev / "idProduct" + if not vid.is_file() or not pid.is_file(): + continue + key = want.get((vid.read_text().strip(), pid.read_text().strip())) + if not key: + continue + speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?" + found[key].append({"port": dev.name, "speed": speed}) + 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"]}, + ] + + +def _listeners(pattern: str) -> bool: + try: + out = subprocess.run( + ["pgrep", "-af", pattern], + capture_output=True, text=True, timeout=2, + ).stdout + except Exception: + return False + for line in out.splitlines(): + if "pgrep" in line or "operator_console" in line: + continue + return True + return False + + +def _services() -> list[dict]: + rows = [ + ("real", "控制栈", "ros2_control_node"), + ("tactile", "视触觉节点", "lib/arm_application/tactile"), + ("camera", "相机", "realsense_color_pub"), + ("gripper", "夹爪", "dh_pgea|dual_gripper_driver"), + ("pipeline", "三段流程", "assembly_pipeline"), + ("gauge", "通止规装配", "gauge_assembly"), + ] + out = [] + for sid, label, pattern in rows: + if "|" in pattern: + up = any(_listeners(part) for part in pattern.split("|")) + else: + up = _listeners(pattern) + out.append({"id": sid, "label": label, "up": up}) + return out + + +def _grippers() -> list[str]: + return sorted(p.name for p in Path("/dev").glob("ttyUSB*")) + + +def _parse_tactile(text: str) -> dict: + got: dict[str, object] = {} + for line in text.splitlines(): + if ":" not in line: + continue + key, _, raw = line.partition(":") + key = key.strip() + raw = raw.strip() + if key in ("fnormal", "fshearx", "fsheary", "fps"): + try: + got[key] = float(raw) + except ValueError: + continue + elif key in ("connected", "contact"): + got[key] = raw.lower() == "true" + return got + + +def _poll_tactile() -> None: + while True: + snap = {} + for side in ("left", "right"): + try: + proc = subprocess.run( + ["timeout", "1.2", "ros2", "topic", "echo", f"/tactile/{side}/status", "--once"], + capture_output=True, text=True, timeout=2.5, + ) + snap[side] = _parse_tactile(proc.stdout) if proc.returncode == 0 else {} + except Exception: + snap[side] = {} + view_on = bool(tactile_view.state().get("on")) + with _LOCK: + _TACTILE["stamp"] = time.time() + if not view_on: + _TACTILE["left"] = snap.get("left") or {} + _TACTILE["right"] = snap.get("right") or {} + now = time.time() + for side in ("left", "right"): + row = snap.get(side) or {} + if row: + _HIST[side].append({ + "t": now, + "fnormal": row.get("fnormal"), + "fshearx": row.get("fshearx"), + "fsheary": row.get("fsheary"), + }) + time.sleep(0.35) + + +def _robot(arm: str) -> dict: + 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: + return robot + return {} + + +def _comment(text: str, key: str) -> str: + match = re.search(rf"^#\s*{re.escape(key)}:\s*(.+)$", text, re.M) + return match.group(1).strip() if match else "" + + +def _pose_count(path: Path) -> int | None: + if not path.is_file(): + return None + match = re.search(r"num_poses:\s*(\d+)", path.read_text(errors="replace")) + return int(match.group(1)) if match else None + + +def _result_card(path: Path, pose_path: Path, board: dict) -> dict: + text = path.read_text(errors="replace") if path.is_file() else "" + trans = None + tool = "" + if text: + try: + data = yaml.safe_load(text) or {} + except yaml.YAMLError: + data = {} + if isinstance(data, dict): + raw = data.get("translation_m") + if isinstance(raw, list) and len(raw) >= 3: + trans = [float(raw[0]), float(raw[1]), float(raw[2])] + tool = str(data.get("gripper_tool") or "") + meaning = "" + for line in text.splitlines(): + if line.startswith("# T_4x4"): + meaning = line[2:].strip() + break + return { + "file": str(path), + "exists": path.is_file(), + "views": _comment(text, "views"), + "utc": _comment(text, "utc"), + "board_note": _comment(text, "board"), + "camera": _comment(text, "camera_source"), + "tool": tool, + "meaning": meaning, + "translation_m": trans, + "poses": _pose_count(pose_path), + "pose_file": str(pose_path), + "board": { + "cols": board.get("cols"), + "rows": board.get("rows"), + "square_m": board.get("square_size_m"), + }, + } + + +def _handeye(arm: str) -> dict: + site = _site() + robot = _robot(arm) + paths = robot.get("paths") if isinstance(robot.get("paths"), dict) else {} + shared = site.get("paths") if isinstance(site.get("paths"), dict) else {} + boards = ((site.get("handeye") or {}).get("boards") or {}) + wrist_file = Path(str( + paths.get("handeye_file") + or shared.get("handeye_file") + or (ROOT / "handeye" / "results" / f"{arm}_eye_in_hand.yaml") + )) + wrist_pose = Path(str( + paths.get("handeye_waypoints") + or shared.get("handeye_waypoints") + or (ROOT / "handeye" / "poses" / f"{arm}_eye_in_hand.yaml") + )) + cam, _others = overhead_camera(site, robot or None) + cam_name = str((cam or {}).get("name") or "") + e2h_name = eye_to_hand_result_name(arm, cam_name) + e2h_file = Path(str( + paths.get("handeye_file_eye_to_hand") + or shared.get("handeye_file_eye_to_hand") + or (ROOT / "handeye" / "results" / e2h_name) + )) + e2h_pose = Path(str( + paths.get("handeye_waypoints_eye_to_hand") + or shared.get("handeye_waypoints_eye_to_hand") + or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml") + )) + 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 {}), + } + + +def status() -> dict: + with _LOCK: + tactile = { + "left": dict(_TACTILE["left"]), + "right": dict(_TACTILE["right"]), + "stamp": _TACTILE["stamp"], + } + history = { + "left": list(_HIST["left"]), + "right": list(_HIST["right"]), + } + return { + "arms": _arms(), + "usb": _usb(), + "services": _services(), + "grippers": _grippers(), + "tactile": tactile, + "history": history, + "view": tactile_view.state(), + "handeye": {"89": _handeye("89"), "91": _handeye("91")}, + "now": time.time(), + } + + +# 会动真机的动作必须 confirm。argv 里的 {arm} 只接受 89 或 91。 +_ACTIONS = { + "real_up": { + "title": "上电控制栈", + "script": "run_real.sh", + "args": ["--arm", "{arm}"], + "moves": True, + "blurb": "连接机械臂并拉起 ros2_control。", + }, + "camera_d405": { + "title": "本臂腕部相机", + "script": "run_camera_d405.sh", + "args": ["--arm", "{arm}", "--no-preview"], + "moves": False, + "blurb": "只拉起本臂腕部相机。彩色和深度显示在程序里，不看左右红外，不另开窗口。", + }, + "camera_d435": { + "title": "手外 D435", + "script": "run_camera_d435.sh", + "args": ["--no-preview"], + "moves": False, + "blurb": "只拉起顶视 D435。彩色和深度显示在程序里，不另开窗口。", + }, + "tactile_up": { + "title": "视触觉三力", + "script": "run_tactile.sh", + "args": [], + "moves": False, + "blurb": "发布左右指法向力与两路切向力。", + }, + "gripper_up": { + "title": "夹爪节点", + "script": "run_gripper.sh", + "args": ["--arm", "{arm}"], + "moves": False, + "blurb": "只拉起夹爪驱动，不主动张合。", + }, + "gripper_probe": { + "title": "夹爪探测", + "script": "run_gripper.sh", + "args": ["--arm", "{arm}", "--cli", "probe"], + "moves": False, + "blurb": "读夹爪是否在线，不动手指。", + }, + "gripper_open": { + "title": "夹爪张开", + "script": "run_gripper.sh", + "args": ["--arm", "{arm}", "--cli", "open"], + "moves": True, + "blurb": "张开当前臂夹爪。", + }, + "gripper_close": { + "title": "夹爪闭合", + "script": "run_gripper.sh", + "args": ["--arm", "{arm}", "--cli", "close"], + "moves": True, + "blurb": "闭合当前臂夹爪。", + }, + "gripper_rotate": { + "title": "右爪转 90°", + "script": "run_gripper.sh", + "args": ["--arm", "{arm}", "--cli", "rotate", "90"], + "moves": True, + "blurb": "右臂旋转爪转到 90°。左臂平行爪没有这一轴。", + }, + "pipeline": { + "title": "整体三段", + "script": "run_all.sh", + "args": ["--arm", "{arm}"], + "moves": True, + "blurb": "跟踪、对孔、搜孔轻压一次跑完。会动臂。", + }, + "stage_track": { + "title": "只测跟踪", + "script": "run_visual_servo_rt.sh", + "args": ["--arm", "{arm}", "--control"], + "moves": True, + "blurb": "只跑视觉跟踪。从当前位姿开始，会动臂。要先有控制栈和相机。", + }, + "stage_locate": { + "title": "只测对孔", + "script": "run_stage2_locate.sh", + "args": ["--arm", "{arm}"], + "moves": True, + "blurb": "只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。", + }, + "stage_search": { + "title": "只测搜孔", + "script": "run_stage3_search.sh", + "args": ["--arm", "{arm}"], + "moves": True, + "blurb": "只跑三阶段：触面、搜孔、轻压。会动臂。臂应已停在孔上方。", + }, + "reset_home": { + "title": "复位到 home", + "script": "go_home.sh", + "args": ["--arm", "{arm}", "--pose", "home"], + "moves": True, + "sync": True, + "timeout": 120, + "blurb": "先停跟踪，再回到命名位姿 home。要控制栈已经起来。会动臂。", + }, + "gauge_dry": { + "title": "装配干跑", + "script": "run_gauge_assembly.sh", + "args": ["--arm", "{arm}", "--dry-run"], + "moves": False, + "blurb": "S00 到 S11 只打印计划，不发运动指令。", + }, + "gauge_run": { + "title": "装配真跑", + "script": "run_gauge_assembly.sh", + "args": ["--arm", "{arm}"], + "moves": True, + "blurb": "通止规状态机。会动臂、会拧。", + }, + "handeye_show": { + "title": "查看标定配置", + "script": "run_handeye.sh", + "args": ["--arm", "{arm}", "show", "--mode", "{mode}"], + "moves": False, + "sync": True, + "blurb": "打印这块板、结果文件和已有点数，不动臂。", + }, + "handeye_teach": { + "title": "示教采点", + "script": "run_handeye.sh", + "args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "12"], + "moves": True, + "blurb": "软拖拽示教，目标 12 个姿态。要先停控制栈，会动臂。", + }, + "handeye_merge": { + "title": "合并示教点", + "script": "run_handeye.sh", + "args": ["--arm", "{arm}", "merge", "--mode", "{mode}"], + "moves": False, + "sync": True, + "blurb": "把刚采的点写入该模式的位姿文件，不动臂。", + }, + "handeye_calibrate": { + "title": "求解手眼", + "script": "run_handeye.sh", + "args": ["--arm", "{arm}", "calibrate", "--mode", "{mode}"], + "moves": True, + "blurb": "按点位走一遍并写结果。需要控制栈和对应相机，会动臂。", + }, +} + + +def catalog() -> dict: + return { + "actions": [ + {"id": key, **{k: v for k, v in spec.items() if k not in ("script", "sync")}} + for key, spec in _ACTIONS.items() + ], + "stages": [ + ("S00", "自检"), + ("S01", "回零预绕"), + ("S02", "夹持确认"), + ("S03", "手外粗定位"), + ("S04", "视觉跟踪"), + ("S05", "精拍"), + ("S06", "正悬停"), + ("S07", "柔顺接触"), + ("S08", "棘轮拧入"), + ("S09", "反向拧出"), + ("S10", "换孔"), + ("S11", "回零结束"), + ], + } + + +def _stop_tactile() -> str: + subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False) + subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False) + subprocess.run(["pkill", "orisys_tac_w"], check=False) + return "已发送停止视触觉节点" + + +def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand") -> dict: + if action == "tactile_down": + tactile_view.stop() + return {"ok": True, "message": _stop_tactile()} + if action == "tactile_view_on": + with _LOCK: + _HIST["left"].clear() + _HIST["right"].clear() + return {"ok": True, "message": tactile_view.start(_site())} + if action == "tactile_view_off": + return {"ok": True, "message": tactile_view.stop()} + 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 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: + return {"ok": False, "message": "这个动作会动真机，需要确认"} + 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"]] + 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)} + + +def _bounded(name: str, raw, low: float, high: float) -> float: + try: + value = float(raw) + except (TypeError, ValueError): + raise ValueError(f"{name} 不是数字") from None + if value != value or value < low or value > high: + raise ValueError(f"{name} 只接受 {low:g}–{high:g}") + return value + + +def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict: + """按界面上的位置、力度、速度下发夹爪。右臂另有角度、圈数。""" + if arm not in ("89", "91"): + return {"ok": False, "message": "臂号只接受 89 或 91"} + if kind not in ("pos", "rotate", "rotate_by", "revolutions"): + return {"ok": False, "message": "没有这种夹爪调节"} + if kind != "pos" and arm != "91": + return {"ok": False, "message": "左臂平行爪没有旋转轴"} + try: + if kind == "pos": + pos = int(round(_bounded("位置", numbers.get("pos"), 0, 1000))) + force = int(round(_bounded("力度", numbers.get("force"), 20, 100))) + speed = int(round(_bounded("速度", numbers.get("speed"), 1, 100))) + cli = ["pos", str(pos)] + summary = f"位置 {pos}（0 合，1000 开） 力度 {force}% 速度 {speed}%" + else: + force = int(round(_bounded("力矩", numbers.get("torque"), 20, 100))) + speed = int(round(_bounded("旋转速度", numbers.get("rot_speed"), 1, 100))) + if kind == "rotate": + angle = _bounded("绝对角", numbers.get("angle"), -32767, 32767) + cli = ["rotate", f"{angle:.3f}"] + summary = f"绝对角 {angle:.3f}° 力矩 {force}% 速度 {speed}%" + elif kind == "rotate_by": + angle = _bounded("相对角", numbers.get("delta"), -32767, 32767) + cli = ["rotate_by", f"{angle:.3f}"] + summary = f"相对转 {angle:.3f}° 力矩 {force}% 速度 {speed}%" + else: + turns = _bounded("圈数", numbers.get("revolutions"), -50, 50) + cli = ["revolutions", f"{turns:.3f}"] + summary = f"转 {turns:.3f} 圈 力矩 {force}% 速度 {speed}%" + except ValueError as exc: + return {"ok": False, "message": str(exc)} + script = ROOT / "scripts" / "run_gripper.sh" + argv = [ + "bash", str(script), + "--arm", arm, + "--force", str(force), + "--speed", str(speed), + "--cli", *cli, + ] + shown = " ".join(argv) + try: + proc = subprocess.run(argv, cwd=str(ROOT), capture_output=True, text=True, timeout=90) + except subprocess.TimeoutExpired: + return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown} + detail = ((proc.stdout or "") + (proc.stderr or "")).strip() + ok = proc.returncode == 0 + return { + "ok": ok, + "message": summary if ok else f"夹爪退出码 {proc.returncode}", + "detail": (summary + "\n" + shown + "\n" + detail)[-4000:], + } + + +class Handler(BaseHTTPRequestHandler): + def log_message(self, fmt: str, *args) -> None: + return + + def _send(self, code: int, body: bytes, content_type: str) -> None: + self.send_response(code) + self.send_header("Content-Type", content_type) + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def _json(self, code: int, payload: dict) -> None: + self._send(code, json.dumps(payload, ensure_ascii=False).encode(), "application/json; charset=utf-8") + + def do_GET(self) -> None: + path = urlparse(self.path).path + if path in ("/", "/index.html"): + page = (ASSETS / "operator_console.html").read_text(encoding="utf-8") + payload = json.dumps( + {"status": status(), "catalog": catalog()}, + ensure_ascii=False, + ).replace("<", "\\u003c") + page = page.replace("/*BOOT*/null", payload, 1) + self._send(200, page.encode(), "text/html; charset=utf-8") + return + if path == "/logo.png": + self._send(200, (ASSETS / "operator_console_logo.png").read_bytes(), "image/png") + return + if path == "/api/status": + self._json(200, status()) + return + if path == "/api/catalog": + self._json(200, catalog()) + return + parts = path.strip("/").split("/") + if len(parts) == 5 and parts[:3] == ["api", "tactile", "frame"] and parts[4].endswith(".jpg"): + side, kind = parts[3], parts[4][:-4] + blob = tactile_view.frame(side, kind) if side in ("left", "right") and kind in ("shear", "thermal") else None + if not blob: + self._json(404, {"ok": False, "message": "还没有这一帧"}) + return + self.send_response(200) + self.send_header("Content-Type", "image/jpeg") + self.send_header("Cache-Control", "no-store") + self.send_header("Content-Length", str(len(blob))) + self.end_headers() + self.wfile.write(blob) + return + self._json(404, {"ok": False, "message": "没有这个路径"}) + + def do_POST(self) -> None: + path = urlparse(self.path).path + if path not in ("/api/action", "/api/tactile/view"): + self._json(404, {"ok": False, "message": "没有这个路径"}) + return + length = int(self.headers.get("Content-Length") or 0) + try: + body = json.loads(self.rfile.read(length) or b"{}") + except json.JSONDecodeError: + self._json(400, {"ok": False, "message": "请求不是 JSON"}) + return + if path == "/api/tactile/view": + action = "tactile_view_on" if body.get("on") else "tactile_view_off" + result = run_action(action, "89", 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"), + bool(body.get("confirm")), + str(body.get("mode") or "eye_in_hand"), + ) + self._json(200 if result.get("ok") else 400, result) + + +def main() -> None: + import atexit + atexit.register(tactile_view.stop) + threading.Thread(target=_poll_tactile, daemon=True).start() + server = ThreadingHTTPServer((HOST, PORT), Handler) + print(f"Orisys 工位界面 http://{HOST}:{PORT}", flush=True) + server.serve_forever() + + +if __name__ == "__main__": + main() diff --git a/src/arm_application/arm_application/operator_console_logo.png b/src/arm_application/arm_application/operator_console_logo.png new file mode 100644 index 0000000..21a82db Binary files /dev/null and b/src/arm_application/arm_application/operator_console_logo.png differ diff --git a/src/arm_application/arm_application/operator_tactile_view.py b/src/arm_application/arm_application/operator_tactile_view.py new file mode 100644 index 0000000..6bb9113 --- /dev/null +++ b/src/arm_application/arm_application/operator_tactile_view.py @@ -0,0 +1,353 @@ +"""工位界面用的视触觉成像。 + +切向力是 SDK 的 ``VSHEAR`` 箭头，热成像是 ``DEPTH`` 的 JET 伪彩。 +相机独占，所以只有页面点了「开始成像」才打开，失败把原因交回去，不造图。 +""" + +from __future__ import annotations + +import os +import re +import threading +import time +from pathlib import Path + +import numpy as np + +_LOCK = threading.Lock() +_STOP = threading.Event() +_THREADS: list[threading.Thread] = [] +_ON_SAMPLE = None +_STATE = { + "on": False, + "error": "", + "stamp": 0.0, + "note": "", + "frames": {}, +} +_JPEG: dict[str, bytes] = {} + + +def set_sample_hook(fn) -> None: + global _ON_SAMPLE + _ON_SAMPLE = fn + + +def state() -> dict: + with _LOCK: + return { + "on": _STATE["on"], + "error": _STATE["error"], + "stamp": _STATE["stamp"], + "note": _STATE["note"], + "frames": dict(_STATE["frames"]), + } + + +def frame(side: str, kind: str) -> bytes | None: + with _LOCK: + return _JPEG.get(f"{side}_{kind}") + + +def _videos() -> list[str]: + found = [] + by_path = Path("/dev/v4l/by-path") + if not by_path.is_dir(): + return found + for link in sorted(by_path.iterdir()): + if not link.name.endswith("-video-index0"): + continue + real = os.path.realpath(link) + base = os.path.basename(real) + sysdev = os.path.realpath(f"/sys/class/video4linux/{base}/device") + dev = "" + for part in reversed(sysdev.split("/")): + if re.fullmatch(r"\d+-\d+(\.\d+)*", part): + dev = part + break + if not dev: + continue + vid = Path(f"/sys/bus/usb/devices/{dev}/idVendor") + pid = Path(f"/sys/bus/usb/devices/{dev}/idProduct") + if ( + vid.is_file() + and pid.is_file() + and vid.read_text().strip() == "1bcf" + and pid.read_text().strip() == "2281" + ): + found.append(str(link)) + return found + + +def _resolve_cfg(site: dict) -> dict: + tactile = site.get("tactile") if isinstance(site.get("tactile"), dict) else {} + left = str(tactile.get("left_video") or "") + right = str(tactile.get("right_video") or "") + if not Path(left).exists() or not Path(right).exists(): + found = _videos() + if len(found) >= 2: + left, right = found[0], found[1] + elif len(found) == 1: + left = right = found[0] + return { + "cfg": str(tactile.get("config_file") or ""), + "backend": str(tactile.get("infer_backend") or "cpu"), + "cuda": bool(tactile.get("cuda", False)), + "isstitch": bool(tactile.get("isstitch", True)), + "baseline": float(tactile.get("baseline_seconds") or 1.5), + "left": left, + "right": right, + } + + +def _as_bgr(img): + if not isinstance(img, np.ndarray) or img.size == 0: + return None + import cv2 + + if img.ndim == 2: + return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) + if img.ndim == 3 and img.shape[2] >= 3: + return np.ascontiguousarray(img[:, :, :3]) + return None + + +def _flat(arr) -> bool: + if not isinstance(arr, np.ndarray) or arr.size == 0 or arr.ndim < 2: + return True + a = arr.reshape(-1, arr.shape[-1]) if arr.ndim == 3 else arr.reshape(-1) + finite = np.isfinite(a) + if not bool(finite.any()): + return True + vals = a[finite] + return float(np.max(vals) - np.min(vals)) < 1e-6 + + +def _thermal(depth): + import cv2 + + if depth is None: + return None + a = np.asarray(depth) + if a.size == 0: + return None + if a.ndim == 3 and a.shape[2] == 2: + a = np.linalg.norm(a.astype(np.float32), axis=2) + if a.ndim != 2: + return None + finite = np.isfinite(a) + if not finite.any(): + return None + vals = a[finite] + lo, hi = float(vals.min()), float(vals.max()) + if hi <= lo: + norm = np.zeros(a.shape, np.uint8) + else: + norm = np.clip((a - lo) / (hi - lo) * 255.0, 0, 255).astype(np.uint8) + return cv2.applyColorMap(norm, cv2.COLORMAP_JET) + + +def _views(sensor, gray): + import cv2 + import orisys + + bgr = _as_bgr(gray) + h, w = (bgr.shape[0], bgr.shape[1]) if bgr is not None else (450, 420) + try: + sensor.compute_deformation(decompose=True, check_motion=False) + except Exception: + pass + try: + sensor.compute_contact() + except Exception: + pass + try: + vshear, vnormal, depth = sensor.read_info( + sensor.info.VSHEAR, sensor.info.VNORMAL, sensor.info.DEPTH + ) + except Exception: + vshear, vnormal, depth = None, None, None + canvas = np.zeros((h, w, 3), np.uint8) + shear = canvas + if isinstance(vshear, np.ndarray) and vshear.ndim == 3 and vshear.shape[2] >= 2 and vshear.size: + field = np.ascontiguousarray(vshear[:, :, :2], dtype=np.float32) + if field.shape[0] != h or field.shape[1] != w: + field = cv2.resize(field, (w, h)) + mag = np.linalg.norm(field, axis=2) + peak = float(np.percentile(mag, 99)) if mag.size else 0.0 + arrow_scale = (2.0 * peak / 12.0) if peak > 1e-6 else 1.0 + try: + shear = orisys.util.draw_arrows( + canvas, + field, + threshold=peak * 0.15 if peak > 1e-6 else 1.0, + grid_spacing=16, + arrow_scale=max(arrow_scale, 1e-6), + below_threshold_color=(40, 40, 40), + ) + except Exception: + shear = canvas + heat = None if _flat(depth) else _thermal(depth) + if heat is None: + heat = _thermal(vnormal) + if not isinstance(heat, np.ndarray) or heat.ndim != 3: + heat = np.zeros((h, w, 3), np.uint8) + elif heat.shape[0] != h or heat.shape[1] != w: + heat = cv2.resize(heat, (w, h)) + return shear, heat + + +def _jpeg(img) -> bytes | None: + import cv2 + + if not isinstance(img, np.ndarray) or img.size == 0: + return None + ok, buf = cv2.imencode(".jpg", img, [int(cv2.IMWRITE_JPEG_QUALITY), 70]) + if not ok: + return None + return buf.tobytes() + + +def _store(side: str, shear, heat, sample: dict | None) -> None: + frames = {} + blobs = {} + for kind, img in (("shear", shear), ("thermal", heat)): + blob = _jpeg(img) + if blob: + blobs[f"{side}_{kind}"] = blob + frames[f"{side}_{kind}"] = True + with _LOCK: + _JPEG.update(blobs) + _STATE["frames"].update(frames) + _STATE["stamp"] = time.time() + if _STATE["error"].startswith(side): + _STATE["error"] = "" + if sample and _ON_SAMPLE is not None: + _ON_SAMPLE(side, sample) + + +def _open_sensor(cfg: dict, side: str): + import orisys + + return orisys.Sensor( + cfg[side], + cuda=cfg["cuda"], + isstitch=cfg["isstitch"], + verbose=False, + config_name=cfg["cfg"], + backend=cfg["backend"], + ) + + +def _worker(side: str, cfg: dict) -> None: + if not cfg.get(side): + with _LOCK: + _STATE["error"] = f"{side} 没有视频节点" + return + sensor = None + try: + sensor = _open_sensor(cfg, side) + except Exception as exc: + with _LOCK: + prev = _STATE["error"] + _STATE["error"] = (prev + "；" if prev else "") + f"{side} 打开失败：{exc}" + return + buckets = {"n": [], "x": [], "y": []} + deadline = time.perf_counter() + float(cfg["baseline"]) + while time.perf_counter() < deadline and not _STOP.is_set(): + try: + img = sensor.get_img() + sensor.compute_deformation(check_motion=False) + n, x, y = sensor.read_info( + sensor.info.FNORMAL, sensor.info.FSHEARX, sensor.info.FSHEARY + ) + buckets["n"].append(float(n)) + buckets["x"].append(float(x)) + buckets["y"].append(float(y)) + except Exception: + pass + time.sleep(0.02) + base = { + "n": float(np.mean(buckets["n"])) if buckets["n"] else 0.0, + "x": float(np.mean(buckets["x"])) if buckets["x"] else 0.0, + "y": float(np.mean(buckets["y"])) if buckets["y"] else 0.0, + } + with _LOCK: + base_note = "曲线已减去打开后短暂基线，单位仍是形变读数。" + if "右指" in str(_STATE["note"]): + _STATE["note"] = _STATE["note"] + " " + base_note + else: + _STATE["note"] = base_note + last_sample = 0.0 + while not _STOP.is_set(): + try: + gray = sensor.get_img() + except Exception: + gray = None + if gray is None: + time.sleep(0.05) + continue + shear, heat = _views(sensor, gray) + sample = None + now = time.time() + if now - last_sample >= 0.1: + try: + n, x, y = sensor.read_info( + sensor.info.FNORMAL, sensor.info.FSHEARX, sensor.info.FSHEARY + ) + sample = { + "t": now, + "fnormal": float(n) - base["n"], + "fshearx": float(x) - base["x"], + "fsheary": float(y) - base["y"], + } + last_sample = now + except Exception: + sample = None + _store(side, shear, heat, sample) + time.sleep(0.04) + try: + sensor.disconnect() + except Exception: + pass + + +def start(site: dict) -> str: + global _THREADS + with _LOCK: + if _STATE["on"]: + return "成像已经开着" + _STATE["on"] = True + _STATE["error"] = "" + _STATE["frames"] = {} + _STOP.clear() + cfg = _resolve_cfg(site) + if not cfg["left"] and not cfg["right"]: + with _LOCK: + _STATE["on"] = False + _STATE["error"] = "没有找到 Orisys 相机" + return _STATE["error"] + sides = ["left"] + if cfg.get("right") and cfg.get("right") != cfg.get("left"): + sides.append("right") + elif not cfg.get("right") or cfg.get("right") == cfg.get("left"): + with _LOCK: + _STATE["note"] = "只找到一路 Orisys，右指没有画面。插上后重新开始成像。" + _THREADS = [ + threading.Thread(target=_worker, args=(side, cfg), daemon=True, name=f"tac-{side}") + for side in sides + ] + for thread in _THREADS: + thread.start() + return "正在打开两只视触觉。切向力和热成像出来之前会先采一小段基线。" + + +def stop() -> str: + _STOP.set() + for thread in list(_THREADS): + thread.join(timeout=2.0) + with _LOCK: + _STATE["on"] = False + _STATE["frames"] = {} + _JPEG.clear() + return "已停止视触觉成像" diff --git a/src/arm_application/arm_application/operator_teach.py b/src/arm_application/arm_application/operator_teach.py new file mode 100644 index 0000000..6761ad8 --- /dev/null +++ b/src/arm_application/arm_application/operator_teach.py @@ -0,0 +1,203 @@ +"""软拖拽示教：开拖拽、记下当前关节角、再回到该点。 + +点位写在 ``handeye/tmp/<臂号>/operator_teach.yaml``。关节角单位是弧度， +直接存桥接返回的 7 个数，不另做姿态换算。 + +软拖拽占用 RDK，和 ``run_real.sh`` 不能同时连同一台臂。 +""" + +from __future__ import annotations + +import threading +from datetime import datetime, timezone +from pathlib import Path + +import yaml + +_LOCK = threading.Lock() +_BRIDGE = None +_SN = "" + + +def _file(arm: str) -> Path: + from arm_application.operator_console import ROOT + + if arm not in ("89", "91"): + raise ValueError("臂号只接受 89 或 91") + return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml" + + +def _bridge_for(sn: str): + global _BRIDGE, _SN + sn = str(sn or "").strip() + if not sn: + raise RuntimeError("site.yaml 里这台臂没有序列号，拒绝连接") + if _BRIDGE is not None and _SN == sn and _BRIDGE._proc.poll() is None: + return _BRIDGE + close() + from arm_application.handeye_teach import RdkBridge + + _BRIDGE = RdkBridge(sn) + _SN = sn + return _BRIDGE + + +def close() -> None: + global _BRIDGE, _SN + bridge = _BRIDGE + _BRIDGE = None + _SN = "" + if bridge is None: + return + try: + bridge.close() + except Exception: + pass + + +def drag_on(sn: str) -> dict: + with _LOCK: + try: + bridge = _bridge_for(sn) + result = bridge.drag_on(mode="soft") + except Exception as exc: + return {"ok": False, "message": str(exc)} + if not result.get("ok"): + return {"ok": False, "message": str(result.get("error") or result)} + return { + "ok": True, + "message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。", + } + + +def drag_off(sn: str) -> dict: + with _LOCK: + try: + bridge = _bridge_for(sn) + result = bridge.drag_off() + except Exception as exc: + return {"ok": False, "message": str(exc)} + if not result.get("ok"): + return {"ok": False, "message": str(result.get("error") or result)} + return {"ok": True, "message": "软拖拽已关闭，臂保持当前位置。"} + + +def list_points(arm: str) -> dict: + try: + path = _file(arm) + except ValueError as exc: + return {"ok": False, "message": str(exc), "points": []} + if not path.is_file(): + return {"ok": True, "points": [], "file": str(path)} + try: + data = yaml.safe_load(path.read_text(encoding="utf-8")) or {} + except Exception as exc: + return {"ok": False, "message": f"示教文件读不了：{exc}", "points": [], "file": str(path)} + points = [] + for row in data.get("points") or []: + if not isinstance(row, dict): + continue + joints = row.get("joints") + if not isinstance(joints, list) or len(joints) < 7: + continue + try: + values = [float(v) for v in joints[:7]] + except (TypeError, ValueError): + continue + if any(v != v for v in values): + continue + points.append({ + "name": str(row.get("name") or "").strip(), + "utc": str(row.get("utc") or ""), + "joints": values, + }) + points = [row for row in points if row["name"]] + return {"ok": True, "points": points, "file": str(path)} + + +def _write(arm: str, points: list[dict]) -> Path: + path = _file(arm) + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text( + yaml.safe_dump({"points": points}, allow_unicode=True, sort_keys=False), + encoding="utf-8", + ) + return path + + +def record(sn: str, arm: str, name: str) -> dict: + title = str(name or "").strip() + if not title or "/" in title or "\\" in title or title in (".", ".."): + return {"ok": False, "message": "点名不能为空，也不能带路径"} + with _LOCK: + try: + bridge = _bridge_for(sn) + result = bridge._ask({"cmd": "joints"}, timeout=2.0) + except Exception as exc: + return {"ok": False, "message": str(exc)} + if not result.get("ok"): + return {"ok": False, "message": str(result.get("error") or "没有读到关节，点位没写下去")} + raw = result.get("q") or [] + if len(raw) < 7: + return {"ok": False, "message": "关节数不够 7 个，点位没写下去"} + try: + values = [round(float(v), 6) for v in raw[:7]] + except (TypeError, ValueError): + return {"ok": False, "message": "关节角不是数字，点位没写下去"} + if any(v != v for v in values): + return {"ok": False, "message": "关节角不是有限值，点位没写下去"} + listed = list_points(arm) + if not listed.get("ok"): + return listed + points = [row for row in listed["points"] if row["name"] != title] + points.append({ + "name": title, + "utc": datetime.now(timezone.utc).strftime("%Y-%m-%dT%H:%M:%SZ"), + "joints": values, + }) + path = _write(arm, points) + text = " ".join(f"{v:.4f}" for v in values) + return {"ok": True, "message": f"已记录 {title}", "detail": f"{path}\n关节 rad：{text}", "points": points} + + +def delete_point(arm: str, name: str) -> dict: + title = str(name or "").strip() + listed = list_points(arm) + if not listed.get("ok"): + return listed + kept = [row for row in listed["points"] if row["name"] != title] + if len(kept) == len(listed["points"]): + return {"ok": False, "message": f"没有名为 {title} 的点"} + path = _write(arm, kept) + return {"ok": True, "message": f"已删除 {title}", "detail": str(path), "points": kept} + + +def goto(sn: str, arm: str, name: str, speed: float) -> dict: + title = str(name or "").strip() + listed = list_points(arm) + if not listed.get("ok"): + return listed + hit = next((row for row in listed["points"] if row["name"] == title), None) + if hit is None: + return {"ok": False, "message": f"没有名为 {title} 的点"} + try: + speed_f = float(speed) + except (TypeError, ValueError): + return {"ok": False, "message": "关节速度不是数字"} + if not 0.05 <= speed_f <= 0.35: + return {"ok": False, "message": "关节速度只接受 0.05–0.35 rad/s"} + with _LOCK: + try: + bridge = _bridge_for(sn) + bridge.drag_off() + result = bridge.move_joints(hit["joints"], speed_f) + except Exception as exc: + return {"ok": False, "message": str(exc)} + if not result.get("ok"): + return {"ok": False, "message": str(result.get("error") or result)} + text = " ".join(f"{float(v):.4f}" for v in hit["joints"]) + return { + "ok": True, + "message": f"已回到 {title}", + "detail": f"速度 {speed_f:.2f} rad/s\n关节 rad：{text}", + }