cheng
794 lines
26 KiB
Python
794 lines
26 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 QEasingCurve, QPropertyAnimation, Qt, QTimer
|
||
from PyQt5.QtGui import QFont, QImage, QPixmap
|
||
from PyQt5.QtWidgets import (
|
||
QApplication,
|
||
QFrame,
|
||
QGraphicsOpacityEffect,
|
||
QGridLayout,
|
||
QHBoxLayout,
|
||
QLabel,
|
||
QMainWindow,
|
||
QPushButton,
|
||
QSizePolicy,
|
||
QSplitter,
|
||
QVBoxLayout,
|
||
QWidget,
|
||
)
|
||
except Exception as exc:
|
||
_QT_IMPORT_ERROR = exc
|
||
Qt = QTimer = QPropertyAnimation = QEasingCurve = QFont = QImage = QPixmap = QApplication = None
|
||
QGridLayout = QHBoxLayout = QLabel = QPushButton = QSizePolicy = QSplitter = None
|
||
QVBoxLayout = QWidget = None
|
||
QFrame = QGraphicsOpacityEffect = 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
|
||
PLOT_SHEAR_Y_MIN_N = -3.0
|
||
PLOT_SHEAR_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}"
|
||
|
||
|
||
STATE_TEXT = {
|
||
"open_wait": "等待触发",
|
||
"closing": "闭合中",
|
||
"gripping": "夹持加力",
|
||
"hold_check": "夹持检测",
|
||
"open_recover": "张开恢复",
|
||
"manual_close": "手动闭合",
|
||
"manual_hold": "手动保持",
|
||
"error": "错误",
|
||
}
|
||
|
||
ACTION_TEXT = {
|
||
"wait": "等待",
|
||
"waiting-for-sensor-samples": "等待传感器数据",
|
||
"open-wait-stabilizing": "等待稳定基准",
|
||
"open-wait-armed": "已就绪",
|
||
"side-trigger-close": "侧边触发闭合",
|
||
"closing-continue": "继续闭合",
|
||
"closing-to-grip": "闭合寻找物体",
|
||
"object-detected-low-force": "检测到物体",
|
||
"close-timeout-open": "闭合超时张开",
|
||
"grip-ramp-up": "夹持加力",
|
||
"grip-hold": "夹持保持",
|
||
"hold-reference-armed": "记录夹持基准",
|
||
"adaptive-grip-ramp": "自适应加力",
|
||
"adaptive-grip-wait": "等待自适应加力",
|
||
"release-arm-delay": "释放检测延时",
|
||
"release-reference-armed": "释放检测就绪",
|
||
"hold-check": "保持检测",
|
||
"force-change-release-open": "力变化张开",
|
||
"open-recover-position": "张开位置恢复",
|
||
"open-recover-position-stabilizing": "张开位置稳定中",
|
||
"open-ready-position": "张开到位",
|
||
"open-recover-quiet": "力稳定恢复",
|
||
"open-recover-stabilizing": "恢复稳定中",
|
||
"open-recover-wait-force-clear": "等待力恢复",
|
||
"open-ready": "恢复完成",
|
||
"manual-open": "手动张开",
|
||
"manual-close": "手动闭合",
|
||
"manual-hold": "手动保持",
|
||
"manual-release": "手动释放",
|
||
}
|
||
|
||
|
||
def label_with_cn(value, mapping):
|
||
value = str(value or "--")
|
||
cn = mapping.get(value)
|
||
if cn is None:
|
||
for prefix, text in mapping.items():
|
||
if value.startswith(f"{prefix} "):
|
||
cn = text
|
||
break
|
||
if cn is None:
|
||
return value
|
||
return f"{value}（{cn}）"
|
||
|
||
|
||
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;
|
||
}
|
||
QFrame#StatusPanel {
|
||
background-color: #101010;
|
||
border: 1px solid #303030;
|
||
border-radius: 4px;
|
||
}
|
||
QLabel#StatusMain {
|
||
color: #00e5ff;
|
||
font-family: "Cascadia Code", "Consolas";
|
||
font-size: 17px;
|
||
font-weight: 600;
|
||
}
|
||
QLabel#StatusSub {
|
||
color: #9a9a9a;
|
||
font-family: "Cascadia Code", "Consolas";
|
||
font-size: 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: #171717;
|
||
color: #d8d8d8;
|
||
border: 1px solid #363636;
|
||
border-radius: 4px;
|
||
padding: 7px 16px;
|
||
font-size: 13px;
|
||
}
|
||
QPushButton:hover {
|
||
background-color: #242424;
|
||
color: #ffffff;
|
||
border: 1px solid #5a5a5a;
|
||
}
|
||
QPushButton:pressed {
|
||
background-color: #0f0f0f;
|
||
border: 1px solid #8a8a8a;
|
||
padding-top: 8px;
|
||
padding-bottom: 6px;
|
||
}
|
||
QPushButton:disabled {
|
||
background-color: #101010;
|
||
color: #606060;
|
||
border: 1px solid #242424;
|
||
}
|
||
QPushButton#StartButton {
|
||
background-color: #171717;
|
||
color: #d8d8d8;
|
||
border: 1px solid #363636;
|
||
}
|
||
QPushButton#StopButton {
|
||
background-color: #171717;
|
||
color: #d8d8d8;
|
||
border: 1px solid #363636;
|
||
}
|
||
QPushButton#RestartButton {
|
||
background-color: #171717;
|
||
color: #d8d8d8;
|
||
border: 1px solid #363636;
|
||
}
|
||
QPushButton#ManualButton {
|
||
background-color: #171717;
|
||
color: #d8d8d8;
|
||
border: 1px solid #363636;
|
||
}
|
||
QPushButton#HoldButton {
|
||
background-color: #171717;
|
||
color: #d8d8d8;
|
||
border: 1px solid #363636;
|
||
}
|
||
QSplitter::handle {
|
||
background-color: #151515;
|
||
}
|
||
QSplitter::handle:hover {
|
||
background-color: #2a2a2a;
|
||
}
|
||
QSplitter::handle:horizontal {
|
||
width: 8px;
|
||
}
|
||
QSplitter::handle:vertical {
|
||
height: 8px;
|
||
}
|
||
"""
|
||
|
||
|
||
if QPushButton is not None:
|
||
class AnimatedButton(QPushButton):
|
||
def __init__(self, text=""):
|
||
super().__init__(text)
|
||
self._opacity = QGraphicsOpacityEffect(self)
|
||
self._opacity.setOpacity(1.0)
|
||
self.setGraphicsEffect(self._opacity)
|
||
self._animation = QPropertyAnimation(self._opacity, b"opacity", self)
|
||
self._animation.setEasingCurve(QEasingCurve.OutCubic)
|
||
|
||
def _animate_opacity(self, target, duration):
|
||
self._animation.stop()
|
||
self._animation.setDuration(duration)
|
||
self._animation.setStartValue(self._opacity.opacity())
|
||
self._animation.setEndValue(target)
|
||
self._animation.start()
|
||
|
||
def mousePressEvent(self, event):
|
||
if self.isEnabled():
|
||
self._animate_opacity(0.72, 70)
|
||
super().mousePressEvent(event)
|
||
|
||
def mouseReleaseEvent(self, event):
|
||
if self.isEnabled():
|
||
self._animate_opacity(1.0, 140)
|
||
super().mouseReleaseEvent(event)
|
||
else:
|
||
AnimatedButton = None
|
||
|
||
|
||
class MetricBox(QFrame):
|
||
def __init__(self, title, value="--"):
|
||
super().__init__()
|
||
self.setObjectName("Panel")
|
||
layout = QVBoxLayout(self)
|
||
layout.setContentsMargins(10, 8, 10, 8)
|
||
layout.setSpacing(4)
|
||
self.title_label = QLabel(title)
|
||
self.title_label.setObjectName("MetricLabel")
|
||
self.value_label = QLabel(value)
|
||
self.value_label.setObjectName("MetricValue")
|
||
layout.addWidget(self.title_label)
|
||
layout.addWidget(self.value_label)
|
||
|
||
def set_value(self, value, sub=None):
|
||
self.value_label.setText(value)
|
||
|
||
|
||
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_panel = QFrame()
|
||
self.status_panel.setObjectName("StatusPanel")
|
||
status_layout = QVBoxLayout(self.status_panel)
|
||
status_layout.setContentsMargins(10, 6, 10, 6)
|
||
status_layout.setSpacing(1)
|
||
self.status_chip = QLabel("state=--")
|
||
self.status_chip.setObjectName("StatusMain")
|
||
self.action_label = QLabel("action=--")
|
||
self.action_label.setObjectName("StatusSub")
|
||
status_layout.addWidget(self.status_chip)
|
||
status_layout.addWidget(self.action_label)
|
||
self.start_button = AnimatedButton("启动")
|
||
self.start_button.setObjectName("StartButton")
|
||
self.start_button.clicked.connect(self.start_control)
|
||
self.stop_button = AnimatedButton("停止")
|
||
self.stop_button.setObjectName("StopButton")
|
||
self.stop_button.clicked.connect(self.stop_control)
|
||
self.stop_button.setEnabled(False)
|
||
self.restart_button = AnimatedButton("重启")
|
||
self.restart_button.setObjectName("RestartButton")
|
||
self.restart_button.clicked.connect(self.restart_control)
|
||
self.manual_open_button = AnimatedButton("夹爪张开")
|
||
self.manual_open_button.setObjectName("ManualButton")
|
||
self.manual_open_button.clicked.connect(lambda: self.send_manual_command("open"))
|
||
self.manual_close_button = AnimatedButton("夹爪闭合")
|
||
self.manual_close_button.setObjectName("ManualButton")
|
||
self.manual_close_button.clicked.connect(lambda: self.send_manual_command("close"))
|
||
self.manual_hold_button = AnimatedButton("保持")
|
||
self.manual_hold_button.setObjectName("HoldButton")
|
||
self.manual_hold_button.clicked.connect(self.toggle_manual_hold)
|
||
self._set_button_state("idle")
|
||
top.addWidget(title)
|
||
top.addWidget(self.status_panel)
|
||
top.addStretch(1)
|
||
top.addWidget(self.start_button)
|
||
top.addWidget(self.stop_button)
|
||
top.addWidget(self.restart_button)
|
||
top.addWidget(self.manual_open_button)
|
||
top.addWidget(self.manual_close_button)
|
||
top.addWidget(self.manual_hold_button)
|
||
outer.addLayout(top)
|
||
|
||
main_splitter = QSplitter(Qt.Horizontal)
|
||
main_splitter.setChildrenCollapsible(False)
|
||
outer.addWidget(main_splitter, 1)
|
||
|
||
flow_splitter = QSplitter(Qt.Vertical)
|
||
flow_splitter.setChildrenCollapsible(False)
|
||
self.left_flow = ImageBox("左侧光流")
|
||
self.right_flow = ImageBox("右侧光流")
|
||
flow_splitter.addWidget(self.left_flow)
|
||
flow_splitter.addWidget(self.right_flow)
|
||
flow_splitter.setStretchFactor(0, 1)
|
||
flow_splitter.setStretchFactor(1, 1)
|
||
flow_splitter.setSizes([360, 360])
|
||
main_splitter.addWidget(flow_splitter)
|
||
|
||
side_widget = QWidget()
|
||
side = QVBoxLayout(side_widget)
|
||
side.setContentsMargins(0, 0, 0, 0)
|
||
side.setSpacing(8)
|
||
main_splitter.addWidget(side_widget)
|
||
main_splitter.setStretchFactor(0, 2)
|
||
main_splitter.setStretchFactor(1, 4)
|
||
main_splitter.setSizes([520, 980])
|
||
|
||
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)
|
||
|
||
def _configure_plot(self):
|
||
self.plots = []
|
||
self.curves = {}
|
||
plot_specs = [
|
||
("left_normal", "左法向力", "#00E5FF", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N),
|
||
("right_normal", "右法向力", "#ffb000", PLOT_FORCE_Y_MIN_N, PLOT_FORCE_Y_MAX_N),
|
||
("left_shear_x", "左切向 X", "#61d394", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
|
||
("left_shear_y", "左切向 Y", "#f07178", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
|
||
("right_shear_x", "右切向 X", "#8ab4f8", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
|
||
("right_shear_y", "右切向 Y", "#d19a66", PLOT_SHEAR_Y_MIN_N, PLOT_SHEAR_Y_MAX_N),
|
||
]
|
||
for idx, (key, title, color, y_min, y_max) in enumerate(plot_specs):
|
||
row = idx // 2
|
||
col = idx % 2
|
||
plot = self.plot_widget.addPlot(row=row, col=col)
|
||
plot.setTitle(title, color="#999999", size="11pt")
|
||
plot.titleLabel.setFont(QFont("SimHei", 11))
|
||
plot.showAxis("top", False)
|
||
plot.showAxis("right", False)
|
||
plot.showGrid(x=False, y=True, alpha=0.12)
|
||
plot.setLabel("bottom", "时间", units="s")
|
||
plot.setLabel("left", "力", units="N")
|
||
plot.getAxis("bottom").setTickSpacing(major=1.0, minor=1.0)
|
||
plot.setYRange(y_min, y_max, padding=0)
|
||
plot.setLimits(yMin=y_min, yMax=y_max)
|
||
plot.enableAutoRange(axis=pg.ViewBox.YAxis, enable=False)
|
||
self.plots.append(plot)
|
||
self.curves[key] = plot.plot(pen=pg.mkPen(color, width=2))
|
||
|
||
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)
|
||
manual_enabled = state == "running"
|
||
self.manual_open_button.setEnabled(manual_enabled)
|
||
self.manual_close_button.setEnabled(manual_enabled)
|
||
self.manual_hold_button.setEnabled(manual_enabled)
|
||
|
||
def send_manual_command(self, command):
|
||
if self.controller is None or self.control_thread is None or not self.control_thread.is_alive():
|
||
return
|
||
try:
|
||
self.controller.request_manual_command(command)
|
||
except Exception as exc:
|
||
print(f"manual command failed: {exc}")
|
||
|
||
def toggle_manual_hold(self):
|
||
command = "release" if self.manual_hold_button.text() == "释放" else "hold"
|
||
self.send_manual_command(command)
|
||
|
||
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
|
||
|
||
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")
|
||
|
||
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()
|
||
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={label_with_cn(state, STATE_TEXT)}")
|
||
self.action_label.setText(f"action={label_with_cn(action, ACTION_TEXT)}")
|
||
self.manual_hold_button.setText(
|
||
"释放" if bool(status.get("manual_hold_active")) else "保持"
|
||
)
|
||
|
||
self.left_force.set_value(
|
||
fmt(status.get("left_normal"), unit),
|
||
)
|
||
self.right_force.set_value(
|
||
fmt(status.get("right_normal"), unit),
|
||
)
|
||
self.left_shear_box.set_value(
|
||
fmt(status.get("left_shear"), shear_unit),
|
||
)
|
||
self.right_shear_box.set_value(
|
||
fmt(status.get("right_shear"), shear_unit),
|
||
)
|
||
self.force_pct_box.set_value(
|
||
str(status.get("force_pct", "--")),
|
||
)
|
||
self.target_force_box.set_value(
|
||
str(status.get("adaptive_target_force", "--")),
|
||
)
|
||
self.speed_box.set_value(
|
||
str(status.get("speed_pct", "--")),
|
||
)
|
||
self.trigger_box.set_value(
|
||
f"{int(bool(status.get('trigger')))} / {int(bool(status.get('trigger_armed')))}",
|
||
)
|
||
self.contact_box.set_value(
|
||
str(int(bool(status.get("grip_contact")))),
|
||
)
|
||
self.release_box.set_value(
|
||
str(int(bool(status.get("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.right_flow.set_image(right_sample.get("flow_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),
|
||
force_display_value(status.get("left_shear_x", 0.0) or 0.0),
|
||
force_display_value(status.get("left_shear_y", 0.0) or 0.0),
|
||
force_display_value(status.get("right_shear_x", 0.0) or 0.0),
|
||
force_display_value(status.get("right_shear_y", 0.0) or 0.0),
|
||
)
|
||
)
|
||
if len(self.force_history) > PLOT_POINTS:
|
||
self.force_history = self.force_history[-PLOT_POINTS:]
|
||
self._update_plot()
|
||
|
||
def _update_plot(self):
|
||
if not self.force_history:
|
||
return
|
||
data = np.asarray(self.force_history, dtype=float)
|
||
x = data[:, 0]
|
||
keys = [
|
||
"left_normal",
|
||
"right_normal",
|
||
"left_shear_x",
|
||
"left_shear_y",
|
||
"right_shear_x",
|
||
"right_shear_y",
|
||
]
|
||
for idx, key in enumerate(keys, start=1):
|
||
self.curves[key].setData(x, data[:, idx])
|
||
if len(x) >= 2:
|
||
x_max = max(6.0, float(np.ceil(x[-1])))
|
||
x_min = max(0.0, x_max - 6.0)
|
||
for plot in self.plots:
|
||
plot.setXRange(x_min, x_max, 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.start_control(from_restart=True)
|
||
return
|
||
self._set_button_state("idle")
|
||
|
||
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:]))
|