orisys/test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v 1.1

Browse Source
This commit is contained in:
cheng
2026-06-09 13:45:08 +08:00
parent 886e5df5ee
commit 74d2bc4853
6 changed files with 101 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gripper_control_02/visualizer.py
+97 -29
View File
@@ -60,6 +60,7 @@ 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"
@@ -74,11 +75,19 @@ def cv2_to_pixmap(img_bgr):
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:
text = f"{float(value):.{precision}f}"
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}"
@@ -169,6 +178,11 @@ QPushButton#StopButton {
color: #f07070;
border: 1px solid #4d1a1a;
}
QPushButton#RestartButton {
background-color: #1d1d12;
color: #e0c96a;
border: 1px solid #4d4520;
}
"""
@@ -232,6 +246,8 @@ class GripperDemo02Viewer(QMainWindow):
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
@@ -265,11 +281,16 @@ class GripperDemo02Viewer(QMainWindow):
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()
@@ -294,10 +315,10 @@ class GripperDemo02Viewer(QMainWindow):
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.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("接触", "--")
@@ -305,8 +326,8 @@ class GripperDemo02Viewer(QMainWindow):
boxes = [
self.left_force,
self.right_force,
self.shear_force,
self.force_diff,
self.left_shear_box,
self.right_shear_box,
self.force_pct_box,
self.target_force_box,
self.speed_box,
@@ -342,7 +363,7 @@ class GripperDemo02Viewer(QMainWindow):
def _configure_plot(self):
self.plot = self.plot_widget.addPlot()
self.plot.setTitle("左右法向力与最大切向力", color="#999999", size="13pt")
self.plot.setTitle("左右法向力", color="#999999", size="13pt")
self.plot.titleLabel.setFont(QFont("SimHei", 13))
self.plot.showAxis("top", False)
self.plot.showAxis("right", False)
@@ -361,20 +382,43 @@ class GripperDemo02Viewer(QMainWindow):
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):
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.start_button.setEnabled(False)
self.stop_button.setEnabled(True)
self._set_button_state("restarting" if from_restart else "starting")
self.controller_finished = False
self.info_label.setText("启动中：正在连接夹爪与两个触觉传感器")
if from_restart:
self.info_label.setText("重启中：正在重新连接夹爪与两个触觉传感器")
else:
self.info_label.setText("启动中：正在连接夹爪与两个触觉传感器")
self.controller = build_controller(
self.argv,
@@ -391,9 +435,20 @@ class GripperDemo02Viewer(QMainWindow):
def stop_control(self):
if self.controller is not None:
self.controller.stop()
self.stop_button.setEnabled(False)
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()
@@ -413,6 +468,13 @@ class GripperDemo02Viewer(QMainWindow):
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", "--")
@@ -429,13 +491,13 @@ class GripperDemo02Viewer(QMainWindow):
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.left_shear_box.set_value(
fmt(status.get("left_shear"), shear_unit),
"left_shear",
)
self.force_diff.set_value(
fmt(status.get("normal_diff"), unit),
f"dN {fmt(status.get('normal_change'), unit)}",
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", "--")),
@@ -473,9 +535,8 @@ class GripperDemo02Viewer(QMainWindow):
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),
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:
@@ -503,7 +564,6 @@ class GripperDemo02Viewer(QMainWindow):
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)
@@ -512,8 +572,15 @@ class GripperDemo02Viewer(QMainWindow):
if not self.controller_finished:
return
self.controller_finished = False
self.start_button.setEnabled(True)
self.stop_button.setEnabled(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("已停止")
@@ -548,6 +615,7 @@ class GripperDemo02Viewer(QMainWindow):
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)