feat: 多任務群組系統 — 四欄式面板、地圖框選分配、可調參數

- 新增 mission_group.py:MissionGroup 資料結構、DroneAssignDialog、GroupPanel
- GroupPanel 四欄式佈局:控制指令 / 任務規劃 / 任務參數 / 選取與分組
- 任務參數欄依任務類型動態切換(間距、高度、偏移量等),規劃時從 UI 讀值
- 框選/全選可直接分配無人機到群組,清除分組一鍵重置
- 地圖清理:移除右下角 overlay,功能整合至 GroupPanel
- 修復 JS clearSelectionMode 引用已移除元素導致框選/Grid Sweep 失效
- 每群組獨立 MissionExecutor,共用 MavlinkSender

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
chiyu
wenchun 1 month ago
parent 693a55c289
commit c2196153ff

@ -0,0 +1,17 @@
# 專案說明
## 專案背景
這是計畫相關的研究,負責 Airtrapmine 無人機系統開發。
使用 SITL、Gazebo 進行模擬,搭配 ArduPilot、MAVLink。
## 技術環境
- Ubuntu Linux
- Python, ROS/Gazebo, SITL
- MAVLink, PyQt5, cmavnode
## 注意事項
- 這個專案跟在 Windows 上做的 MADDPG/MATLAB 異質載具對抗模擬是完全不同的工作,不要把兩者的內容混在一起
## 工作流程Model 使用偏好)
- **規劃、討論、設計階段**:使用 Opus model啟動時加 `--model opusplan` 或手動 `/model opus`
- **實作、寫程式階段**:使用 Sonnet model預設`/model sonnet` 切換回來)

@ -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()

@ -178,35 +178,8 @@ class DroneMap:
</head> </head>
<body> <body>
<div id="map"></div> <div id="map"></div>
<div class="map-controls"> <div class="map-controls" id="map-controls-panel" style="display: none;">
<button class="selection-button" onclick="clearAllTrajectories()">清除軌跡</button> <button class="confirm-route-button" id="confirm-route-btn" onclick="confirmRoute()">確認路徑</button>
<div style="border-top: 1px solid #ddd; padding-top: 5px; margin-top: 5px;">
<label style="font-size: 12px; color: #555; font-weight: bold;">任務模式</label>
<select id="mission-mode-select" onchange="onMissionModeChanged(this.value)" style="width: 100%; padding: 6px; border-radius: 4px; border: 1px solid #ccc; font-size: 12px; margin-top: 3px;">
<option value="M_FORMATION">列隊飛行</option>
<option value="CIRCLE_FORMATION">環狀包圍</option>
<option value="LEADER_FOLLOWER">跟隨模式</option>
<option value="GRID_SWEEP">柵狀偵查</option>
</select>
</div>
<button class="confirm-route-button" id="confirm-route-btn" onclick="confirmRoute()" style="display: none;">確認路徑</button>
<button class="selection-button" id="select-polygon-btn" onclick="togglePolygonSelection()">多點選擇區域 (開發中)</button>
</div>
<div class="mission-info-panel">
<div class="selection-buttons">
<button class="selection-button-blue" onclick="toggleSelectAllDrones()">全選無人機</button>
<button class="selection-button-blue" id="select-drones-btn" onclick="toggleDroneSelection()">框選無人機</button>
</div>
<div class="mission-info-row">
<span class="mission-info-label">中心點: </span>
<span class="mission-info-value" id="center-position">未設定</span>
</div>
<div class="mission-info-row">
<span class="mission-info-label">目標點: </span>
<span class="mission-info-value" id="target-position">未設定</span>
</div>
<button class="mission-start-button" id="start-mission-btn" onclick="startMission()" disabled>開始任務</button>
<button class="mission-start-button" id="pause-mission-btn" onclick="pauseMission()">暫停任務</button>
</div> </div>
<script> <script>
@ -374,6 +347,9 @@ class DroneMap:
var missionLine = null; var missionLine = null;
var centerPosition = null; var centerPosition = null;
var targetPosition = null; var targetPosition = null;
// 多群組任務規劃 每個 group 各自的 layer group
var groupMissionLayers = {}; // group_id L.layerGroup
// 選擇工具變量 // 選擇工具變量
var selectionMode = null; // 'drones', 'rect', 'polygon', 'route', null var selectionMode = null; // 'drones', 'rect', 'polygon', 'route', null
@ -393,30 +369,8 @@ class DroneMap:
var routeLayer = L.layerGroup().addTo(map); var routeLayer = L.layerGroup().addTo(map);
// ================================================================================ // ================================================================================
// 更新任務信息面板 // 任務信息已移至 GroupPanel 顯示
function updateMissionInfo() { function updateMissionInfo() {}
const centerElem = document.getElementById('center-position');
const targetElem = document.getElementById('target-position');
const startBtn = document.getElementById('start-mission-btn');
if (centerPosition) {
centerElem.textContent = `${centerPosition.lat.toFixed(6)}°, ${centerPosition.lng.toFixed(6)}°`;
} else {
centerElem.textContent = '未設定';
}
if (targetPosition) {
targetElem.textContent = `${targetPosition.lat.toFixed(6)}°, ${targetPosition.lng.toFixed(6)}°`;
} else {
targetElem.textContent = '未設定';
}
if (centerPosition && targetPosition) {
startBtn.disabled = false;
} else {
startBtn.disabled = true;
}
}
// ================================================================================ // ================================================================================
// 選擇工具函數 // 選擇工具函數
@ -429,14 +383,14 @@ class DroneMap:
} }
function toggleDroneSelection() { function toggleDroneSelection() {
clearSelectionMode();
if (selectionMode === 'drones') { if (selectionMode === 'drones') {
selectionMode = null; selectionMode = null;
document.getElementById('select-drones-btn').classList.remove('active'); selectionLayer.clearLayers();
tempRectangle = null;
map.dragging.enable(); map.dragging.enable();
} else { } else {
clearSelectionMode();
selectionMode = 'drones'; selectionMode = 'drones';
document.getElementById('select-drones-btn').classList.add('active');
map.dragging.disable(); map.dragging.disable();
} }
} }
@ -450,7 +404,7 @@ class DroneMap:
clearSelectionMode(); clearSelectionMode();
clearPolygonPoints(); clearPolygonPoints();
selectionMode = null; selectionMode = null;
document.getElementById('select-polygon-btn').classList.remove('active'); // select-polygon-btn removed from map overlay
} }
} else { } else {
clearSelectionMode(); clearSelectionMode();
@ -462,17 +416,12 @@ class DroneMap:
} }
function clearSelectionMode() { function clearSelectionMode() {
document.getElementById('select-drones-btn').classList.remove('active');
document.getElementById('select-polygon-btn').classList.remove('active');
selectionLayer.clearLayers(); selectionLayer.clearLayers();
tempRectangle = null; tempRectangle = null;
// 不清除 routeLayer clearRoutePoints 單獨管理 // 不清除 routeLayer clearRoutePoints 單獨管理
map.dragging.enable(); map.dragging.enable();
// 如果離開 route 模式重置 selectionMode
if (selectionMode !== 'route') { if (selectionMode !== 'route') {
selectionMode = null; selectionMode = null;
} }
@ -527,7 +476,7 @@ class DroneMap:
} }
selectionMode = null; selectionMode = null;
document.getElementById('select-polygon-btn').classList.remove('active'); // select-polygon-btn removed from map overlay
map.dragging.enable(); map.dragging.enable();
} }
@ -554,6 +503,10 @@ class DroneMap:
} }
}); });
console.log('框選無人機:', selectedDrones); console.log('框選無人機:', selectedDrones);
// 通知 Python 完整的框選結果
if (bridge && selectedDrones.length > 0) {
bridge.droneBoxSelected(JSON.stringify(selectedDrones));
}
} else if (selectionMode === 'rect') { } else if (selectionMode === 'rect') {
var rectPoints = [ var rectPoints = [
@ -588,18 +541,7 @@ class DroneMap:
// 重置狀態 // 重置狀態
selectionMode = null; selectionMode = null;
document.getElementById('select-drones-btn').classList.remove('active');
map.dragging.enable(); map.dragging.enable();
// 如果仍在 Grid Sweep 模式重新進入框選
var currentMode = document.getElementById('mission-mode-select').value;
if (currentMode === 'GRID_SWEEP') {
setTimeout(function() {
selectionMode = 'rect';
map.dragging.disable();
console.log('Grid Sweep: 重新進入框選模式');
}, 500);
}
} }
// ================================================================================ // ================================================================================
@ -708,19 +650,19 @@ class DroneMap:
selectionMode = null; selectionMode = null;
clearSelectionMode(); clearSelectionMode();
clearRoutePoints(); clearRoutePoints();
// 隱藏確認路徑按鈕 // 預設隱藏控制面板
document.getElementById('confirm-route-btn').style.display = 'none'; document.getElementById('map-controls-panel').style.display = 'none';
if (mode === 'GRID_SWEEP') { if (mode === 'GRID_SWEEP') {
// 自動進入框選模式 // 自動進入框選模式
selectionMode = 'rect'; selectionMode = 'rect';
map.dragging.disable(); map.dragging.disable();
console.log('Grid Sweep: 進入框選模式'); console.log('Grid Sweep: 進入框選模式');
} else if (mode === 'LEADER_FOLLOWER') { } else if (mode === 'LEADER_FOLLOWER') {
// 進入路徑標記模式 // 進入路徑標記模式 + 顯示確認路徑按鈕
selectionMode = 'route'; selectionMode = 'route';
document.getElementById('confirm-route-btn').style.display = 'block'; document.getElementById('map-controls-panel').style.display = 'flex';
console.log('跟隨模式: 進入路徑標記模式,點擊地圖添加路徑點'); console.log('跟隨模式: 進入路徑標記模式,點擊地圖添加路徑點');
} }
@ -839,76 +781,74 @@ class DroneMap:
// 任務規劃視覺化函式 // 任務規劃視覺化函式
// ================================================================================ // ================================================================================
function drawMissionPlan(centerLat, centerLon, targetLat, targetLon) { function drawMissionPlan(centerLat, centerLon, targetLat, targetLon) {
clearMissionPlan(); drawMissionPlanForGroup('_default', '#FF4444', centerLat, centerLon, targetLat, targetLon);
}
function drawMissionPlanForGroup(groupId, color, centerLat, centerLon, targetLat, targetLon) {
clearMissionPlanForGroup(groupId);
centerPosition = {lat: centerLat, lng: centerLon}; centerPosition = {lat: centerLat, lng: centerLon};
targetPosition = {lat: targetLat, lng: targetLon}; targetPosition = {lat: targetLat, lng: targetLon};
updateMissionInfo(); updateMissionInfo();
var layerGroup = L.layerGroup().addTo(map);
groupMissionLayers[groupId] = layerGroup;
var centerIcon = L.divIcon({ var centerIcon = L.divIcon({
className: 'mission-center', className: 'mission-center',
html: `<div style=" html: '<div style="' +
background-color: #FF4444; 'background-color: ' + color + ';' +
color: white; 'color: white;' +
width: 20px; 'width: 22px; height: 22px;' +
height: 20px; 'border-radius: 50%;' +
border-radius: 50%; 'display: flex; align-items: center; justify-content: center;' +
display: flex; 'font-weight: bold; font-size: 10px;' +
align-items: center; 'border: 2px solid white;' +
justify-content: center; 'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
font-weight: bold; '">' + groupId + '</div>',
font-size: 12px; iconSize: [22, 22],
border: 2px solid white; iconAnchor: [11, 11]
box-shadow: 0 2px 5px rgba(0,0,0,0.3);
">C</div>`,
iconSize: [20, 20],
iconAnchor: [10, 10]
}); });
centerMarker = L.marker([centerLat, centerLon], { L.marker([centerLat, centerLon], {
icon: centerIcon, icon: centerIcon, zIndexOffset: 2000
zIndexOffset: 2000 }).addTo(layerGroup);
}).addTo(missionPlanGroup);
var targetIcon = L.divIcon({ var targetIcon = L.divIcon({
className: 'mission-target', className: 'mission-target',
html: `<div style=" html: '<div style="' +
background-color: #FFD700; 'background-color: #FFD700;' +
color: #FF4444; 'color: ' + color + ';' +
width: 20px; 'width: 22px; height: 22px;' +
height: 20px; 'border-radius: 50%;' +
border-radius: 50%; 'display: flex; align-items: center; justify-content: center;' +
display: flex; 'font-weight: bold; font-size: 14px;' +
align-items: center; 'border: 2px solid white;' +
justify-content: center; 'box-shadow: 0 2px 5px rgba(0,0,0,0.3);' +
font-weight: bold; '">★</div>',
font-size: 14px; iconSize: [22, 22],
border: 2px solid white; iconAnchor: [11, 11]
box-shadow: 0 2px 5px rgba(0,0,0,0.3);
">★</div>`,
iconSize: [20, 20],
iconAnchor: [10, 10]
}); });
targetMarker = L.marker([targetLat, targetLon], { L.marker([targetLat, targetLon], {
icon: targetIcon, icon: targetIcon, zIndexOffset: 2000
zIndexOffset: 2000 }).addTo(layerGroup);
}).addTo(missionPlanGroup);
console.log('Group ' + groupId + ' 任務規劃已繪製');
console.log('任務規劃已繪製: C(' + centerLat + ', ' + centerLon + ') -> T(' + targetLat + ', ' + targetLon + ')');
} }
function clearMissionPlan() { function clearMissionPlanForGroup(groupId) {
if (centerMarker) { if (groupMissionLayers[groupId]) {
missionPlanGroup.removeLayer(centerMarker); map.removeLayer(groupMissionLayers[groupId]);
centerMarker = null; delete groupMissionLayers[groupId];
} }
}
if (targetMarker) {
missionPlanGroup.removeLayer(targetMarker); function clearAllMissionPlans() {
targetMarker = null; for (var gid in groupMissionLayers) {
map.removeLayer(groupMissionLayers[gid]);
} }
groupMissionLayers = {};
centerPosition = null; centerPosition = null;
targetPosition = null; targetPosition = null;
updateMissionInfo(); updateMissionInfo();
@ -969,17 +909,45 @@ class DroneMap:
# 任務規劃視覺化方法 # 任務規劃視覺化方法
# ================================================================================ # ================================================================================
def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon): def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon):
"""在地圖上繪製任務規劃""" """在地圖上繪製任務規劃(舊介面,相容用)"""
self.draw_mission_plan_for_group('_default', '#FF4444',
center_lat, center_lon, target_lat, target_lon)
def draw_mission_plan_for_group(self, group_id, color,
center_lat, center_lon, target_lat, target_lon):
"""在地圖上繪製指定群組的任務規劃(帶顏色區分)"""
if self.map_loaded: if self.map_loaded:
js_code = f"drawMissionPlan({center_lat:.6f}, {center_lon:.6f}, {target_lat:.6f}, {target_lon:.6f});" js_code = (
f"drawMissionPlanForGroup("
f"'{group_id}', '{color}', "
f"{center_lat:.6f}, {center_lon:.6f}, "
f"{target_lat:.6f}, {target_lon:.6f});"
)
self.map_view.page().runJavaScript(js_code) self.map_view.page().runJavaScript(js_code)
print(f"📍 地圖已繪製任務規劃: C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})") print(f"📍 地圖已繪製 Group {group_id} 任務規劃: "
f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
def clear_mission_plan(self): def clear_mission_plan(self):
"""清除地圖上的任務規劃標記""" """清除地圖上所有任務規劃標記"""
if self.map_loaded:
self.map_view.page().runJavaScript("clearAllMissionPlans();")
print("🗑️ 地圖已清除所有任務規劃")
def clear_mission_plan_for_group(self, group_id):
"""清除指定群組的任務規劃標記"""
if self.map_loaded: if self.map_loaded:
self.map_view.page().runJavaScript("clearMissionPlan();") self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
print("🗑️ 地圖已清除任務規劃") print(f"🗑️ 地圖已清除 Group {group_id} 任務規劃")
def set_mission_mode(self, mode):
"""從 Python 端切換地圖的任務模式(觸發框選/路徑標記等)"""
if self.map_loaded:
self.map_view.page().runJavaScript(f"onMissionModeChanged('{mode}');")
def toggle_drone_box_select(self):
"""切換地圖上的無人機框選模式"""
if self.map_loaded:
self.map_view.page().runJavaScript("toggleDroneSelection();")
# ================================================================================ # ================================================================================
def get_widget(self): def get_widget(self):
@ -1029,6 +997,10 @@ class DroneMap:
def get_route_confirmed_signal(self): def get_route_confirmed_signal(self):
"""獲取路徑確認信號""" """獲取路徑確認信號"""
return self.bridge.route_confirmed return self.bridge.route_confirmed
def get_drone_box_selected_signal(self):
"""獲取框選無人機完成信號"""
return self.bridge.drone_box_selected
class MapBridge(QObject): class MapBridge(QObject):
"""JavaScript 和 Python 之間的橋接類""" """JavaScript 和 Python 之間的橋接類"""
@ -1042,7 +1014,8 @@ class MapBridge(QObject):
polygon_selected = pyqtSignal(str) polygon_selected = pyqtSignal(str)
mission_mode_changed = pyqtSignal(str) mission_mode_changed = pyqtSignal(str)
route_confirmed = pyqtSignal(str) # 路徑確認 (JSON 字串) route_confirmed = pyqtSignal(str) # 路徑確認 (JSON 字串)
drone_box_selected = pyqtSignal(str) # 框選無人機完成 (JSON 字串)
def __init__(self): def __init__(self):
super().__init__() super().__init__()
@ -1102,4 +1075,10 @@ class MapBridge(QObject):
def routeConfirmed(self, points_json): def routeConfirmed(self, points_json):
"""供 JavaScript 調用的方法 - 路徑確認""" """供 JavaScript 調用的方法 - 路徑確認"""
self.route_confirmed.emit(points_json) self.route_confirmed.emit(points_json)
print(f"📍 路徑已確認: {points_json}") print(f"📍 路徑已確認: {points_json}")
@pyqtSlot(str)
def droneBoxSelected(self, drone_ids_json):
"""供 JavaScript 調用的方法 - 框選無人機完成"""
self.drone_box_selected.emit(drone_ids_json)
print(f"📦 框選無人機: {drone_ids_json}")

@ -0,0 +1,495 @@
#!/usr/bin/env python3
"""
任務群組模組
管理多任務群組的資料結構與無人機分配對話框
"""
from PyQt6.QtWidgets import (
QWidget, QVBoxLayout, QHBoxLayout, QLabel, QPushButton,
QComboBox, QDialog, QCheckBox, QScrollArea, QFrame, QLineEdit
)
from PyQt6.QtCore import Qt, pyqtSignal
from mission_executor import MissionExecutor, MissionState
# 群組顏色(循環使用)
GROUP_COLORS = [
'#4A9EFF', # 藍
'#FF8C42', # 橘
'#56C87A', # 綠
'#E85D75', # 紅
'#B07CED', # 紫
'#F5C542', # 黃
'#42C9C9', # 青
'#FF6B9D', # 粉
]
class MissionGroup:
"""單一任務群組的資料"""
def __init__(self, group_id, color):
self.group_id = group_id # 'A', 'B', 'C', ...
self.color = color # 顏色 hex
self.drone_ids = set() # 已分配的無人機 ID
self.mission_type = 'M_FORMATION' # 預設任務類型
self.planned_waypoints = None # 規劃結果 dict
self.executor = None # MissionExecutor 實例(延遲建立)
self.center_origin = None # 規劃原點
@property
def state(self):
if self.executor is None:
return MissionState.IDLE
return self.executor.state
@property
def display_name(self):
return f"Group {self.group_id}"
class DroneAssignDialog(QDialog):
"""無人機分配對話框"""
def __init__(self, parent, all_drone_ids, current_assigned, other_assigned):
"""
Args:
parent: widget
all_drone_ids: 所有可用無人機 ID 列表
current_assigned: 當前群組已分配的無人機 set
other_assigned: 其他群組已佔用的無人機 dict {drone_id: group_id}
"""
super().__init__(parent)
self.setWindowTitle("分配無人機")
self.setMinimumWidth(280)
self.setStyleSheet("""
QDialog { background-color: #2B2B2B; }
QLabel { color: #DDD; }
QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
QCheckBox::indicator { width: 16px; height: 16px; }
QCheckBox:disabled { color: #666; }
""")
layout = QVBoxLayout(self)
title = QLabel("選擇要分配到此群組的無人機:")
title.setStyleSheet("font-size: 13px; font-weight: bold; padding-bottom: 6px;")
layout.addWidget(title)
# 滾動區域
scroll = QScrollArea()
scroll.setWidgetResizable(True)
scroll.setMaximumHeight(300)
scroll_widget = QWidget()
scroll_layout = QVBoxLayout(scroll_widget)
scroll_layout.setSpacing(2)
self.checkboxes = {}
sorted_ids = sorted(all_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
for drone_id in sorted_ids:
cb = QCheckBox(drone_id)
if drone_id in current_assigned:
cb.setChecked(True)
elif drone_id in other_assigned:
cb.setEnabled(False)
cb.setToolTip(f"已分配到 Group {other_assigned[drone_id]}")
cb.setText(f"{drone_id} (Group {other_assigned[drone_id]})")
self.checkboxes[drone_id] = cb
scroll_layout.addWidget(cb)
scroll_layout.addStretch()
scroll.setWidget(scroll_widget)
layout.addWidget(scroll)
# 按鈕
btn_layout = QHBoxLayout()
ok_btn = QPushButton("確定")
ok_btn.setStyleSheet("""
QPushButton { background-color: #4A9EFF; color: white; border: none;
padding: 8px 20px; border-radius: 4px; font-weight: bold; }
QPushButton:hover { background-color: #3A8EEF; }
""")
ok_btn.clicked.connect(self.accept)
cancel_btn = QPushButton("取消")
cancel_btn.setStyleSheet("""
QPushButton { background-color: #555; color: #DDD; border: none;
padding: 8px 20px; border-radius: 4px; }
QPushButton:hover { background-color: #666; }
""")
cancel_btn.clicked.connect(self.reject)
btn_layout.addStretch()
btn_layout.addWidget(cancel_btn)
btn_layout.addWidget(ok_btn)
layout.addLayout(btn_layout)
def get_selected(self):
"""回傳勾選的無人機 ID set"""
return {did for did, cb in self.checkboxes.items() if cb.isChecked() and cb.isEnabled()}
class GroupPanel(QWidget):
"""單一群組的 UI 面板(嵌入到 tab 中)— 三欄式佈局"""
# 信號
assign_drones_requested = pyqtSignal(str) # group_id
mission_type_changed = pyqtSignal(str, str) # group_id, mission_type
start_requested = pyqtSignal(str) # group_id
pause_requested = pyqtSignal(str) # group_id
stop_requested = pyqtSignal(str) # group_id
mode_change_requested = pyqtSignal(str, str) # group_id, mode
arm_requested = pyqtSignal(str) # group_id
takeoff_requested = pyqtSignal(str, float) # group_id, altitude
box_select_requested = pyqtSignal(str) # group_id — 框選直接分配
select_all_requested = pyqtSignal(str) # group_id — 全選直接分配
clear_group_requested = pyqtSignal(str) # group_id — 清除分組
BUTTON_STYLE = """
QPushButton {{ background-color: {bg}; color: {fg}; border: none;
padding: 5px 8px; border-radius: 4px; font-size: 11px; }}
QPushButton:hover {{ background-color: {hover}; }}
QPushButton:disabled {{ background-color: #444; color: #666; }}
"""
def __init__(self, group: MissionGroup, parent=None):
super().__init__(parent)
self.group = group
self._build_ui()
def _make_sep(self):
"""建立垂直分隔線"""
sep = QFrame()
sep.setFrameShape(QFrame.Shape.VLine)
sep.setStyleSheet("color: #444;")
return sep
def _build_ui(self):
layout = QVBoxLayout(self)
layout.setContentsMargins(6, 4, 6, 4)
layout.setSpacing(0)
COMBO = ("QComboBox { background-color: #333; color: #DDD; "
"border-radius: 3px; padding: 2px 6px; font-size: 11px; }")
BTN = self.BUTTON_STYLE
LBL = "color: #AAA; font-size: 11px;"
TITLE = "color: #DDD; font-size: 11px; font-weight: bold; padding-bottom: 2px;"
# ── 三欄主佈局 ──
cols = QHBoxLayout()
cols.setSpacing(6)
# ============================
# 左欄:控制指令
# ============================
left = QVBoxLayout()
left.setSpacing(3)
left_title = QLabel("控制指令")
left_title.setStyleSheet(TITLE)
left.addWidget(left_title)
# 模式切換
mode_row = QHBoxLayout()
mode_row.setSpacing(3)
self.mode_combo = QComboBox()
self.mode_combo.addItems([
"GUIDED", "AUTO", "LAND", "LOITER",
"STABILIZE", "ALT_HOLD", "RTL", "POSHOLD", "SMART_RTL"
])
self.mode_combo.setStyleSheet(COMBO)
mode_btn = QPushButton("切換")
mode_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
mode_btn.clicked.connect(
lambda: self.mode_change_requested.emit(
self.group.group_id, self.mode_combo.currentText()))
mode_row.addWidget(self.mode_combo, 1)
mode_row.addWidget(mode_btn)
left.addLayout(mode_row)
# 解鎖
arm_btn = QPushButton("解鎖")
arm_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
arm_btn.clicked.connect(
lambda: self.arm_requested.emit(self.group.group_id))
left.addWidget(arm_btn)
# 起飛
takeoff_row = QHBoxLayout()
takeoff_row.setSpacing(3)
self.alt_input = QComboBox()
self.alt_input.setEditable(True)
self.alt_input.addItems(["5", "10", "15", "20"])
self.alt_input.setCurrentText("10")
self.alt_input.setStyleSheet(COMBO)
alt_lbl = QLabel("m")
alt_lbl.setStyleSheet(LBL)
takeoff_btn = QPushButton("起飛")
takeoff_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
takeoff_btn.clicked.connect(self._on_takeoff)
takeoff_row.addWidget(self.alt_input, 1)
takeoff_row.addWidget(alt_lbl)
takeoff_row.addWidget(takeoff_btn)
left.addLayout(takeoff_row)
left.addStretch()
# ============================
# 中欄:任務規劃(左右分割)
# ============================
mid = QVBoxLayout()
mid.setSpacing(2)
mid_title = QLabel("任務規劃")
mid_title.setStyleSheet(TITLE)
mid.addWidget(mid_title)
mid_body = QHBoxLayout()
mid_body.setSpacing(4)
# 左側:類型 + 狀態 + 座標
mid_left = QVBoxLayout()
mid_left.setSpacing(2)
self.type_combo = QComboBox()
self.type_combo.addItems([
"M_FORMATION", "CIRCLE_FORMATION", "LEADER_FOLLOWER", "GRID_SWEEP"
])
self.type_combo.setStyleSheet(COMBO)
self.type_combo.currentTextChanged.connect(
lambda t: self.mission_type_changed.emit(self.group.group_id, t))
mid_left.addWidget(self.type_combo)
self.status_label = QLabel("○ 未規劃")
self.status_label.setStyleSheet("color: #888; font-size: 11px;")
mid_left.addWidget(self.status_label)
self.center_label = QLabel("中心: --")
self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
mid_left.addWidget(self.center_label)
self.target_label = QLabel("目標: --")
self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
mid_left.addWidget(self.target_label)
mid_left.addStretch()
# 右側:執行 / 暫停 / 停止(垂直排列)
mid_right = QVBoxLayout()
mid_right.setSpacing(3)
self.start_btn = QPushButton("▶ 執行")
self.start_btn.setStyleSheet(BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
self.start_btn.clicked.connect(
lambda: self.start_requested.emit(self.group.group_id))
self.pause_btn = QPushButton("⏸ 暫停")
self.pause_btn.setStyleSheet(BTN.format(bg='#F57F17', fg='white', hover='#F9A825'))
self.pause_btn.clicked.connect(
lambda: self.pause_requested.emit(self.group.group_id))
self.stop_btn = QPushButton("■ 停止")
self.stop_btn.setStyleSheet(BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
self.stop_btn.clicked.connect(
lambda: self.stop_requested.emit(self.group.group_id))
mid_right.addWidget(self.start_btn)
mid_right.addWidget(self.pause_btn)
mid_right.addWidget(self.stop_btn)
mid_right.addStretch()
mid_body.addLayout(mid_left, 1)
mid_body.addLayout(mid_right)
mid.addLayout(mid_body)
# ============================
# 選取與分組2x2 按鈕)
# ============================
right = QVBoxLayout()
right.setSpacing(3)
right_title = QLabel("選取與分組")
right_title.setStyleSheet(TITLE)
right.addWidget(right_title)
self.drone_list_label = QLabel("尚未分配")
self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
self.drone_list_label.setWordWrap(True)
right.addWidget(self.drone_list_label)
# 2x2 按鈕網格
grid_r1 = QHBoxLayout()
grid_r1.setSpacing(3)
assign_btn = QPushButton("編輯分配")
assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
assign_btn.clicked.connect(
lambda: self.assign_drones_requested.emit(self.group.group_id))
clear_btn = QPushButton("清除分組")
clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
clear_btn.clicked.connect(
lambda: self.clear_group_requested.emit(self.group.group_id))
grid_r1.addWidget(assign_btn)
grid_r1.addWidget(clear_btn)
right.addLayout(grid_r1)
grid_r2 = QHBoxLayout()
grid_r2.setSpacing(3)
box_btn = QPushButton("框選")
box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
box_btn.clicked.connect(
lambda: self.box_select_requested.emit(self.group.group_id))
all_btn = QPushButton("全選")
all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
all_btn.clicked.connect(
lambda: self.select_all_requested.emit(self.group.group_id))
grid_r2.addWidget(box_btn)
grid_r2.addWidget(all_btn)
right.addLayout(grid_r2)
right.addStretch()
# ============================
# 第四欄:任務參數
# ============================
param_col = QVBoxLayout()
param_col.setSpacing(2)
param_title = QLabel("任務參數")
param_title.setStyleSheet(TITLE)
param_col.addWidget(param_title)
INPUT = ("QLineEdit { background-color: #333; color: #DDD; "
"border: 1px solid #555; border-radius: 3px; "
"padding: 1px 4px; font-size: 11px; }")
# 每種任務類型的參數定義: (key, label, default_value)
self._param_defs = {
'M_FORMATION': [
('spacing', '間距 (m)', '5.0'),
('base_altitude', '基準高度 (m)', '10.0'),
('altitude_diff', '高低差 (m)', '2.0'),
],
'CIRCLE_FORMATION': [
('radius', '半徑 (m)', '10.0'),
('altitude', '高度 (m)', '10.0'),
('start_angle', '起始角 (°)', '0'),
],
'LEADER_FOLLOWER': [
('lateral_offset', '橫向偏移 (m)', '3.0'),
('longitudinal_spacing', '縱向間距 (m)', '5.0'),
('altitude', '高度 (m)', '10.0'),
],
'GRID_SWEEP': [
('line_spacing', '掃描線距 (m)', '5.0'),
('altitude', '高度 (m)', '10.0'),
],
}
# 建立所有參數列的 widget先全部建好切換時顯示/隱藏)
self._param_widgets = {} # key → (label_widget, input_widget)
self._param_rows = [] # 所有 row layout 對應的 container widget
for mission_type, defs in self._param_defs.items():
for key, label_text, default in defs:
if key in self._param_widgets:
continue # 同名參數只建一次
row_w = QWidget()
row_l = QHBoxLayout(row_w)
row_l.setContentsMargins(0, 0, 0, 0)
row_l.setSpacing(3)
lbl = QLabel(label_text)
lbl.setStyleSheet(LBL)
inp = QLineEdit(default)
inp.setStyleSheet(INPUT)
inp.setFixedWidth(50)
row_l.addWidget(lbl, 1)
row_l.addWidget(inp)
param_col.addWidget(row_w)
self._param_widgets[key] = (row_w, inp)
self._param_rows.append(row_w)
param_col.addStretch()
# 初始顯示對應的參數
self._update_param_visibility()
# 當任務類型切換時更新參數顯示
self.type_combo.currentTextChanged.connect(self._update_param_visibility)
# ── 組裝四欄:控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
cols.addLayout(left, 1)
cols.addWidget(self._make_sep())
cols.addLayout(mid, 1)
cols.addWidget(self._make_sep())
cols.addLayout(param_col, 1)
cols.addWidget(self._make_sep())
cols.addLayout(right, 1)
layout.addLayout(cols)
def update_drone_list(self):
"""更新無人機列表顯示"""
if not self.group.drone_ids:
self.drone_list_label.setText("尚未分配")
self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
else:
sorted_ids = sorted(self.group.drone_ids,
key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
self.drone_list_label.setText("".join(sorted_ids))
self.drone_list_label.setStyleSheet(
f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
def update_status(self):
"""更新任務狀態顯示"""
state = self.group.state
if self.group.planned_waypoints is None:
self.status_label.setText("○ 未規劃")
self.status_label.setStyleSheet("color: #888; font-size: 11px;")
elif state == MissionState.RUNNING:
self.status_label.setText("▶ 執行中")
self.status_label.setStyleSheet("color: #4CAF50; font-size: 11px; font-weight: bold;")
elif state == MissionState.PAUSED:
self.status_label.setText("⏸ 已暫停")
self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
else:
n = len(self.group.drone_ids)
total_wps = sum(len(wps) for wps in self.group.planned_waypoints['waypoints'])
self.status_label.setText(f"● 已規劃 ({n}架/{total_wps}點)")
self.status_label.setStyleSheet(
f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
def _update_param_visibility(self, _=None):
"""根據當前任務類型,顯示/隱藏對應的參數列"""
mission_type = self.type_combo.currentText()
visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
for key, (row_w, _inp) in self._param_widgets.items():
row_w.setVisible(key in visible_keys)
def get_mission_params(self):
"""讀取當前顯示的參數值,回傳 dict"""
mission_type = self.type_combo.currentText()
params = {}
for key, _label, default in self._param_defs.get(mission_type, []):
if key in self._param_widgets:
_row_w, inp = self._param_widgets[key]
try:
params[key] = float(inp.text())
except ValueError:
params[key] = float(default)
return params
def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
"""更新中心點 / 目標點顯示"""
info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°")
self.center_label.setStyleSheet(info_style)
self.target_label.setText(f"目標: {target_lat:.6f}°, {target_lon:.6f}°")
self.target_label.setStyleSheet(info_style)
def clear_mission_info(self):
"""清除中心點 / 目標點顯示"""
self.center_label.setText("中心: --")
self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
self.target_label.setText("目標: --")
self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
def _on_takeoff(self):
try:
alt = float(self.alt_input.currentText())
except ValueError:
alt = 10.0
self.takeoff_requested.emit(self.group.group_id, alt)
Loading…
Cancel
Save