#!/usr/bin/env python3 import rclpy from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, QHeaderView, QPushButton, QCheckBox, QLineEdit) from PyQt6.QtCore import Qt, QTimer import sys import asyncio import json import subprocess import time # 導入分離的類別 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver from map_layout import DroneMap from drone_panel import DronePanel, SocketGroupPanel from comm_panel import CommPanel from overview_table import OverviewTable # ================================================================================ # 導入任務規劃器、執行器、發送器 # ================================================================================ from mission_planner import FormationPlanner, MissionType from command_sender import MavlinkSender from mission_executor import MissionExecutor # ================================================================================ class ControlStationUI(QMainWindow): VERSION = '1.0.1' def __init__(self): super().__init__() self.setWindowTitle(f'GCS v{self.VERSION}') self.resize(1400, 900) # 初始化ROS2 rclpy.init() self.monitor = DroneMonitor() self.monitor.signals.update_signal.connect(self.update_ui) # ROS执行器配置 self.executor = rclpy.executors.SingleThreadedExecutor() self.executor.add_node(self.monitor) # 定时处理ROS事件 self.timer = QTimer() self.timer.timeout.connect(self.spin_ros) self.timer.start(10) # 初始化 panel 和 map 更新（10Hz） self.panel_map_timer = QTimer() self.panel_map_timer.timeout.connect(self._update_panel_and_map) self.panel_map_timer.start(100) # 10Hz # 快取消息數據，以便在沒有新消息時使用上一次的值 self._message_cache = {} # 初始化UI self.drones = {} self.socket_groups = {} self.socket_types = {} self.socket_colors = { '0': '#00BFFF', # 天藍色 (DeepSkyBlue) '1': '#FFD700', # 金色 (Gold) '2': '#FF6969', # 淺紅色 (Light Red) '3': '#FF69B4', # 熱粉紅 (HotPink) '4': '#00FA9A', # 中春綠 (MediumSpringGreen) '5': '#9370DB', # 中紫色 (MediumPurple) - 串口 '6': '#FFA500', # 橙色 (Orange) '7': '#20B2AA', # 淺海綠 (LightSeaGreen) '8': '#7CFC00', # 草綠色 (LawnGreen) '9': '#FF8C00', # 深橙色 (DarkOrange) 'default': '#AAAAAA' # 灰色 } self.drone_positions = {} self.drone_headings = {} # 初始化地圖 self.drone_map = DroneMap() # 初始化連接列表 self.udp_receivers = [] self.udp_connections = [] self.ws_connections = [] self.serial_receivers = [] self.serial_connections = [] # ================================================================================ # 初始化任務規劃器 # ================================================================================ self.mission_planner = FormationPlanner( spacing=5.0, # 5 公尺間距 base_altitude=10.0, # 基準高度 10 公尺 altitude_diff=2.0 # 高低差 2 公尺 ) self.planned_waypoints = None # 儲存規劃結果 # ================================================================================ # ================================================================================ # 當前任務模式 (由地圖右上角下拉選單控制) # ================================================================================ self.current_mission_mode = 'M_FORMATION' # ================================================================================ # ================================================================================ # 初始化指令發送器與任務執行器 # ================================================================================ self.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死 self.mission_executor = MissionExecutor( sender=self.command_sender, drone_gps=self.monitor.drone_gps, # 直接傳引用，即時讀取 arrival_radius=2.0, send_rate_hz=2.0 ) self.mission_executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached) self.mission_executor.mission_completed.connect(self.on_mission_completed) # ================================================================================ self.init_ui() def init_ui(self): main_splitter = QSplitter(Qt.Orientation.Horizontal) # 左側 TabWidget self.left_tab = QTabWidget() # — 分頁 1：Drone Panel self.drone_panel_container = QWidget() self.drone_panel_layout = QVBoxLayout(self.drone_panel_container) self.drone_panel_layout.setAlignment(Qt.AlignmentFlag.AlignTop) self.drone_panel_layout.setSpacing(0) self.drone_panel_layout.setContentsMargins(10, 10, 10, 10) scroll = QScrollArea() scroll.setWidget(self.drone_panel_container) scroll.setWidgetResizable(True) self.left_tab.addTab(scroll, "無人載具") # — 分頁 2：Overview Table self.overview_table = OverviewTable() self.left_tab.addTab(self.overview_table, "總覽") # — 分頁 3：通訊設定 self.comm_panel = CommPanel() self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) self.comm_panel.serial_connection_added.connect(self.handle_serial_connection_added) self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection) self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection) self.comm_panel.serial_connection_toggled.connect(self.toggle_serial_connection) self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection) self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection) self.comm_panel.serial_connection_removed.connect(self.remove_serial_connection) self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout)) self.left_tab.addTab(self.comm_panel, "通訊") # 右侧容器 right_container = QWidget() right_layout = QVBoxLayout(right_container) right_layout.setContentsMargins(10, 10, 10, 10) right_layout.setSpacing(10) # ========== 批次控制區域 ========== batch_control_layout = QHBoxLayout() batch_title = QLabel("批次操作") batch_title.setStyleSheet(""" color: #DDD; font-size: 16px; font-weight: bold; padding: 5px; background-color: #333; border-radius: 4px; """) batch_control_layout.addWidget(batch_title) first_row = QHBoxLayout() select_all_btn = QPushButton("全選") select_all_btn.clicked.connect(self.handle_select_all) select_all_btn.setStyleSheet(""" QPushButton { background-color: #444; color: #DDD; border: none; padding: 8px 12px; border-radius: 4px; min-width: 80px; } QPushButton:hover { background-color: #555; } """) first_row.addWidget(select_all_btn) first_row.addStretch() mode_layout = QHBoxLayout() mode_label = QLabel("模式：") mode_label.setStyleSheet("color: #DDD; min-width: 40px;") from PyQt6.QtWidgets import QComboBox self.mode_combo = QComboBox() self.mode_combo.addItems([ "GUIDED", "AUTO", "LAND", "LOITER", "STABILIZE", "ACRO", "ALT_HOLD", "RTL", "CIRCLE", "DRIFT", "SPORT", "FLIP", "AUTOTUNE", "POSHOLD", "BRAKE", "THROW", "AVOID_ADSB", "GUIDED_NOGPS", "SMART_RTL", "FLOWHOLD", "FOLLOW", "ZIGZAG", "SYSTEMID", "AUTOROTATE", "AUTO_RTL" ]) self.mode_combo.setCurrentIndex(1) self.mode_combo.setStyleSheet(""" QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} """) batch_mode_btn = QPushButton("切換") batch_mode_btn.clicked.connect(self.handle_batch_mode_change) batch_mode_btn.setStyleSheet(""" QPushButton { background-color: #444; color: #DDD; border: none; padding: 8px 12px; border-radius: 4px; min-width: 80px; } QPushButton:hover { background-color: #555; } """) mode_layout.addWidget(mode_label) mode_layout.addWidget(self.mode_combo) mode_layout.addWidget(batch_mode_btn) mode_layout.addStretch() third_row = QHBoxLayout() arm_all_btn = QPushButton("解鎖") arm_all_btn.clicked.connect(self.handle_arm_selected) arm_all_btn.setStyleSheet(""" QPushButton { background-color: #444; color: #DDD; border: none; padding: 8px 12px; border-radius: 4px; min-width: 80px; } QPushButton:hover { background-color: #555; } """) third_row.addWidget(arm_all_btn) third_row.addStretch() fourth_row = QHBoxLayout() self.z_input = QLineEdit() self.z_input.setFixedWidth(60) self.z_input.setStyleSheet(""" QLineEdit { background-color: #333; color: #DDD; border: 1px solid #555; border-radius: 4px; padding: 3px; } """) takeoff_all_btn = QPushButton("起飛") takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) takeoff_all_btn.setStyleSheet(""" QPushButton { background-color: #444; color: #DDD; border: none; padding: 8px 12px; border-radius: 4px; min-width: 80px; } QPushButton:hover { background-color: #555; } """) fourth_row.addWidget(QLabel("高度:", styleSheet="color: #DDD;")) fourth_row.addWidget(self.z_input) fourth_row.addWidget(takeoff_all_btn) fourth_row.addStretch() batch_control_layout.addLayout(first_row) batch_control_layout.addLayout(mode_layout) batch_control_layout.addLayout(third_row) batch_control_layout.addLayout(fourth_row) right_layout.addLayout(batch_control_layout) # 添加地圖 right_layout.addWidget(self.drone_map.get_widget()) self.drone_map.get_gps_signal().connect(self.handle_map_click) self.drone_map.get_drone_clicked_signal().connect(self.handle_drone_clicked) self.drone_map.get_clear_all_drone_selection_signal().connect(self.handle_clear_all_drone_selection) self.drone_map.get_toggle_select_all_drones_signal().connect(self.handle_toggle_select_all_drones) # ================================================================================ # 連接任務控制 + 矩形選取 + 任務模式切換 + 路徑確認信號 # ================================================================================ self.drone_map.get_start_mission_signal().connect(self.handle_start_mission) self.drone_map.get_pause_mission_signal().connect(self.handle_pause_mission) self.drone_map.get_rectangle_selected_signal().connect(self.handle_rectangle_selected) self.drone_map.get_mission_mode_changed_signal().connect(self.on_mission_mode_changed) self.drone_map.get_route_confirmed_signal().connect(self.handle_route_confirmed) # ================================================================================ main_splitter.addWidget(self.left_tab) main_splitter.addWidget(right_container) main_splitter.setSizes([400, 1000]) self.setCentralWidget(main_splitter) # ================================================================================ # 連線管理 # ================================================================================ def handle_udp_connection_added(self, ip, port): new_conn = {'name': f'UDP {len(self.udp_connections) + 1}', 'ip': ip, 'port': port, 'enabled': True} receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'], self.monitor) receiver.start() self.udp_receivers.append(receiver) new_conn['receiver'] = receiver self.udp_connections.append(new_conn) self.comm_panel.add_udp_panel(new_conn) self.statusBar().showMessage(f"已添加 UDP 連接: {ip}:{port}", 3000) def handle_ws_connection_added(self, url): new_conn = {'name': f'WS {len(self.ws_connections) + 1}', 'url': url, 'enabled': True} receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'], self.monitor) receiver.start() self.monitor.ws_receivers.append(receiver) new_conn['receiver'] = receiver self.ws_connections.append(new_conn) self.comm_panel.add_ws_panel(new_conn) self.statusBar().showMessage(f"已添加 WebSocket 連接: {url}", 3000) def create_drone_panel(self, drone_id): panel = DronePanel(drone_id) panel.mode_change_requested.connect(self.handle_mode_change) panel.arm_requested.connect(self.handle_arm) panel.takeoff_requested.connect(self.handle_takeoff) panel.setpoint_requested.connect(self.handle_single_setpoint) panel.selection_changed.connect(self.handle_drone_selection) return panel def toggle_ws_connection(self, conn, btn, status_label): if conn.get('enabled', False): if 'receiver' in conn and conn['receiver']: conn['receiver'].stop() conn['enabled'] = False btn.setText("啟動") status_label.setStyleSheet("color: #888; font-size: 16px;") status_label.setToolTip("已停止") self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000) else: receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name']) receiver.start() self.monitor.ws_receivers.append(receiver) conn['receiver'] = receiver conn['enabled'] = True btn.setText("停止") status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") status_label.setToolTip("運行中") self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) def remove_ws_connection(self, conn, panel): if 'receiver' in conn and conn['receiver']: conn['receiver'].stop() if conn['receiver'] in self.monitor.ws_receivers: self.monitor.ws_receivers.remove(conn['receiver']) if conn in self.ws_connections: self.ws_connections.remove(conn) self.comm_panel.remove_ws_connection_from_list(conn) panel.setParent(None) panel.deleteLater() self.statusBar().showMessage(f"已移除 WebSocket 連接: {conn['url']}", 3000) def toggle_udp_connection(self, conn, btn, status_label): if conn.get('enabled', False): if 'receiver' in conn and conn['receiver']: conn['receiver'].stop() conn['enabled'] = False btn.setText("啟動") status_label.setStyleSheet("color: #888; font-size: 16px;") status_label.setToolTip("已停止") self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000) else: receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name']) receiver.start() self.udp_receivers.append(receiver) conn['receiver'] = receiver conn['enabled'] = True btn.setText("停止") status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") status_label.setToolTip("運行中") self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) def remove_udp_connection(self, conn, panel): if 'receiver' in conn and conn['receiver']: conn['receiver'].stop() if conn['receiver'] in self.udp_receivers: self.udp_receivers.remove(conn['receiver']) if conn in self.udp_connections: self.udp_connections.remove(conn) self.comm_panel.remove_udp_connection_from_list(conn) panel.setParent(None) panel.deleteLater() self.statusBar().showMessage(f"已移除 UDP 連接: {conn['ip']}:{conn['port']}", 3000) def handle_serial_connection_added(self, port, baudrate): conn = {'name': 'Serial', 'port': port, 'baudrate': baudrate, 'enabled': False, 'receiver': None} self.serial_connections.append(conn) self.comm_panel.add_serial_panel(conn) self.statusBar().showMessage(f"已添加 Serial 連接: {port} @ {baudrate}", 3000) def toggle_serial_connection(self, conn, btn, status_label): if conn.get('enabled', False): if conn.get('receiver'): conn['receiver'].stop() conn['receiver'] = None conn['enabled'] = False btn.setText("啟動") status_label.setStyleSheet("color: #888; font-size: 16px;") status_label.setToolTip("已停止") self.statusBar().showMessage(f"已停止 Serial 連接: {conn['port']}", 3000) else: try: receiver = self.monitor.start_serial_connection(conn['port'], conn['baudrate']) conn['receiver'] = receiver conn['enabled'] = True btn.setText("停止") status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") status_label.setToolTip("運行中") self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000) except Exception as e: self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000) def remove_serial_connection(self, conn, panel): if conn.get('enabled', False) and conn.get('receiver'): conn['receiver'].stop() if conn in self.serial_connections: self.serial_connections.remove(conn) self.comm_panel.remove_serial_connection_from_list(conn) panel.setParent(None) panel.deleteLater() self.statusBar().showMessage(f"已移除 Serial 連接: {conn['port']}", 3000) def create_socket_group_panel(self, socket_id): color = self.socket_colors.get(socket_id, self.socket_colors['default']) socket_type = self.socket_types.get(socket_id, None) panel = SocketGroupPanel(socket_id, color, socket_type) panel.group_selection_changed.connect(self.handle_group_selection) return panel # ================================================================================ # 無人機操作 # ================================================================================ def handle_mode_change(self, drone_id): mode = self.mode_combo.currentText() loop = asyncio.get_event_loop() future = self.monitor.set_mode(drone_id, mode) loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}")) def handle_arm(self, drone_id): loop = asyncio.get_event_loop() arm_state = not self.monitor.get_arm_state(drone_id) future = self.monitor.arm_drone(drone_id, arm_state) loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}")) def handle_takeoff(self, drone_id): loop = asyncio.get_event_loop() future = self.monitor.takeoff_drone(drone_id, 10.0) loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}")) def handle_setpoint_selected(self): try: x = float(self.x_input.text() or '0') y = float(self.y_input.text() or '0') z = float(self.z_input.text() or '0') for drone_id in self.monitor.selected_drones: if self.monitor.send_setpoint(drone_id, x, y, z): self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000) else: self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000) except ValueError: self.statusBar().showMessage("座標格式錯誤", 3000) def handle_single_setpoint(self, drone_id): try: x = float(self.x_input.text() or '0') y = float(self.y_input.text() or '0') z = float(self.z_input.text() or '0') if self.monitor.send_setpoint(drone_id, x, y, z): self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000) else: self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000) except ValueError: self.statusBar().showMessage("座標格式錯誤", 3000) async def handle_service_response(self, future, action): try: result = await future if result: self.statusBar().showMessage(f"{action} 成功", 3000) else: self.statusBar().showMessage(f"{action} 失敗", 3000) except Exception as e: self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) def handle_arm_selected(self): loop = asyncio.get_event_loop() for drone_id in self.monitor.selected_drones: future = self.monitor.arm_drone(drone_id, True) loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}")) def handle_takeoff_selected(self): loop = asyncio.get_event_loop() for drone_id in self.monitor.selected_drones: future = self.monitor.takeoff_drone(drone_id, 10.0) loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) def handle_batch_mode_change(self): mode = self.mode_combo.currentText() loop = asyncio.get_event_loop() for drone_id in self.monitor.selected_drones: future = self.monitor.set_mode(drone_id, mode) loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) # ================================================================================ # UI 更新 # ================================================================================ def update_ui(self, msg_type, drone_id, data): """只做數據快取，不在這裡更新 UI""" if msg_type == 'connection_type': conn_type = data.get('type', 'Unknown') parts = drone_id.split('_') if len(parts) >= 2 and parts[0].startswith('s'): socket_id = parts[0][1:] if socket_id not in self.socket_types: self.socket_types[socket_id] = conn_type if socket_id in self.socket_groups: self.socket_groups[socket_id].set_socket_type(conn_type) return if drone_id not in self.drones: self.add_drone(drone_id) return # 只做資料快取，不更新 UI - 所有 UI 更新都在 _update_panel_and_map 中進行 if drone_id not in self._message_cache: self._message_cache[drone_id] = {} self._message_cache[drone_id][msg_type] = data # ================================================================================ # 勾選管理 # ================================================================================ def handle_group_selection(self, socket_id, state): group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id] is_checked = state == Qt.CheckState.Checked.value for drone_id in group_drones: checkbox = self.drones[drone_id].get_checkbox() if checkbox: checkbox.blockSignals(True) checkbox.setChecked(is_checked) checkbox.blockSignals(False) if is_checked: self.monitor.selected_drones.add(drone_id) else: self.monitor.selected_drones.discard(drone_id) def handle_drone_selection(self, drone_id, state): if state == Qt.CheckState.Checked.value: self.monitor.selected_drones.add(drone_id) else: self.monitor.selected_drones.discard(drone_id) self.update_group_checkbox_state(self.get_socket_id(drone_id)) def update_group_checkbox_state(self, socket_id): group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id] if not group_drones: return checked_count = sum(1 for did in group_drones if self.drones[did].is_checked()) if socket_id in self.socket_groups: group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox") if group_checkbox: group_checkbox.blockSignals(True) if checked_count == 0: group_checkbox.setCheckState(Qt.CheckState.Unchecked) elif checked_count == len(group_drones): group_checkbox.setCheckState(Qt.CheckState.Checked) else: group_checkbox.setCheckState(Qt.CheckState.PartiallyChecked) group_checkbox.blockSignals(False) def handle_select_all(self): all_selected = all(self.drones[did].is_checked() for did in self.drones) new_state = not all_selected for drone_id in self.drones: self.drones[drone_id].set_checked(new_state) for socket_id in self.socket_groups: group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox") if group_checkbox: group_checkbox.blockSignals(True) group_checkbox.setChecked(new_state) group_checkbox.blockSignals(False) def handle_drone_clicked(self, drone_id): if drone_id in self.drones: checkbox = self.drones[drone_id].get_checkbox() checkbox.setChecked(not checkbox.isChecked()) def handle_clear_all_drone_selection(self): for drone_id in self.drones.keys(): checkbox = self.drones[drone_id].get_checkbox() if checkbox: checkbox.blockSignals(True) checkbox.setChecked(False) checkbox.blockSignals(False) self.monitor.selected_drones.clear() for socket_id in self.socket_groups.keys(): self.update_group_checkbox_state(socket_id) def handle_toggle_select_all_drones(self): all_selected = all(self.drones[did].get_checkbox().isChecked() for did in self.drones.keys()) if all_selected: for drone_id in self.drones.keys(): checkbox = self.drones[drone_id].get_checkbox() if checkbox: checkbox.blockSignals(True) checkbox.setChecked(False) checkbox.blockSignals(False) self.monitor.selected_drones.clear() else: for drone_id in self.drones.keys(): checkbox = self.drones[drone_id].get_checkbox() if checkbox: checkbox.blockSignals(True) checkbox.setChecked(True) checkbox.blockSignals(False) self.monitor.selected_drones.add(drone_id) for socket_id in self.socket_groups.keys(): self.update_group_checkbox_state(socket_id) # ================================================================================ # 任務模式切換 # ================================================================================ def on_mission_mode_changed(self, mode): self.current_mission_mode = mode mode_names = { 'M_FORMATION': '列隊飛行', 'CIRCLE_FORMATION': '環狀包圍', 'LEADER_FOLLOWER': '跟隨模式', 'GRID_SWEEP': '柵狀偵查' } display_name = mode_names.get(mode, mode) self.statusBar().showMessage(f"🔄 任務模式: {display_name}", 3000) print(f"任務模式切換: {mode}") # ================================================================================ # 任務規劃 — 點擊地圖 (M-Formation / Circle) # ================================================================================ def handle_map_click(self, lat, lon): """處理地圖點擊事件 — 根據選單模式規劃""" print(f"地圖點擊位置: {lat:.6f}, {lon:.6f} (模式: {self.current_mission_mode})") # Grid Sweep 和 Leader-Follower 由各自的觸發方式處理，點擊地圖不處理 mode_map = { 'M_FORMATION': MissionType.M_FORMATION, 'CIRCLE_FORMATION': MissionType.CIRCLE_FORMATION, } mission_type = mode_map.get(self.current_mission_mode) if mission_type is None: # Grid Sweep / Leader-Follower 模式下點擊地圖不處理 return selected_drones = self.get_selected_drones() if len(selected_drones) == 0: self.statusBar().showMessage("⚠ 請先選擇無人機（勾選 checkbox）", 3000) return base_alt = 10.0 target_gps = (lat, lon, base_alt) self.statusBar().showMessage(f"⏳ 正在規劃 {self.current_mission_mode} ({len(selected_drones)} 台) ...", 2000) try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) if drone_gps_positions is None: return waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( drone_gps_positions, target_gps, mission_type ) self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} self.show_planned_waypoints() center_lat, center_lon, _ = center_origin self.drone_map.draw_mission_plan(center_lat, center_lon, lat, lon) self._launch_verification( self.current_mission_mode, drone_gps_positions, selected_drones, waypoints_per_drone, center_origin, target_gps=target_gps ) total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( f"✓ {self.current_mission_mode} 規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000 ) except Exception as e: self.statusBar().showMessage(f"❌ 規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() # ================================================================================ # 任務規劃 — 矩形選取 (Grid Sweep) # ================================================================================ def handle_rectangle_selected(self, points_json): print(f"矩形選取: {points_json}") selected_drones = self.get_selected_drones() if len(selected_drones) == 0: self.statusBar().showMessage("⚠ 請先選擇無人機再框選區域", 3000) return try: rect_corners = [(p[0], p[1]) for p in json.loads(points_json)] except (json.JSONDecodeError, IndexError): self.statusBar().showMessage("❌ 矩形座標解析失敗", 3000) return base_alt = 10.0 self.statusBar().showMessage(f"⏳ 正在規劃 Grid Sweep ({len(selected_drones)} 台) ...", 2000) try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) if drone_gps_positions is None: return target_lat = sum(c[0] for c in rect_corners) / 4 target_lon = sum(c[1] for c in rect_corners) / 4 target_gps = (target_lat, target_lon, base_alt) waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( drone_gps_positions, target_gps, MissionType.GRID_SWEEP, params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': base_alt} ) self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} self.show_planned_waypoints() center_lat, center_lon, _ = center_origin self.drone_map.draw_mission_plan(center_lat, center_lon, target_lat, target_lon) self._launch_verification( 'grid_sweep', drone_gps_positions, selected_drones, waypoints_per_drone, center_origin, rect_corners=rect_corners ) total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( f"✓ Grid Sweep 規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000 ) except Exception as e: self.statusBar().showMessage(f"❌ Grid Sweep 規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() # ================================================================================ # 任務規劃 — 路徑確認 (Leader-Follower 跟隨模式) # ================================================================================ def handle_route_confirmed(self, points_json): """路徑確認 → Leader-Follower 任務規劃""" print(f"路徑確認: {points_json}") selected_drones = self.get_selected_drones() if len(selected_drones) == 0: self.statusBar().showMessage("⚠ 請先選擇無人機再標記路徑", 3000) return try: route_points = json.loads(points_json) # [[lat, lon], ...] route_waypoints = [(p[0], p[1]) for p in route_points] except (json.JSONDecodeError, IndexError): self.statusBar().showMessage("❌ 路徑座標解析失敗", 3000) return if len(route_waypoints) < 2: self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000) return base_alt = 10.0 self.statusBar().showMessage(f"⏳ 正在規劃跟隨模式 ({len(selected_drones)} 台, {len(route_waypoints)} 個路徑點) ...", 2000) try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) if drone_gps_positions is None: return # 目標點 = 路徑中心（供 origin 計算） target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints) target_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints) target_gps = (target_lat, target_lon, base_alt) waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER, params={ 'route_waypoints': route_waypoints, 'lateral_offset': 3.0, 'longitudinal_spacing': 5.0, 'altitude': base_alt } ) self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} self.show_planned_waypoints() center_lat, center_lon, _ = center_origin self.drone_map.draw_mission_plan(center_lat, center_lon, target_lat, target_lon) # 啟動視覺化驗證 self._launch_verification( 'LEADER_FOLLOWER', drone_gps_positions, selected_drones, waypoints_per_drone, center_origin, target_gps=target_gps, route_waypoints=route_waypoints ) total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( f"✓ 跟隨模式規劃完成！{len(selected_drones)} 台，{len(route_waypoints)} 個路徑點，共 {total_wps} 個航點", 5000 ) except Exception as e: self.statusBar().showMessage(f"❌ 跟隨模式規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() # ================================================================================ # 任務執行控制 # ================================================================================ def handle_start_mission(self, center_lat, center_lon, target_lat, target_lon): if self.planned_waypoints is None: self.statusBar().showMessage("⚠ 請先規劃任務", 3000) return self.mission_executor.start(self.planned_waypoints) self.statusBar().showMessage("🚀 任務已啟動", 3000) def handle_pause_mission(self): if self.mission_executor.state.value == "running": self.mission_executor.pause() self.statusBar().showMessage("⏸ 任務已暫停", 3000) elif self.mission_executor.state.value == "paused": self.mission_executor.resume() self.statusBar().showMessage("▶ 任務已恢復", 3000) def on_drone_waypoint_reached(self, drone_id, wp_index, total): if wp_index >= total: self.statusBar().showMessage(f"📍 {drone_id} 完成所有航點", 3000) else: self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000) def on_mission_completed(self): self.statusBar().showMessage("✅ 所有無人機已完成任務", 5000) # ================================================================================ # 輔助方法 # ================================================================================ def _collect_drone_gps(self, selected_drones, base_alt): drone_gps_positions = [] for drone_id in selected_drones: if hasattr(self.monitor, 'drone_gps') and drone_id in self.monitor.drone_gps: gps_data = self.monitor.drone_gps[drone_id] drone_gps_positions.append((gps_data['lat'], gps_data['lon'], gps_data.get('alt', base_alt))) elif drone_id in self.drone_positions: pos = self.drone_positions[drone_id] lat_drone = 24.0 + pos[1] / 111000 lon_drone = 120.0 + pos[0] / (111000 * 0.9) alt_drone = pos[2] if len(pos) > 2 else base_alt drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) else: self.statusBar().showMessage(f"⚠ 找不到 {drone_id} 的位置資料", 3000) return None return drone_gps_positions def _launch_verification(self, mission_type, drone_gps_positions, selected_drones, waypoints_per_drone, origin, target_gps=None, rect_corners=None, route_waypoints=None): """存 JSON + 啟動 matplotlib 視覺化驗證 (獨立 process)""" import os data = { 'mission_type': mission_type, 'drone_ids': selected_drones, 'drones_gps': drone_gps_positions, 'waypoints': waypoints_per_drone, 'origin': list(origin), } if target_gps: data['target'] = list(target_gps) if rect_corners: data['rect_corners'] = rect_corners if route_waypoints: data['route_waypoints'] = route_waypoints json_path = '/tmp/mission_plan.json' with open(json_path, 'w') as f: json.dump(data, f, indent=2) script_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'validation', 'verify_waypoints.py') subprocess.Popen([sys.executable, script_path, '--file', json_path]) print(f"驗證視窗已啟動: {json_path}") def show_planned_waypoints(self): if not self.planned_waypoints: return print("\n" + "=" * 60) print("任務規劃結果") print("=" * 60) drone_ids = self.planned_waypoints['drone_ids'] waypoints = self.planned_waypoints['waypoints'] print(f"\n共 {len(drone_ids)} 台無人機") for i, drone_id in enumerate(drone_ids): wps = waypoints[i] print(f"\n【{drone_id}】({len(wps)} 個航點)") for j, wp in enumerate(wps): print(f" WP{j}: ({wp[0]:.6f}°, {wp[1]:.6f}°, {wp[2]:.1f}m)") print("\n" + "=" * 60) def get_selected_drones(self): return [did for did, panel in self.drones.items() if hasattr(panel, 'checkbox') and panel.checkbox.isChecked()] def update_field(self, panel, drone_id, field, text, color=None): if isinstance(panel, DronePanel): panel.update_field(field, text, color) def update_overview_table(self, drone_id=None, field=None, value=None): if not hasattr(self, 'overview_table') or self.overview_table is None: return self.overview_table.set_drones(self.drones) self.overview_table.update_table(drone_id, field, value) def get_socket_id(self, drone_id): import re match = re.match(r's(\d+)_(\d+)', drone_id) return match.group(1) if match else 'unknown' def add_drone(self, drone_id): if drone_id in self.drones: return socket_id = self.get_socket_id(drone_id) panel = self.create_drone_panel(drone_id) self.drones[drone_id] = panel if socket_id not in self.socket_groups: self.socket_groups[socket_id] = self.create_socket_group_panel(socket_id) self.socket_groups[socket_id].drones_layout.addWidget(panel) self.reorganize_socket_groups() self.update_group_checkbox_state(socket_id) self.update_overview_table() def reorganize_socket_groups(self): while self.drone_panel_layout.count(): w = self.drone_panel_layout.takeAt(0).widget() if w: w.setParent(None) for socket_id, group_panel in self.socket_groups.items(): group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id] while group_panel.drones_layout.count(): w = group_panel.drones_layout.takeAt(0).widget() if w: w.setParent(None) def sort_key(x): parts = x[1:].split('_') return (int(parts[0]), int(parts[1])) for did in sorted(group_drones, key=sort_key): group_panel.drones_layout.addWidget(self.drones[did]) for socket_id in sorted(self.socket_groups.keys(), key=lambda x: int(x)): self.drone_panel_layout.addWidget(self.socket_groups[socket_id]) def _update_panel_and_map(self): """30Hz 定時更新 panel 和 map，批量更新 UI 以避免過度重繪""" if not hasattr(self, '_message_cache') or not self._message_cache: return # 頻率監控 if not hasattr(self, '_map_update_time'): self._map_update_time = time.time() self._map_update_count = 0 self._map_update_count += 1 now = time.time() if now - self._map_update_time >= 1.0: print(f"[Panel/Map Update] {self._map_update_count} Hz") self._map_update_time = now self._map_update_count = 0 # ✅ 步驟 1: 暫停表格的即時重繪 if hasattr(self, 'overview_table') and self.overview_table: self.overview_table.setUpdatesEnabled(False) try: start_time = time.time() # ✅ 步驟 2: 遍歷快取中最新的資料來更新 UI for drone_id in list(self._message_cache.keys()): if drone_id not in self.drones: continue panel = self.drones[drone_id] cached_data = self._message_cache[drone_id] # 處理所有快取的消息類型 for msg_type, data in cached_data.items(): if msg_type == 'state': mode = data.get('mode', 'UNKNOWN') armed = data.get('armed', None) mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' if armed is True: arm_text, arm_color = "ARMED", '#55FF55' elif armed is False: arm_text, arm_color = "DISARMED", '#FF5555' else: arm_text, arm_color = "--", '#AAAAAA' self.update_field(panel, drone_id, 'mode', mode, mode_color) self.update_field(panel, drone_id, 'armed', arm_text, arm_color) self.update_overview_table(drone_id, 'mode', mode) self.update_overview_table(drone_id, 'armed', arm_text) elif msg_type == 'battery': voltage = data.get('voltage', 16) cells = round(voltage / 3.95) percentage = (voltage / cells - 3.7) / 0.5 * 100 if cells > 0 else 0 if percentage < 20: voltage_color = '#FF6464' elif percentage < 50: voltage_color = '#FFA500' else: voltage_color = '#FFFFFF' percentage = data.get('percentage', percentage) self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color) self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V") self.update_field(panel, drone_id, 'battery_cells', f"{cells}S") self.update_overview_table(drone_id, 'battery', f"{voltage:.2f}V") elif msg_type == 'altitude': altitude = data.get('altitude', 0) text = f"{altitude:.1f} m" self.update_field(panel, drone_id, 'altitude', text) self.update_overview_table(drone_id, 'altitude', text) elif msg_type == 'local_pose': x, y = data.get('x', 0), data.get('y', 0) if not hasattr(self.monitor, 'drone_local'): self.monitor.drone_local = {} self.monitor.drone_local[drone_id] = {'x': x, 'y': y} self.update_overview_table(drone_id, 'local', f"{x:.1f}, {y:.1f}") elif msg_type == 'loss_rate': text = f"{data.get('loss_rate', 0):.1f}%" self.update_field(panel, drone_id, 'loss_rate', text) self.update_overview_table(drone_id, 'loss_rate', text) elif msg_type == 'ping': text = f"{data.get('ping', 0):.1f} ms" self.update_field(panel, drone_id, 'ping', text) self.update_overview_table(drone_id, 'ping', text) elif msg_type == 'velocity': self.update_overview_table(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}") elif msg_type == 'attitude': roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0) self.update_overview_table(drone_id, 'roll', f"{roll:.1f}°") self.update_overview_table(drone_id, 'pitch', f"{pitch:.1f}°") self.update_overview_table(drone_id, 'yaw', f"{yaw:.1f}°") panel._last_roll = roll panel._last_pitch = pitch if hasattr(panel, 'update_attitude'): heading = self.drone_headings.get(drone_id, 0) panel.update_attitude(heading, roll, pitch) elif msg_type == 'gps': gps_data = data lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0) self.drone_positions[drone_id] = (lat, lon) alt = gps_data.get('alt', 0) if not hasattr(self.monitor, 'drone_gps'): self.monitor.drone_gps = {} self.monitor.drone_gps[drone_id] = {'lat': lat, 'lon': lon, 'alt': alt} self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°") self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°") elif msg_type == 'hud': hud_data = data heading = hud_data.get('heading', 0) self.drone_headings[drone_id] = heading groundspeed = hud_data.get('groundspeed', 0) airspeed = hud_data.get('airspeed', 0) throttle = hud_data.get('throttle', 0) hud_alt = hud_data.get('alt', 0) climb = hud_data.get('climb', 0) self.update_overview_table(drone_id, 'heading', f"{heading:.1f}°") self.update_overview_table(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") self.update_overview_table(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--") self.update_overview_table(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--") self.update_overview_table(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--") self.update_overview_table(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--") self.update_field(panel, drone_id, 'heading', f"{heading:.1f}°") self.update_field(panel, drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") self.update_field(panel, drone_id, 'speed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") if drone_id in self.drone_positions: lat, lon = self.drone_positions[drone_id] self.drone_map.update_drone_position(drone_id, lat, lon, heading) elapsed = (time.time() - start_time) * 1000 if elapsed > 33: print(f"[WARNING] UI update took {elapsed:.1f}ms (target: <33ms)") finally: # ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪） if hasattr(self, 'overview_table') and self.overview_table: self.overview_table.setUpdatesEnabled(True) self.overview_table.viewport().update() def spin_ros(self): try: self.executor.spin_once(timeout_sec=0.01) for (drone_id, msg_type), data in self.monitor.latest_data.items(): self.monitor.signals.update_signal.emit(msg_type, drone_id, data) self.monitor.latest_data.clear() except Exception as e: print(f"ROS spin error: {e}") def closeEvent(self, event): self.mission_executor.stop() self.command_sender.close() for receiver in self.udp_receivers: receiver.stop() for receiver in self.monitor.ws_receivers: receiver.stop() self.monitor.destroy_node() self.executor.shutdown() rclpy.shutdown() event.accept() if __name__ == '__main__': app = QApplication(sys.argv) station = ControlStationUI() station.show() app.exec()