cheng
593 lines
19 KiB
Python
593 lines
19 KiB
Python
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
|
|
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 fmt(value, unit="", width=0, precision=3):
|
|
if value is None:
|
|
text = "--"
|
|
else:
|
|
text = f"{float(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;
|
|
}
|
|
"""
|
|
|
|
|
|
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.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)
|
|
top.addWidget(title)
|
|
top.addWidget(self.status_chip)
|
|
top.addWidget(self.action_chip, 1)
|
|
top.addWidget(self.start_button)
|
|
top.addWidget(self.stop_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.shear_force = MetricBox("最大切向力", "--")
|
|
self.force_diff = MetricBox("左右差值", "--")
|
|
self.force_pct_box = MetricBox("force_pct", "--")
|
|
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.shear_force,
|
|
self.force_diff,
|
|
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)",
|
|
)
|
|
self.shear_curve = self.plot.plot(
|
|
pen=pg.mkPen("#7ddc8a", width=2),
|
|
name="最大切向 (N)",
|
|
)
|
|
|
|
def start_control(self):
|
|
if self.control_thread is not None and self.control_thread.is_alive():
|
|
return
|
|
self.force_history.clear()
|
|
self.start_time = time.time()
|
|
self.start_button.setEnabled(False)
|
|
self.stop_button.setEnabled(True)
|
|
self.controller_finished = False
|
|
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.stop_button.setEnabled(False)
|
|
self.info_label.setText("停止中：正在释放相机与夹爪")
|
|
|
|
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()
|
|
|
|
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.shear_force.set_value(
|
|
fmt(status.get("max_shear"), shear_unit),
|
|
f"dShear {fmt(status.get('shear_change'), shear_unit)}",
|
|
)
|
|
self.force_diff.set_value(
|
|
fmt(status.get("normal_diff"), unit),
|
|
f"dN {fmt(status.get('normal_change'), unit)}",
|
|
)
|
|
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,
|
|
float(status.get("left_normal", 0.0) or 0.0),
|
|
float(status.get("right_normal", 0.0) or 0.0),
|
|
float(status.get("max_shear", 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.shear_curve.setData(x, data[:, 3])
|
|
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
|
|
self.start_button.setEnabled(True)
|
|
self.stop_button.setEnabled(False)
|
|
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.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:]))
|