wenchun/AirTrapMine
1
0
Fork 0
forked from chiyu1468/AirTrapMine
Code Pull Requests Activity

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

Browse Source
This commit is contained in:
ken910606
2026-04-29 20:47:39 +08:00
parent 7f8babfa5e
commit edd15df3fc
5 changed files with 410 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GUI/command_sender.py
+7 -3
View File
@@ -16,6 +16,10 @@ from PyQt6.QtCore import QObject, pyqtSignal
from pymavlink import mavutil from pymavlink import mavutil
def _log(level, message):
print(f"[{level}] {message}")
class CommandSender(ABC): class CommandSender(ABC):
"""指令發送抽象介面""" """指令發送抽象介面"""
@@ -63,7 +67,7 @@ class MavlinkSender(CommandSender):
""" """
self.connection_string = connection_string self.connection_string = connection_string
self.mav = mavutil.mavlink_connection(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): def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。""" """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
@@ -86,7 +90,7 @@ class MavlinkSender(CommandSender):
if self.mav: if self.mav:
self.mav.close() self.mav.close()
self.mav = None self.mav = None
print("MavlinkSender 已關閉") _log("INFO", "MavlinkSender 已關閉")
class Ros2CommandSender(QObject): class Ros2CommandSender(QObject):
@@ -162,4 +166,4 @@ class Ros2CommandSender(QObject):
if not task.done(): if not task.done():
task.cancel() task.cancel()
self._pending.clear() self._pending.clear()
print("Ros2CommandSender 已關閉") _log("INFO", "Ros2CommandSender 已關閉")
src/GUI/communication.py
+108 -41
View File
@@ -20,6 +20,11 @@ from mavros_msgs.msg import State, VfrHud
from nav_msgs.msg import Odometry from nav_msgs.msg import Odometry
from mavros_msgs.srv import CommandBool, CommandTOL from mavros_msgs.srv import CommandBool, CommandTOL
def _log(level, message):
print(f"[{level}] {message}")
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入） # 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) _src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if _src_path not in sys.path: if _src_path not in sys.path:
@@ -30,10 +35,9 @@ try:
from fc_network_apps.longCommand import CommandLongClient from fc_network_apps.longCommand import CommandLongClient
except ImportError as e: except ImportError as e:
import traceback import traceback
print(f"⚠️ 警告: 無法導入 CommandLongClient") _log("WARN", "無法導入 CommandLongClient")
print(f" 错误: {e}") _log("ERROR", f"錯誤: {e}")
print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装") _log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整")
print(f" 完整堆栈跟踪:")
traceback.print_exc() traceback.print_exc()
CommandLongClient = None CommandLongClient = None
@@ -42,11 +46,18 @@ try:
from fc_network_apps.navigation import PositionTargetGlobalIntClient from fc_network_apps.navigation import PositionTargetGlobalIntClient
except ImportError as e: except ImportError as e:
import traceback import traceback
print(f"⚠️ 警告: 無法導入 PositionTargetGlobalIntClient") _log("WARN", "無法導入 PositionTargetGlobalIntClient")
print(f" 错误: {e}") _log("ERROR", f"錯誤: {e}")
traceback.print_exc() traceback.print_exc()
PositionTargetGlobalIntClient = None 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): class DroneSignals(QObject):
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) 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}" unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
client = CommandLongClient(node_name=unique_name) client = CommandLongClient(node_name=unique_name)
self.command_long_clients[drone_id] = client 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 添加到主執行器（這樣它的回調才能被處理） # 將新 client 添加到主執行器（這樣它的回調才能被處理）
if self.executor: if self.executor:
self.executor.add_node(client) self.executor.add_node(client)
print(f" ✓ 將 {drone_id} 的 client 添加到主執行器") _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
except TypeError: except TypeError:
# 舊版 CommandLongClient 不支持 node_name 參數，使用預設 # 舊版 CommandLongClient 不支持 node_name 參數，使用預設
client = CommandLongClient() client = CommandLongClient()
self.command_long_clients[drone_id] = client self.command_long_clients[drone_id] = client
print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)") _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)")
if self.executor: if self.executor:
self.executor.add_node(client) self.executor.add_node(client)
print(f" ✓ 將 {drone_id} 的 client 添加到主執行器") _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
except Exception as e: except Exception as e:
print(f"⚠️ 無法為 {drone_id} 創建 CommandLongClient: {e}") _log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}")
return None return None
return self.command_long_clients[drone_id] 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}" unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
client = PositionTargetGlobalIntClient(node_name=unique_name) client = PositionTargetGlobalIntClient(node_name=unique_name)
self.position_target_clients[drone_id] = client 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: if self.executor:
self.executor.add_node(client) self.executor.add_node(client)
print(f" ✓ 將 {drone_id} 的 position client 添加到主執行器") _log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器")
except Exception as e: except Exception as e:
print(f"⚠️ 無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}") _log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}")
return None return None
return self.position_target_clients[drone_id] return self.position_target_clients[drone_id]
@@ -593,7 +604,7 @@ class DroneMonitor(Node):
if not hasattr(self, subs_attr): if not hasattr(self, subs_attr):
self.setup_drone(sys_id) self.setup_drone(sys_id)
else: else:
# 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱） # 檢查既有訂閱是否包含 position_ned / attitude，如果不包含就添加（兼容舊訂閱）
subs = getattr(self, subs_attr, {}) subs = getattr(self, subs_attr, {})
if isinstance(subs, dict) and 'position_ned' not in subs: if isinstance(subs, dict) and 'position_ned' not in subs:
base_topic = f'/fc_network/vehicle/{sys_id}' base_topic = f'/fc_network/vehicle/{sys_id}'
@@ -608,6 +619,19 @@ class DroneMonitor(Node):
setattr(self, subs_attr, subs) # 明確保存更新後的字典 setattr(self, subs_attr, subs) # 明確保存更新後的字典
except Exception as e: except Exception as e:
pass 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): def setup_drone(self, sys_id):
# sys_id 格式： sys11, sys12, ... # sys_id 格式： sys11, sys12, ...
@@ -663,6 +687,14 @@ class DroneMonitor(Node):
) )
} }
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) setattr(self, f'drone_{sys_id}_subs', subs)
# ================================================================================ # ================================================================================
@@ -699,22 +731,22 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid # 解析 drone_id 提取 sysid
parts = drone_id.split('_') parts = drone_id.split('_')
if len(parts) < 2: if len(parts) < 2:
print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}") _log("ERROR", f"[SET_MODE] 無效的 drone_id 格式: {drone_id}")
return False return False
sysid = int(parts[-1]) sysid = int(parts[-1])
# 獲取模式對應的 custom_mode 值 # 獲取模式對應的 custom_mode 值
custom_mode = self.MODE_MAPPING.get(mode_name) custom_mode = self.MODE_MAPPING.get(mode_name)
if custom_mode is None: if custom_mode is None:
print(f"❌ [SET_MODE] 未知模式: {mode_name}") _log("ERROR", f"[SET_MODE] 未知模式: {mode_name}")
return False 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（避免多機並行時的競態條件） # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id) client = self.get_or_create_client(drone_id)
if not client: if not client:
print(f"❌ [SET_MODE] CommandLongClient 無法初始化") _log("ERROR", "[SET_MODE] CommandLongClient 無法初始化")
return False return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -727,13 +759,13 @@ class DroneMonitor(Node):
) )
if result and result.success: if result and result.success:
print(f"✅ [SET_MODE] 模式切換成功") _log("INFO", f"[SET_MODE] {drone_id} 模式切換成功")
return True return True
else: 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 return False
except Exception as e: except Exception as e:
print(f"❌ [SET_MODE] 例外錯誤: {e}") _log("ERROR", f"[SET_MODE] 例外錯誤: {e}")
traceback.print_exc() traceback.print_exc()
return False return False
@@ -743,17 +775,17 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid # 解析 drone_id 提取 sysid
parts = drone_id.split('_') parts = drone_id.split('_')
if len(parts) < 2: if len(parts) < 2:
print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}") _log("ERROR", f"[ARM] 無效的 drone_id 格式: {drone_id}")
return False return False
sysid = int(parts[-1]) sysid = int(parts[-1])
action_name = "解鎖" if arm else "上鎖" action_name = "解鎖" if arm else "上鎖"
print(f"\n📢 [ARM] {drone_id} → {action_name}") _log("INFO", f"[ARM] {drone_id} -> {action_name}")
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件） # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id) client = self.get_or_create_client(drone_id)
if not client: if not client:
print(f"❌ [ARM] CommandLongClient 無法初始化") _log("ERROR", "[ARM] CommandLongClient 無法初始化")
return False return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -765,13 +797,13 @@ class DroneMonitor(Node):
) )
if result and result.success: if result and result.success:
print(f"✅ [ARM] {action_name}成功") _log("INFO", f"[ARM] {drone_id} {action_name}成功")
return True return True
else: 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 return False
except Exception as e: except Exception as e:
print(f"❌ [ARM] 例外錯誤: {e}") _log("ERROR", f"[ARM] 例外錯誤: {e}")
traceback.print_exc() traceback.print_exc()
return False return False
@@ -781,16 +813,16 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid # 解析 drone_id 提取 sysid
parts = drone_id.split('_') parts = drone_id.split('_')
if len(parts) < 2: if len(parts) < 2:
print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}") _log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}")
return False return False
sysid = int(parts[-1]) sysid = int(parts[-1])
print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)") _log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)")
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件） # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
client = self.get_or_create_client(drone_id) client = self.get_or_create_client(drone_id)
if not client: if not client:
print(f"❌ [TAKEOFF] CommandLongClient 無法初始化") _log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化")
return False return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
@@ -802,13 +834,13 @@ class DroneMonitor(Node):
) )
if result and result.success: if result and result.success:
print(f"✅ [TAKEOFF] 起飛成功") _log("INFO", f"[TAKEOFF] {drone_id} 起飛成功")
return True return True
else: else:
print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})") _log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
return False return False
except Exception as e: except Exception as e:
print(f"❌ [TAKEOFF] 例外錯誤: {e}") _log("ERROR", f"[TAKEOFF] 例外錯誤: {e}")
traceback.print_exc() traceback.print_exc()
return False return False
@@ -840,17 +872,42 @@ class DroneMonitor(Node):
return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
# callbacks # callbacks
def attitude_callback(self, drone_id, msg): def attitude_callback(self, sys_id, msg):
if hasattr(msg, 'orientation'): """處理姿態 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) roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
self.latest_data[(drone_id, 'attitude')] = { data = {
'roll': roll, 'roll': roll,
'pitch': pitch, 'pitch': pitch,
'yaw': yaw, 'yaw': yaw,
'rates': (msg.angular_velocity.x, 'rates': (
msg.angular_velocity.x,
msg.angular_velocity.y, msg.angular_velocity.y,
msg.angular_velocity.z) 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): def battery_callback(self, sys_id, msg):
# 使用映射獲取實際的 drone_id # 使用映射獲取實際的 drone_id
@@ -903,7 +960,7 @@ class DroneMonitor(Node):
sys_num = sys_id.replace('sys', '') sys_num = sys_id.replace('sys', '')
actual_drone_id = f's{assigned_socket_id}_{sys_num}' actual_drone_id = f's{assigned_socket_id}_{sys_num}'
self.sys_to_actual_id[sys_id] = actual_drone_id 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, { 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 對齐） x = msg.pose.pose.position.y # NED 座標系中交換 x/y（與 local_pose 對齐）
y = msg.pose.pose.position.x y = msg.pose.pose.position.x
z = -msg.pose.pose.position.z # 將向下的 NED z 轉換為向上的高度（z 為負表示向下） 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')] = { self.latest_data[(actual_drone_id, 'altitude')] = {
@@ -1016,6 +1076,13 @@ class DroneMonitor(Node):
'z': z 'z': z
} }
# 儲存速度資訊，供總覽頁「XY速度」欄位顯示
self.latest_data[(actual_drone_id, 'velocity')] = {
'vx': vx,
'vy': vy,
'vz': vz
}
# 發送信號給 GUI 更新高度顯示 # 發送信號給 GUI 更新高度顯示
self.signals.update_signal.emit('altitude', actual_drone_id, { self.signals.update_signal.emit('altitude', actual_drone_id, {
'altitude': z 'altitude': z
@@ -1039,5 +1106,5 @@ class DroneMonitor(Node):
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self) receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
receiver.start() receiver.start()
self.serial_receivers.append(receiver) self.serial_receivers.append(receiver)
print(f"Started serial connection on {port} at {baudrate} baud") _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud")
return receiver return receiver
src/GUI/gui.py
+227 -149
View File
@@ -4,8 +4,8 @@ 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) QComboBox, QDialog, QPlainTextEdit)
from PyQt6.QtCore import Qt, QTimer from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal
from PyQt6.QtGui import QColor from PyQt6.QtGui import QColor
import sys import sys
import asyncio import asyncio
@@ -14,6 +14,11 @@ import subprocess
import time import time
import traceback import traceback
def _log(level, message):
print(f"[{level}] {message}", flush=True)
# 導入分離的類別 # 導入分離的類別
from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
from map_layout import DroneMap 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): class ControlStationUI(QMainWindow):
VERSION = '2.0.8' VERSION = '2.1.0'
def __init__(self): def __init__(self):
super().__init__() super().__init__()
@@ -78,6 +116,8 @@ class ControlStationUI(QMainWindow):
# 快取消息數據，以便在沒有新消息時使用上一次的值 # 快取消息數據，以便在沒有新消息時使用上一次的值
self._message_cache = {} self._message_cache = {}
self.message_history = []
self.max_message_history = 500
# 初始化UI # 初始化UI
self.drones = {} self.drones = {}
@@ -137,6 +177,7 @@ class ControlStationUI(QMainWindow):
# ================================================================================ # ================================================================================
self.init_ui() self.init_ui()
self._setup_stream_redirector()
def init_ui(self): def init_ui(self):
main_splitter = QSplitter(Qt.Orientation.Horizontal) main_splitter = QSplitter(Qt.Orientation.Horizontal)
@@ -160,7 +201,11 @@ class ControlStationUI(QMainWindow):
self.overview_table = OverviewTable() self.overview_table = OverviewTable()
self.left_tab.addTab(self.overview_table, "總覽") 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 = CommPanel()
self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added)
self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added)
@@ -259,6 +304,95 @@ class ControlStationUI(QMainWindow):
main_splitter.setSizes([400, 1000]) main_splitter.setSizes([400, 1000])
self.setCentralWidget(main_splitter) 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("運行中") status_label.setToolTip("運行中")
self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000) self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000)
except Exception as e: except Exception as e:
print(f"❌ Serial 連接啟動失敗: {str(e)}") _log("ERROR", f"Serial 連接啟動失敗: {str(e)}")
traceback.print_exc() traceback.print_exc()
self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000) self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000)
@@ -410,39 +544,25 @@ class ControlStationUI(QMainWindow):
# ================================================================================ # ================================================================================
def handle_mode_change(self, drone_id): def handle_mode_change(self, drone_id):
print(f"\n📢 [GUI] handle_mode_change 被调用")
print(f" drone_id: {drone_id}")
# 從 active group 的 mode_combo 讀取模式 # 從 active group 的 mode_combo 讀取模式
group = self._get_active_group() group = self._get_active_group()
if group: if group:
panel = self.group_panels.get(group.group_id) panel = self.group_panels.get(group.group_id)
mode = panel.mode_combo.currentText() if panel else "GUIDED" mode = panel.mode_combo.currentText() if panel else "GUIDED"
print(f" 从群组读取模式: {mode}")
else: else:
mode = "GUIDED" mode = "GUIDED"
print(f" 没有活跃群组，使用默认模式: {mode}") _log("INFO", f"切換模式請求: drone={drone_id}, mode={mode}")
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}"))
def handle_arm(self, 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() loop = asyncio.get_event_loop()
print(f" 獲得事件循環: {loop}")
arm_state = not self.monitor.get_arm_state(drone_id) 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) 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}" action_text = f"{'解鎖' if arm_state else '上鎖'} {drone_id}"
print(f" 準備提交到事件循環: {action_text}") _log("INFO", f"{action_text} 請求已送出")
task = loop.create_task(self.handle_service_response(coro, action_text)) loop.create_task(self.handle_service_response(coro, action_text))
print(f" task 已創建: {task}")
print(f" 已提交到事件循環\n")
def handle_takeoff(self, drone_id): def handle_takeoff(self, drone_id):
loop = asyncio.get_event_loop() loop = asyncio.get_event_loop()
@@ -453,7 +573,7 @@ class ControlStationUI(QMainWindow):
"""發送位置命令到 active group 的所有選中無人機""" """發送位置命令到 active group 的所有選中無人機"""
group = self._get_active_group() group = self._get_active_group()
if not group: if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) self.statusBar().showMessage("請先建立任務群組", 3000)
return return
try: try:
@@ -481,34 +601,18 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage("座標格式錯誤", 3000) self.statusBar().showMessage("座標格式錯誤", 3000)
async def handle_service_response(self, future, action): 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: try:
print(f" ├─ 等待 future/coroutine 完成...")
print(f" └─ (這是一個阻塞等待，直到服務返回)\n")
result = await future 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: if result:
print(f"\n✅ {action} 成功 (result={result})") _log("INFO", f"{action} 成功")
self.statusBar().showMessage(f"{action} 成功", 3000) self.statusBar().showMessage(f"{action} 成功", 3000)
else: else:
print(f"\n❌ {action} 失敗 (result={result})") _log("WARN", f"{action} 失敗")
self.statusBar().showMessage(f"{action} 失敗", 3000) self.statusBar().showMessage(f"{action} 失敗", 3000)
except asyncio.CancelledError: except asyncio.CancelledError:
print(f"⚠️ {action} 被取消") _log("WARN", f"{action} 已取消")
except Exception as e: except Exception as e:
print(f"❌ {action} 錯誤: {str(e)}") _log("ERROR", f"{action} 錯誤: {str(e)}")
traceback.print_exc() traceback.print_exc()
self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
@@ -516,38 +620,33 @@ class ControlStationUI(QMainWindow):
"""解鎖 active group 的所有選中無人機""" """解鎖 active group 的所有選中無人機"""
group = self._get_active_group() group = self._get_active_group()
if not group: if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) self.statusBar().showMessage("請先建立任務群組", 3000)
return return
selected = list(group.selected_drone_ids) selected = list(group.selected_drone_ids)
if not selected: if not selected:
self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000) self.statusBar().showMessage("尚未選擇任何無人機", 3000)
return return
_log("INFO", f"批次解鎖請求: {', '.join(selected)}")
print(f"\n📢 [GUI] handle_arm_selected 被調用")
print(f" 已選擇的無人機: {selected}")
loop = asyncio.get_event_loop() loop = asyncio.get_event_loop()
for drone_id in selected: for drone_id in selected:
print(f" 準備批次解鎖: {drone_id}")
coro = self.monitor.arm_drone(drone_id, True) coro = self.monitor.arm_drone(drone_id, True)
print(f" arm_drone 返回: {coro}")
# 使用 run_coroutine_threadsafe() 正確地在事件循環中運行 # 使用 run_coroutine_threadsafe() 正確地在事件循環中運行
asyncio.run_coroutine_threadsafe( asyncio.run_coroutine_threadsafe(
self.handle_service_response(coro, f"批次解鎖 {drone_id}"), self.handle_service_response(coro, f"批次解鎖 {drone_id}"),
loop loop
) )
print(f" handle_arm_selected 完成\n")
def handle_takeoff_selected(self): def handle_takeoff_selected(self):
"""起飛 active group 的所有選中無人機""" """起飛 active group 的所有選中無人機"""
group = self._get_active_group() group = self._get_active_group()
if not group: if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) self.statusBar().showMessage("請先建立任務群組", 3000)
return return
selected = list(group.selected_drone_ids) selected = list(group.selected_drone_ids)
if not selected: if not selected:
self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000) self.statusBar().showMessage("尚未選擇任何無人機", 3000)
return return
loop = asyncio.get_event_loop() loop = asyncio.get_event_loop()
@@ -774,7 +873,7 @@ class ControlStationUI(QMainWindow):
if blocked: if blocked:
self.statusBar().showMessage( self.statusBar().showMessage(
f"⚠ Group {group_id}: {len(blocked)} 台已被其他 group 勾選，未加入", 4000 f"Group {group_id}: {len(blocked)} 台已被其他 group 勾選，未加入", 4000
) )
else: else:
self.statusBar().showMessage( self.statusBar().showMessage(
@@ -811,7 +910,7 @@ class ControlStationUI(QMainWindow):
if not group: if not group:
return return
if group.planned_waypoints is None: if group.planned_waypoints is None:
self.statusBar().showMessage(f"⚠ Group {group_id}: 請先規劃任務", 3000) self.statusBar().showMessage(f"Group {group_id}: 請先規劃任務", 3000)
return return
if group.executor is None: if group.executor is None:
self._create_executor_for_group(group) self._create_executor_for_group(group)
@@ -819,7 +918,7 @@ class ControlStationUI(QMainWindow):
panel = self.group_panels.get(group_id) panel = self.group_panels.get(group_id)
if panel: if panel:
panel.update_status() 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): def _handle_group_pause(self, group_id):
"""暫停/恢復群組任務""" """暫停/恢復群組任務"""
@@ -828,10 +927,10 @@ class ControlStationUI(QMainWindow):
return return
if group.executor.state == MissionState.RUNNING: if group.executor.state == MissionState.RUNNING:
group.executor.pause() 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: elif group.executor.state == MissionState.PAUSED:
group.executor.resume() 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) panel = self.group_panels.get(group_id)
if panel: if panel:
panel.update_status() panel.update_status()
@@ -849,54 +948,44 @@ class ControlStationUI(QMainWindow):
if panel: if panel:
panel.update_status() panel.update_status()
panel.clear_mission_info() 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): 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) group = self.mission_groups.get(group_id)
if not group: if not group:
print(f"❌ 找不到群組: {group_id}", flush=True) _log("ERROR", f"找不到群組: {group_id}")
return return
if not group.selected_drone_ids: if not group.selected_drone_ids:
print(f"⚠️ 群組中沒有無人機", flush=True) _log("WARN", f"Group {group_id} 中沒有無人機")
self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000) self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
return return
_log("INFO", f"Group {group_id} 準備切換模式為 {mode}")
print(f" 準備為 {len(group.selected_drone_ids)} 台無人機切換模式...", flush=True) self.statusBar().showMessage("正在切換模式", 1000)
self.statusBar().showMessage(f"正在切換模式...", 1000)
# 使用 asyncio 執行（通過事件循環） # 使用 asyncio 執行（通過事件循環）
async def do_mode_changes_async(): async def do_mode_changes_async():
print(f"\n 【異步任務】開始執行模式切換", flush=True)
for drone_id in group.selected_drone_ids: for drone_id in group.selected_drone_ids:
print(f"\n 【切換】{drone_id} → {mode}", flush=True)
try: try:
result = await self.monitor.set_mode(drone_id, mode) result = await self.monitor.set_mode(drone_id, mode)
if result: if result:
msg = f"✅ {drone_id} 切換成功" msg = f"{drone_id} 切換成功"
print(f" {msg}", flush=True) _log("INFO", msg)
self.message_queue.put((msg, 2000)) self.message_queue.put((msg, 2000))
else: else:
msg = f"❌ {drone_id} 切換失敗" msg = f"{drone_id} 切換失敗"
print(f" {msg}", flush=True) _log("WARN", msg)
self.message_queue.put((msg, 2000)) self.message_queue.put((msg, 2000))
except Exception as e: except Exception as e:
msg = f"❌ {drone_id} 錯誤: {str(e)}" msg = f"{drone_id} 錯誤: {str(e)}"
print(f" {msg}", flush=True) _log("ERROR", msg)
traceback.print_exc() traceback.print_exc()
self.message_queue.put((msg, 2000)) self.message_queue.put((msg, 2000))
print(f"\n 【完成】模式切換任務結束\n", flush=True)
# 通過事件循環提交異步任務 # 通過事件循環提交異步任務
print(f" 【排隊】將任務提交至事件循環", flush=True)
loop = asyncio.get_event_loop() loop = asyncio.get_event_loop()
asyncio.run_coroutine_threadsafe( asyncio.run_coroutine_threadsafe(
do_mode_changes_async(), do_mode_changes_async(),
@@ -905,34 +994,22 @@ class ControlStationUI(QMainWindow):
def _handle_group_arm(self, group_id): 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) group = self.mission_groups.get(group_id)
print(f" 群組存在: {group is not None}")
if not group: if not group:
print(f" ⚠️ 群組不存在，返回\n") _log("WARN", f"找不到群組: {group_id}")
return return
print(f" 群組內無人機: {group.selected_drone_ids}")
loop = asyncio.get_event_loop() 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: 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) coro = self.monitor.arm_drone(drone_id, True)
print(f" ├─ arm_drone 返回類型: {type(coro)}")
action_text = f"解鎖 {drone_id}" action_text = f"解鎖 {drone_id}"
print(f" ├─ 準備提交到事件循環: {action_text}")
# 使用 run_coroutine_threadsafe() 正確地在事件循環中運行 coroutine # 使用 run_coroutine_threadsafe() 正確地在事件循環中運行 coroutine
# 這是 Qt + asyncio 整合的正確方式 # 這是 Qt + asyncio 整合的正確方式
future = asyncio.run_coroutine_threadsafe( asyncio.run_coroutine_threadsafe(
self.handle_service_response(coro, action_text), self.handle_service_response(coro, action_text),
loop loop
) )
print(f" ├─ Future 已創建: {future}")
print(f" └─ Future 將在事件循環中執行")
print(f"\n _handle_group_arm 完成（coroutine 已提交至事件循環）\n")
def _handle_group_takeoff(self, group_id, altitude): def _handle_group_takeoff(self, group_id, altitude):
"""群組內所有無人機起飛""" """群組內所有無人機起飛"""
@@ -983,7 +1060,7 @@ class ControlStationUI(QMainWindow):
if blocked: if blocked:
self.statusBar().showMessage( self.statusBar().showMessage(
f"⚠ Group {group_id}: 部分 drone 已被其他 group 勾選，僅加入 {len(allowed)} 台", 4000 f"Group {group_id}: 部分 drone 已被其他 group 勾選，僅加入 {len(allowed)} 台", 4000
) )
else: else:
self.statusBar().showMessage( self.statusBar().showMessage(
@@ -1049,7 +1126,7 @@ class ControlStationUI(QMainWindow):
"""刪除一個任務群組""" """刪除一個任務群組"""
# 檢查是否只有一個群組，如果是就禁止刪除 # 檢查是否只有一個群組，如果是就禁止刪除
if len(self.mission_groups) <= 1: if len(self.mission_groups) <= 1:
self.statusBar().showMessage("⚠️ 至少需要保留一個群組！", 3000) self.statusBar().showMessage("至少需要保留一個群組", 3000)
return return
if group_id not in self.mission_groups: if group_id not in self.mission_groups:
@@ -1100,7 +1177,7 @@ class ControlStationUI(QMainWindow):
panel = self.group_panels.get(group_id) panel = self.group_panels.get(group_id)
if panel: if panel:
panel.update_status() panel.update_status()
self.statusBar().showMessage(f"✅ Group {group_id}: 所有無人機已完成任務", 5000) self.statusBar().showMessage(f"Group {group_id}: 所有無人機已完成任務", 5000)
# ================================================================================ # ================================================================================
# UI 更新 # UI 更新
@@ -1152,7 +1229,7 @@ class ControlStationUI(QMainWindow):
if not selectable: if not selectable:
if blocked: if blocked:
self.statusBar().showMessage( self.statusBar().showMessage(
f"⚠ Socket {socket_id} 的 drone 已被其他 group 勾選，無法操作", 4000 f"Socket {socket_id} 的 drone 已被其他 group 勾選，無法操作", 4000
) )
self.refresh_selection_ui() self.refresh_selection_ui()
return return
@@ -1165,7 +1242,7 @@ class ControlStationUI(QMainWindow):
group.selected_drone_ids.update(selectable) group.selected_drone_ids.update(selectable)
if blocked: if blocked:
self.statusBar().showMessage( self.statusBar().showMessage(
f"⚠ 以下 drone 已被其他 group 勾選，略過: {', '.join(blocked)}", 4000 f"以下 drone 已被其他 group 勾選，略過: {', '.join(blocked)}", 4000
) )
self.refresh_selection_ui() self.refresh_selection_ui()
@@ -1217,7 +1294,7 @@ class ControlStationUI(QMainWindow):
if is_checked: if is_checked:
if self._is_drone_selected_by_other_group(drone_id, group.group_id): if self._is_drone_selected_by_other_group(drone_id, group.group_id):
self.statusBar().showMessage( 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() self.refresh_selection_ui()
return return
@@ -1239,7 +1316,7 @@ class ControlStationUI(QMainWindow):
# 嘗試勾選前先檢查是否被其他 group 使用 # 嘗試勾選前先檢查是否被其他 group 使用
if self._is_drone_selected_by_other_group(drone_id, group.group_id): if self._is_drone_selected_by_other_group(drone_id, group.group_id):
self.statusBar().showMessage( 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() self.refresh_selection_ui()
return return
@@ -1285,7 +1362,7 @@ class ControlStationUI(QMainWindow):
"""處理地圖點擊事件 — 根據 active group 的任務類型規劃""" """處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
group = self._get_active_group() group = self._get_active_group()
if not group: if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) self.statusBar().showMessage("請先建立任務群組", 3000)
return return
mode_map = { mode_map = {
@@ -1298,16 +1375,16 @@ class ControlStationUI(QMainWindow):
selected_drones = self._get_group_drones(group) selected_drones = self._get_group_drones(group)
if len(selected_drones) == 0: if len(selected_drones) == 0:
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000)
return 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) panel = self.group_panels.get(group.group_id)
params = panel.get_mission_params() if panel else {} params = panel.get_mission_params() if panel else {}
base_alt = params.get('base_altitude', params.get('altitude', 10.0)) 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( 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: try:
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) 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) total_wps = sum(len(wps) for wps in waypoints_per_drone)
self.statusBar().showMessage( 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: 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() traceback.print_exc()
# ================================================================================ # ================================================================================
@@ -1357,28 +1434,28 @@ class ControlStationUI(QMainWindow):
def handle_rectangle_selected(self, points_json): def handle_rectangle_selected(self, points_json):
group = self._get_active_group() group = self._get_active_group()
if not group: if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) self.statusBar().showMessage("請先建立任務群組", 3000)
return return
if group.mission_type != 'GRID_SWEEP': if group.mission_type != 'GRID_SWEEP':
return # 非 Grid Sweep 模式不處理矩形選取 return # 非 Grid Sweep 模式不處理矩形選取
selected_drones = self._get_group_drones(group) selected_drones = self._get_group_drones(group)
if len(selected_drones) == 0: if len(selected_drones) == 0:
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000)
return return
print(f"矩形選取 → Group {group.group_id}: {points_json}") _log("INFO", 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
panel = self.group_panels.get(group.group_id) panel = self.group_panels.get(group.group_id)
params = panel.get_mission_params() if panel else {} params = panel.get_mission_params() if panel else {}
base_alt = params.get('altitude', 10.0) base_alt = params.get('altitude', 10.0)
self.statusBar().showMessage( 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: 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:
@@ -1419,10 +1496,10 @@ class ControlStationUI(QMainWindow):
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"✓ 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: 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() traceback.print_exc()
# ================================================================================ # ================================================================================
@@ -1432,14 +1509,14 @@ class ControlStationUI(QMainWindow):
def handle_route_confirmed(self, points_json): def handle_route_confirmed(self, points_json):
group = self._get_active_group() group = self._get_active_group()
if not group: if not group:
self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) self.statusBar().showMessage("請先建立任務群組", 3000)
return return
if group.mission_type != 'LEADER_FOLLOWER': if group.mission_type != 'LEADER_FOLLOWER':
return return
selected_drones = self._get_group_drones(group) selected_drones = self._get_group_drones(group)
if len(selected_drones) == 0: if len(selected_drones) == 0:
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000)
return return
# LEADER_FOLLOWER：把指定的領隊放到 rank 0 # LEADER_FOLLOWER：把指定的領隊放到 rank 0
@@ -1447,23 +1524,23 @@ class ControlStationUI(QMainWindow):
if leader and leader in selected_drones: if leader and leader in selected_drones:
selected_drones = [leader] + [d for d in selected_drones if d != leader] 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: try:
route_points = json.loads(points_json) 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)
return return
if len(route_waypoints) < 2: if len(route_waypoints) < 2:
self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000) self.statusBar().showMessage("至少需要 2 個路徑點", 3000)
return return
panel = self.group_panels.get(group.group_id) panel = self.group_panels.get(group.group_id)
params = panel.get_mission_params() if panel else {} params = panel.get_mission_params() if panel else {}
base_alt = params.get('altitude', 10.0) base_alt = params.get('altitude', 10.0)
self.statusBar().showMessage( self.statusBar().showMessage(
f"⏳ Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台) ...", 2000) 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)
@@ -1507,10 +1584,10 @@ class ControlStationUI(QMainWindow):
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"✓ 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: 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() traceback.print_exc()
# ================================================================================ # ================================================================================
@@ -1519,20 +1596,20 @@ class ControlStationUI(QMainWindow):
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_task_status_changed(self, drone_id, status, message): def _on_task_status_changed(self, drone_id, status, message):
"""MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar""" """MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar"""
if status == "retrying": if status == "retrying":
self.statusBar().showMessage(f"⚠ {drone_id} {message}", 4000) self.statusBar().showMessage(f"{drone_id} {message}", 4000)
elif status == "fallback_loiter": 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": elif status == "waiting_at_barrier":
self.statusBar().showMessage(f"⏸ {drone_id} {message}", 3000) self.statusBar().showMessage(f"{drone_id} {message}", 3000)
elif status == "normal": 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 alt_drone = pos[2] if len(pos) > 2 else base_alt
drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) drone_gps_positions.append((lat_drone, lon_drone, alt_drone))
else: else:
self.statusBar().showMessage(f"⚠ 找不到 {drone_id} 的位置資料", 3000) self.statusBar().showMessage(f"找不到 {drone_id} 的位置資料", 3000)
return None return None
return drone_gps_positions 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') 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]) 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): def show_planned_waypoints(self, group=None):
pw = group.planned_waypoints if group else None pw = group.planned_waypoints if group else None
@@ -1739,7 +1816,7 @@ class ControlStationUI(QMainWindow):
panel._last_roll = roll panel._last_roll = roll
panel._last_pitch = pitch panel._last_pitch = pitch
if hasattr(panel, 'update_attitude'): 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) panel.update_attitude(heading, roll, pitch)
elif msg_type == 'gps': elif msg_type == 'gps':
@@ -1780,7 +1857,7 @@ class ControlStationUI(QMainWindow):
elapsed = (time.time() - start_time) * 1000 elapsed = (time.time() - start_time) * 1000
if elapsed > 33: if elapsed > 33:
print(f"[WARNING] UI update took {elapsed:.1f}ms (target: <33ms)") _log("WARN", f"UI 更新耗時 {elapsed:.1f}ms (目標 <33ms)")
finally: finally:
# ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪） # ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪）
@@ -1800,7 +1877,7 @@ class ControlStationUI(QMainWindow):
except: except:
break break
except Exception as e: except Exception as e:
print(f"❌ 消息隊列處理錯誤: {e}", flush=True) _log("ERROR", f"訊息佇列處理錯誤: {e}")
traceback.print_exc() traceback.print_exc()
def _spin_asyncio(self): def _spin_asyncio(self):
@@ -1831,15 +1908,16 @@ class ControlStationUI(QMainWindow):
except RuntimeError as e: except RuntimeError as e:
# ROS2 context 被破坏或不可用 # ROS2 context 被破坏或不可用
if "Context" in str(e) or "context" in str(e).lower(): 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: else:
print(f"ROS spin error: {e}", flush=True) _log("ERROR", f"ROS spin error: {e}")
traceback.print_exc() traceback.print_exc()
except Exception as e: except Exception as e:
print(f"ROS spin error: {e}", flush=True) _log("ERROR", f"ROS spin error: {e}")
traceback.print_exc() traceback.print_exc()
def closeEvent(self, event): def closeEvent(self, event):
self._restore_stream_redirector()
try: try:
for group in self.mission_groups.values(): for group in self.mission_groups.values():
if group.executor: if group.executor:
@@ -1867,7 +1945,7 @@ class ControlStationUI(QMainWindow):
self.monitor.destroy_node() self.monitor.destroy_node()
self.executor.shutdown() self.executor.shutdown()
except Exception as e: except Exception as e:
print(f"⚠️ 清理資源時出錯: {e}", flush=True) _log("WARN", f"清理資源時發生錯誤: {e}")
traceback.print_exc() traceback.print_exc()
# 安全地 shutdown ROS2 # 安全地 shutdown ROS2
@@ -1875,7 +1953,7 @@ class ControlStationUI(QMainWindow):
if rclpy.ok(): if rclpy.ok():
rclpy.shutdown() rclpy.shutdown()
except RuntimeError as e: except RuntimeError as e:
print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True) _log("WARN", f"ROS2 shutdown 錯誤: {e}")
traceback.print_exc() traceback.print_exc()
event.accept() event.accept()
@@ -1894,36 +1972,36 @@ def main():
""" """
# 第一步：在最外層只初始化一次 ROS2（終極防護） # 第一步：在最外層只初始化一次 ROS2（終極防護）
# 添加 rclpy.ok() 檢查，防止重複初始化導致 "Context.init() must only be called once" 錯誤 # 添加 rclpy.ok() 檢查，防止重複初始化導致 "Context.init() must only be called once" 錯誤
print("🚀 [GUI 主程式] 檢查 ROS2 初始化狀態...", flush=True) _log("INFO", "GUI 主程式檢查 ROS2 初始化狀態")
if not rclpy.ok(): if not rclpy.ok():
print("🚀 [GUI 主程式] ROS2 未初始化，開始初始化...", flush=True) _log("INFO", "ROS2 尚未初始化，開始初始化")
rclpy.init() rclpy.init()
print("✅ [GUI 主程式] ROS2 已全局初始化（由 GUI 主程式霸佔）", flush=True) _log("INFO", "ROS2 已完成全域初始化")
else: else:
print("ℹ️ [GUI 主程式] ROS2 已初始化，跳過重複初始化", flush=True) _log("INFO", "ROS2 已初始化，略過重複初始化")
try: try:
# 第二步：啟動 Qt 應用程式 # 第二步：啟動 Qt 應用程式
print("🚀 [GUI 主程式] 啟動 Qt 應用程式...", flush=True) _log("INFO", "啟動 Qt 應用程式")
app = QApplication(sys.argv) app = QApplication(sys.argv)
station = ControlStationUI() station = ControlStationUI()
station.show() station.show()
print("✓ [GUI 主程式] 應用程式已啟動", flush=True) _log("INFO", "應用程式已啟動")
# 第三步：進入 Qt 事件循環（阻塞直到用戶關閉應用） # 第三步：進入 Qt 事件循環（阻塞直到用戶關閉應用）
print("🎯 [GUI 主程式] 進入主事件循環，等待用戶操作...", flush=True) _log("INFO", "進入主事件循環")
sys.exit(app.exec()) sys.exit(app.exec())
finally: finally:
# 第四步：只有當 GUI 視窗被關閉時，才做 ROS2 cleanup（終極防護） # 第四步：只有當 GUI 視窗被關閉時，才做 ROS2 cleanup（終極防護）
# 這裡確保 ROS2 被正確且安全地關閉 # 這裡確保 ROS2 被正確且安全地關閉
print("\n🛑 [GUI 主程式] 應用程式關閉，正在清理 ROS2 資源...", flush=True) _log("INFO", "應用程式關閉，開始清理 ROS2 資源")
if rclpy.ok(): if rclpy.ok():
rclpy.shutdown() rclpy.shutdown()
print("✓ [GUI 主程式] ROS2 已安全關閉", flush=True) _log("INFO", "ROS2 已安全關閉")
else: else:
print("ℹ️ [GUI 主程式] ROS2 已關閉或不可用，無需重複 shutdown", flush=True) _log("INFO", "ROS2 已關閉或不可用，無需重複 shutdown")
print("✓ [GUI 主程式] 應用程式完全退出", flush=True) _log("INFO", "應用程式已完全退出")
if __name__ == '__main__': if __name__ == '__main__':
src/GUI/map_layout.py
+27 -14
View File
@@ -3,6 +3,11 @@ from PyQt6.QtWebEngineWidgets import QWebEngineView
from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
from PyQt6.QtWebChannel import QWebChannel from PyQt6.QtWebChannel import QWebChannel
def _log(level, message):
print(f"[{level}] {message}")
class DroneMap: class DroneMap:
"""無人機地圖類別 - 負責管理 Leaflet 地圖顯示""" """無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
@@ -874,7 +879,7 @@ class DroneMap:
if ok: if ok:
self.map_loaded = True self.map_loaded = True
else: else:
print("⚠️ 地圖加載失敗") _log("ERROR", "地圖載入失敗")
def update_drone_position(self, drone_id, lat, lon, heading): def update_drone_position(self, drone_id, lat, lon, heading):
"""更新無人機位置（加入待處理隊列）""" """更新無人機位置（加入待處理隊列）"""
@@ -924,20 +929,24 @@ class DroneMap:
f"{target_lat:.6f}, {target_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"📍 地圖已繪製 Group {group_id} 任務規劃: " _log(
f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})") "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): def clear_mission_plan(self):
"""清除地圖上所有任務規劃標記""" """清除地圖上所有任務規劃標記"""
if self.map_loaded: if self.map_loaded:
self.map_view.page().runJavaScript("clearAllMissionPlans();") self.map_view.page().runJavaScript("clearAllMissionPlans();")
print("🗑️ 地圖已清除所有任務規劃") _log("INFO", "地圖已清除所有任務規劃")
def clear_mission_plan_for_group(self, group_id): def clear_mission_plan_for_group(self, group_id):
"""清除指定群組的任務規劃標記""" """清除指定群組的任務規劃標記"""
if self.map_loaded: if self.map_loaded:
self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');") 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): def set_mission_mode(self, mode):
"""從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）""" """從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
@@ -1033,52 +1042,56 @@ class MapBridge(QObject):
def clearAllDroneSelection(self): def clearAllDroneSelection(self):
"""供 JavaScript 調用的方法 - 清除所有無人機選擇""" """供 JavaScript 調用的方法 - 清除所有無人機選擇"""
self.clear_all_drone_selection.emit() self.clear_all_drone_selection.emit()
print("🗑️ 清除所有無人機選擇") _log("INFO", "已清除所有無人機選擇")
@pyqtSlot() @pyqtSlot()
def toggleSelectAllDrones(self): def toggleSelectAllDrones(self):
"""供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機""" """供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機"""
self.select_all_drones.emit() self.select_all_drones.emit()
print("🔄 切換全選無人機") _log("INFO", "已切換全選無人機")
@pyqtSlot(float, float, float, float) @pyqtSlot(float, float, float, float)
def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon): def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon):
"""供 JavaScript 調用的方法 - 開始任務""" """供 JavaScript 調用的方法 - 開始任務"""
self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon) 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() @pyqtSlot()
def pauseMissionSignal(self): def pauseMissionSignal(self):
"""供 JavaScript 調用的方法 - 暫停任務""" """供 JavaScript 調用的方法 - 暫停任務"""
self.pause_mission_signal.emit() self.pause_mission_signal.emit()
print("⏸️ 暫停任務信號已發出") _log("INFO", "已發出暫停任務信號")
@pyqtSlot(str) @pyqtSlot(str)
def rectangleSelected(self, points_json): def rectangleSelected(self, points_json):
"""供 JavaScript 調用的方法 - 矩形選擇完成""" """供 JavaScript 調用的方法 - 矩形選擇完成"""
self.rectangle_selected.emit(points_json) self.rectangle_selected.emit(points_json)
print(f"📦 矩形區域已選擇: {points_json}") _log("INFO", f"矩形區域已選擇: {points_json}")
@pyqtSlot(str) @pyqtSlot(str)
def polygonSelected(self, points_json): def polygonSelected(self, points_json):
"""供 JavaScript 調用的方法 - 多邊形選擇完成""" """供 JavaScript 調用的方法 - 多邊形選擇完成"""
self.polygon_selected.emit(points_json) self.polygon_selected.emit(points_json)
print(f"🔷 多邊形區域已選擇: {points_json}") _log("INFO", f"多邊形區域已選擇: {points_json}")
@pyqtSlot(str) @pyqtSlot(str)
def missionModeChanged(self, mode): def missionModeChanged(self, mode):
"""供 JavaScript 調用的方法 - 任務模式切換""" """供 JavaScript 調用的方法 - 任務模式切換"""
self.mission_mode_changed.emit(mode) self.mission_mode_changed.emit(mode)
print(f"🔄 任務模式切換: {mode}") _log("INFO", f"任務模式已切換: {mode}")
@pyqtSlot(str) @pyqtSlot(str)
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}") _log("INFO", f"路徑已確認: {points_json}")
@pyqtSlot(str) @pyqtSlot(str)
def droneBoxSelected(self, drone_ids_json): def droneBoxSelected(self, drone_ids_json):
"""供 JavaScript 調用的方法 - 框選無人機完成""" """供 JavaScript 調用的方法 - 框選無人機完成"""
self.drone_box_selected.emit(drone_ids_json) self.drone_box_selected.emit(drone_ids_json)
print(f"📦 框選無人機: {drone_ids_json}") _log("INFO", f"框選無人機完成: {drone_ids_json}")
src/GUI/mission_executor.py
+30 -18
View File
@@ -19,6 +19,10 @@ from enum import Enum
from PyQt6.QtCore import QObject, QTimer, pyqtSignal from PyQt6.QtCore import QObject, QTimer, pyqtSignal
def _log(level, message):
print(f"[{level}] {message}")
class MissionState(Enum): class MissionState(Enum):
"""整體任務狀態""" """整體任務狀態"""
IDLE = "idle" IDLE = "idle"
@@ -109,7 +113,7 @@ class MissionExecutor(QObject):
def start(self, planned_waypoints): def start(self, planned_waypoints):
if self.state == MissionState.RUNNING: if self.state == MissionState.RUNNING:
print("任務已在執行中") _log("WARN", "任務已在執行中")
return return
self.tasks.clear() self.tasks.clear()
@@ -132,31 +136,34 @@ class MissionExecutor(QObject):
f"rendezvous WP={sorted(self.rendezvous_indices)}" f"rendezvous WP={sorted(self.rendezvous_indices)}"
if self.rendezvous_indices else "無 rendezvous (各自跑)" if self.rendezvous_indices else "無 rendezvous (各自跑)"
) )
print(f"任務啟動: {len(self.tasks)} 架無人機, " _log(
"INFO",
f"任務已啟動: {len(self.tasks)} 架無人機, "
f"共 {total_wps} 個航點, " f"共 {total_wps} 個航點, "
f"到達半徑={self.arrival_radius}m, " f"到達半徑={self.arrival_radius}m, "
f"tick 週期={self._interval_ms}ms, " f"tick 週期={self._interval_ms}ms, "
f"barrier timeout={self.barrier_timeout_sec}s, " f"barrier timeout={self.barrier_timeout_sec}s, "
f"{rv_info}") f"{rv_info}",
)
def pause(self): def pause(self):
if self.state == MissionState.RUNNING: if self.state == MissionState.RUNNING:
self._timer.stop() self._timer.stop()
self.state = MissionState.PAUSED self.state = MissionState.PAUSED
print("任務暫停") _log("INFO", "任務已暫停")
def resume(self): def resume(self):
if self.state == MissionState.PAUSED: if self.state == MissionState.PAUSED:
self._timer.start(self._interval_ms) self._timer.start(self._interval_ms)
self.state = MissionState.RUNNING self.state = MissionState.RUNNING
print("任務恢復") _log("INFO", "任務已恢復")
def stop(self): def stop(self):
self._timer.stop() self._timer.stop()
self.tasks.clear() self.tasks.clear()
self.rendezvous_indices = set() self.rendezvous_indices = set()
self.state = MissionState.IDLE self.state = MissionState.IDLE
print("任務停止") _log("INFO", "任務已停止")
# ------------------------------------------------------------------ 控制迴圈 # ------------------------------------------------------------------ 控制迴圈
@@ -197,7 +204,7 @@ class MissionExecutor(QObject):
# rendezvous 點 → 不推進，進入 barrier 等待 # rendezvous 點 → 不推進，進入 barrier 等待
task.status = TaskStatus.WAITING_AT_BARRIER task.status = TaskStatus.WAITING_AT_BARRIER
task.waiting_since = now 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( self.task_status_changed.emit(
task.drone_id, task.status.value, task.drone_id, task.status.value,
f"waiting at WP {task.wp_index}" f"waiting at WP {task.wp_index}"
@@ -236,7 +243,7 @@ class MissionExecutor(QObject):
self._timer.stop() self._timer.stop()
self.state = MissionState.IDLE self.state = MissionState.IDLE
self.mission_completed.emit() self.mission_completed.emit()
print("===== 任務全部完成 =====") _log("INFO", "任務全部完成")
def _advance_waypoint(self, task, arrived_distance): def _advance_waypoint(self, task, arrived_distance):
"""把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。""" """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
@@ -248,14 +255,20 @@ class MissionExecutor(QObject):
self.drone_waypoint_reached.emit( self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints task.drone_id, task.wp_index, task.total_waypoints
) )
print(f" {task.drone_id} → DONE " _log(
f"({task.total_waypoints}/{task.total_waypoints})") "INFO",
f"{task.drone_id} 已完成所有航點 "
f"({task.total_waypoints}/{task.total_waypoints})",
)
return return
self.drone_waypoint_reached.emit( self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints task.drone_id, task.wp_index, task.total_waypoints
) )
print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} " _log(
f"(到達距離: {arrived_distance:.1f}m)") "INFO",
f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} "
f"(到達距離: {arrived_distance:.1f}m)",
)
def _check_barriers(self, now): def _check_barriers(self, now):
"""檢查每個有人在等的 barrier 能不能釋放。""" """檢查每個有人在等的 barrier 能不能釋放。"""
@@ -289,14 +302,13 @@ class MissionExecutor(QObject):
oldest_wait = min(t.waiting_since for t in waiting_tasks) oldest_wait = min(t.waiting_since for t in waiting_tasks)
if now - oldest_wait >= self.barrier_timeout_sec: if now - oldest_wait >= self.barrier_timeout_sec:
force_reason = f"timeout {self.barrier_timeout_sec:.0f}s" 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: else:
continue # 還沒到齊、也還沒 timeout → 繼續等 continue # 還沒到齊、也還沒 timeout → 繼續等
# 釋放：把所有在此 barrier 等待的機一起推進 # 釋放：把所有在此 barrier 等待的機一起推進
tag = "全員到齊" if force_reason is None else force_reason tag = "全員到齊" if force_reason is None else force_reason
print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，" _log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag})，推進 {len(waiting_tasks)} 架")
f"推進 {len(waiting_tasks)} 架")
for task in waiting_tasks: for task in waiting_tasks:
task.status = TaskStatus.NORMAL task.status = TaskStatus.NORMAL
msg = f"barrier WP {barrier_idx} released ({tag})" msg = f"barrier WP {barrier_idx} released ({tag})"
@@ -324,7 +336,7 @@ class MissionExecutor(QObject):
return return
task.fail_count += 1 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: if task.fail_count < self.MAX_RETRY:
task.status = TaskStatus.RETRYING task.status = TaskStatus.RETRYING
@@ -339,13 +351,13 @@ class MissionExecutor(QObject):
drone_id, task.status.value, drone_id, task.status.value,
f"fallback LOITER after {self.MAX_RETRY} fails: {message}" 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) self._fallback_to_loiter(drone_id)
def _fallback_to_loiter(self, drone_id): def _fallback_to_loiter(self, drone_id):
"""用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。""" """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
if self.monitor is None: if self.monitor is None:
print(f"⚠️ 無 monitor，無法將 {drone_id} 切到 LOITER") _log("WARN", f"無 monitor，無法將 {drone_id} 切換至 LOITER")
return return
try: try:
loop = asyncio.get_event_loop() loop = asyncio.get_event_loop()