chiyu1468
/
AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update GUI 2.1.0: logs history tab, drone panel attitude, linear twist overview

Browse Source
wenchun
ken910606 1 week ago
parent 7f8babfa5e
commit edd15df3fc
5 changed files with 410 additions and 236 deletions
Show Stats Download Patch File Download Diff File

10
src/GUI/command_sender.py
Unescape Escape View File

@ -16,6 +16,10 @@ from PyQt6.QtCore import QObject, pyqtSignal
from pymavlink import mavutil
def _log(level, message):
print(f"[{level}] {message}")
class CommandSender(ABC):
"""指令發送抽象介面"""
@ -63,7 +67,7 @@ class MavlinkSender(CommandSender):
"""
self.connection_string = connection_string
self.mav = mavutil.mavlink_connection(connection_string)
print(f"MavlinkSender 已建立連線: {connection_string}")
_log("INFO", f"MavlinkSender 已建立連線: {connection_string}")
def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
@ -86,7 +90,7 @@ class MavlinkSender(CommandSender):
if self.mav:
self.mav.close()
self.mav = None
print("MavlinkSender 已關閉")
_log("INFO", "MavlinkSender 已關閉")
class Ros2CommandSender(QObject):
@ -162,4 +166,4 @@ class Ros2CommandSender(QObject):
if not task.done():
task.cancel()
self._pending.clear()
print("Ros2CommandSender 已關閉")
_log("INFO", "Ros2CommandSender 已關閉")

163
src/GUI/communication.py
Unescape Escape View File

@ -20,6 +20,11 @@ from mavros_msgs.msg import State, VfrHud
from nav_msgs.msg import Odometry
from mavros_msgs.srv import CommandBool, CommandTOL
def _log(level, message):
print(f"[{level}] {message}")
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if _src_path not in sys.path:
@ -30,10 +35,9 @@ try:
from fc_network_apps.longCommand import CommandLongClient
except ImportError as e:
import traceback
print(f"⚠️ 警告: 無法導入 CommandLongClient")
print(f" 错误: {e}")
print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
print(f" 完整堆栈跟踪:")
_log("WARN", "無法導入 CommandLongClient")
_log("ERROR", f"錯誤: {e}")
_log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整")
traceback.print_exc()
CommandLongClient = None
@ -42,11 +46,18 @@ try:
from fc_network_apps.navigation import PositionTargetGlobalIntClient
except ImportError as e:
import traceback
print(f"⚠️ 警告: 無法導入 PositionTargetGlobalIntClient")
print(f" 错误: {e}")
_log("WARN", "無法導入 PositionTargetGlobalIntClient")
_log("ERROR", f"錯誤: {e}")
traceback.print_exc()
PositionTargetGlobalIntClient = None
try:
from fc_interfaces.msg import AttitudeRaw
except ImportError as e:
_log("WARN", "無法導入 AttitudeRaw")
_log("ERROR", f"錯誤: {e}")
AttitudeRaw = None
class DroneSignals(QObject):
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
@ -528,25 +539,25 @@ class DroneMonitor(Node):
unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
client = CommandLongClient(node_name=unique_name)
self.command_long_clients[drone_id] = client
print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
_log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})")
# 將新 client 添加到主執行器（這樣它的回調才能被處理）
if self.executor:
self.executor.add_node(client)
print(f" ✓ 將 {drone_id} 的 client 添加到主執行器")
_log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
except TypeError:
# 舊版 CommandLongClient 不支持 node_name 參數，使用預設
client = CommandLongClient()
self.command_long_clients[drone_id] = client
print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
_log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)")
if self.executor:
self.executor.add_node(client)
print(f" ✓ 將 {drone_id} 的 client 添加到主執行器")
_log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
except Exception as e:
print(f"⚠️ 無法為 {drone_id} 創建 CommandLongClient: {e}")
_log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}")
return None
return self.command_long_clients[drone_id]
@ -561,12 +572,12 @@ class DroneMonitor(Node):
unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
client = PositionTargetGlobalIntClient(node_name=unique_name)
self.position_target_clients[drone_id] = client
print(f" ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})")
_log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})")
if self.executor:
self.executor.add_node(client)
print(f" ✓ 將 {drone_id} 的 position client 添加到主執行器")
_log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器")
except Exception as e:
print(f"⚠️ 無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}")
_log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}")
return None
return self.position_target_clients[drone_id]
@ -593,7 +604,7 @@ class DroneMonitor(Node):
if not hasattr(self, subs_attr):
self.setup_drone(sys_id)
else:
# 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱）
# 檢查既有訂閱是否包含 position_ned / attitude，如果不包含就添加（兼容舊訂閱）
subs = getattr(self, subs_attr, {})
if isinstance(subs, dict) and 'position_ned' not in subs:
base_topic = f'/fc_network/vehicle/{sys_id}'
@ -608,6 +619,19 @@ class DroneMonitor(Node):
setattr(self, subs_attr, subs) # 明確保存更新後的字典
except Exception as e:
pass
if isinstance(subs, dict) and 'attitude' not in subs and AttitudeRaw is not None:
base_topic = f'/fc_network/vehicle/{sys_id}'
try:
attitude_sub = self.create_subscription(
AttitudeRaw,
f'{base_topic}/attitude',
lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
10
)
subs['attitude'] = attitude_sub
setattr(self, subs_attr, subs)
except Exception:
pass
def setup_drone(self, sys_id):
# sys_id 格式： sys11, sys12, ...
@ -662,6 +686,14 @@ class DroneMonitor(Node):
10
)
}
if AttitudeRaw is not None:
subs['attitude'] = self.create_subscription(
AttitudeRaw,
f'{base_topic}/attitude',
lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
10
)
setattr(self, f'drone_{sys_id}_subs', subs)
@ -699,22 +731,22 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
_log("ERROR", f"[SET_MODE] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
# 獲取模式對應的 custom_mode 值
custom_mode = self.MODE_MAPPING.get(mode_name)
if custom_mode is None:
print(f"❌ [SET_MODE] 未知模式: {mode_name}")
_log("ERROR", f"[SET_MODE] 未知模式: {mode_name}")
return False
print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
_log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})")
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id)
if not client:
print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
_log("ERROR", "[SET_MODE] CommandLongClient 無法初始化")
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@ -727,13 +759,13 @@ class DroneMonitor(Node):
)
if result and result.success:
print(f"✅ [SET_MODE] 模式切換成功")
_log("INFO", f"[SET_MODE] {drone_id} 模式切換成功")
return True
else:
print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
_log("ERROR", f"[SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
return False
except Exception as e:
print(f"❌ [SET_MODE] 例外錯誤: {e}")
_log("ERROR", f"[SET_MODE] 例外錯誤: {e}")
traceback.print_exc()
return False
@ -743,17 +775,17 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
_log("ERROR", f"[ARM] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
action_name = "解鎖" if arm else "上鎖"
print(f"\n📢 [ARM] {drone_id} → {action_name}")
_log("INFO", f"[ARM] {drone_id} -> {action_name}")
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id)
if not client:
print(f"❌ [ARM] CommandLongClient 無法初始化")
_log("ERROR", "[ARM] CommandLongClient 無法初始化")
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@ -765,13 +797,13 @@ class DroneMonitor(Node):
)
if result and result.success:
print(f"✅ [ARM] {action_name}成功")
_log("INFO", f"[ARM] {drone_id} {action_name}成功")
return True
else:
print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
_log("ERROR", f"[ARM] {drone_id} {action_name}失敗 (message={result.message if result else 'None'})")
return False
except Exception as e:
print(f"❌ [ARM] 例外錯誤: {e}")
_log("ERROR", f"[ARM] 例外錯誤: {e}")
traceback.print_exc()
return False
@ -781,16 +813,16 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
_log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
_log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)")
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id)
if not client:
print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
_log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化")
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@ -802,13 +834,13 @@ class DroneMonitor(Node):
)
if result and result.success:
print(f"✅ [TAKEOFF] 起飛成功")
_log("INFO", f"[TAKEOFF] {drone_id} 起飛成功")
return True
else:
print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
_log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
return False
except Exception as e:
print(f"❌ [TAKEOFF] 例外錯誤: {e}")
_log("ERROR", f"[TAKEOFF] 例外錯誤: {e}")
traceback.print_exc()
return False
@ -840,17 +872,42 @@ class DroneMonitor(Node):
return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
# callbacks
def attitude_callback(self, drone_id, msg):
if hasattr(msg, 'orientation'):
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
self.latest_data[(drone_id, 'attitude')] = {
'roll': roll,
'pitch': pitch,
'yaw': yaw,
'rates': (msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z)
}
def attitude_callback(self, sys_id, msg):
"""處理姿態 topic，支援 AttitudeRaw 與 IMU 四元數格式。"""
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
if actual_drone_id is None:
return
try:
if hasattr(msg, 'roll') and hasattr(msg, 'pitch') and hasattr(msg, 'yaw'):
data = {
'roll': math.degrees(msg.roll),
'pitch': math.degrees(msg.pitch),
'yaw': math.degrees(msg.yaw),
'rates': (
getattr(msg, 'rollspeed', 0.0),
getattr(msg, 'pitchspeed', 0.0),
getattr(msg, 'yawspeed', 0.0),
)
}
elif hasattr(msg, 'orientation'):
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
data = {
'roll': roll,
'pitch': pitch,
'yaw': yaw,
'rates': (
msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z,
)
}
else:
return
self.latest_data[(actual_drone_id, 'attitude')] = data
except Exception as e:
print(f"Error parsing attitude msg for {sys_id}: {e}")
def battery_callback(self, sys_id, msg):
# 使用映射獲取實際的 drone_id
@ -903,7 +960,7 @@ class DroneMonitor(Node):
sys_num = sys_id.replace('sys', '')
actual_drone_id = f's{assigned_socket_id}_{sys_num}'
self.sys_to_actual_id[sys_id] = actual_drone_id
print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
_log("INFO", f"summary_callback 已建立映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
# 先發送連接類型資訊
self.signals.update_signal.emit('connection_type', actual_drone_id, {
@ -1003,6 +1060,9 @@ class DroneMonitor(Node):
x = msg.pose.pose.position.y # NED 座標系中交換 x/y（與 local_pose 對齐）
y = msg.pose.pose.position.x
z = -msg.pose.pose.position.z # 將向下的 NED z 轉換為向上的高度（z 為負表示向下）
vx = msg.twist.twist.linear.y
vy = msg.twist.twist.linear.x
vz = -msg.twist.twist.linear.z
# 儲存高度信息
self.latest_data[(actual_drone_id, 'altitude')] = {
@ -1015,6 +1075,13 @@ class DroneMonitor(Node):
'y': y,
'z': z
}
# 儲存速度資訊，供總覽頁「XY速度」欄位顯示
self.latest_data[(actual_drone_id, 'velocity')] = {
'vx': vx,
'vy': vy,
'vz': vz
}
# 發送信號給 GUI 更新高度顯示
self.signals.update_signal.emit('altitude', actual_drone_id, {
@ -1039,5 +1106,5 @@ class DroneMonitor(Node):
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
receiver.start()
self.serial_receivers.append(receiver)
print(f"Started serial connection on {port} at {baudrate} baud")
return receiver
_log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud")
return receiver

376
src/GUI/gui.py
Unescape Escape View File

@ -4,8 +4,8 @@ from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout
QWidget, QLabel, QSplitter, QScrollArea,
QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem,
QHeaderView, QPushButton, QCheckBox, QLineEdit,
QComboBox, QDialog)
from PyQt6.QtCore import Qt, QTimer
QComboBox, QDialog, QPlainTextEdit)
from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal
from PyQt6.QtGui import QColor
import sys
import asyncio
@ -14,6 +14,11 @@ import subprocess
import time
import traceback
def _log(level, message):
print(f"[{level}] {message}", flush=True)
# 導入分離的類別
from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
from map_layout import DroneMap
@ -32,8 +37,41 @@ from mission_group import (
)
# ================================================================================
class StreamRedirector(QObject):
"""將 stdout/stderr 同步轉發到 Qt UI。"""
text_written = pyqtSignal(str)
def __init__(self, original_stream=None, parent=None):
super().__init__(parent)
self.original_stream = original_stream
self._buffer = ""
def write(self, text):
if self.original_stream:
self.original_stream.write(text)
if not text:
return
self._buffer += text
while "\n" in self._buffer:
line, self._buffer = self._buffer.split("\n", 1)
line = line.strip()
if line:
self.text_written.emit(line)
def flush(self):
if self.original_stream:
self.original_stream.flush()
line = self._buffer.strip()
if line:
self.text_written.emit(line)
self._buffer = ""
class ControlStationUI(QMainWindow):
VERSION = '2.0.8'
VERSION = '2.1.0'
def __init__(self):
super().__init__()
@ -78,6 +116,8 @@ class ControlStationUI(QMainWindow):
# 快取消息數據，以便在沒有新消息時使用上一次的值
self._message_cache = {}
self.message_history = []
self.max_message_history = 500
# 初始化UI
self.drones = {}
@ -137,6 +177,7 @@ class ControlStationUI(QMainWindow):
# ================================================================================
self.init_ui()
self._setup_stream_redirector()
def init_ui(self):
main_splitter = QSplitter(Qt.Orientation.Horizontal)
@ -160,7 +201,11 @@ class ControlStationUI(QMainWindow):
self.overview_table = OverviewTable()
self.left_tab.addTab(self.overview_table, "總覽")
# — 分頁 3：通訊設定
# — 分頁 3：訊息歷史
self.message_history_tab = self._create_message_history_tab()
self.left_tab.addTab(self.message_history_tab, "紀錄")
# — 分頁 4：通訊設定
self.comm_panel = CommPanel()
self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added)
self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added)
@ -259,6 +304,95 @@ class ControlStationUI(QMainWindow):
main_splitter.setSizes([400, 1000])
self.setCentralWidget(main_splitter)
self.statusBar().messageChanged.connect(self._on_status_bar_message_changed)
def _create_message_history_tab(self):
"""建立左側訊息歷史分頁。"""
widget = QWidget()
layout = QVBoxLayout(widget)
layout.setContentsMargins(10, 10, 10, 10)
layout.setSpacing(8)
header_layout = QHBoxLayout()
title = QLabel("操作與訊息歷史")
title.setStyleSheet("color: #DDD; font-size: 14px; font-weight: bold;")
header_layout.addWidget(title)
header_layout.addStretch()
clear_btn = QPushButton("清空")
clear_btn.setStyleSheet("""
QPushButton { background-color: #555; color: white; border: none;
padding: 5px 10px; border-radius: 4px; font-size: 12px; }
QPushButton:hover { background-color: #666; }
""")
clear_btn.clicked.connect(self._clear_message_history)
header_layout.addWidget(clear_btn)
layout.addLayout(header_layout)
self.message_history_view = QPlainTextEdit()
self.message_history_view.setReadOnly(True)
self.message_history_view.setStyleSheet("""
QPlainTextEdit {
background-color: #1E1E1E;
color: #DDD;
border: 1px solid #444;
border-radius: 6px;
padding: 6px;
font-family: monospace;
font-size: 12px;
}
""")
self.message_history_view.setPlaceholderText("左下角狀態訊息會顯示在這裡...")
layout.addWidget(self.message_history_view)
return widget
def _clear_message_history(self):
"""清空訊息歷史。"""
self.message_history.clear()
if hasattr(self, 'message_history_view'):
self.message_history_view.clear()
def _on_status_bar_message_changed(self, message):
"""同步狀態列訊息到歷史紀錄。"""
if not message:
return
timestamp = time.strftime("%H:%M:%S")
entry = f"[{timestamp}] {message}"
self.message_history.append(entry)
if len(self.message_history) > self.max_message_history:
self.message_history = self.message_history[-self.max_message_history:]
if hasattr(self, 'message_history_view'):
self.message_history_view.setPlainText("\n".join(self.message_history))
scrollbar = self.message_history_view.verticalScrollBar()
scrollbar.setValue(scrollbar.maximum())
def _setup_stream_redirector(self):
"""將 stdout/stderr 同步到左下角狀態列與訊息紀錄。"""
self._original_stdout = sys.stdout
self._original_stderr = sys.stderr
self.stdout_redirector = StreamRedirector(self._original_stdout, self)
self.stderr_redirector = StreamRedirector(self._original_stderr, self)
self.stdout_redirector.text_written.connect(self.show_in_bottom_left)
self.stderr_redirector.text_written.connect(self.show_in_bottom_left)
sys.stdout = self.stdout_redirector
sys.stderr = self.stderr_redirector
def _restore_stream_redirector(self):
"""還原 stdout/stderr，避免關閉視窗後仍寫入 UI。"""
if getattr(self, '_original_stdout', None) is not None:
sys.stdout = self._original_stdout
if getattr(self, '_original_stderr', None) is not None:
sys.stderr = self._original_stderr
def show_in_bottom_left(self, text):
"""將重導向的輸出顯示在左下角狀態列。"""
if not text:
return
self.statusBar().showMessage(text, 5000)
# ================================================================================
@ -384,7 +518,7 @@ class ControlStationUI(QMainWindow):
status_label.setToolTip("運行中")
self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000)
except Exception as e:
print(f"❌ Serial 連接啟動失敗: {str(e)}")
_log("ERROR", f"Serial 連接啟動失敗: {str(e)}")
traceback.print_exc()
self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000)
@ -410,39 +544,25 @@ class ControlStationUI(QMainWindow):
# ================================================================================
def handle_mode_change(self, drone_id):
print(f"\n📢 [GUI] handle_mode_change 被调用")
print(f" drone_id: {drone_id}")
# 從 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"
print(f" 从群组读取模式: {mode}")
else:
mode = "GUIDED"
print(f" 没有活跃群组，使用默认模式: {mode}")
_log("INFO", f"切換模式請求: drone={drone_id}, mode={mode}")
loop = asyncio.get_event_loop()
future = self.monitor.set_mode(drone_id, mode)
loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}"))
def handle_arm(self, drone_id):
print(f"\n📢 [GUI] handle_arm 被調用")
print(f" drone_id: {drone_id}")
loop = asyncio.get_event_loop()
print(f" 獲得事件循環: {loop}")
arm_state = not self.monitor.get_arm_state(drone_id)
print(f" 當前鎖定狀態: {not arm_state}, 目標狀態: {arm_state}")
print(f" 準備調用 arm_drone(drone_id={drone_id}, arm={arm_state})")
coro = self.monitor.arm_drone(drone_id, arm_state)
print(f" arm_drone() 返回類型: {type(coro)}")
print(f" coroutine 對象: {coro}")
action_text = f"{'解鎖' if arm_state else '上鎖'} {drone_id}"
print(f" 準備提交到事件循環: {action_text}")
task = loop.create_task(self.handle_service_response(coro, action_text))
print(f" task 已創建: {task}")
print(f" 已提交到事件循環\n")
_log("INFO", f"{action_text} 請求已送出")
loop.create_task(self.handle_service_response(coro, action_text))
def handle_takeoff(self, drone_id):
loop = asyncio.get_event_loop()
@ -453,7 +573,7 @@ class ControlStationUI(QMainWindow):
"""發送位置命令到 active group 的所有選中無人機"""
group = self._get_active_group()
if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
self.statusBar().showMessage("請先建立任務群組", 3000)
return
try:
@ -481,34 +601,18 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage("座標格式錯誤", 3000)
async def handle_service_response(self, future, action):
print(f"\n📥 [GUI] handle_service_response 開始處理: {action}")
print(f" Future/Coroutine 類型: {type(future)}")
print(f" Future/Coroutine 對象: {future}")
try:
print(f" ├─ 等待 future/coroutine 完成...")
print(f" └─ (這是一個阻塞等待，直到服務返回)\n")
result = await future
print(f"\n ✓ Future/Coroutine 完成，接收到返回值！")
print(f" ├─ 返回值類型: {type(result)}")
print(f" ├─ 返回值內容: {result}")
print(f" ├─ 返回值 == True: {result == True}")
print(f" ├─ 返回值 is True: {result is True}")
print(f" └─ bool(返回值): {bool(result)}")
# 詳細檢查 result 結構（用於調試 fc_network 回傳值）
if hasattr(result, '__dict__'):
print(f" └─ 返回值屬性: {result.__dict__}")
if result:
print(f"\n✅ {action} 成功 (result={result})")
_log("INFO", f"{action} 成功")
self.statusBar().showMessage(f"{action} 成功", 3000)
else:
print(f"\n❌ {action} 失敗 (result={result})")
_log("WARN", f"{action} 失敗")
self.statusBar().showMessage(f"{action} 失敗", 3000)
except asyncio.CancelledError:
print(f"⚠️ {action} 被取消")
_log("WARN", f"{action} 已取消")
except Exception as e:
print(f"❌ {action} 錯誤: {str(e)}")
_log("ERROR", f"{action} 錯誤: {str(e)}")
traceback.print_exc()
self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
@ -516,38 +620,33 @@ class ControlStationUI(QMainWindow):
"""解鎖 active group 的所有選中無人機"""
group = self._get_active_group()
if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
self.statusBar().showMessage("請先建立任務群組", 3000)
return
selected = list(group.selected_drone_ids)
if not selected:
self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000)
self.statusBar().showMessage("尚未選擇任何無人機", 3000)
return
print(f"\n📢 [GUI] handle_arm_selected 被調用")
print(f" 已選擇的無人機: {selected}")
_log("INFO", f"批次解鎖請求: {', '.join(selected)}")
loop = asyncio.get_event_loop()
for drone_id in selected:
print(f" 準備批次解鎖: {drone_id}")
coro = self.monitor.arm_drone(drone_id, True)
print(f" arm_drone 返回: {coro}")
# 使用 run_coroutine_threadsafe() 正確地在事件循環中運行
asyncio.run_coroutine_threadsafe(
self.handle_service_response(coro, f"批次解鎖 {drone_id}"),
loop
)
print(f" handle_arm_selected 完成\n")
def handle_takeoff_selected(self):
"""起飛 active group 的所有選中無人機"""
group = self._get_active_group()
if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
self.statusBar().showMessage("請先建立任務群組", 3000)
return
selected = list(group.selected_drone_ids)
if not selected:
self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000)
self.statusBar().showMessage("尚未選擇任何無人機", 3000)
return
loop = asyncio.get_event_loop()
@ -774,7 +873,7 @@ class ControlStationUI(QMainWindow):
if blocked:
self.statusBar().showMessage(
f"⚠ Group {group_id}: {len(blocked)} 台已被其他 group 勾選，未加入", 4000
f"Group {group_id}: {len(blocked)} 台已被其他 group 勾選，未加入", 4000
)
else:
self.statusBar().showMessage(
@ -811,7 +910,7 @@ class ControlStationUI(QMainWindow):
if not group:
return
if group.planned_waypoints is None:
self.statusBar().showMessage(f"⚠ Group {group_id}: 請先規劃任務", 3000)
self.statusBar().showMessage(f"Group {group_id}: 請先規劃任務", 3000)
return
if group.executor is None:
self._create_executor_for_group(group)
@ -819,7 +918,7 @@ class ControlStationUI(QMainWindow):
panel = self.group_panels.get(group_id)
if panel:
panel.update_status()
self.statusBar().showMessage(f"🚀 Group {group_id}: 任務已啟動", 3000)
self.statusBar().showMessage(f"Group {group_id}: 任務已啟動", 3000)
def _handle_group_pause(self, group_id):
"""暫停/恢復群組任務"""
@ -828,10 +927,10 @@ class ControlStationUI(QMainWindow):
return
if group.executor.state == MissionState.RUNNING:
group.executor.pause()
self.statusBar().showMessage(f"⏸ Group {group_id}: 任務已暫停", 3000)
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)
self.statusBar().showMessage(f"Group {group_id}: 任務已恢復", 3000)
panel = self.group_panels.get(group_id)
if panel:
panel.update_status()
@ -849,54 +948,44 @@ class ControlStationUI(QMainWindow):
if panel:
panel.update_status()
panel.clear_mission_info()
self.statusBar().showMessage(f"■ Group {group_id}: 任務已停止", 3000)
self.statusBar().showMessage(f"Group {group_id}: 任務已停止", 3000)
def _handle_group_mode_change(self, group_id, mode):
"""切換群組內所有無人機的飛行模式"""
print(f"\n📢 [GUI] _handle_group_mode_change 被调用", flush=True)
print(f" group_id: {group_id}, mode: {mode}", flush=True)
group = self.mission_groups.get(group_id)
if not group:
print(f"❌ 找不到群組: {group_id}", flush=True)
_log("ERROR", f"找不到群組: {group_id}")
return
if not group.selected_drone_ids:
print(f"⚠️ 群組中沒有無人機", flush=True)
_log("WARN", f"Group {group_id} 中沒有無人機")
self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
return
print(f" 準備為 {len(group.selected_drone_ids)} 台無人機切換模式...", flush=True)
self.statusBar().showMessage(f"正在切換模式...", 1000)
_log("INFO", f"Group {group_id} 準備切換模式為 {mode}")
self.statusBar().showMessage("正在切換模式", 1000)
# 使用 asyncio 執行（通過事件循環）
async def do_mode_changes_async():
print(f"\n 【異步任務】開始執行模式切換", flush=True)
for drone_id in group.selected_drone_ids:
print(f"\n 【切換】{drone_id} → {mode}", flush=True)
try:
result = await self.monitor.set_mode(drone_id, mode)
if result:
msg = f"✅ {drone_id} 切換成功"
print(f" {msg}", flush=True)
msg = f"{drone_id} 切換成功"
_log("INFO", msg)
self.message_queue.put((msg, 2000))
else:
msg = f"❌ {drone_id} 切換失敗"
print(f" {msg}", flush=True)
msg = f"{drone_id} 切換失敗"
_log("WARN", msg)
self.message_queue.put((msg, 2000))
except Exception as e:
msg = f"❌ {drone_id} 錯誤: {str(e)}"
print(f" {msg}", flush=True)
msg = f"{drone_id} 錯誤: {str(e)}"
_log("ERROR", msg)
traceback.print_exc()
self.message_queue.put((msg, 2000))
print(f"\n 【完成】模式切換任務結束\n", flush=True)
# 通過事件循環提交異步任務
print(f" 【排隊】將任務提交至事件循環", flush=True)
loop = asyncio.get_event_loop()
asyncio.run_coroutine_threadsafe(
do_mode_changes_async(),
@ -905,34 +994,22 @@ class ControlStationUI(QMainWindow):
def _handle_group_arm(self, group_id):
"""解鎖群組內所有無人機"""
print(f"\n📢 [GUI] _handle_group_arm 被調用")
print(f" 群組 ID: {group_id}")
group = self.mission_groups.get(group_id)
print(f" 群組存在: {group is not None}")
if not group:
print(f" ⚠️ 群組不存在，返回\n")
_log("WARN", f"找不到群組: {group_id}")
return
print(f" 群組內無人機: {group.selected_drone_ids}")
loop = asyncio.get_event_loop()
print(f" 事件循環: {loop}")
_log("INFO", f"Group {group_id} 批次解鎖 {len(group.selected_drone_ids)} 台無人機")
for drone_id in group.selected_drone_ids:
print(f"\n ┌─ 處理無人機: {drone_id}")
print(f" ├─ 準備調用 arm_drone(drone_id={drone_id}, arm=True)")
coro = self.monitor.arm_drone(drone_id, True)
print(f" ├─ arm_drone 返回類型: {type(coro)}")
action_text = f"解鎖 {drone_id}"
print(f" ├─ 準備提交到事件循環: {action_text}")
# 使用 run_coroutine_threadsafe() 正確地在事件循環中運行 coroutine
# 這是 Qt + asyncio 整合的正確方式
future = asyncio.run_coroutine_threadsafe(
asyncio.run_coroutine_threadsafe(
self.handle_service_response(coro, action_text),
loop
)
print(f" ├─ Future 已創建: {future}")
print(f" └─ Future 將在事件循環中執行")
print(f"\n _handle_group_arm 完成（coroutine 已提交至事件循環）\n")
def _handle_group_takeoff(self, group_id, altitude):
"""群組內所有無人機起飛"""
@ -983,7 +1060,7 @@ class ControlStationUI(QMainWindow):
if blocked:
self.statusBar().showMessage(
f"⚠ Group {group_id}: 部分 drone 已被其他 group 勾選，僅加入 {len(allowed)} 台", 4000
f"Group {group_id}: 部分 drone 已被其他 group 勾選，僅加入 {len(allowed)} 台", 4000
)
else:
self.statusBar().showMessage(
@ -1049,7 +1126,7 @@ class ControlStationUI(QMainWindow):
"""刪除一個任務群組"""
# 檢查是否只有一個群組，如果是就禁止刪除
if len(self.mission_groups) <= 1:
self.statusBar().showMessage("⚠️ 至少需要保留一個群組！", 3000)
self.statusBar().showMessage("至少需要保留一個群組", 3000)
return
if group_id not in self.mission_groups:
@ -1100,7 +1177,7 @@ class ControlStationUI(QMainWindow):
panel = self.group_panels.get(group_id)
if panel:
panel.update_status()
self.statusBar().showMessage(f"✅ Group {group_id}: 所有無人機已完成任務", 5000)
self.statusBar().showMessage(f"Group {group_id}: 所有無人機已完成任務", 5000)
# ================================================================================
# UI 更新
@ -1152,7 +1229,7 @@ class ControlStationUI(QMainWindow):
if not selectable:
if blocked:
self.statusBar().showMessage(
f"⚠ Socket {socket_id} 的 drone 已被其他 group 勾選，無法操作", 4000
f"Socket {socket_id} 的 drone 已被其他 group 勾選，無法操作", 4000
)
self.refresh_selection_ui()
return
@ -1165,7 +1242,7 @@ class ControlStationUI(QMainWindow):
group.selected_drone_ids.update(selectable)
if blocked:
self.statusBar().showMessage(
f"⚠ 以下 drone 已被其他 group 勾選，略過: {', '.join(blocked)}", 4000
f"以下 drone 已被其他 group 勾選，略過: {', '.join(blocked)}", 4000
)
self.refresh_selection_ui()
@ -1217,7 +1294,7 @@ class ControlStationUI(QMainWindow):
if is_checked:
if self._is_drone_selected_by_other_group(drone_id, group.group_id):
self.statusBar().showMessage(
f"⚠ {drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
f"{drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
)
self.refresh_selection_ui()
return
@ -1239,7 +1316,7 @@ class ControlStationUI(QMainWindow):
# 嘗試勾選前先檢查是否被其他 group 使用
if self._is_drone_selected_by_other_group(drone_id, group.group_id):
self.statusBar().showMessage(
f"⚠ {drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
f"{drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
)
self.refresh_selection_ui()
return
@ -1285,7 +1362,7 @@ class ControlStationUI(QMainWindow):
"""處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
group = self._get_active_group()
if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
self.statusBar().showMessage("請先建立任務群組", 3000)
return
mode_map = {
@ -1298,16 +1375,16 @@ class ControlStationUI(QMainWindow):
selected_drones = self._get_group_drones(group)
if len(selected_drones) == 0:
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000)
return
print(f"地圖點擊: {lat:.6f}, {lon:.6f} → Group {group.group_id} ({group.mission_type})")
_log("INFO", 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"⏳ Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台) ...", 2000)
f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000)
try:
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
@ -1344,10 +1421,10 @@ class ControlStationUI(QMainWindow):
total_wps = sum(len(wps) for wps in waypoints_per_drone)
self.statusBar().showMessage(
f"✓ Group {group.group_id}: {group.mission_type} 規劃完成！{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"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
self.statusBar().showMessage(f"Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
traceback.print_exc()
# ================================================================================
@ -1357,28 +1434,28 @@ class ControlStationUI(QMainWindow):
def handle_rectangle_selected(self, points_json):
group = self._get_active_group()
if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
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(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000)
return
print(f"矩形選取 → Group {group.group_id}: {points_json}")
_log("INFO", 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)
self.statusBar().showMessage("矩形座標解析失敗", 3000)
return
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)
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:
@ -1419,10 +1496,10 @@ class ControlStationUI(QMainWindow):
total_wps = sum(len(wps) for wps in waypoints_per_drone)
self.statusBar().showMessage(
f"✓ Group {group.group_id}: 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"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
self.statusBar().showMessage(f"Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
traceback.print_exc()
# ================================================================================
@ -1432,14 +1509,14 @@ class ControlStationUI(QMainWindow):
def handle_route_confirmed(self, points_json):
group = self._get_active_group()
if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
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(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000)
return
# LEADER_FOLLOWER：把指定的領隊放到 rank 0
@ -1447,23 +1524,23 @@ class ControlStationUI(QMainWindow):
if leader and leader in selected_drones:
selected_drones = [leader] + [d for d in selected_drones if d != leader]
print(f"路徑確認 → Group {group.group_id}: {points_json}")
_log("INFO", f"路徑確認 -> Group {group.group_id}: {points_json}")
try:
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)
self.statusBar().showMessage("路徑座標解析失敗", 3000)
return
if len(route_waypoints) < 2:
self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000)
self.statusBar().showMessage("至少需要 2 個路徑點", 3000)
return
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)
f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000)
try:
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
@ -1507,10 +1584,10 @@ class ControlStationUI(QMainWindow):
total_wps = sum(len(wps) for wps in waypoints_per_drone)
self.statusBar().showMessage(
f"✓ Group {group.group_id}: 跟隨模式規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
f"Group {group.group_id}: 跟隨模式規劃完成，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
)
except Exception as e:
self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
self.statusBar().showMessage(f"Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
traceback.print_exc()
# ================================================================================
@ -1519,20 +1596,20 @@ class ControlStationUI(QMainWindow):
def on_drone_waypoint_reached(self, drone_id, wp_index, total):
if wp_index >= total:
self.statusBar().showMessage(f"📍 {drone_id} 完成所有航點", 3000)
self.statusBar().showMessage(f"{drone_id} 已完成所有航點", 3000)
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_task_status_changed(self, drone_id, status, message):
"""MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar"""
if status == "retrying":
self.statusBar().showMessage(f"⚠ {drone_id} {message}", 4000)
self.statusBar().showMessage(f"{drone_id} {message}", 4000)
elif status == "fallback_loiter":
self.statusBar().showMessage(f"❌ {drone_id} {message}", 8000)
self.statusBar().showMessage(f"{drone_id} {message}", 8000)
elif status == "waiting_at_barrier":
self.statusBar().showMessage(f"⏸ {drone_id} {message}", 3000)
self.statusBar().showMessage(f"{drone_id} {message}", 3000)
elif status == "normal":
self.statusBar().showMessage(f"▶ {drone_id} {message or '已恢復'}", 2000)
self.statusBar().showMessage(f"{drone_id} {message or '已恢復'}", 2000)
# ================================================================================
# 輔助方法
@ -1551,7 +1628,7 @@ class ControlStationUI(QMainWindow):
alt_drone = pos[2] if len(pos) > 2 else base_alt
drone_gps_positions.append((lat_drone, lon_drone, alt_drone))
else:
self.statusBar().showMessage(f"⚠ 找不到 {drone_id} 的位置資料", 3000)
self.statusBar().showMessage(f"找不到 {drone_id} 的位置資料", 3000)
return None
return drone_gps_positions
@ -1580,7 +1657,7 @@ class ControlStationUI(QMainWindow):
script_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'validation', 'verify_waypoints.py')
subprocess.Popen([sys.executable, script_path, '--file', json_path])
print(f"驗證視窗已啟動: {json_path}")
_log("INFO", f"驗證視窗已啟動: {json_path}")
def show_planned_waypoints(self, group=None):
pw = group.planned_waypoints if group else None
@ -1739,7 +1816,7 @@ class ControlStationUI(QMainWindow):
panel._last_roll = roll
panel._last_pitch = pitch
if hasattr(panel, 'update_attitude'):
heading = self.drone_headings.get(drone_id, 0)
heading = self.drone_headings.get(drone_id, yaw)
panel.update_attitude(heading, roll, pitch)
elif msg_type == 'gps':
@ -1780,7 +1857,7 @@ class ControlStationUI(QMainWindow):
elapsed = (time.time() - start_time) * 1000
if elapsed > 33:
print(f"[WARNING] UI update took {elapsed:.1f}ms (target: <33ms)")
_log("WARN", f"UI 更新耗時 {elapsed:.1f}ms (目標 <33ms)")
finally:
# ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪）
@ -1800,7 +1877,7 @@ class ControlStationUI(QMainWindow):
except:
break
except Exception as e:
print(f"❌ 消息隊列處理錯誤: {e}", flush=True)
_log("ERROR", f"訊息佇列處理錯誤: {e}")
traceback.print_exc()
def _spin_asyncio(self):
@ -1831,15 +1908,16 @@ class ControlStationUI(QMainWindow):
except RuntimeError as e:
# ROS2 context 被破坏或不可用
if "Context" in str(e) or "context" in str(e).lower():
print(f"⚠️ ROS2 context 錯誤（忽略）: {e}", flush=True)
_log("WARN", f"ROS2 context 錯誤（已忽略）: {e}")
else:
print(f"ROS spin error: {e}", flush=True)
_log("ERROR", f"ROS spin error: {e}")
traceback.print_exc()
except Exception as e:
print(f"ROS spin error: {e}", flush=True)
_log("ERROR", f"ROS spin error: {e}")
traceback.print_exc()
def closeEvent(self, event):
self._restore_stream_redirector()
try:
for group in self.mission_groups.values():
if group.executor:
@ -1867,7 +1945,7 @@ class ControlStationUI(QMainWindow):
self.monitor.destroy_node()
self.executor.shutdown()
except Exception as e:
print(f"⚠️ 清理資源時出錯: {e}", flush=True)
_log("WARN", f"清理資源時發生錯誤: {e}")
traceback.print_exc()
# 安全地 shutdown ROS2
@ -1875,7 +1953,7 @@ class ControlStationUI(QMainWindow):
if rclpy.ok():
rclpy.shutdown()
except RuntimeError as e:
print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True)
_log("WARN", f"ROS2 shutdown 錯誤: {e}")
traceback.print_exc()
event.accept()
@ -1894,36 +1972,36 @@ def main():
"""
# 第一步：在最外層只初始化一次 ROS2（終極防護）
# 添加 rclpy.ok() 檢查，防止重複初始化導致 "Context.init() must only be called once" 錯誤
print("🚀 [GUI 主程式] 檢查 ROS2 初始化狀態...", flush=True)
_log("INFO", "GUI 主程式檢查 ROS2 初始化狀態")
if not rclpy.ok():
print("🚀 [GUI 主程式] ROS2 未初始化，開始初始化...", flush=True)
_log("INFO", "ROS2 尚未初始化，開始初始化")
rclpy.init()
print("✅ [GUI 主程式] ROS2 已全局初始化（由 GUI 主程式霸佔）", flush=True)
_log("INFO", "ROS2 已完成全域初始化")
else:
print("ℹ️ [GUI 主程式] ROS2 已初始化，跳過重複初始化", flush=True)
_log("INFO", "ROS2 已初始化，略過重複初始化")
try:
# 第二步：啟動 Qt 應用程式
print("🚀 [GUI 主程式] 啟動 Qt 應用程式...", flush=True)
_log("INFO", "啟動 Qt 應用程式")
app = QApplication(sys.argv)
station = ControlStationUI()
station.show()
print("✓ [GUI 主程式] 應用程式已啟動", flush=True)
_log("INFO", "應用程式已啟動")
# 第三步：進入 Qt 事件循環（阻塞直到用戶關閉應用）
print("🎯 [GUI 主程式] 進入主事件循環，等待用戶操作...", flush=True)
_log("INFO", "進入主事件循環")
sys.exit(app.exec())
finally:
# 第四步：只有當 GUI 視窗被關閉時，才做 ROS2 cleanup（終極防護）
# 這裡確保 ROS2 被正確且安全地關閉
print("\n🛑 [GUI 主程式] 應用程式關閉，正在清理 ROS2 資源...", flush=True)
_log("INFO", "應用程式關閉，開始清理 ROS2 資源")
if rclpy.ok():
rclpy.shutdown()
print("✓ [GUI 主程式] ROS2 已安全關閉", flush=True)
_log("INFO", "ROS2 已安全關閉")
else:
print("ℹ️ [GUI 主程式] ROS2 已關閉或不可用，無需重複 shutdown", flush=True)
print("✓ [GUI 主程式] 應用程式完全退出", flush=True)
_log("INFO", "ROS2 已關閉或不可用，無需重複 shutdown")
_log("INFO", "應用程式已完全退出")
if __name__ == '__main__':

41
src/GUI/map_layout.py
Unescape Escape View File

@ -3,6 +3,11 @@ from PyQt6.QtWebEngineWidgets import QWebEngineView
from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
from PyQt6.QtWebChannel import QWebChannel
def _log(level, message):
print(f"[{level}] {message}")
class DroneMap:
"""無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
@ -874,7 +879,7 @@ class DroneMap:
if ok:
self.map_loaded = True
else:
print("⚠️ 地圖加載失敗")
_log("ERROR", "地圖載入失敗")
def update_drone_position(self, drone_id, lat, lon, heading):
"""更新無人機位置（加入待處理隊列）"""
@ -924,20 +929,24 @@ class DroneMap:
f"{target_lat:.6f}, {target_lon:.6f});"
)
self.map_view.page().runJavaScript(js_code)
print(f"📍 地圖已繪製 Group {group_id} 任務規劃: "
f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
_log(
"INFO",
f"地圖已繪製 Group {group_id} 任務規劃: "
f"C({center_lat:.6f}, {center_lon:.6f}) -> "
f"T({target_lat:.6f}, {target_lon:.6f})",
)
def clear_mission_plan(self):
"""清除地圖上所有任務規劃標記"""
if self.map_loaded:
self.map_view.page().runJavaScript("clearAllMissionPlans();")
print("🗑️ 地圖已清除所有任務規劃")
_log("INFO", "地圖已清除所有任務規劃")
def clear_mission_plan_for_group(self, group_id):
"""清除指定群組的任務規劃標記"""
if self.map_loaded:
self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
print(f"🗑️ 地圖已清除 Group {group_id} 任務規劃")
_log("INFO", f"地圖已清除 Group {group_id} 任務規劃")
def set_mission_mode(self, mode):
"""從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
@ -1033,52 +1042,56 @@ class MapBridge(QObject):
def clearAllDroneSelection(self):
"""供 JavaScript 調用的方法 - 清除所有無人機選擇"""
self.clear_all_drone_selection.emit()
print("🗑️ 清除所有無人機選擇")
_log("INFO", "已清除所有無人機選擇")
@pyqtSlot()
def toggleSelectAllDrones(self):
"""供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機"""
self.select_all_drones.emit()
print("🔄 切換全選無人機")
_log("INFO", "已切換全選無人機")
@pyqtSlot(float, float, float, float)
def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon):
"""供 JavaScript 調用的方法 - 開始任務"""
self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon)
print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})")
_log(
"INFO",
f"已發出開始任務信號: "
f"C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})",
)
@pyqtSlot()
def pauseMissionSignal(self):
"""供 JavaScript 調用的方法 - 暫停任務"""
self.pause_mission_signal.emit()
print("⏸️ 暫停任務信號已發出")
_log("INFO", "已發出暫停任務信號")
@pyqtSlot(str)
def rectangleSelected(self, points_json):
"""供 JavaScript 調用的方法 - 矩形選擇完成"""
self.rectangle_selected.emit(points_json)
print(f"📦 矩形區域已選擇: {points_json}")
_log("INFO", f"矩形區域已選擇: {points_json}")
@pyqtSlot(str)
def polygonSelected(self, points_json):
"""供 JavaScript 調用的方法 - 多邊形選擇完成"""
self.polygon_selected.emit(points_json)
print(f"🔷 多邊形區域已選擇: {points_json}")
_log("INFO", f"多邊形區域已選擇: {points_json}")
@pyqtSlot(str)
def missionModeChanged(self, mode):
"""供 JavaScript 調用的方法 - 任務模式切換"""
self.mission_mode_changed.emit(mode)
print(f"🔄 任務模式切換: {mode}")
_log("INFO", f"任務模式已切換: {mode}")
@pyqtSlot(str)
def routeConfirmed(self, points_json):
"""供 JavaScript 調用的方法 - 路徑確認"""
self.route_confirmed.emit(points_json)
print(f"📍 路徑已確認: {points_json}")
_log("INFO", 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}")
_log("INFO", f"框選無人機完成: {drone_ids_json}")

56
src/GUI/mission_executor.py
Unescape Escape View File

@ -19,6 +19,10 @@ from enum import Enum
from PyQt6.QtCore import QObject, QTimer, pyqtSignal
def _log(level, message):
print(f"[{level}] {message}")
class MissionState(Enum):
"""整體任務狀態"""
IDLE = "idle"
@ -109,7 +113,7 @@ class MissionExecutor(QObject):
def start(self, planned_waypoints):
if self.state == MissionState.RUNNING:
print("任務已在執行中")
_log("WARN", "任務已在執行中")
return
self.tasks.clear()
@ -132,31 +136,34 @@ class MissionExecutor(QObject):
f"rendezvous WP={sorted(self.rendezvous_indices)}"
if self.rendezvous_indices else "無 rendezvous (各自跑)"
)
print(f"任務啟動: {len(self.tasks)} 架無人機, "
f"共 {total_wps} 個航點, "
f"到達半徑={self.arrival_radius}m, "
f"tick 週期={self._interval_ms}ms, "
f"barrier timeout={self.barrier_timeout_sec}s, "
f"{rv_info}")
_log(
"INFO",
f"任務已啟動: {len(self.tasks)} 架無人機, "
f"共 {total_wps} 個航點, "
f"到達半徑={self.arrival_radius}m, "
f"tick 週期={self._interval_ms}ms, "
f"barrier timeout={self.barrier_timeout_sec}s, "
f"{rv_info}",
)
def pause(self):
if self.state == MissionState.RUNNING:
self._timer.stop()
self.state = MissionState.PAUSED
print("任務暫停")
_log("INFO", "任務已暫停")
def resume(self):
if self.state == MissionState.PAUSED:
self._timer.start(self._interval_ms)
self.state = MissionState.RUNNING
print("任務恢復")
_log("INFO", "任務已恢復")
def stop(self):
self._timer.stop()
self.tasks.clear()
self.rendezvous_indices = set()
self.state = MissionState.IDLE
print("任務停止")
_log("INFO", "任務已停止")
# ------------------------------------------------------------------ 控制迴圈
@ -197,7 +204,7 @@ class MissionExecutor(QObject):
# rendezvous 點 → 不推進，進入 barrier 等待
task.status = TaskStatus.WAITING_AT_BARRIER
task.waiting_since = now
print(f" {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
_log("INFO", f"{task.drone_id} 抵達 barrier WP {task.wp_index}，等待同伴")
self.task_status_changed.emit(
task.drone_id, task.status.value,
f"waiting at WP {task.wp_index}"
@ -236,7 +243,7 @@ class MissionExecutor(QObject):
self._timer.stop()
self.state = MissionState.IDLE
self.mission_completed.emit()
print("===== 任務全部完成 =====")
_log("INFO", "任務全部完成")
def _advance_waypoint(self, task, arrived_distance):
"""把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
@ -248,14 +255,20 @@ class MissionExecutor(QObject):
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
print(f" {task.drone_id} → DONE "
f"({task.total_waypoints}/{task.total_waypoints})")
_log(
"INFO",
f"{task.drone_id} 已完成所有航點 "
f"({task.total_waypoints}/{task.total_waypoints})",
)
return
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
f"(到達距離: {arrived_distance:.1f}m)")
_log(
"INFO",
f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} "
f"(到達距離: {arrived_distance:.1f}m)",
)
def _check_barriers(self, now):
"""檢查每個有人在等的 barrier 能不能釋放。"""
@ -289,14 +302,13 @@ class MissionExecutor(QObject):
oldest_wait = min(t.waiting_since for t in waiting_tasks)
if now - oldest_wait >= self.barrier_timeout_sec:
force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
print(f"⚠️ barrier WP {barrier_idx} {force_reason}，強制放行")
_log("WARN", f"barrier WP {barrier_idx} {force_reason}，強制放行")
else:
continue # 還沒到齊、也還沒 timeout → 繼續等
# 釋放：把所有在此 barrier 等待的機一起推進
tag = "全員到齊" if force_reason is None else force_reason
print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，"
f"推進 {len(waiting_tasks)} 架")
_log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag})，推進 {len(waiting_tasks)} 架")
for task in waiting_tasks:
task.status = TaskStatus.NORMAL
msg = f"barrier WP {barrier_idx} released ({tag})"
@ -324,7 +336,7 @@ class MissionExecutor(QObject):
return
task.fail_count += 1
print(f"⚠️ {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
_log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
if task.fail_count < self.MAX_RETRY:
task.status = TaskStatus.RETRYING
@ -339,13 +351,13 @@ class MissionExecutor(QObject):
drone_id, task.status.value,
f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
)
print(f"❌ {drone_id} 連續失敗 {self.MAX_RETRY} 次 → 切換 LOITER")
_log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次，切換至 LOITER")
self._fallback_to_loiter(drone_id)
def _fallback_to_loiter(self, drone_id):
"""用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
if self.monitor is None:
print(f"⚠️ 無 monitor，無法將 {drone_id} 切到 LOITER")
_log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER")
return
try:
loop = asyncio.get_event_loop()

Write Preview
Loading…
Cancel
Save