import argparse import multiprocessing as mp import sys import threading import time import cv2 import numpy as np def _fix_qt_plugin_path(): try: from PyQt5.QtCore import QLibraryInfo import os os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = QLibraryInfo.location( QLibraryInfo.PluginsPath ) except Exception: pass _fix_qt_plugin_path() _QT_IMPORT_ERROR = None try: from PyQt5.QtCore import Qt, QTimer from PyQt5.QtGui import QFont, QImage, QPixmap from PyQt5.QtWidgets import ( QApplication, QFrame, QGridLayout, QHBoxLayout, QLabel, QMainWindow, QPushButton, QSizePolicy, QVBoxLayout, QWidget, ) except Exception as exc: _QT_IMPORT_ERROR = exc Qt = QTimer = QFont = QImage = QPixmap = QApplication = None QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = None QVBoxLayout = QWidget = None QFrame = QMainWindow = object try: import pyqtgraph as pg except Exception as exc: pg = None _PYQTGRAPH_IMPORT_ERROR = exc else: _PYQTGRAPH_IMPORT_ERROR = None from .main import build_controller PLOT_POINTS = 180 PLOT_FORCE_Y_MIN_N = 0.0 PLOT_FORCE_Y_MAX_N = 3.0 FORCE_DISPLAY_ZERO_THRESHOLD_N = 0.05 CURVE_COLOR = "#00E5FF" def cv2_to_pixmap(img_bgr): if img_bgr is None: return QPixmap() if len(img_bgr.shape) == 2: img_bgr = cv2.cvtColor(img_bgr, cv2.COLOR_GRAY2BGR) rgb = cv2.cvtColor(np.ascontiguousarray(img_bgr), cv2.COLOR_BGR2RGB) h, w, ch = rgb.shape qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888).copy() return QPixmap.fromImage(qimg) def force_display_value(value): value = float(value) if abs(value) < FORCE_DISPLAY_ZERO_THRESHOLD_N: return 0.0 return value def fmt(value, unit="", width=0, precision=3): if value is None: text = "--" else: value = force_display_value(value) text = "0" if value == 0.0 else f"{value:.{precision}f}" if width: text = text.rjust(width) return f"{text}{unit}" STYLE = """ QMainWindow { background-color: #000000; } QWidget { background-color: #000000; color: #9a9a9a; font-family: "Microsoft YaHei", "SimHei"; } QLabel#Title { color: #d7d7d7; font-size: 18px; font-weight: 600; } QLabel#Subtle { color: #777777; font-size: 12px; } QLabel#MetricLabel { color: #888888; font-size: 12px; } QLabel#MetricValue { color: #f2f2f2; font-family: "Cascadia Code", "Consolas"; font-size: 22px; font-weight: 600; } QLabel#SmallValue { color: #d8d8d8; font-family: "Cascadia Code", "Consolas"; font-size: 15px; } QLabel#StatusChip { background-color: #101010; border: 1px solid #303030; border-radius: 4px; color: #00e5ff; font-family: "Cascadia Code", "Consolas"; font-size: 18px; font-weight: 600; padding: 8px 12px; } QLabel#ActionChip { background-color: #0b0b0b; border: 1px solid #262626; border-radius: 4px; color: #c8c8c8; font-family: "Cascadia Code", "Consolas"; font-size: 13px; padding: 7px 10px; } QLabel#ImagePanel { background-color: #0b0b0b; border: 1px solid #262626; border-radius: 4px; } QFrame#Panel { background-color: #050505; border: 1px solid #222222; border-radius: 4px; } QPushButton { background-color: #1a1a1a; color: #a8a8a8; border: 1px solid #333333; border-radius: 4px; padding: 7px 16px; font-size: 13px; } QPushButton:hover { background-color: #262626; color: #d4d4d4; border: 1px solid #555555; } QPushButton#StartButton { background-color: #0d2818; color: #67d79a; border: 1px solid #1a4d30; } QPushButton#StopButton { background-color: #2b1010; color: #f07070; border: 1px solid #4d1a1a; } QPushButton#RestartButton { background-color: #1d1d12; color: #e0c96a; border: 1px solid #4d4520; } """ class MetricBox(QFrame): def __init__(self, title, value="--", sub=""): super().__init__() self.setObjectName("Panel") layout = QVBoxLayout(self) layout.setContentsMargins(10, 8, 10, 8) layout.setSpacing(3) self.title_label = QLabel(title) self.title_label.setObjectName("MetricLabel") self.value_label = QLabel(value) self.value_label.setObjectName("MetricValue") self.sub_label = QLabel(sub) self.sub_label.setObjectName("Subtle") layout.addWidget(self.title_label) layout.addWidget(self.value_label) layout.addWidget(self.sub_label) def set_value(self, value, sub=None): self.value_label.setText(value) if sub is not None: self.sub_label.setText(sub) class ImageBox(QFrame): def __init__(self, title): super().__init__() self.setObjectName("Panel") layout = QVBoxLayout(self) layout.setContentsMargins(8, 8, 8, 8) layout.setSpacing(6) self.title_label = QLabel(title) self.title_label.setObjectName("MetricLabel") self.image_label = QLabel() self.image_label.setObjectName("ImagePanel") self.image_label.setAlignment(Qt.AlignCenter) self.image_label.setMinimumSize(180, 180) self.image_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.image_label.setScaledContents(True) layout.addWidget(self.title_label) layout.addWidget(self.image_label, 1) def set_image(self, img): self.image_label.setPixmap(cv2_to_pixmap(img)) class GripperDemo02Viewer(QMainWindow): def __init__(self, argv=None): super().__init__() if _QT_IMPORT_ERROR is not None: raise RuntimeError(f"PyQt5 import failed: {_QT_IMPORT_ERROR}") if pg is None: raise RuntimeError(f"pyqtgraph import failed: {_PYQTGRAPH_IMPORT_ERROR}") self.setWindowTitle("Gripper Demo 02") self.setStyleSheet(STYLE) self.controller = None self.control_thread = None self.latest_status = None self.status_lock = threading.Lock() self.controller_finished = False self.restart_pending = False self.button_state = "idle" self.start_time = time.time() self.force_history = [] self.argv = argv self._build_ui() self._configure_plot() self.timer = QTimer(self) self.timer.timeout.connect(self._refresh) self.timer.start(33) def _build_ui(self): root = QWidget() self.setCentralWidget(root) outer = QVBoxLayout(root) outer.setContentsMargins(8, 8, 8, 8) outer.setSpacing(8) top = QHBoxLayout() top.setSpacing(8) title = QLabel("Orisys 夹爪") title.setObjectName("Title") self.status_chip = QLabel("state=--") self.status_chip.setObjectName("StatusChip") self.action_chip = QLabel("action=--") self.action_chip.setObjectName("ActionChip") self.start_button = QPushButton("启动") self.start_button.setObjectName("StartButton") self.start_button.clicked.connect(self.start_control) self.stop_button = QPushButton("停止") self.stop_button.setObjectName("StopButton") self.stop_button.clicked.connect(self.stop_control) self.stop_button.setEnabled(False) self.restart_button = QPushButton("重启") self.restart_button.setObjectName("RestartButton") self.restart_button.clicked.connect(self.restart_control) self._set_button_state("idle") top.addWidget(title) top.addWidget(self.status_chip) top.addWidget(self.action_chip, 1) top.addWidget(self.start_button) top.addWidget(self.stop_button) top.addWidget(self.restart_button) outer.addLayout(top) main = QGridLayout() main.setSpacing(8) outer.addLayout(main, 1) self.left_flow = ImageBox("左侧光流") self.left_mag = ImageBox("左侧幅值图") self.right_flow = ImageBox("右侧光流") self.right_mag = ImageBox("右侧幅值图") main.addWidget(self.left_flow, 0, 0) main.addWidget(self.left_mag, 0, 1) main.addWidget(self.right_flow, 1, 0) main.addWidget(self.right_mag, 1, 1) side = QVBoxLayout() side.setSpacing(8) main.addLayout(side, 0, 2, 2, 1) metrics_grid = QGridLayout() metrics_grid.setSpacing(8) side.addLayout(metrics_grid) self.left_force = MetricBox("左法向力", "--") self.right_force = MetricBox("右法向力", "--") self.left_shear_box = MetricBox("左切向力", "--") self.right_shear_box = MetricBox("右切向力", "--") self.force_pct_box = MetricBox("夹爪当前力比例（%）", "--") self.target_force_box = MetricBox("夹爪目标力比例（%）", "--") self.speed_box = MetricBox("速度", "--") self.trigger_box = MetricBox("是否触发/系统是否允许触发", "--") self.contact_box = MetricBox("接触", "--") self.release_box = MetricBox("释放检测", "--") boxes = [ self.left_force, self.right_force, self.left_shear_box, self.right_shear_box, self.force_pct_box, self.target_force_box, self.speed_box, self.trigger_box, self.contact_box, self.release_box, ] for idx, box in enumerate(boxes): metrics_grid.addWidget(box, idx // 2, idx % 2) pg.setConfigOption("background", "#000000") pg.setConfigOption("foreground", "#777777") pg.setConfigOptions(antialias=True) self.plot_widget = pg.GraphicsLayoutWidget() self.plot_widget.setMinimumHeight(240) side.addWidget(self.plot_widget, 1) self.info_box = QFrame() self.info_box.setObjectName("Panel") info_layout = QVBoxLayout(self.info_box) info_layout.setContentsMargins(10, 8, 10, 8) self.info_label = QLabel("等待启动") self.info_label.setObjectName("SmallValue") self.info_label.setWordWrap(True) info_layout.addWidget(self.info_label) side.addWidget(self.info_box) main.setColumnStretch(0, 2) main.setColumnStretch(1, 2) main.setColumnStretch(2, 3) main.setRowStretch(0, 1) main.setRowStretch(1, 1) def _configure_plot(self): self.plot = self.plot_widget.addPlot() self.plot.setTitle("左右法向力", color="#999999", size="13pt") self.plot.titleLabel.setFont(QFont("SimHei", 13)) self.plot.showAxis("top", False) self.plot.showAxis("right", False) self.plot.showGrid(x=False, y=True, alpha=0.12) self.plot.setLabel("bottom", "时间", units="s") self.plot.setLabel("left", "力", units="N") self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=0) self.plot.setLimits(yMin=PLOT_FORCE_Y_MIN_N, yMax=PLOT_FORCE_Y_MAX_N) self.plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False) self.plot.addLegend(offset=(-12, 12), labelTextColor="#d0d0d0") self.left_curve = self.plot.plot( pen=pg.mkPen(CURVE_COLOR, width=2), name="左法向 (N)", ) self.right_curve = self.plot.plot( pen=pg.mkPen("#ffb000", width=2), name="右法向 (N)", ) def _set_button_state(self, state): labels = { "idle": ("启动", "停止", "重启"), "starting": ("启动中", "停止", "重启"), "running": ("启动", "停止", "重启"), "stopping": ("启动", "停止中", "重启"), "restarting": ("启动", "停止", "重启中"), } enabled = { "idle": (True, False, True), "starting": (False, True, False), "running": (False, True, True), "stopping": (False, False, False), "restarting": (False, False, False), } self.button_state = state start_text, stop_text, restart_text = labels[state] start_enabled, stop_enabled, restart_enabled = enabled[state] self.start_button.setText(start_text) self.stop_button.setText(stop_text) self.restart_button.setText(restart_text) self.start_button.setEnabled(start_enabled) self.stop_button.setEnabled(stop_enabled) self.restart_button.setEnabled(restart_enabled) def start_control(self, from_restart=False): if self.control_thread is not None and self.control_thread.is_alive(): return self.force_history.clear() self.start_time = time.time() self._set_button_state("restarting" if from_restart else "starting") self.controller_finished = False if from_restart: self.info_label.setText("重启中：正在重新连接夹爪与两个触觉传感器") else: self.info_label.setText("启动中：正在连接夹爪与两个触觉传感器") self.controller = build_controller( self.argv, status_callback=self._on_status, include_visuals=True, visual_size=320, ) self.control_thread = threading.Thread( target=self._run_controller, daemon=True, ) self.control_thread.start() def stop_control(self): if self.controller is not None: self.controller.stop() self._set_button_state("stopping") self.info_label.setText("停止中：正在释放相机与夹爪") def restart_control(self): if self.control_thread is not None and self.control_thread.is_alive(): self.restart_pending = True self._set_button_state("restarting") if self.controller is not None: self.controller.stop() self.info_label.setText("重启中：正在停止当前控制线程") return self.restart_pending = False self.start_control(from_restart=True) def _run_controller(self): try: self.controller.run() except Exception as exc: self._on_status({"state": "error", "action": str(exc), "timestamp": time.time()}) finally: self.controller_finished = True def _on_status(self, status): with self.status_lock: self.latest_status = status def _refresh(self): with self.status_lock: status = self.latest_status if not status: self._update_thread_buttons() return self._update_thread_buttons() if ( self.control_thread is not None and self.control_thread.is_alive() and not self.restart_pending and self.button_state in ("starting", "restarting") ): self._set_button_state("running") state = status.get("state", "--") action = status.get("action", "--") unit = status.get("normal_unit", "N") shear_unit = status.get("shear_unit", "N") self.status_chip.setText(f"state={state}") self.action_chip.setText(f"action={action}") self.left_force.set_value( fmt(status.get("left_normal"), unit), f"raw FPS {self._sample_fps(status, 'left_sample')}", ) self.right_force.set_value( fmt(status.get("right_normal"), unit), f"raw FPS {self._sample_fps(status, 'right_sample')}", ) self.left_shear_box.set_value( fmt(status.get("left_shear"), shear_unit), "left_shear", ) self.right_shear_box.set_value( fmt(status.get("right_shear"), shear_unit), "right_shear", ) self.force_pct_box.set_value( str(status.get("force_pct", "--")), f"hold_pos={status.get('hold_pos')}", ) self.target_force_box.set_value( str(status.get("adaptive_target_force", "--")), f"adaptive={int(bool(status.get('adaptive_grip_enabled')))}", ) self.speed_box.set_value( str(status.get("speed_pct", "--")), "speed_pct", ) self.trigger_box.set_value( f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}", "trigger / armed", ) self.contact_box.set_value( str(int(bool(status.get("grip_contact")))), self._contact_text(status), ) self.release_box.set_value( str(int(bool(status.get("release_armed")))), "release_armed", ) left_sample = status.get("left_sample") or {} right_sample = status.get("right_sample") or {} self.left_flow.set_image(left_sample.get("flow_view")) self.left_mag.set_image(left_sample.get("magnitude_view")) self.right_flow.set_image(right_sample.get("flow_view")) self.right_mag.set_image(right_sample.get("magnitude_view")) elapsed = time.time() - self.start_time self.force_history.append( ( elapsed, force_display_value(status.get("left_normal", 0.0) or 0.0), force_display_value(status.get("right_normal", 0.0) or 0.0), ) ) if len(self.force_history) > PLOT_POINTS: self.force_history = self.force_history[-PLOT_POINTS:] self._update_plot() self.info_label.setText( " | ".join( [ f"min={fmt(status.get('min_normal'), unit)}", f"max={fmt(status.get('max_normal'), unit)}", f"L_shear={fmt(status.get('left_shear'), shear_unit)}", f"R_shear={fmt(status.get('right_shear'), shear_unit)}", f"target={status.get('adaptive_target_force', '--')}", f"release_armed={int(bool(status.get('release_armed')))}", f"flow L/R={self._flow_text(left_sample)} / {self._flow_text(right_sample)}", ] ) ) def _update_plot(self): if not self.force_history: return data = np.asarray(self.force_history, dtype=float) x = data[:, 0] self.left_curve.setData(x, data[:, 1]) self.right_curve.setData(x, data[:, 2]) self.plot.setYRange(PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N, padding=0) if len(x) >= 2: self.plot.setXRange(max(0.0, x[-1] - 6.0), max(6.0, x[-1]), padding=0) def _update_thread_buttons(self): if not self.controller_finished: return self.controller_finished = False if self.control_thread is not None: self.control_thread.join(timeout=0.1) self.control_thread = None if self.restart_pending: self.restart_pending = False self.info_label.setText("重启中：正在重新启动") self.start_control(from_restart=True) return self._set_button_state("idle") if self.controller is not None: self.info_label.setText("已停止") def _sample_fps(self, status, key): sample = status.get(key) or {} fps = sample.get("fps") if fps is None: return "--" return f"{fps:.1f}" def _contact_text(self, status): left = status.get("left_sample") or {} right = status.get("right_sample") or {} return f"L={int(bool(left.get('is_contact')))} R={int(bool(right.get('is_contact')))}" def _flow_text(self, sample): if not sample: return "--" return f"{sample.get('flow_mean', 0.0):.2f}/{sample.get('flow_max', 0.0):.2f}" def keyPressEvent(self, event): if event.key() == Qt.Key_Q: self.close() return if event.key() == Qt.Key_Escape: if self.isFullScreen(): self.showNormal() else: self.showFullScreen() return super().keyPressEvent(event) def closeEvent(self, event): self.timer.stop() self.restart_pending = False self.stop_control() if self.control_thread is not None: self.control_thread.join(timeout=3.0) event.accept() def main(argv=None): parser = argparse.ArgumentParser() parser.add_argument("--windowed", action="store_true") ui_args, controller_argv = parser.parse_known_args(argv) if _QT_IMPORT_ERROR is not None: print( "PyQt5 is required for the visualizer. " "Install it in the active environment, for example: pip install PyQt5 pyqtgraph" ) print(f"Import error: {_QT_IMPORT_ERROR}") return 1 if pg is None: print( "pyqtgraph is required for the visualizer. " "Install it in the active environment, for example: pip install pyqtgraph" ) print(f"Import error: {_PYQTGRAPH_IMPORT_ERROR}") return 1 QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True) QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True) app = QApplication(sys.argv[:1]) window = GripperDemo02Viewer(controller_argv) if ui_args.windowed: window.resize(1500, 900) window.show() else: window.showFullScreen() window.start_control() return app.exec_() if __name__ == "__main__": mp.freeze_support() raise SystemExit(main(sys.argv[1:]))