init
This commit is contained in:
cheng
@@ -0,0 +1,877 @@
|
||||
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 apply_visual_transform(img, transform):
|
||||
if img is None:
|
||||
return None
|
||||
angle = int(transform.get("rotation_angle", 0)) % 360
|
||||
if angle == 90:
|
||||
img = cv2.rotate(img, cv2.ROTATE_90_CLOCKWISE)
|
||||
elif angle == 180:
|
||||
img = cv2.rotate(img, cv2.ROTATE_180)
|
||||
elif angle == 270:
|
||||
img = cv2.rotate(img, cv2.ROTATE_90_COUNTERCLOCKWISE)
|
||||
|
||||
flip_x = bool(transform.get("flip_x"))
|
||||
flip_y = bool(transform.get("flip_y"))
|
||||
if flip_x and flip_y:
|
||||
img = cv2.flip(img, -1)
|
||||
elif flip_x:
|
||||
img = cv2.flip(img, 1)
|
||||
elif flip_y:
|
||||
img = cv2.flip(img, 0)
|
||||
return img
|
||||
|
||||
|
||||
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, on_rotate=None, on_flip_x=None, on_flip_y=None, on_reset=None):
|
||||
super().__init__()
|
||||
self.title = title
|
||||
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)
|
||||
controls = QHBoxLayout()
|
||||
controls.setSpacing(6)
|
||||
self.rotate_button = AnimatedButton("旋转")
|
||||
self.flip_x_button = AnimatedButton("水平")
|
||||
self.flip_y_button = AnimatedButton("垂直")
|
||||
self.reset_button = AnimatedButton("重置")
|
||||
for button, callback in (
|
||||
(self.rotate_button, on_rotate),
|
||||
(self.flip_x_button, on_flip_x),
|
||||
(self.flip_y_button, on_flip_y),
|
||||
(self.reset_button, on_reset),
|
||||
):
|
||||
button.setObjectName("ManualButton")
|
||||
if callback is not None:
|
||||
button.clicked.connect(callback)
|
||||
controls.addWidget(button)
|
||||
layout.addLayout(controls)
|
||||
|
||||
def set_image(self, img):
|
||||
self.image_label.setPixmap(cv2_to_pixmap(img))
|
||||
|
||||
def set_transform_label(self, transform):
|
||||
angle = int(transform.get("rotation_angle", 0)) % 360
|
||||
flip_x = "X1" if transform.get("flip_x") else "X0"
|
||||
flip_y = "Y1" if transform.get("flip_y") else "Y0"
|
||||
self.title_label.setText(f"{self.title} {angle}deg {flip_x} {flip_y}")
|
||||
|
||||
|
||||
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.visual_transforms = {
|
||||
"left": {"rotation_angle": 0, "flip_x": False, "flip_y": False},
|
||||
"right": {"rotation_angle": 0, "flip_x": False, "flip_y": False},
|
||||
}
|
||||
|
||||
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(
|
||||
"左侧光流",
|
||||
on_rotate=lambda: self.adjust_visual_transform("left", "rotate"),
|
||||
on_flip_x=lambda: self.adjust_visual_transform("left", "flip_x"),
|
||||
on_flip_y=lambda: self.adjust_visual_transform("left", "flip_y"),
|
||||
on_reset=lambda: self.adjust_visual_transform("left", "reset"),
|
||||
)
|
||||
self.right_flow = ImageBox(
|
||||
"右侧光流",
|
||||
on_rotate=lambda: self.adjust_visual_transform("right", "rotate"),
|
||||
on_flip_x=lambda: self.adjust_visual_transform("right", "flip_x"),
|
||||
on_flip_y=lambda: self.adjust_visual_transform("right", "flip_y"),
|
||||
on_reset=lambda: self.adjust_visual_transform("right", "reset"),
|
||||
)
|
||||
self.left_flow.set_transform_label(self.visual_transforms["left"])
|
||||
self.right_flow.set_transform_label(self.visual_transforms["right"])
|
||||
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 adjust_visual_transform(self, side, action):
|
||||
transform = self.visual_transforms[side]
|
||||
if action == "rotate":
|
||||
transform["rotation_angle"] = (int(transform["rotation_angle"]) + 90) % 360
|
||||
elif action == "flip_x":
|
||||
transform["flip_x"] = not bool(transform["flip_x"])
|
||||
elif action == "flip_y":
|
||||
transform["flip_y"] = not bool(transform["flip_y"])
|
||||
elif action == "reset":
|
||||
transform.update({"rotation_angle": 0, "flip_x": False, "flip_y": False})
|
||||
|
||||
if side == "left":
|
||||
self.left_flow.set_transform_label(transform)
|
||||
else:
|
||||
self.right_flow.set_transform_label(transform)
|
||||
|
||||
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(
|
||||
apply_visual_transform(left_sample.get("flow_view"), self.visual_transforms["left"])
|
||||
)
|
||||
self.right_flow.set_image(
|
||||
apply_visual_transform(right_sample.get("flow_view"), self.visual_transforms["right"])
|
||||
)
|
||||
|
||||
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:]))
|
||||
Reference in New Issue
Block a user