From c2196153ffd1860f6f3f1444e654252e94724898 Mon Sep 17 00:00:00 2001 From: wenchun Date: Wed, 1 Apr 2026 18:15:49 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=A4=9A=E4=BB=BB=E5=8B=99=E7=BE=A4?= =?UTF-8?q?=E7=B5=84=E7=B3=BB=E7=B5=B1=20=E2=80=94=20=E5=9B=9B=E6=AC=84?= =?UTF-8?q?=E5=BC=8F=E9=9D=A2=E6=9D=BF=E3=80=81=E5=9C=B0=E5=9C=96=E6=A1=86?= =?UTF-8?q?=E9=81=B8=E5=88=86=E9=85=8D=E3=80=81=E5=8F=AF=E8=AA=BF=E5=8F=83?= =?UTF-8?q?=E6=95=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 新增 mission_group.py：MissionGroup 資料結構、DroneAssignDialog、GroupPanel - GroupPanel 四欄式佈局：控制指令 / 任務規劃 / 任務參數 / 選取與分組 - 任務參數欄依任務類型動態切換（間距、高度、偏移量等），規劃時從 UI 讀值 - 框選/全選可直接分配無人機到群組，清除分組一鍵重置 - 地圖清理：移除右下角 overlay，功能整合至 GroupPanel - 修復 JS clearSelectionMode 引用已移除元素導致框選/Grid Sweep 失效 - 每群組獨立 MissionExecutor，共用 MavlinkSender Co-Authored-By: Claude Opus 4.6 --- CLAUDE.md | 17 + src/GUI/gui.py | 695 ++++++++++++++++++++++++++------------- src/GUI/map_layout.py | 277 +++++++--------- src/GUI/mission_group.py | 495 ++++++++++++++++++++++++++++ 4 files changed, 1103 insertions(+), 381 deletions(-) create mode 100644 CLAUDE.md create mode 100644 src/GUI/mission_group.py diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..a5c0c5d --- /dev/null +++ b/CLAUDE.md @@ -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` 切換回來） diff --git a/src/GUI/gui.py b/src/GUI/gui.py index a850097..1886305 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -1,10 +1,12 @@ #!/usr/bin/env python3 import rclpy from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, - QWidget, QLabel, QSplitter, QScrollArea, + QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, - QHeaderView, QPushButton, QCheckBox, QLineEdit) + QHeaderView, QPushButton, QCheckBox, QLineEdit, + QComboBox, QDialog) from PyQt6.QtCore import Qt, QTimer +from PyQt6.QtGui import QColor import sys import asyncio import json @@ -19,11 +21,14 @@ from comm_panel import CommPanel from overview_table import OverviewTable # ================================================================================ -# 導入任務規劃器、執行器、發送器 +# 導入任務規劃器、執行器、發送器、群組 # ================================================================================ from mission_planner import FormationPlanner, MissionType from command_sender import MavlinkSender -from mission_executor import MissionExecutor +from mission_executor import MissionExecutor, MissionState +from mission_group import ( + MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS +) # ================================================================================ class ControlStationUI(QMainWindow): @@ -94,28 +99,22 @@ class ControlStationUI(QMainWindow): base_altitude=10.0, # 基準高度 10 公尺 altitude_diff=2.0 # 高低差 2 公尺 ) - self.planned_waypoints = None # 儲存規劃結果 # ================================================================================ # ================================================================================ - # 當前任務模式 (由地圖右上角下拉選單控制) + # 初始化指令發送器（所有群組共用） # ================================================================================ - self.current_mission_mode = 'M_FORMATION' + self.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死 # ================================================================================ # ================================================================================ - # 初始化指令發送器與任務執行器 + # 多任務群組管理 # ================================================================================ - self.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死 - - self.mission_executor = MissionExecutor( - sender=self.command_sender, - drone_gps=self.monitor.drone_gps, # 直接傳引用，即時讀取 - arrival_radius=2.0, - send_rate_hz=2.0 - ) - self.mission_executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached) - self.mission_executor.mission_completed.connect(self.on_mission_completed) + self.mission_groups = {} # group_id → MissionGroup + self.group_panels = {} # group_id → GroupPanel + self.active_group_id = None # 當前 active 的 group + self._group_counter = 0 # 用來產生 group_id + self._pending_box_assign = None # 框選後直接分配到的 group_id # ================================================================================ self.init_ui() @@ -163,97 +162,51 @@ class ControlStationUI(QMainWindow): right_layout.setContentsMargins(10, 10, 10, 10) right_layout.setSpacing(10) - # ========== 批次控制區域 ========== - batch_control_layout = QHBoxLayout() - - batch_title = QLabel("批次操作") - batch_title.setStyleSheet(""" - color: #DDD; font-size: 16px; font-weight: bold; - padding: 5px; background-color: #333; border-radius: 4px; - """) - batch_control_layout.addWidget(batch_title) - - first_row = QHBoxLayout() - select_all_btn = QPushButton("全選") - select_all_btn.clicked.connect(self.handle_select_all) - select_all_btn.setStyleSheet(""" - QPushButton { background-color: #444; color: #DDD; border: none; - padding: 8px 12px; border-radius: 4px; min-width: 80px; } - QPushButton:hover { background-color: #555; } + # ========== 任務群組 Tab ========== + group_header = QHBoxLayout() + group_title = QLabel("任務群組") + group_title.setStyleSheet( + "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;") + group_header.addWidget(group_title) + group_header.addStretch() + + add_group_btn = QPushButton("+ 新增群組") + add_group_btn.setStyleSheet(""" + QPushButton { background-color: #4A9EFF; color: white; border: none; + padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; } + QPushButton:hover { background-color: #3A8EEF; } """) - first_row.addWidget(select_all_btn) - first_row.addStretch() - - mode_layout = QHBoxLayout() - mode_label = QLabel("模式：") - mode_label.setStyleSheet("color: #DDD; min-width: 40px;") - - from PyQt6.QtWidgets import QComboBox - self.mode_combo = QComboBox() - self.mode_combo.addItems([ - "GUIDED", "AUTO", "LAND", "LOITER", - "STABILIZE", "ACRO", "ALT_HOLD", "RTL", - "CIRCLE", "DRIFT", "SPORT", "FLIP", - "AUTOTUNE", "POSHOLD", "BRAKE", "THROW", - "AVOID_ADSB", "GUIDED_NOGPS", "SMART_RTL", - "FLOWHOLD", "FOLLOW", "ZIGZAG", "SYSTEMID", - "AUTOROTATE", "AUTO_RTL" - ]) - self.mode_combo.setCurrentIndex(1) - self.mode_combo.setStyleSheet(""" - QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} - """) - - batch_mode_btn = QPushButton("切換") - batch_mode_btn.clicked.connect(self.handle_batch_mode_change) - batch_mode_btn.setStyleSheet(""" - QPushButton { background-color: #444; color: #DDD; border: none; - padding: 8px 12px; border-radius: 4px; min-width: 80px; } - QPushButton:hover { background-color: #555; } + add_group_btn.clicked.connect(self._add_mission_group) + group_header.addWidget(add_group_btn) + + clear_traj_btn = QPushButton("清除軌跡") + clear_traj_btn.setStyleSheet(""" + QPushButton { background-color: #EF5350; color: white; border: none; + padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; } + QPushButton:hover { background-color: #E53935; } """) - mode_layout.addWidget(mode_label) - mode_layout.addWidget(self.mode_combo) - mode_layout.addWidget(batch_mode_btn) - mode_layout.addStretch() - - third_row = QHBoxLayout() - arm_all_btn = QPushButton("解鎖") - arm_all_btn.clicked.connect(self.handle_arm_selected) - arm_all_btn.setStyleSheet(""" - QPushButton { background-color: #444; color: #DDD; border: none; - padding: 8px 12px; border-radius: 4px; min-width: 80px; } - QPushButton:hover { background-color: #555; } - """) - third_row.addWidget(arm_all_btn) - third_row.addStretch() - - fourth_row = QHBoxLayout() - self.z_input = QLineEdit() - self.z_input.setFixedWidth(60) - self.z_input.setStyleSheet(""" - QLineEdit { background-color: #333; color: #DDD; - border: 1px solid #555; border-radius: 4px; padding: 3px; } - """) - - takeoff_all_btn = QPushButton("起飛") - takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) - takeoff_all_btn.setStyleSheet(""" - QPushButton { background-color: #444; color: #DDD; border: none; - padding: 8px 12px; border-radius: 4px; min-width: 80px; } - QPushButton:hover { background-color: #555; } + clear_traj_btn.clicked.connect(self.drone_map.clear_trajectories) + group_header.addWidget(clear_traj_btn) + + right_layout.addLayout(group_header) + + self.group_tab_widget = QTabWidget() + self.group_tab_widget.setStyleSheet(""" + QTabWidget::pane { border: 1px solid #444; background-color: #2B2B2B; } + QTabBar::tab { + background-color: #333; color: #AAA; border: 1px solid #444; + padding: 5px 12px; margin-right: 2px; border-top-left-radius: 4px; + border-top-right-radius: 4px; font-size: 12px; + } + QTabBar::tab:selected { background-color: #2B2B2B; color: #FFF; border-bottom-color: #2B2B2B; } + QTabBar::tab:hover { background-color: #3A3A3A; } """) - - fourth_row.addWidget(QLabel("高度:", styleSheet="color: #DDD;")) - fourth_row.addWidget(self.z_input) - fourth_row.addWidget(takeoff_all_btn) - fourth_row.addStretch() - - batch_control_layout.addLayout(first_row) - batch_control_layout.addLayout(mode_layout) - batch_control_layout.addLayout(third_row) - batch_control_layout.addLayout(fourth_row) + self.group_tab_widget.setFixedHeight(150) + self.group_tab_widget.currentChanged.connect(self._on_group_tab_changed) + right_layout.addWidget(self.group_tab_widget) - right_layout.addLayout(batch_control_layout) + # 預設建立 Group A + self._add_mission_group() # 添加地圖 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) # ================================================================================ - # 連接任務控制 + 矩形選取 + 任務模式切換 + 路徑確認信號 + # 連接地圖信號（任務模式改由 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_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_drone_box_selected_signal().connect(self._handle_drone_box_selected) # ================================================================================ main_splitter.addWidget(self.left_tab) @@ -426,7 +377,13 @@ class ControlStationUI(QMainWindow): # ================================================================================ 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() future = self.monitor.set_mode(drone_id, mode) 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: future = self.monitor.takeoff_drone(drone_id, 10.0) loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) - - def handle_batch_mode_change(self): - mode = self.mode_combo.currentText() + + # ================================================================================ + # 任務群組管理 + # ================================================================================ + + 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() - for drone_id in self.monitor.selected_drones: + for drone_id in group.drone_ids: 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 更新 @@ -610,73 +820,76 @@ class ControlStationUI(QMainWindow): self.update_group_checkbox_state(socket_id) # ================================================================================ - # 任務模式切換 + # 任務規劃 — 點擊地圖（路由到 active group） # ================================================================================ - def on_mission_mode_changed(self, mode): - self.current_mission_mode = mode - mode_names = { - 'M_FORMATION': '列隊飛行', - 'CIRCLE_FORMATION': '環狀包圍', - 'LEADER_FOLLOWER': '跟隨模式', - 'GRID_SWEEP': '柵狀偵查' - } - display_name = mode_names.get(mode, mode) - self.statusBar().showMessage(f"🔄 任務模式: {display_name}", 3000) - print(f"任務模式切換: {mode}") + def _get_group_drones(self, group): + """取得群組的無人機 ID 列表（排序後）""" + return sorted(group.drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1]))) - # ================================================================================ - # 任務規劃 — 點擊地圖 (M-Formation / Circle) - # ================================================================================ - def handle_map_click(self, lat, lon): - """處理地圖點擊事件 — 根據選單模式規劃""" - print(f"地圖點擊位置: {lat:.6f}, {lon:.6f} (模式: {self.current_mission_mode})") - - # Grid Sweep 和 Leader-Follower 由各自的觸發方式處理，點擊地圖不處理 + """處理地圖點擊事件 — 根據 active group 的任務類型規劃""" + group = self._get_active_group() + if not group: + self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + return + mode_map = { 'M_FORMATION': MissionType.M_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: - # Grid Sweep / Leader-Follower 模式下點擊地圖不處理 - return - - selected_drones = self.get_selected_drones() + return # Grid Sweep / Leader-Follower 由各自的觸發方式處理 + + selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: - self.statusBar().showMessage("⚠ 請先選擇無人機（勾選 checkbox）", 3000) + self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) 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) - 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: 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( - 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} - self.show_planned_waypoints() - + + group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.center_origin = center_origin + self.show_planned_waypoints(group) + 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.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 ) - + + 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) 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: - self.statusBar().showMessage(f"❌ 規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() @@ -685,66 +898,93 @@ class ControlStationUI(QMainWindow): # ================================================================================ def handle_rectangle_selected(self, points_json): - print(f"矩形選取: {points_json}") - selected_drones = self.get_selected_drones() + group = self._get_active_group() + 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: - self.statusBar().showMessage("⚠ 請先選擇無人機再框選區域", 3000) + self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) return + + print(f"矩形選取 → Group {group.group_id}: {points_json}") try: rect_corners = [(p[0], p[1]) for p in json.loads(points_json)] except (json.JSONDecodeError, IndexError): self.statusBar().showMessage("❌ 矩形座標解析失敗", 3000) return - base_alt = 10.0 - self.statusBar().showMessage(f"⏳ 正在規劃 Grid Sweep ({len(selected_drones)} 台) ...", 2000) + panel = self.group_panels.get(group.group_id) + 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: 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_lon = sum(c[1] for c in rect_corners) / 4 target_gps = (target_lat, target_lon, base_alt) + params['rect_corners'] = rect_corners waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( drone_gps_positions, target_gps, MissionType.GRID_SWEEP, - params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': base_alt} + params=params ) - - self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} - self.show_planned_waypoints() - + + group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.center_origin = center_origin + self.show_planned_waypoints(group) + 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( 'grid_sweep', drone_gps_positions, selected_drones, 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) 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: - self.statusBar().showMessage(f"❌ Grid Sweep 規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() # ================================================================================ - # 任務規劃 — 路徑確認 (Leader-Follower 跟隨模式) + # 任務規劃 — 路徑確認 (Leader-Follower) # ================================================================================ def handle_route_confirmed(self, points_json): - """路徑確認 → Leader-Follower 任務規劃""" - print(f"路徑確認: {points_json}") - - selected_drones = self.get_selected_drones() + group = self._get_active_group() + if not group: + self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + return + if group.mission_type != 'LEADER_FOLLOWER': + return + + selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: - self.statusBar().showMessage("⚠ 請先選擇無人機再標記路徑", 3000) + self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) return - + + print(f"路徑確認 → Group {group.group_id}: {points_json}") 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] except (json.JSONDecodeError, IndexError): self.statusBar().showMessage("❌ 路徑座標解析失敗", 3000) @@ -754,81 +994,67 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000) return - base_alt = 10.0 - self.statusBar().showMessage(f"⏳ 正在規劃跟隨模式 ({len(selected_drones)} 台, {len(route_waypoints)} 個路徑點) ...", 2000) - + panel = self.group_panels.get(group.group_id) + 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: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) if drone_gps_positions is None: return - # 目標點 = 路徑中心（供 origin 計算） target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints) target_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints) target_gps = (target_lat, target_lon, base_alt) + params['route_waypoints'] = route_waypoints waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( - drone_gps_positions, - target_gps, - MissionType.LEADER_FOLLOWER, - params={ - 'route_waypoints': route_waypoints, - 'lateral_offset': 3.0, - 'longitudinal_spacing': 5.0, - 'altitude': base_alt - } + drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER, + params=params ) - - self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} - self.show_planned_waypoints() - + + group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.center_origin = center_origin + self.show_planned_waypoints(group) + 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( 'LEADER_FOLLOWER', drone_gps_positions, selected_drones, waypoints_per_drone, center_origin, 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) 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: - self.statusBar().showMessage(f"❌ 跟隨模式規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() # ================================================================================ - # 任務執行控制 + # 任務執行回呼 # ================================================================================ - def handle_start_mission(self, center_lat, center_lon, target_lat, target_lon): - if self.planned_waypoints is None: - self.statusBar().showMessage("⚠ 請先規劃任務", 3000) - return - self.mission_executor.start(self.planned_waypoints) - self.statusBar().showMessage("🚀 任務已啟動", 3000) - - def handle_pause_mission(self): - if self.mission_executor.state.value == "running": - self.mission_executor.pause() - self.statusBar().showMessage("⏸ 任務已暫停", 3000) - elif self.mission_executor.state.value == "paused": - self.mission_executor.resume() - self.statusBar().showMessage("▶ 任務已恢復", 3000) - def on_drone_waypoint_reached(self, drone_id, wp_index, total): if wp_index >= total: self.statusBar().showMessage(f"📍 {drone_id} 完成所有航點", 3000) else: self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000) - def on_mission_completed(self): - self.statusBar().showMessage("✅ 所有無人機已完成任務", 5000) - # ================================================================================ # 輔助方法 # ================================================================================ @@ -877,20 +1103,23 @@ class ControlStationUI(QMainWindow): subprocess.Popen([sys.executable, script_path, '--file', json_path]) print(f"驗證視窗已啟動: {json_path}") - def show_planned_waypoints(self): - if not self.planned_waypoints: return - print("\n" + "=" * 60) - print("任務規劃結果") - print("=" * 60) - drone_ids = self.planned_waypoints['drone_ids'] - waypoints = self.planned_waypoints['waypoints'] + def show_planned_waypoints(self, group=None): + pw = group.planned_waypoints if group else None + if not pw: + return + gid = group.group_id if group else "?" + print(f"\n{'=' * 60}") + print(f"任務規劃結果 — Group {gid}") + print(f"{'=' * 60}") + drone_ids = pw['drone_ids'] + waypoints = pw['waypoints'] print(f"\n共 {len(drone_ids)} 台無人機") for i, drone_id in enumerate(drone_ids): wps = waypoints[i] print(f"\n【{drone_id}】({len(wps)} 個航點)") for j, wp in enumerate(wps): print(f" WP{j}: ({wp[0]:.6f}°, {wp[1]:.6f}°, {wp[2]:.1f}m)") - print("\n" + "=" * 60) + print(f"\n{'=' * 60}") def get_selected_drones(self): 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}") 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() for receiver in self.udp_receivers: receiver.stop() diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py index 0e0a252..2be9cb9 100644 --- a/src/GUI/map_layout.py +++ b/src/GUI/map_layout.py @@ -178,35 +178,8 @@ class DroneMap:
-
- -
- - -
- - -
-
-
- - -
-
- 中心點: - 未設定 -
-
- 目標點: - 未設定 -
- - +