缩放功能欧克，箭头好了

yincheng.zhong

2025-11-24 08da431bc5693c77659d664bb131f2bfdd14d8e4

缩放功能欧克，箭头好了

 python/hitl/__pycache__/run_sim.cpython-310.pyc

Binary files differ

 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:

 python/hitl/runlog.txt

ÎļþÌ«´ó

 python/realtime_control.py

@@ -865,7 +865,7 @@
    def disconnect(self):
        """断开连接"""
        if self.receiver:
            self.receiver.disconnect()
        self.receiver.disconnect()
            self.receiver = None
        self.cmd_sender = None
    
@@ -1099,7 +1099,7 @@
            
            if self._stop_event.is_set():
                return

            
            print("[INFO] å¼€å§‹æŽ§åˆ¶å¾ªçŽ¯")
            
            while not self._stop_event.is_set():
@@ -1158,7 +1158,7 @@
                            # æ•°å­¦è§’度转地理角度：heading_err_deg
                            # æ³¨æ„ control_output['info']['heading_err'] æ˜¯å¼§åº¦
                            heading_err_deg = math.degrees(control_info.get('heading_err', 0))

                            
                            print(f"[LOG] POS_ENU: {x:.3f}, {y:.3f}, {z:.3f} | "
                                  f"ATT(H/P/R): {gps_heading:.2f}°, {gps_pitch:.2f}°, {gps_roll:.2f}° | "
                                  f"CTRL: S={status}, F={forward_signal}, T={turn_signal}, Err={xte:.2f}m")
@@ -1211,10 +1211,10 @@
            self._stop_event.clear()
            # å‘送停止命令 (中位值)
            if self.cmd_sender:
                print("[INFO] å‘送停止命令...")
                for _ in range(5):
                    self.cmd_sender.send_control_command(1500, 1500)
                    time.sleep(0.02)
            print("[INFO] å‘送停止命令...")
            for _ in range(5):
                self.cmd_sender.send_control_command(1500, 1500)
                time.sleep(0.02)
            
            # æ‰“印最终统计
            print("\n========== æœ€ç»ˆç»Ÿè®¡ ==========")
@@ -1223,7 +1223,7 @@
                print(f"GPS数据包: {receiver.gps_count}")
                print(f"IMU数据包: {receiver.imu_count}")
            if self.cmd_sender:
                print(f"命令发送: {self.cmd_sender.get_stats()}")
            print(f"命令发送: {self.cmd_sender.get_stats()}")
            if receiver:
                print(f"错误计数: {receiver.error_count}")
            print("==============================\n")
@@ -1252,7 +1252,7 @@
        origin_alt_m=None,
        gui_bridge=gui_bridge,
    )

    
    # é¢„加载路径文件（若存在）
    candidate_path = path_file or DEFAULT_PATH_FILE
    if candidate_path and Path(candidate_path).exists():
@@ -1275,13 +1275,13 @@
        # æ— Qt环境时，沿用旧的命令行工作流
        if not controller.path_points and Path(DEFAULT_PATH_FILE).exists():
            controller.load_path(DEFAULT_PATH_FILE)
        if not controller.connect():
            print("[ERROR] ä¸²å£è¿žæŽ¥å¤±è´¥ï¼Œé€€å‡º")
            return
        try:
            controller.run()
        finally:
            controller.disconnect()
    if not controller.connect():
        print("[ERROR] ä¸²å£è¿žæŽ¥å¤±è´¥ï¼Œé€€å‡º")
        return
    try:
        controller.run()
    finally:
        controller.disconnect()


if __name__ == "__main__":