|
|
|
@ -1,10 +1,12 @@
|
|
|
|
#!/usr/bin/env python3
|
|
|
|
#!/usr/bin/env python3
|
|
|
|
import rclpy
|
|
|
|
import rclpy
|
|
|
|
from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout,
|
|
|
|
from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout,
|
|
|
|
QWidget, QLabel, QSplitter, QScrollArea,
|
|
|
|
QWidget, QLabel, QSplitter, QScrollArea,
|
|
|
|
QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem,
|
|
|
|
QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem,
|
|
|
|
QHeaderView, QPushButton, QCheckBox, QLineEdit)
|
|
|
|
QHeaderView, QPushButton, QCheckBox, QLineEdit,
|
|
|
|
|
|
|
|
QComboBox, QDialog)
|
|
|
|
from PyQt6.QtCore import Qt, QTimer
|
|
|
|
from PyQt6.QtCore import Qt, QTimer
|
|
|
|
|
|
|
|
from PyQt6.QtGui import QColor
|
|
|
|
import sys
|
|
|
|
import sys
|
|
|
|
import asyncio
|
|
|
|
import asyncio
|
|
|
|
import json
|
|
|
|
import json
|
|
|
|
@ -19,11 +21,14 @@ from comm_panel import CommPanel
|
|
|
|
from overview_table import OverviewTable
|
|
|
|
from overview_table import OverviewTable
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
# 導入任務規劃器、執行器、發送器
|
|
|
|
# 導入任務規劃器、執行器、發送器、群組
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
from mission_planner import FormationPlanner, MissionType
|
|
|
|
from mission_planner import FormationPlanner, MissionType
|
|
|
|
from command_sender import MavlinkSender
|
|
|
|
from command_sender import MavlinkSender
|
|
|
|
from mission_executor import MissionExecutor
|
|
|
|
from mission_executor import MissionExecutor, MissionState
|
|
|
|
|
|
|
|
from mission_group import (
|
|
|
|
|
|
|
|
MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS
|
|
|
|
|
|
|
|
)
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
class ControlStationUI(QMainWindow):
|
|
|
|
class ControlStationUI(QMainWindow):
|
|
|
|
@ -94,28 +99,22 @@ class ControlStationUI(QMainWindow):
|
|
|
|
base_altitude=10.0, # 基準高度 10 公尺
|
|
|
|
base_altitude=10.0, # 基準高度 10 公尺
|
|
|
|
altitude_diff=2.0 # 高低差 2 公尺
|
|
|
|
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.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死
|
|
|
|
self.mission_groups = {} # group_id → MissionGroup
|
|
|
|
|
|
|
|
self.group_panels = {} # group_id → GroupPanel
|
|
|
|
self.mission_executor = MissionExecutor(
|
|
|
|
self.active_group_id = None # 當前 active 的 group
|
|
|
|
sender=self.command_sender,
|
|
|
|
self._group_counter = 0 # 用來產生 group_id
|
|
|
|
drone_gps=self.monitor.drone_gps, # 直接傳引用,即時讀取
|
|
|
|
self._pending_box_assign = None # 框選後直接分配到的 group_id
|
|
|
|
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()
|
|
|
|
self.init_ui()
|
|
|
|
@ -163,97 +162,51 @@ class ControlStationUI(QMainWindow):
|
|
|
|
right_layout.setContentsMargins(10, 10, 10, 10)
|
|
|
|
right_layout.setContentsMargins(10, 10, 10, 10)
|
|
|
|
right_layout.setSpacing(10)
|
|
|
|
right_layout.setSpacing(10)
|
|
|
|
|
|
|
|
|
|
|
|
# ========== 批次控制區域 ==========
|
|
|
|
# ========== 任務群組 Tab ==========
|
|
|
|
batch_control_layout = QHBoxLayout()
|
|
|
|
group_header = QHBoxLayout()
|
|
|
|
|
|
|
|
group_title = QLabel("任務群組")
|
|
|
|
batch_title = QLabel("批次操作")
|
|
|
|
group_title.setStyleSheet(
|
|
|
|
batch_title.setStyleSheet("""
|
|
|
|
"color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
|
|
|
|
color: #DDD; font-size: 16px; font-weight: bold;
|
|
|
|
group_header.addWidget(group_title)
|
|
|
|
padding: 5px; background-color: #333; border-radius: 4px;
|
|
|
|
group_header.addStretch()
|
|
|
|
""")
|
|
|
|
|
|
|
|
batch_control_layout.addWidget(batch_title)
|
|
|
|
add_group_btn = QPushButton("+ 新增群組")
|
|
|
|
|
|
|
|
add_group_btn.setStyleSheet("""
|
|
|
|
first_row = QHBoxLayout()
|
|
|
|
QPushButton { background-color: #4A9EFF; color: white; border: none;
|
|
|
|
select_all_btn = QPushButton("全選")
|
|
|
|
padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; }
|
|
|
|
select_all_btn.clicked.connect(self.handle_select_all)
|
|
|
|
QPushButton:hover { background-color: #3A8EEF; }
|
|
|
|
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)
|
|
|
|
add_group_btn.clicked.connect(self._add_mission_group)
|
|
|
|
first_row.addStretch()
|
|
|
|
group_header.addWidget(add_group_btn)
|
|
|
|
|
|
|
|
|
|
|
|
mode_layout = QHBoxLayout()
|
|
|
|
clear_traj_btn = QPushButton("清除軌跡")
|
|
|
|
mode_label = QLabel("模式:")
|
|
|
|
clear_traj_btn.setStyleSheet("""
|
|
|
|
mode_label.setStyleSheet("color: #DDD; min-width: 40px;")
|
|
|
|
QPushButton { background-color: #EF5350; color: white; border: none;
|
|
|
|
|
|
|
|
padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; }
|
|
|
|
from PyQt6.QtWidgets import QComboBox
|
|
|
|
QPushButton:hover { background-color: #E53935; }
|
|
|
|
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)
|
|
|
|
clear_traj_btn.clicked.connect(self.drone_map.clear_trajectories)
|
|
|
|
mode_layout.addWidget(self.mode_combo)
|
|
|
|
group_header.addWidget(clear_traj_btn)
|
|
|
|
mode_layout.addWidget(batch_mode_btn)
|
|
|
|
|
|
|
|
mode_layout.addStretch()
|
|
|
|
right_layout.addLayout(group_header)
|
|
|
|
|
|
|
|
|
|
|
|
third_row = QHBoxLayout()
|
|
|
|
self.group_tab_widget = QTabWidget()
|
|
|
|
arm_all_btn = QPushButton("解鎖")
|
|
|
|
self.group_tab_widget.setStyleSheet("""
|
|
|
|
arm_all_btn.clicked.connect(self.handle_arm_selected)
|
|
|
|
QTabWidget::pane { border: 1px solid #444; background-color: #2B2B2B; }
|
|
|
|
arm_all_btn.setStyleSheet("""
|
|
|
|
QTabBar::tab {
|
|
|
|
QPushButton { background-color: #444; color: #DDD; border: none;
|
|
|
|
background-color: #333; color: #AAA; border: 1px solid #444;
|
|
|
|
padding: 8px 12px; border-radius: 4px; min-width: 80px; }
|
|
|
|
padding: 5px 12px; margin-right: 2px; border-top-left-radius: 4px;
|
|
|
|
QPushButton:hover { background-color: #555; }
|
|
|
|
border-top-right-radius: 4px; font-size: 12px;
|
|
|
|
""")
|
|
|
|
}
|
|
|
|
third_row.addWidget(arm_all_btn)
|
|
|
|
QTabBar::tab:selected { background-color: #2B2B2B; color: #FFF; border-bottom-color: #2B2B2B; }
|
|
|
|
third_row.addStretch()
|
|
|
|
QTabBar::tab:hover { background-color: #3A3A3A; }
|
|
|
|
|
|
|
|
|
|
|
|
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; }
|
|
|
|
|
|
|
|
""")
|
|
|
|
""")
|
|
|
|
|
|
|
|
self.group_tab_widget.setFixedHeight(150)
|
|
|
|
fourth_row.addWidget(QLabel("高度:", styleSheet="color: #DDD;"))
|
|
|
|
self.group_tab_widget.currentChanged.connect(self._on_group_tab_changed)
|
|
|
|
fourth_row.addWidget(self.z_input)
|
|
|
|
right_layout.addWidget(self.group_tab_widget)
|
|
|
|
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)
|
|
|
|
# 預設建立 Group A
|
|
|
|
|
|
|
|
self._add_mission_group()
|
|
|
|
|
|
|
|
|
|
|
|
# 添加地圖
|
|
|
|
# 添加地圖
|
|
|
|
right_layout.addWidget(self.drone_map.get_widget())
|
|
|
|
right_layout.addWidget(self.drone_map.get_widget())
|
|
|
|
@ -263,13 +216,11 @@ class ControlStationUI(QMainWindow):
|
|
|
|
self.drone_map.get_toggle_select_all_drones_signal().connect(self.handle_toggle_select_all_drones)
|
|
|
|
self.drone_map.get_toggle_select_all_drones_signal().connect(self.handle_toggle_select_all_drones)
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
# 連接任務控制 + 矩形選取 + 任務模式切換 + 路徑確認信號
|
|
|
|
# 連接地圖信號(任務模式改由 Group Tab 控制,不再從地圖下拉選單)
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
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_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)
|
|
|
|
self.drone_map.get_route_confirmed_signal().connect(self.handle_route_confirmed)
|
|
|
|
|
|
|
|
self.drone_map.get_drone_box_selected_signal().connect(self._handle_drone_box_selected)
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
main_splitter.addWidget(self.left_tab)
|
|
|
|
main_splitter.addWidget(self.left_tab)
|
|
|
|
@ -426,7 +377,13 @@ class ControlStationUI(QMainWindow):
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
def handle_mode_change(self, drone_id):
|
|
|
|
def handle_mode_change(self, drone_id):
|
|
|
|
mode = self.mode_combo.currentText()
|
|
|
|
# 從 active group 的 mode_combo 讀取模式
|
|
|
|
|
|
|
|
group = self._get_active_group()
|
|
|
|
|
|
|
|
if group:
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group.group_id)
|
|
|
|
|
|
|
|
mode = panel.mode_combo.currentText() if panel else "GUIDED"
|
|
|
|
|
|
|
|
else:
|
|
|
|
|
|
|
|
mode = "GUIDED"
|
|
|
|
loop = asyncio.get_event_loop()
|
|
|
|
loop = asyncio.get_event_loop()
|
|
|
|
future = self.monitor.set_mode(drone_id, mode)
|
|
|
|
future = self.monitor.set_mode(drone_id, mode)
|
|
|
|
loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}"))
|
|
|
|
loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}"))
|
|
|
|
@ -488,13 +445,266 @@ class ControlStationUI(QMainWindow):
|
|
|
|
for drone_id in self.monitor.selected_drones:
|
|
|
|
for drone_id in self.monitor.selected_drones:
|
|
|
|
future = self.monitor.takeoff_drone(drone_id, 10.0)
|
|
|
|
future = self.monitor.takeoff_drone(drone_id, 10.0)
|
|
|
|
loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
|
|
|
|
loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
|
|
|
|
|
|
|
|
|
|
|
|
def handle_batch_mode_change(self):
|
|
|
|
# ================================================================================
|
|
|
|
mode = self.mode_combo.currentText()
|
|
|
|
# 任務群組管理
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _next_group_id(self):
|
|
|
|
|
|
|
|
"""產生下一個群組 ID (A, B, C, ...)"""
|
|
|
|
|
|
|
|
gid = chr(ord('A') + self._group_counter)
|
|
|
|
|
|
|
|
self._group_counter += 1
|
|
|
|
|
|
|
|
return gid
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _add_mission_group(self):
|
|
|
|
|
|
|
|
"""新增一個任務群組"""
|
|
|
|
|
|
|
|
gid = self._next_group_id()
|
|
|
|
|
|
|
|
color = GROUP_COLORS[(self._group_counter - 1) % len(GROUP_COLORS)]
|
|
|
|
|
|
|
|
group = MissionGroup(gid, color)
|
|
|
|
|
|
|
|
self.mission_groups[gid] = group
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
panel = GroupPanel(group)
|
|
|
|
|
|
|
|
panel.assign_drones_requested.connect(self._handle_assign_drones)
|
|
|
|
|
|
|
|
panel.mission_type_changed.connect(self._handle_mission_type_changed)
|
|
|
|
|
|
|
|
panel.start_requested.connect(self._handle_group_start)
|
|
|
|
|
|
|
|
panel.pause_requested.connect(self._handle_group_pause)
|
|
|
|
|
|
|
|
panel.stop_requested.connect(self._handle_group_stop)
|
|
|
|
|
|
|
|
panel.mode_change_requested.connect(self._handle_group_mode_change)
|
|
|
|
|
|
|
|
panel.arm_requested.connect(self._handle_group_arm)
|
|
|
|
|
|
|
|
panel.takeoff_requested.connect(self._handle_group_takeoff)
|
|
|
|
|
|
|
|
panel.box_select_requested.connect(self._handle_box_select)
|
|
|
|
|
|
|
|
panel.select_all_requested.connect(self._handle_select_all_for_group)
|
|
|
|
|
|
|
|
panel.clear_group_requested.connect(self._handle_clear_group)
|
|
|
|
|
|
|
|
self.group_panels[gid] = panel
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 用帶顏色的 tab 標題
|
|
|
|
|
|
|
|
scroll = QScrollArea()
|
|
|
|
|
|
|
|
scroll.setWidget(panel)
|
|
|
|
|
|
|
|
scroll.setWidgetResizable(True)
|
|
|
|
|
|
|
|
idx = self.group_tab_widget.addTab(scroll, f"Group {gid}")
|
|
|
|
|
|
|
|
self.group_tab_widget.tabBar().setTabTextColor(idx, QColor(color))
|
|
|
|
|
|
|
|
self.group_tab_widget.setCurrentIndex(idx)
|
|
|
|
|
|
|
|
self.active_group_id = gid
|
|
|
|
|
|
|
|
self.statusBar().showMessage(f"已新增 Group {gid}", 2000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _on_group_tab_changed(self, index):
|
|
|
|
|
|
|
|
"""Tab 切換時更新 active group 並同步地圖模式"""
|
|
|
|
|
|
|
|
if index < 0:
|
|
|
|
|
|
|
|
self.active_group_id = None
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
# tab 標題是 "Group X"
|
|
|
|
|
|
|
|
tab_text = self.group_tab_widget.tabText(index)
|
|
|
|
|
|
|
|
gid = tab_text.replace("Group ", "")
|
|
|
|
|
|
|
|
if gid in self.mission_groups:
|
|
|
|
|
|
|
|
self.active_group_id = gid
|
|
|
|
|
|
|
|
group = self.mission_groups[gid]
|
|
|
|
|
|
|
|
self.drone_map.set_mission_mode(group.mission_type)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _get_active_group(self):
|
|
|
|
|
|
|
|
"""取得當前 active 的 MissionGroup"""
|
|
|
|
|
|
|
|
if self.active_group_id and self.active_group_id in self.mission_groups:
|
|
|
|
|
|
|
|
return self.mission_groups[self.active_group_id]
|
|
|
|
|
|
|
|
return None
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _get_other_assigned(self, exclude_gid):
|
|
|
|
|
|
|
|
"""取得其他群組已佔用的無人機 {drone_id: group_id}"""
|
|
|
|
|
|
|
|
assigned = {}
|
|
|
|
|
|
|
|
for gid, group in self.mission_groups.items():
|
|
|
|
|
|
|
|
if gid == exclude_gid:
|
|
|
|
|
|
|
|
continue
|
|
|
|
|
|
|
|
for did in group.drone_ids:
|
|
|
|
|
|
|
|
assigned[did] = gid
|
|
|
|
|
|
|
|
return assigned
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_assign_drones(self, group_id):
|
|
|
|
|
|
|
|
"""開啟無人機分配對話框(已勾選的 checkbox 會預先帶入)"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
all_ids = list(self.drones.keys())
|
|
|
|
|
|
|
|
other_assigned = self._get_other_assigned(group_id)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 將目前 checkbox 已勾選的無人機(且未被其他群組佔用)合併進 pre-selected
|
|
|
|
|
|
|
|
currently_checked = self.get_selected_drones()
|
|
|
|
|
|
|
|
pre_selected = set(group.drone_ids)
|
|
|
|
|
|
|
|
for did in currently_checked:
|
|
|
|
|
|
|
|
if did not in other_assigned:
|
|
|
|
|
|
|
|
pre_selected.add(did)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
dialog = DroneAssignDialog(self, all_ids, pre_selected, other_assigned)
|
|
|
|
|
|
|
|
if dialog.exec() == QDialog.DialogCode.Accepted:
|
|
|
|
|
|
|
|
group.drone_ids = dialog.get_selected()
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_drone_list()
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
|
|
|
|
f"Group {group_id}: 已分配 {len(group.drone_ids)} 台無人機", 3000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_mission_type_changed(self, group_id, mission_type):
|
|
|
|
|
|
|
|
"""群組任務類型切換"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if group:
|
|
|
|
|
|
|
|
group.mission_type = mission_type
|
|
|
|
|
|
|
|
# 如果是 active group,同步更新地圖的選擇模式
|
|
|
|
|
|
|
|
if group_id == self.active_group_id:
|
|
|
|
|
|
|
|
self.drone_map.set_mission_mode(mission_type)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _create_executor_for_group(self, group):
|
|
|
|
|
|
|
|
"""為群組建立 MissionExecutor"""
|
|
|
|
|
|
|
|
executor = MissionExecutor(
|
|
|
|
|
|
|
|
sender=self.command_sender,
|
|
|
|
|
|
|
|
drone_gps=self.monitor.drone_gps,
|
|
|
|
|
|
|
|
arrival_radius=2.0,
|
|
|
|
|
|
|
|
send_rate_hz=2.0
|
|
|
|
|
|
|
|
)
|
|
|
|
|
|
|
|
executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached)
|
|
|
|
|
|
|
|
executor.mission_completed.connect(
|
|
|
|
|
|
|
|
lambda gid=group.group_id: self._on_group_mission_completed(gid))
|
|
|
|
|
|
|
|
group.executor = executor
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_group_start(self, group_id):
|
|
|
|
|
|
|
|
"""啟動群組任務"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
if group.planned_waypoints is None:
|
|
|
|
|
|
|
|
self.statusBar().showMessage(f"⚠ Group {group_id}: 請先規劃任務", 3000)
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
if group.executor is None:
|
|
|
|
|
|
|
|
self._create_executor_for_group(group)
|
|
|
|
|
|
|
|
group.executor.start(group.planned_waypoints)
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(f"🚀 Group {group_id}: 任務已啟動", 3000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_group_pause(self, group_id):
|
|
|
|
|
|
|
|
"""暫停/恢復群組任務"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group or not group.executor:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
if group.executor.state == MissionState.RUNNING:
|
|
|
|
|
|
|
|
group.executor.pause()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(f"⏸ Group {group_id}: 任務已暫停", 3000)
|
|
|
|
|
|
|
|
elif group.executor.state == MissionState.PAUSED:
|
|
|
|
|
|
|
|
group.executor.resume()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(f"▶ Group {group_id}: 任務已恢復", 3000)
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_group_stop(self, group_id):
|
|
|
|
|
|
|
|
"""停止群組任務"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
if group.executor:
|
|
|
|
|
|
|
|
group.executor.stop()
|
|
|
|
|
|
|
|
group.planned_waypoints = None
|
|
|
|
|
|
|
|
self.drone_map.clear_mission_plan_for_group(group_id)
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
panel.clear_mission_info()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(f"■ Group {group_id}: 任務已停止", 3000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_group_mode_change(self, group_id, mode):
|
|
|
|
|
|
|
|
"""切換群組內所有無人機的飛行模式"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
loop = asyncio.get_event_loop()
|
|
|
|
loop = asyncio.get_event_loop()
|
|
|
|
for drone_id in self.monitor.selected_drones:
|
|
|
|
for drone_id in group.drone_ids:
|
|
|
|
future = self.monitor.set_mode(drone_id, mode)
|
|
|
|
future = self.monitor.set_mode(drone_id, mode)
|
|
|
|
loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}"))
|
|
|
|
loop.create_task(self.handle_service_response(future, f"{drone_id} 切換 {mode}"))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_group_arm(self, group_id):
|
|
|
|
|
|
|
|
"""解鎖群組內所有無人機"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
loop = asyncio.get_event_loop()
|
|
|
|
|
|
|
|
for drone_id in group.drone_ids:
|
|
|
|
|
|
|
|
future = self.monitor.arm_drone(drone_id, True)
|
|
|
|
|
|
|
|
loop.create_task(self.handle_service_response(future, f"解鎖 {drone_id}"))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_group_takeoff(self, group_id, altitude):
|
|
|
|
|
|
|
|
"""群組內所有無人機起飛"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
loop = asyncio.get_event_loop()
|
|
|
|
|
|
|
|
for drone_id in group.drone_ids:
|
|
|
|
|
|
|
|
future = self.monitor.takeoff_drone(drone_id, altitude)
|
|
|
|
|
|
|
|
loop.create_task(self.handle_service_response(future, f"起飛 {drone_id} ({altitude}m)"))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_box_select(self, group_id):
|
|
|
|
|
|
|
|
"""觸發地圖框選 → 框選完成後直接分配到該群組"""
|
|
|
|
|
|
|
|
self._pending_box_assign = group_id
|
|
|
|
|
|
|
|
self.drone_map.toggle_drone_box_select()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
|
|
|
|
f"請在地圖上框選要分配到 Group {group_id} 的無人機", 5000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_drone_box_selected(self, drone_ids_json):
|
|
|
|
|
|
|
|
"""地圖框選完成 — 直接分配到指定群組"""
|
|
|
|
|
|
|
|
group_id = self._pending_box_assign
|
|
|
|
|
|
|
|
self._pending_box_assign = None
|
|
|
|
|
|
|
|
if not group_id:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
drone_ids = json.loads(drone_ids_json)
|
|
|
|
|
|
|
|
other = self._get_other_assigned(group_id)
|
|
|
|
|
|
|
|
valid = {did for did in drone_ids if did not in other}
|
|
|
|
|
|
|
|
group.drone_ids = valid
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_drone_list()
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
|
|
|
|
f"Group {group_id}: 框選分配 {len(valid)} 台無人機", 3000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_select_all_for_group(self, group_id):
|
|
|
|
|
|
|
|
"""全選所有可用無人機,直接分配到該群組"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
other = self._get_other_assigned(group_id)
|
|
|
|
|
|
|
|
available = {did for did in self.drones.keys() if did not in other}
|
|
|
|
|
|
|
|
group.drone_ids = available
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_drone_list()
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
|
|
|
|
f"Group {group_id}: 全選分配 {len(available)} 台無人機", 3000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _handle_clear_group(self, group_id):
|
|
|
|
|
|
|
|
"""清除群組的無人機分配"""
|
|
|
|
|
|
|
|
group = self.mission_groups.get(group_id)
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
group.drone_ids = set()
|
|
|
|
|
|
|
|
group.planned_waypoints = None
|
|
|
|
|
|
|
|
if group.executor:
|
|
|
|
|
|
|
|
group.executor.stop()
|
|
|
|
|
|
|
|
self.drone_map.clear_mission_plan_for_group(group_id)
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_drone_list()
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
panel.clear_mission_info()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
|
|
|
|
f"Group {group_id}: 已清除分組", 3000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _on_group_mission_completed(self, group_id):
|
|
|
|
|
|
|
|
"""群組任務完成回呼"""
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
self.statusBar().showMessage(f"✅ Group {group_id}: 所有無人機已完成任務", 5000)
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
# UI 更新
|
|
|
|
# UI 更新
|
|
|
|
@ -610,73 +820,76 @@ class ControlStationUI(QMainWindow):
|
|
|
|
self.update_group_checkbox_state(socket_id)
|
|
|
|
self.update_group_checkbox_state(socket_id)
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
# 任務模式切換
|
|
|
|
# 任務規劃 — 點擊地圖(路由到 active group)
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
def on_mission_mode_changed(self, mode):
|
|
|
|
def _get_group_drones(self, group):
|
|
|
|
self.current_mission_mode = mode
|
|
|
|
"""取得群組的無人機 ID 列表(排序後)"""
|
|
|
|
mode_names = {
|
|
|
|
return sorted(group.drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
|
|
|
|
'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):
|
|
|
|
def handle_map_click(self, lat, lon):
|
|
|
|
"""處理地圖點擊事件 — 根據選單模式規劃"""
|
|
|
|
"""處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
|
|
|
|
print(f"地圖點擊位置: {lat:.6f}, {lon:.6f} (模式: {self.current_mission_mode})")
|
|
|
|
group = self._get_active_group()
|
|
|
|
|
|
|
|
if not group:
|
|
|
|
# Grid Sweep 和 Leader-Follower 由各自的觸發方式處理,點擊地圖不處理
|
|
|
|
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
mode_map = {
|
|
|
|
mode_map = {
|
|
|
|
'M_FORMATION': MissionType.M_FORMATION,
|
|
|
|
'M_FORMATION': MissionType.M_FORMATION,
|
|
|
|
'CIRCLE_FORMATION': MissionType.CIRCLE_FORMATION,
|
|
|
|
'CIRCLE_FORMATION': MissionType.CIRCLE_FORMATION,
|
|
|
|
}
|
|
|
|
}
|
|
|
|
mission_type = mode_map.get(self.current_mission_mode)
|
|
|
|
mission_type = mode_map.get(group.mission_type)
|
|
|
|
if mission_type is None:
|
|
|
|
if mission_type is None:
|
|
|
|
# Grid Sweep / Leader-Follower 模式下點擊地圖不處理
|
|
|
|
return # Grid Sweep / Leader-Follower 由各自的觸發方式處理
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
selected_drones = self._get_group_drones(group)
|
|
|
|
selected_drones = self.get_selected_drones()
|
|
|
|
|
|
|
|
if len(selected_drones) == 0:
|
|
|
|
if len(selected_drones) == 0:
|
|
|
|
self.statusBar().showMessage("⚠ 請先選擇無人機(勾選 checkbox)", 3000)
|
|
|
|
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
|
|
|
|
return
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
base_alt = 10.0
|
|
|
|
print(f"地圖點擊: {lat:.6f}, {lon:.6f} → Group {group.group_id} ({group.mission_type})")
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group.group_id)
|
|
|
|
|
|
|
|
params = panel.get_mission_params() if panel else {}
|
|
|
|
|
|
|
|
base_alt = params.get('base_altitude', params.get('altitude', 10.0))
|
|
|
|
target_gps = (lat, lon, base_alt)
|
|
|
|
target_gps = (lat, lon, base_alt)
|
|
|
|
self.statusBar().showMessage(f"⏳ 正在規劃 {self.current_mission_mode} ({len(selected_drones)} 台) ...", 2000)
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
|
|
|
|
f"⏳ Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台) ...", 2000)
|
|
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
try:
|
|
|
|
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
|
|
|
|
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
|
|
|
|
if drone_gps_positions is None: return
|
|
|
|
if drone_gps_positions is None:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
|
|
|
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
|
|
|
drone_gps_positions, target_gps, mission_type
|
|
|
|
drone_gps_positions, target_gps, mission_type, params=params
|
|
|
|
)
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
|
|
|
|
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
|
|
|
|
self.show_planned_waypoints()
|
|
|
|
group.center_origin = center_origin
|
|
|
|
|
|
|
|
self.show_planned_waypoints(group)
|
|
|
|
|
|
|
|
|
|
|
|
center_lat, center_lon, _ = center_origin
|
|
|
|
center_lat, center_lon, _ = center_origin
|
|
|
|
self.drone_map.draw_mission_plan(center_lat, center_lon, lat, lon)
|
|
|
|
self.drone_map.draw_mission_plan_for_group(
|
|
|
|
|
|
|
|
group.group_id, group.color,
|
|
|
|
|
|
|
|
center_lat, center_lon, lat, lon
|
|
|
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
self._launch_verification(
|
|
|
|
self._launch_verification(
|
|
|
|
self.current_mission_mode, drone_gps_positions, selected_drones,
|
|
|
|
group.mission_type, drone_gps_positions, selected_drones,
|
|
|
|
waypoints_per_drone, center_origin, target_gps=target_gps
|
|
|
|
waypoints_per_drone, center_origin, target_gps=target_gps
|
|
|
|
)
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group.group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
panel.update_mission_info(center_lat, center_lon, lat, lon)
|
|
|
|
|
|
|
|
|
|
|
|
total_wps = sum(len(wps) for wps in waypoints_per_drone)
|
|
|
|
total_wps = sum(len(wps) for wps in waypoints_per_drone)
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
f"✓ {self.current_mission_mode} 規劃完成!{len(selected_drones)} 台,共 {total_wps} 個航點", 5000
|
|
|
|
f"✓ Group {group.group_id}: {group.mission_type} 規劃完成!{len(selected_drones)} 台,共 {total_wps} 個航點", 5000
|
|
|
|
)
|
|
|
|
)
|
|
|
|
except Exception as e:
|
|
|
|
except Exception as e:
|
|
|
|
self.statusBar().showMessage(f"❌ 規劃失敗: {str(e)}", 5000)
|
|
|
|
self.statusBar().showMessage(f"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
|
|
|
|
import traceback
|
|
|
|
import traceback
|
|
|
|
traceback.print_exc()
|
|
|
|
traceback.print_exc()
|
|
|
|
|
|
|
|
|
|
|
|
@ -685,66 +898,93 @@ class ControlStationUI(QMainWindow):
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
def handle_rectangle_selected(self, points_json):
|
|
|
|
def handle_rectangle_selected(self, points_json):
|
|
|
|
print(f"矩形選取: {points_json}")
|
|
|
|
group = self._get_active_group()
|
|
|
|
selected_drones = self.get_selected_drones()
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
if group.mission_type != 'GRID_SWEEP':
|
|
|
|
|
|
|
|
return # 非 Grid Sweep 模式不處理矩形選取
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
selected_drones = self._get_group_drones(group)
|
|
|
|
if len(selected_drones) == 0:
|
|
|
|
if len(selected_drones) == 0:
|
|
|
|
self.statusBar().showMessage("⚠ 請先選擇無人機再框選區域", 3000)
|
|
|
|
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
|
|
|
|
return
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
print(f"矩形選取 → Group {group.group_id}: {points_json}")
|
|
|
|
try:
|
|
|
|
try:
|
|
|
|
rect_corners = [(p[0], p[1]) for p in json.loads(points_json)]
|
|
|
|
rect_corners = [(p[0], p[1]) for p in json.loads(points_json)]
|
|
|
|
except (json.JSONDecodeError, IndexError):
|
|
|
|
except (json.JSONDecodeError, IndexError):
|
|
|
|
self.statusBar().showMessage("❌ 矩形座標解析失敗", 3000)
|
|
|
|
self.statusBar().showMessage("❌ 矩形座標解析失敗", 3000)
|
|
|
|
return
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
base_alt = 10.0
|
|
|
|
panel = self.group_panels.get(group.group_id)
|
|
|
|
self.statusBar().showMessage(f"⏳ 正在規劃 Grid Sweep ({len(selected_drones)} 台) ...", 2000)
|
|
|
|
params = panel.get_mission_params() if panel else {}
|
|
|
|
|
|
|
|
base_alt = params.get('altitude', 10.0)
|
|
|
|
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
|
|
|
|
f"⏳ Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台) ...", 2000)
|
|
|
|
try:
|
|
|
|
try:
|
|
|
|
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
|
|
|
|
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
|
|
|
|
if drone_gps_positions is None: return
|
|
|
|
if drone_gps_positions is None:
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
target_lat = sum(c[0] for c in rect_corners) / 4
|
|
|
|
target_lat = sum(c[0] for c in rect_corners) / 4
|
|
|
|
target_lon = sum(c[1] 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)
|
|
|
|
target_gps = (target_lat, target_lon, base_alt)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
params['rect_corners'] = rect_corners
|
|
|
|
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
|
|
|
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
|
|
|
drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
|
|
|
|
drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
|
|
|
|
params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': base_alt}
|
|
|
|
params=params
|
|
|
|
)
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
|
|
|
|
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
|
|
|
|
self.show_planned_waypoints()
|
|
|
|
group.center_origin = center_origin
|
|
|
|
|
|
|
|
self.show_planned_waypoints(group)
|
|
|
|
|
|
|
|
|
|
|
|
center_lat, center_lon, _ = center_origin
|
|
|
|
center_lat, center_lon, _ = center_origin
|
|
|
|
self.drone_map.draw_mission_plan(center_lat, center_lon, target_lat, target_lon)
|
|
|
|
self.drone_map.draw_mission_plan_for_group(
|
|
|
|
|
|
|
|
group.group_id, group.color,
|
|
|
|
|
|
|
|
center_lat, center_lon, target_lat, target_lon
|
|
|
|
|
|
|
|
)
|
|
|
|
self._launch_verification(
|
|
|
|
self._launch_verification(
|
|
|
|
'grid_sweep', drone_gps_positions, selected_drones,
|
|
|
|
'grid_sweep', drone_gps_positions, selected_drones,
|
|
|
|
waypoints_per_drone, center_origin, rect_corners=rect_corners
|
|
|
|
waypoints_per_drone, center_origin, rect_corners=rect_corners
|
|
|
|
)
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group.group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
|
|
|
|
|
|
|
|
|
|
|
|
total_wps = sum(len(wps) for wps in waypoints_per_drone)
|
|
|
|
total_wps = sum(len(wps) for wps in waypoints_per_drone)
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
f"✓ Grid Sweep 規劃完成!{len(selected_drones)} 台,共 {total_wps} 個航點", 5000
|
|
|
|
f"✓ Group {group.group_id}: Grid Sweep 規劃完成!{len(selected_drones)} 台,共 {total_wps} 個航點", 5000
|
|
|
|
)
|
|
|
|
)
|
|
|
|
except Exception as e:
|
|
|
|
except Exception as e:
|
|
|
|
self.statusBar().showMessage(f"❌ Grid Sweep 規劃失敗: {str(e)}", 5000)
|
|
|
|
self.statusBar().showMessage(f"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
|
|
|
|
import traceback
|
|
|
|
import traceback
|
|
|
|
traceback.print_exc()
|
|
|
|
traceback.print_exc()
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
# 任務規劃 — 路徑確認 (Leader-Follower 跟隨模式)
|
|
|
|
# 任務規劃 — 路徑確認 (Leader-Follower)
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
def handle_route_confirmed(self, points_json):
|
|
|
|
def handle_route_confirmed(self, points_json):
|
|
|
|
"""路徑確認 → Leader-Follower 任務規劃"""
|
|
|
|
group = self._get_active_group()
|
|
|
|
print(f"路徑確認: {points_json}")
|
|
|
|
if not group:
|
|
|
|
|
|
|
|
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
|
|
|
|
selected_drones = self.get_selected_drones()
|
|
|
|
return
|
|
|
|
|
|
|
|
if group.mission_type != 'LEADER_FOLLOWER':
|
|
|
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
selected_drones = self._get_group_drones(group)
|
|
|
|
if len(selected_drones) == 0:
|
|
|
|
if len(selected_drones) == 0:
|
|
|
|
self.statusBar().showMessage("⚠ 請先選擇無人機再標記路徑", 3000)
|
|
|
|
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
|
|
|
|
return
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
print(f"路徑確認 → Group {group.group_id}: {points_json}")
|
|
|
|
try:
|
|
|
|
try:
|
|
|
|
route_points = json.loads(points_json) # [[lat, lon], ...]
|
|
|
|
route_points = json.loads(points_json)
|
|
|
|
route_waypoints = [(p[0], p[1]) for p in route_points]
|
|
|
|
route_waypoints = [(p[0], p[1]) for p in route_points]
|
|
|
|
except (json.JSONDecodeError, IndexError):
|
|
|
|
except (json.JSONDecodeError, IndexError):
|
|
|
|
self.statusBar().showMessage("❌ 路徑座標解析失敗", 3000)
|
|
|
|
self.statusBar().showMessage("❌ 路徑座標解析失敗", 3000)
|
|
|
|
@ -754,81 +994,67 @@ class ControlStationUI(QMainWindow):
|
|
|
|
self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000)
|
|
|
|
self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000)
|
|
|
|
return
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
base_alt = 10.0
|
|
|
|
panel = self.group_panels.get(group.group_id)
|
|
|
|
self.statusBar().showMessage(f"⏳ 正在規劃跟隨模式 ({len(selected_drones)} 台, {len(route_waypoints)} 個路徑點) ...", 2000)
|
|
|
|
params = panel.get_mission_params() if panel else {}
|
|
|
|
|
|
|
|
base_alt = params.get('altitude', 10.0)
|
|
|
|
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
|
|
|
|
f"⏳ Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台) ...", 2000)
|
|
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
try:
|
|
|
|
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
|
|
|
|
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
|
|
|
|
if drone_gps_positions is None:
|
|
|
|
if drone_gps_positions is None:
|
|
|
|
return
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
|
|
# 目標點 = 路徑中心(供 origin 計算)
|
|
|
|
|
|
|
|
target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints)
|
|
|
|
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_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints)
|
|
|
|
target_gps = (target_lat, target_lon, base_alt)
|
|
|
|
target_gps = (target_lat, target_lon, base_alt)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
params['route_waypoints'] = route_waypoints
|
|
|
|
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
|
|
|
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
|
|
|
|
drone_gps_positions,
|
|
|
|
drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER,
|
|
|
|
target_gps,
|
|
|
|
params=params
|
|
|
|
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}
|
|
|
|
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
|
|
|
|
self.show_planned_waypoints()
|
|
|
|
group.center_origin = center_origin
|
|
|
|
|
|
|
|
self.show_planned_waypoints(group)
|
|
|
|
|
|
|
|
|
|
|
|
center_lat, center_lon, _ = center_origin
|
|
|
|
center_lat, center_lon, _ = center_origin
|
|
|
|
self.drone_map.draw_mission_plan(center_lat, center_lon, target_lat, target_lon)
|
|
|
|
self.drone_map.draw_mission_plan_for_group(
|
|
|
|
|
|
|
|
group.group_id, group.color,
|
|
|
|
# 啟動視覺化驗證
|
|
|
|
center_lat, center_lon, target_lat, target_lon
|
|
|
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
self._launch_verification(
|
|
|
|
self._launch_verification(
|
|
|
|
'LEADER_FOLLOWER', drone_gps_positions, selected_drones,
|
|
|
|
'LEADER_FOLLOWER', drone_gps_positions, selected_drones,
|
|
|
|
waypoints_per_drone, center_origin,
|
|
|
|
waypoints_per_drone, center_origin,
|
|
|
|
target_gps=target_gps, route_waypoints=route_waypoints
|
|
|
|
target_gps=target_gps, route_waypoints=route_waypoints
|
|
|
|
)
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
panel = self.group_panels.get(group.group_id)
|
|
|
|
|
|
|
|
if panel:
|
|
|
|
|
|
|
|
panel.update_status()
|
|
|
|
|
|
|
|
panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
|
|
|
|
|
|
|
|
|
|
|
|
total_wps = sum(len(wps) for wps in waypoints_per_drone)
|
|
|
|
total_wps = sum(len(wps) for wps in waypoints_per_drone)
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
self.statusBar().showMessage(
|
|
|
|
f"✓ 跟隨模式規劃完成!{len(selected_drones)} 台,{len(route_waypoints)} 個路徑點,共 {total_wps} 個航點", 5000
|
|
|
|
f"✓ Group {group.group_id}: 跟隨模式規劃完成!{len(selected_drones)} 台,共 {total_wps} 個航點", 5000
|
|
|
|
)
|
|
|
|
)
|
|
|
|
except Exception as e:
|
|
|
|
except Exception as e:
|
|
|
|
self.statusBar().showMessage(f"❌ 跟隨模式規劃失敗: {str(e)}", 5000)
|
|
|
|
self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
|
|
|
|
import traceback
|
|
|
|
import traceback
|
|
|
|
traceback.print_exc()
|
|
|
|
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):
|
|
|
|
def on_drone_waypoint_reached(self, drone_id, wp_index, total):
|
|
|
|
if wp_index >= total:
|
|
|
|
if wp_index >= total:
|
|
|
|
self.statusBar().showMessage(f"📍 {drone_id} 完成所有航點", 3000)
|
|
|
|
self.statusBar().showMessage(f"📍 {drone_id} 完成所有航點", 3000)
|
|
|
|
else:
|
|
|
|
else:
|
|
|
|
self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000)
|
|
|
|
self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000)
|
|
|
|
|
|
|
|
|
|
|
|
def on_mission_completed(self):
|
|
|
|
|
|
|
|
self.statusBar().showMessage("✅ 所有無人機已完成任務", 5000)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
# 輔助方法
|
|
|
|
# 輔助方法
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
@ -877,20 +1103,23 @@ class ControlStationUI(QMainWindow):
|
|
|
|
subprocess.Popen([sys.executable, script_path, '--file', json_path])
|
|
|
|
subprocess.Popen([sys.executable, script_path, '--file', json_path])
|
|
|
|
print(f"驗證視窗已啟動: {json_path}")
|
|
|
|
print(f"驗證視窗已啟動: {json_path}")
|
|
|
|
|
|
|
|
|
|
|
|
def show_planned_waypoints(self):
|
|
|
|
def show_planned_waypoints(self, group=None):
|
|
|
|
if not self.planned_waypoints: return
|
|
|
|
pw = group.planned_waypoints if group else None
|
|
|
|
print("\n" + "=" * 60)
|
|
|
|
if not pw:
|
|
|
|
print("任務規劃結果")
|
|
|
|
return
|
|
|
|
print("=" * 60)
|
|
|
|
gid = group.group_id if group else "?"
|
|
|
|
drone_ids = self.planned_waypoints['drone_ids']
|
|
|
|
print(f"\n{'=' * 60}")
|
|
|
|
waypoints = self.planned_waypoints['waypoints']
|
|
|
|
print(f"任務規劃結果 — Group {gid}")
|
|
|
|
|
|
|
|
print(f"{'=' * 60}")
|
|
|
|
|
|
|
|
drone_ids = pw['drone_ids']
|
|
|
|
|
|
|
|
waypoints = pw['waypoints']
|
|
|
|
print(f"\n共 {len(drone_ids)} 台無人機")
|
|
|
|
print(f"\n共 {len(drone_ids)} 台無人機")
|
|
|
|
for i, drone_id in enumerate(drone_ids):
|
|
|
|
for i, drone_id in enumerate(drone_ids):
|
|
|
|
wps = waypoints[i]
|
|
|
|
wps = waypoints[i]
|
|
|
|
print(f"\n【{drone_id}】({len(wps)} 個航點)")
|
|
|
|
print(f"\n【{drone_id}】({len(wps)} 個航點)")
|
|
|
|
for j, wp in enumerate(wps):
|
|
|
|
for j, wp in enumerate(wps):
|
|
|
|
print(f" WP{j}: ({wp[0]:.6f}°, {wp[1]:.6f}°, {wp[2]:.1f}m)")
|
|
|
|
print(f" WP{j}: ({wp[0]:.6f}°, {wp[1]:.6f}°, {wp[2]:.1f}m)")
|
|
|
|
print("\n" + "=" * 60)
|
|
|
|
print(f"\n{'=' * 60}")
|
|
|
|
|
|
|
|
|
|
|
|
def get_selected_drones(self):
|
|
|
|
def get_selected_drones(self):
|
|
|
|
return [did for did, panel in self.drones.items() if hasattr(panel, 'checkbox') and panel.checkbox.isChecked()]
|
|
|
|
return [did for did, panel in self.drones.items() if hasattr(panel, 'checkbox') and panel.checkbox.isChecked()]
|
|
|
|
@ -1095,7 +1324,9 @@ class ControlStationUI(QMainWindow):
|
|
|
|
print(f"ROS spin error: {e}")
|
|
|
|
print(f"ROS spin error: {e}")
|
|
|
|
|
|
|
|
|
|
|
|
def closeEvent(self, event):
|
|
|
|
def closeEvent(self, event):
|
|
|
|
self.mission_executor.stop()
|
|
|
|
for group in self.mission_groups.values():
|
|
|
|
|
|
|
|
if group.executor:
|
|
|
|
|
|
|
|
group.executor.stop()
|
|
|
|
self.command_sender.close()
|
|
|
|
self.command_sender.close()
|
|
|
|
for receiver in self.udp_receivers:
|
|
|
|
for receiver in self.udp_receivers:
|
|
|
|
receiver.stop()
|
|
|
|
receiver.stop()
|
|
|
|
|