From 08da431bc5693c77659d664bb131f2bfdd14d8e4 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期一, 24 十一月 2025 16:58:17 +0800
Subject: [PATCH] 缩放功能欧克，箭头好了
---
python/hitl/run_sim.py | 191 ++++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 176 insertions(+), 15 deletions(-)
diff --git a/python/hitl/run_sim.py b/python/hitl/run_sim.py
index cf0e9d3..c14a638 100644
--- a/python/hitl/run_sim.py
+++ b/python/hitl/run_sim.py
@@ -97,6 +97,57 @@
pass
+class ZoomableGraphicsView(QtWidgets.QGraphicsView):
+ """鏀寔榧犳爣婊氳疆缂╂斁 + 鎸変綇宸﹂敭骞崇Щ鐨勮鍥�"""
+
+ def __init__(self, *args, **kwargs):
+ super().__init__(*args, **kwargs)
+ self.setRenderHint(QtGui.QPainter.Antialiasing, True)
+ self.setTransformationAnchor(QtWidgets.QGraphicsView.AnchorUnderMouse)
+ self.setResizeAnchor(QtWidgets.QGraphicsView.AnchorUnderMouse)
+ self.setDragMode(QtWidgets.QGraphicsView.NoDrag)
+ self.setCursor(QtCore.Qt.OpenHandCursor)
+ self._zoom_factor = 1.15 # 姣忔婊氳疆鐨勭缉鏀惧洜瀛�
+ self._panning = False
+ self._pan_last_pos = QtCore.QPoint()
+ self.on_user_pan = None # 鐢卞閮ㄦ敞鍏ョ殑鍥炶皟
+
+ def wheelEvent(self, event: QtGui.QWheelEvent):
+ delta = event.angleDelta().y()
+ scale_factor = self._zoom_factor if delta > 0 else 1.0 / self._zoom_factor
+ self.scale(scale_factor, scale_factor)
+
+ def mousePressEvent(self, event: QtGui.QMouseEvent):
+ if event.button() == QtCore.Qt.LeftButton:
+ self._panning = True
+ self._pan_last_pos = event.pos()
+ self.setCursor(QtCore.Qt.ClosedHandCursor)
+ event.accept()
+ return
+ super().mousePressEvent(event)
+
+ def mouseMoveEvent(self, event: QtGui.QMouseEvent):
+ if self._panning:
+ scene_pos = self.mapToScene(event.pos())
+ last_scene_pos = self.mapToScene(self._pan_last_pos)
+ delta = scene_pos - last_scene_pos
+ self._pan_last_pos = event.pos()
+ self.translate(-delta.x(), -delta.y())
+ if self.on_user_pan:
+ self.on_user_pan()
+ event.accept()
+ return
+ super().mouseMoveEvent(event)
+
+ def mouseReleaseEvent(self, event: QtGui.QMouseEvent):
+ if event.button() == QtCore.Qt.LeftButton and self._panning:
+ self._panning = False
+ self.setCursor(QtCore.Qt.OpenHandCursor)
+ event.accept()
+ return
+ super().mouseReleaseEvent(event)
+
+
class HitlDashboard(QtWidgets.QMainWindow):
def __init__(self,
simulator: HitlSimulator,
@@ -113,6 +164,7 @@
self.pose_status: Optional[PoseStatus] = None
self.state_status: Optional[StateStatus] = None
self.stack_status: Dict[str, StackStatus] = {}
+ self._auto_follow = True
self.setWindowTitle("HITL 浠跨湡鐘舵�侀潰鏉�")
self.resize(1280, 720)
@@ -133,11 +185,21 @@
self.setCentralWidget(central)
self.scene = QtWidgets.QGraphicsScene(self)
- self.view = QtWidgets.QGraphicsView(self.scene)
- self.view.setRenderHint(QtGui.QPainter.Antialiasing, True)
- self.view.setDragMode(QtWidgets.QGraphicsView.ScrollHandDrag)
- self.view.setTransformationAnchor(QtWidgets.QGraphicsView.AnchorUnderMouse)
- layout.addWidget(self.view, stretch=3)
+ left_panel = QtWidgets.QWidget()
+ left_layout = QtWidgets.QVBoxLayout(left_panel)
+ left_layout.setContentsMargins(0, 0, 0, 0)
+ left_layout.setSpacing(4)
+
+ self.view = ZoomableGraphicsView(self.scene)
+ self.view.on_user_pan = self._handle_user_pan
+ left_layout.addWidget(self.view, stretch=1)
+
+ self.follow_checkbox = QtWidgets.QCheckBox("鑷姩璺熼殢杞﹁締")
+ self.follow_checkbox.setChecked(True)
+ self.follow_checkbox.toggled.connect(self._on_follow_toggled)
+ left_layout.addWidget(self.follow_checkbox, alignment=QtCore.Qt.AlignLeft)
+
+ layout.addWidget(left_panel, stretch=3)
self._port_refresh_supported = list_ports is not None
@@ -238,8 +300,21 @@
# Scene items
self.path_item = self.scene.addPath(QtGui.QPainterPath(), QtGui.QPen(QtGui.QColor("gray"), 0.8))
self.trail_item = self.scene.addPath(QtGui.QPainterPath(), QtGui.QPen(QtGui.QColor("blue"), 1.0))
- self.robot_item = self.scene.addEllipse(-0.3, -0.3, 0.6, 0.6, QtGui.QPen(QtGui.QColor("red")), QtGui.QBrush(QtGui.QColor("red")))
- self.heading_item = self.scene.addLine(0, 0, 0, 0, QtGui.QPen(QtGui.QColor("red"), 2))
+ self.robot_item = self.scene.addEllipse(
+ -0.3,
+ -0.3,
+ 0.6,
+ 0.6,
+ QtGui.QPen(QtGui.QColor("#d11d29"), 1.0),
+ QtGui.QBrush(QtGui.QColor("#ff4b5c")),
+ )
+ arrow_pen = QtGui.QPen(QtGui.QColor("#ff8c00"), 0)
+ arrow_pen.setJoinStyle(QtCore.Qt.RoundJoin)
+ self.heading_item = self.scene.addPath(
+ QtGui.QPainterPath(),
+ arrow_pen,
+ QtGui.QBrush(QtGui.QColor("#ff8c00")),
+ )
self.target_item = self.scene.addEllipse(-0.2, -0.2, 0.4, 0.4, QtGui.QPen(QtGui.QColor("green")), QtGui.QBrush(QtGui.QColor("green")))
self.path_browse_btn.clicked.connect(self._browse_path)
@@ -253,6 +328,18 @@
self._set_controls_enabled(False)
self._update_serial_ui()
+ def _handle_user_pan(self):
+ if self._auto_follow:
+ self.follow_checkbox.blockSignals(True)
+ self.follow_checkbox.setChecked(False)
+ self.follow_checkbox.blockSignals(False)
+ self._auto_follow = False
+
+ def _on_follow_toggled(self, checked: bool):
+ self._auto_follow = checked
+ if checked and self.pose_status:
+ self.view.centerOn(self.pose_status.east, -self.pose_status.north)
+
def _refresh_serial_ports(self, initial_uart2: Optional[str] = None, initial_uart5: Optional[str] = None):
ports: List[str] = []
if list_ports:
@@ -399,7 +486,7 @@
def _init_scene(self):
if not self.path_points:
- self.scene.setSceneRect(-10, -10, 20, 20)
+ self.scene.setSceneRect(-250, -250, 500, 500)
return
path = QtGui.QPainterPath()
first = True
@@ -414,8 +501,21 @@
else:
path.lineTo(px, -py)
self.path_item.setPath(path)
- rect = QtCore.QRectF(min(xs) - 2, min(ys) - 2, (max(xs) - min(xs)) + 4, (max(ys) - min(ys)) + 4)
- self.scene.setSceneRect(rect)
+ min_x = min(xs)
+ max_x = max(xs)
+ min_y = min(ys)
+ max_y = max(ys)
+ width = max_x - min_x
+ height = max_y - min_y
+ pad_x = max(10.0, width * 0.75)
+ pad_y = max(10.0, height * 0.75)
+ scene_rect = QtCore.QRectF(
+ min_x - pad_x,
+ min_y - pad_y,
+ (width if width > 0 else 1.0) + pad_x * 2.0,
+ (height if height > 0 else 1.0) + pad_y * 2.0,
+ )
+ self.scene.setSceneRect(scene_rect)
def _append_log(self, text: str):
self.log_view.appendPlainText(text)
@@ -516,12 +616,73 @@
x = pose.east
y = -pose.north
self.robot_item.setRect(x - 0.3, y - 0.3, 0.6, 0.6)
- heading_rad = math.radians(pose.heading_deg)
- dx = math.cos(heading_rad)
- dy = math.sin(heading_rad)
- self.heading_item.setLine(x, y, x + dx, y - dy)
+
+ # 缁樺埗鑸悜瑙掔澶达紙涓庤溅浣撳垎绂荤殑姗欒壊鎸囧悜绠ご锛�
+ # pose.heading_deg 涓虹綏鐩樿锛堝寳=0掳锛岄『鏃堕拡涓烘锛夛紝闇�杞崲鍒版暟瀛﹀潗鏍囷紙涓�=0掳锛岄�嗘椂閽堜负姝ｏ級
+ heading_rad = math.radians(90.0 - pose.heading_deg)
+ dir_x = math.cos(heading_rad)
+ dir_y = -math.sin(heading_rad) # 瑙嗗浘鍧愭爣 Y 鍚戜笅锛屽洜姝ゅ彇鍙�
+ perp_x = -dir_y
+ perp_y = dir_x
+
+ # 灏勭嚎鍙傛暟
+ offset = 0.25 # 绠ご绂昏溅浣撲腑蹇冪殑鍋忕Щ锛岄伩鍏嶉伄鎸�
+ shaft_length = 0.85
+ shaft_half_width = 0.07
+ head_length = 0.45
+ head_half_width = 0.28
+
+ start = (
+ x + dir_x * offset,
+ y + dir_y * offset,
+ )
+ shaft_end = (
+ start[0] + dir_x * shaft_length,
+ start[1] + dir_y * shaft_length,
+ )
+ tip = (
+ shaft_end[0] + dir_x * head_length,
+ shaft_end[1] + dir_y * head_length,
+ )
+
+ points = [
+ (
+ start[0] + perp_x * shaft_half_width,
+ start[1] + perp_y * shaft_half_width,
+ ),
+ (
+ shaft_end[0] + perp_x * shaft_half_width,
+ shaft_end[1] + perp_y * shaft_half_width,
+ ),
+ (
+ shaft_end[0] + perp_x * head_half_width,
+ shaft_end[1] + perp_y * head_half_width,
+ ),
+ tip,
+ (
+ shaft_end[0] - perp_x * head_half_width,
+ shaft_end[1] - perp_y * head_half_width,
+ ),
+ (
+ shaft_end[0] - perp_x * shaft_half_width,
+ shaft_end[1] - perp_y * shaft_half_width,
+ ),
+ (
+ start[0] - perp_x * shaft_half_width,
+ start[1] - perp_y * shaft_half_width,
+ ),
+ ]
+
+ arrow_path = QtGui.QPainterPath()
+ arrow_path.moveTo(*points[0])
+ for pt in points[1:]:
+ arrow_path.lineTo(*pt)
+ arrow_path.closeSubpath()
+
+ self.heading_item.setPath(arrow_path)
self.target_item.setRect(pose.target_east - 0.2, -pose.target_north - 0.2, 0.4, 0.4)
- self.view.centerOn(x, y)
+ if self._auto_follow:
+ self.view.centerOn(x, y)
def parse_args() -> argparse.Namespace:
--
Gitblit v1.10.0