|
|
|
|
|
#!/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()
|
