Compare commits
2
Commits
c4f178bb75
...
dbcd76be35
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
dbcd76be35 | ||
|
|
71321d4839 |
+365
-28
@@ -8,6 +8,8 @@ import asyncio
|
||||
import websockets
|
||||
import json
|
||||
import socket
|
||||
import sys
|
||||
import os
|
||||
from pymavlink import mavutil
|
||||
from geometry_msgs.msg import Point, Vector3
|
||||
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
|
||||
@@ -15,6 +17,24 @@ from std_msgs.msg import Float64, String
|
||||
from mavros_msgs.msg import State, VfrHud
|
||||
from mavros_msgs.srv import CommandBool, CommandTOL
|
||||
|
||||
# 確保 src 目錄在 Python 路徑中(用於 fc_network_apps 導入)
|
||||
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
|
||||
if _src_path not in sys.path:
|
||||
sys.path.insert(0, _src_path)
|
||||
|
||||
# 導入 fc_network_apps 的函數
|
||||
try:
|
||||
from fc_network_apps import change_mode, takeoff
|
||||
except ImportError as e:
|
||||
import traceback
|
||||
print(f"⚠️ 警告: 無法導入 fc_network_apps")
|
||||
print(f" 错误: {e}")
|
||||
print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
|
||||
print(f" 完整堆栈跟踪:")
|
||||
traceback.print_exc()
|
||||
change_mode = None
|
||||
takeoff = None
|
||||
|
||||
class DroneSignals(QObject):
|
||||
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
|
||||
|
||||
@@ -536,44 +556,361 @@ class DroneMonitor(Node):
|
||||
|
||||
setattr(self, f'drone_{sys_id}_subs', subs)
|
||||
|
||||
# ================================================================================
|
||||
# 【新增】模式名稱到 custom_mode 值的映射(基於 Copter 模式)
|
||||
# ================================================================================
|
||||
MODE_MAPPING = {
|
||||
"STABILIZE": 0,
|
||||
"ACRO": 1,
|
||||
"ALT_HOLD": 2,
|
||||
"AUTO": 3,
|
||||
"GUIDED": 4,
|
||||
"LOITER": 5,
|
||||
"RTL": 6,
|
||||
"CIRCLE": 7,
|
||||
"POSITION": 8,
|
||||
"LAND": 9,
|
||||
"OF_LOITER": 10,
|
||||
"DRIFT": 11,
|
||||
"SPORT": 13,
|
||||
"FLIP": 14,
|
||||
"AUTOTUNE": 15,
|
||||
"POSHOLD": 16,
|
||||
"BRAKE": 17,
|
||||
"THROW": 18,
|
||||
"AVOID_ADSB": 19,
|
||||
"GUIDED_NOGPS": 20,
|
||||
"SMART_RTL": 21,
|
||||
}
|
||||
# ================================================================================
|
||||
|
||||
async def set_mode(self, drone_id, mode_name):
|
||||
"""
|
||||
使用 fc_network_apps 的 change_mode 函數切換無人機飛行模式
|
||||
|
||||
參數:
|
||||
drone_id: 無人機ID (格式: "s{socket_id}_{sysid}")
|
||||
mode_name: 模式名稱 (例如: "GUIDED", "AUTO", "LOITER")
|
||||
|
||||
返回:
|
||||
bool: 模式切換是否成功
|
||||
"""
|
||||
import asyncio
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
|
||||
print(f"\n🔵 [SET_MODE] set_mode() 異步函數被調用 (drone_id={drone_id}, mode={mode_name})", flush=True)
|
||||
print(f" 事件循环: {asyncio.get_event_loop()}", flush=True)
|
||||
print(f" 当前任务: {asyncio.current_task()}\n", flush=True)
|
||||
|
||||
# 解析 drone_id 以提取 sysid
|
||||
# drone_id 格式: "s{socket_id}_{sysid}" (例如: "s0_1", "s0_11")
|
||||
try:
|
||||
parts = drone_id.split('_')
|
||||
if len(parts) < 2:
|
||||
self.get_logger().error(f"Invalid drone_id format: {drone_id}")
|
||||
print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
|
||||
print(f" 返回: False")
|
||||
return False
|
||||
sysid = int(parts[-1])
|
||||
print(f"✓ [SET_MODE] 解析 drone_id: {drone_id} → sysid={sysid}")
|
||||
except (ValueError, IndexError) as e:
|
||||
self.get_logger().error(f"Failed to parse drone_id {drone_id}: {e}")
|
||||
print(f"❌ [SET_MODE] 無法解析 drone_id {drone_id}: {e}")
|
||||
print(f" 返回: False")
|
||||
return False
|
||||
|
||||
# 獲取模式對應的 custom_mode 值
|
||||
custom_mode = self.MODE_MAPPING.get(mode_name)
|
||||
if custom_mode is None:
|
||||
self.get_logger().error(f"Unknown mode: {mode_name}. Available modes: {list(self.MODE_MAPPING.keys())}")
|
||||
print(f"❌ [SET_MODE] 未知模式: {mode_name}")
|
||||
print(f" 支持的模式: {list(self.MODE_MAPPING.keys())}")
|
||||
return False
|
||||
|
||||
print(f"✓ [SET_MODE] 模式對應: {mode_name} → custom_mode={custom_mode}")
|
||||
|
||||
# 檢查 fc_network_apps 的 change_mode 函數是否可用
|
||||
if change_mode is None:
|
||||
self.get_logger().error("fc_network_apps is not available. Cannot change mode.")
|
||||
print(f"❌ [SET_MODE] fc_network_apps 不可用")
|
||||
return False
|
||||
|
||||
# 使用 fc_network_apps 的 change_mode 函數
|
||||
try:
|
||||
msg = f"ROS2 服務呼叫: target_sysid={sysid}, custom_mode={custom_mode}, base_mode=1.0"
|
||||
self.get_logger().info(f"Changing mode for drone {drone_id} to {mode_name} (custom_mode={custom_mode})")
|
||||
print(f"\n📢 [SET_MODE] 開始切換模式")
|
||||
print(f" Drone ID: {drone_id}")
|
||||
print(f" 模式: {mode_name}")
|
||||
print(f" {msg}")
|
||||
|
||||
# 在線程池中運行同步的 change_mode 函數
|
||||
loop = asyncio.get_event_loop()
|
||||
executor = ThreadPoolExecutor(max_workers=1)
|
||||
|
||||
def _call_change_mode():
|
||||
print(f"\n 🔄 [_call_change_mode] 在線程池中調用 change_mode...")
|
||||
print(f" ├─ 線程開始時間: {__import__('time').time()}")
|
||||
print(f" ├─ 目標: sysid={sysid}, mode={custom_mode}\n")
|
||||
result = change_mode(
|
||||
target_sysid=sysid,
|
||||
custom_mode=float(custom_mode),
|
||||
target_compid=0,
|
||||
base_mode=1.0,
|
||||
confirmation=0,
|
||||
timeout_sec=2.0,
|
||||
)
|
||||
print(f"\n ├─ change_mode() 返回結果對象: {result}")
|
||||
print(f" └─ 線程任務完成")
|
||||
return result
|
||||
|
||||
print(f" 📢 [SET_MODE] 提交 change_mode 到線程池...")
|
||||
result = await loop.run_in_executor(executor, _call_change_mode)
|
||||
print(f"\n ✓ [SET_MODE] 從線程池接收到返回值")
|
||||
|
||||
print(f"\n📥 [SET_MODE] 從 change_mode() 接收服務響應:")
|
||||
print(f" ├─ result 对象类型: {type(result)}")
|
||||
print(f" ├─ result.success: {result.success} (类型: {type(result.success)})")
|
||||
print(f" ├─ result.message: '{result.message}' (类型: {type(result.message)})")
|
||||
print(f" └─ result.ack_result: {result.ack_result} (类型: {type(result.ack_result)})")
|
||||
|
||||
print(f"\n【返回值確認】")
|
||||
print(f" success == True: {result.success == True}")
|
||||
print(f" success is True: {result.success is True}")
|
||||
print(f" bool(success): {bool(result.success)}")
|
||||
|
||||
print(f"\n【FC_NETWORK SERVICE 回传值确认】")
|
||||
print(f" ├─ result.success: {result.success}")
|
||||
print(f" ├─ result.message: '{result.message}'")
|
||||
print(f" └─ result.ack_result: {result.ack_result}")
|
||||
|
||||
if result.success:
|
||||
self.get_logger().info(f"Mode change successful for {drone_id}: {result.message}")
|
||||
print(f"\n✅ [SET_MODE] 模式切換成功!")
|
||||
print(f" ├─ fc_network 確認: success=True")
|
||||
print(f" ├─ 訊息: {result.message}")
|
||||
print(f" ├─ ACK代碼: {result.ack_result}")
|
||||
print(f" └─ 返回到 GUI: True")
|
||||
return True
|
||||
else:
|
||||
self.get_logger().warning(f"Mode change failed for {drone_id}: {result.message} (ack={result.ack_result})")
|
||||
print(f"\n❌ [SET_MODE] 模式切換失敗!")
|
||||
print(f" ├─ fc_network 確認: success=False")
|
||||
print(f" ├─ 原因: {result.message}")
|
||||
print(f" ├─ ACK代碼: {result.ack_result}")
|
||||
print(f" └─ 返回到 GUI: False")
|
||||
return False
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Exception during mode change for {drone_id}: {e}")
|
||||
print(f"\n❌ [SET_MODE] 例外錯誤: {e}")
|
||||
import traceback
|
||||
traceback.print_exc()
|
||||
print(f" 返回: False (异常)")
|
||||
return False
|
||||
|
||||
async def arm_drone(self, drone_id, arm):
|
||||
if drone_id not in self.arm_clients:
|
||||
return False
|
||||
"""
|
||||
使用 fc_network_apps 的 arm_disarm 函數上鎖/解鎖無人機
|
||||
|
||||
client = self.arm_clients[drone_id]
|
||||
if not client.wait_for_service(timeout_sec=1.0):
|
||||
return False
|
||||
參數:
|
||||
drone_id: 無人機ID (格式: "s{socket_id}_{sysid}")
|
||||
arm: True 為上鎖, False 為解鎖
|
||||
|
||||
request = CommandBool.Request()
|
||||
request.value = arm
|
||||
返回:
|
||||
bool: 操作是否成功
|
||||
"""
|
||||
import asyncio
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
|
||||
future = client.call_async(request)
|
||||
action_name = "上鎖" if arm else "解鎖"
|
||||
print(f"\n🔵 [ARM_DISARM] arm_drone() 異步函數被調用 (drone_id={drone_id}, arm={arm}, 動作={action_name})", flush=True)
|
||||
|
||||
# 解析 drone_id 以提取 sysid
|
||||
try:
|
||||
response = await future
|
||||
return response.success
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Arm service call failed: {e}')
|
||||
parts = drone_id.split('_')
|
||||
if len(parts) < 2:
|
||||
self.get_logger().error(f"Invalid drone_id format: {drone_id}")
|
||||
print(f"❌ [ARM_DISARM] 無效的 drone_id 格式: {drone_id}")
|
||||
return False
|
||||
sysid = int(parts[-1])
|
||||
print(f"✓ [ARM_DISARM] 解析 drone_id: {drone_id} → sysid={sysid}")
|
||||
except (ValueError, IndexError) as e:
|
||||
self.get_logger().error(f"Failed to parse drone_id {drone_id}: {e}")
|
||||
print(f"❌ [ARM_DISARM] 無法解析 drone_id {drone_id}: {e}")
|
||||
return False
|
||||
|
||||
async def takeoff_drone(self, drone_id, altitude=10.0):
|
||||
if drone_id not in self.takeoff_clients:
|
||||
return False
|
||||
|
||||
client = self.takeoff_clients[drone_id]
|
||||
if not client.wait_for_service(timeout_sec=1.0):
|
||||
return False
|
||||
|
||||
request = CommandTOL.Request()
|
||||
request.altitude = altitude
|
||||
request.min_pitch = 0.0
|
||||
request.yaw = 0.0
|
||||
|
||||
future = client.call_async(request)
|
||||
try:
|
||||
response = await future
|
||||
return response.success
|
||||
msg = f"ROS2 服務呼叫: target_sysid={sysid}, arm={arm}"
|
||||
self.get_logger().info(f"Changing arm state for drone {drone_id} to {action_name}")
|
||||
print(f"\n📢 [ARM_DISARM] 開始{action_name}無人機")
|
||||
print(f" Drone ID: {drone_id}")
|
||||
print(f" 動作: {action_name}")
|
||||
print(f" {msg}")
|
||||
|
||||
# 在線程池中直接調用 ROS2 服務(避免 arm_disarm() 導致的初始化衝突)
|
||||
from fc_interfaces.srv import MavCommandLong
|
||||
loop = asyncio.get_event_loop()
|
||||
executor = ThreadPoolExecutor(max_workers=1)
|
||||
|
||||
def _call_ros2_arm_service():
|
||||
"""直接調用 ROS2 服務"""
|
||||
import time
|
||||
print(f"\n 🔄 [_call_ros2_arm_service] 在線程池中調用 ROS2 服務...")
|
||||
print(f" ├─ 時間: {time.time()}")
|
||||
print(f" ├─ 目標: sysid={sysid}, arm={arm}")
|
||||
print(f" └─ 直接調用ROS2服務(避免rclpy.init()衝突)\n")
|
||||
|
||||
try:
|
||||
# 建立 ROS2 客戶端(使用現有 context)
|
||||
client = self.create_client(MavCommandLong, "/fc_network/vehicle/send_command_long")
|
||||
|
||||
# 等待服務
|
||||
if not client.wait_for_service(timeout_sec=2.0):
|
||||
print(f" ❌ 服務不可用")
|
||||
return {'success': False, 'message': 'Service not available', 'ack_result': -1}
|
||||
|
||||
print(f" ✓ 服務已連接")
|
||||
|
||||
# 準備請求
|
||||
req = MavCommandLong.Request()
|
||||
req.target_sysid = sysid
|
||||
req.target_compid = 0
|
||||
req.command = 400 # MAV_CMD_COMPONENT_ARM_DISARM
|
||||
req.confirmation = 0
|
||||
req.param1 = 1.0 if arm else 0.0
|
||||
req.param2 = 0.0
|
||||
req.param3 = 0.0
|
||||
req.param4 = 0.0
|
||||
req.param5 = 0.0
|
||||
req.param6 = 0.0
|
||||
req.param7 = 0.0
|
||||
req.timeout_sec = 2.0
|
||||
|
||||
# 調用服務
|
||||
future = client.call_async(req)
|
||||
|
||||
# 簡單等待
|
||||
timeout = time.time() + 3.0
|
||||
while not future.done() and time.time() < timeout:
|
||||
time.sleep(0.01)
|
||||
|
||||
if future.done() and future.result():
|
||||
response = future.result()
|
||||
return {
|
||||
'success': response.success,
|
||||
'message': response.message,
|
||||
'ack_result': response.ack_result,
|
||||
}
|
||||
else:
|
||||
return {'success': False, 'message': 'Service call timeout', 'ack_result': -1}
|
||||
|
||||
except Exception as e:
|
||||
print(f" ❌ 異常: {e}")
|
||||
return {'success': False, 'message': str(e), 'ack_result': -1}
|
||||
|
||||
print(f" 📢 [ARM_DISARM] 提交 ROS2 服務呼叫到線程池...")
|
||||
result_dict = await loop.run_in_executor(executor, _call_ros2_arm_service)
|
||||
|
||||
if result_dict['success']:
|
||||
self.get_logger().info(f"Arm state change successful for {drone_id}")
|
||||
print(f"\n✅ [ARM_DISARM] 無人機{action_name}成功!")
|
||||
return True
|
||||
else:
|
||||
self.get_logger().warning(f"Arm state change failed for {drone_id}")
|
||||
print(f"\n❌ [ARM_DISARM] 無人機{action_name}失敗!")
|
||||
return False
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Takeoff service call failed: {e}')
|
||||
self.get_logger().error(f"Exception during arm state change for {drone_id}: {e}")
|
||||
print(f"\n❌ [ARM_DISARM] 例外錯誤: {e}")
|
||||
return False
|
||||
|
||||
async def takeoff_drone(self, drone_id, altitude):
|
||||
"""
|
||||
使用 fc_network_apps 的 takeoff 函數執行無人機起飛
|
||||
|
||||
參數:
|
||||
drone_id: 無人機ID (格式: "s{socket_id}_{sysid}")
|
||||
altitude: 目標高度 (米)
|
||||
|
||||
返回:
|
||||
bool: 起飛是否成功
|
||||
"""
|
||||
import asyncio
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
|
||||
print(f"\n🔵 [TAKEOFF] takeoff_drone() 異步函數被調用 (drone_id={drone_id}, altitude={altitude})", flush=True)
|
||||
|
||||
# 解析 drone_id 以提取 sysid
|
||||
try:
|
||||
parts = drone_id.split('_')
|
||||
if len(parts) < 2:
|
||||
self.get_logger().error(f"Invalid drone_id format: {drone_id}")
|
||||
print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
|
||||
return False
|
||||
sysid = int(parts[-1])
|
||||
print(f"✓ [TAKEOFF] 解析 drone_id: {drone_id} → sysid={sysid}")
|
||||
except (ValueError, IndexError) as e:
|
||||
self.get_logger().error(f"Failed to parse drone_id {drone_id}: {e}")
|
||||
print(f"❌ [TAKEOFF] 無法解析 drone_id {drone_id}: {e}")
|
||||
return False
|
||||
|
||||
# 檢查 fc_network_apps 的 takeoff 函數是否可用
|
||||
if takeoff is None:
|
||||
self.get_logger().error("fc_network_apps takeoff is not available. Cannot takeoff drone.")
|
||||
print(f"❌ [TAKEOFF] fc_network_apps takeoff 不可用")
|
||||
return False
|
||||
|
||||
try:
|
||||
print(f"\n📢 [TAKEOFF] 開始起飛無人機")
|
||||
print(f" Drone ID: {drone_id}")
|
||||
print(f" ROS2 服務呼叫: target_sysid={sysid}, altitude_m={altitude}")
|
||||
|
||||
# 在線程池中運行同步的 takeoff 函數
|
||||
loop = asyncio.get_event_loop()
|
||||
executor = ThreadPoolExecutor(max_workers=1)
|
||||
|
||||
def _call_takeoff():
|
||||
print(f"\n 🔄 [_call_takeoff] 在線程池中調用 takeoff(altitude={altitude})...")
|
||||
result = takeoff(
|
||||
target_sysid=sysid,
|
||||
altitude_m=float(altitude),
|
||||
target_compid=0,
|
||||
min_pitch_deg=0.0,
|
||||
yaw_deg=0.0,
|
||||
timeout_sec=2.0,
|
||||
)
|
||||
print(f"\n └─ takeoff() 返回結果")
|
||||
return result
|
||||
|
||||
print(f" 📢 [TAKEOFF] 提交 takeoff 到線程池...")
|
||||
result = await loop.run_in_executor(executor, _call_takeoff)
|
||||
|
||||
print(f"\n📥 [TAKEOFF] 從 takeoff() 接收服務響應:")
|
||||
print(f" ├─ result.success: {result.success}")
|
||||
print(f" ├─ result.message: '{result.message}'")
|
||||
print(f" └─ result.ack_result: {result.ack_result}")
|
||||
|
||||
if result.success:
|
||||
self.get_logger().info(f"Drone {drone_id} takeoff successfully: {result.message}")
|
||||
print(f"\n✅ [TAKEOFF] 無人機起飛成功!")
|
||||
print(f" ├─ fc_network 確認: success=True")
|
||||
print(f" ├─ 訊息: {result.message}")
|
||||
print(f" └─ ACK代碼: {result.ack_result}")
|
||||
return True
|
||||
else:
|
||||
self.get_logger().warning(f"Failed to takeoff drone {drone_id}: {result.message} (ack={result.ack_result})")
|
||||
print(f"\n❌ [TAKEOFF] 無人機起飛失敗!")
|
||||
print(f" ├─ fc_network 確認: success=False")
|
||||
print(f" ├─ 原因: {result.message}")
|
||||
print(f" └─ ACK代碼: {result.ack_result}")
|
||||
return False
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Exception during takeoff for {drone_id}: {e}")
|
||||
print(f"\n❌ [TAKEOFF] 例外錯誤: {e}")
|
||||
import traceback
|
||||
traceback.print_exc()
|
||||
return False
|
||||
|
||||
def send_setpoint(self, drone_id, x, y, z):
|
||||
|
||||
+427
-42
@@ -32,13 +32,17 @@ from mission_group import (
|
||||
# ================================================================================
|
||||
|
||||
class ControlStationUI(QMainWindow):
|
||||
VERSION = '1.0.1'
|
||||
VERSION = '2.0.0'
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.setWindowTitle(f'GCS v{self.VERSION}')
|
||||
self.resize(1400, 900)
|
||||
|
||||
# 初始化消息隊列(用於線程安全的 GUI 更新)
|
||||
import queue
|
||||
self.message_queue = queue.Queue()
|
||||
|
||||
# 初始化ROS2
|
||||
rclpy.init()
|
||||
self.monitor = DroneMonitor()
|
||||
@@ -53,11 +57,22 @@ class ControlStationUI(QMainWindow):
|
||||
self.timer.timeout.connect(self.spin_ros)
|
||||
self.timer.start(10)
|
||||
|
||||
# 驅動 asyncio 事件循環的定時器(新增 - 關鍵!)
|
||||
# 這允許異步任務(如 arm_drone)能夠執行
|
||||
self.asyncio_timer = QTimer()
|
||||
self.asyncio_timer.timeout.connect(self._spin_asyncio)
|
||||
self.asyncio_timer.start(10) # 每 10ms 驅動一次 asyncio
|
||||
|
||||
# 初始化 panel 和 map 更新(10Hz)
|
||||
self.panel_map_timer = QTimer()
|
||||
self.panel_map_timer.timeout.connect(self._update_panel_and_map)
|
||||
self.panel_map_timer.start(100) # 10Hz
|
||||
|
||||
# 消息隊列處理定時器(來自線程的 GUI 更新)
|
||||
self.msg_queue_timer = QTimer()
|
||||
self.msg_queue_timer.timeout.connect(self._process_message_queue)
|
||||
self.msg_queue_timer.start(50) # 每 50ms 檢查一次
|
||||
|
||||
# 快取消息數據,以便在沒有新消息時使用上一次的值
|
||||
self._message_cache = {}
|
||||
|
||||
@@ -164,20 +179,29 @@ class ControlStationUI(QMainWindow):
|
||||
|
||||
# ========== 任務群組 Tab ==========
|
||||
group_header = QHBoxLayout()
|
||||
|
||||
# 標題 + 收起/展開按鈕
|
||||
title_layout = QHBoxLayout()
|
||||
group_title = QLabel("任務群組")
|
||||
group_title.setStyleSheet(
|
||||
"color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
|
||||
group_header.addWidget(group_title)
|
||||
group_header.addStretch()
|
||||
title_layout.addWidget(group_title)
|
||||
|
||||
add_group_btn = QPushButton("+ 新增群組")
|
||||
add_group_btn.setStyleSheet("""
|
||||
QPushButton { background-color: #4A9EFF; color: white; border: none;
|
||||
padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; }
|
||||
QPushButton:hover { background-color: #3A8EEF; }
|
||||
# 收起/展開按鈕
|
||||
self.toggle_group_btn = QPushButton("▼")
|
||||
self.toggle_group_btn.setStyleSheet("""
|
||||
QPushButton { background-color: #555; color: white; border: none;
|
||||
padding: 3px 8px; border-radius: 3px; font-size: 12px; font-weight: bold;
|
||||
min-width: 30px; max-width: 30px; }
|
||||
QPushButton:hover { background-color: #666; }
|
||||
""")
|
||||
add_group_btn.clicked.connect(self._add_mission_group)
|
||||
group_header.addWidget(add_group_btn)
|
||||
self.toggle_group_btn.setToolTip("收起/展開任務群組")
|
||||
self.toggle_group_btn.clicked.connect(self._toggle_group_panel)
|
||||
title_layout.addWidget(self.toggle_group_btn)
|
||||
title_layout.addStretch()
|
||||
|
||||
group_header.addLayout(title_layout)
|
||||
group_header.addStretch()
|
||||
|
||||
clear_traj_btn = QPushButton("清除軌跡")
|
||||
clear_traj_btn.setStyleSheet("""
|
||||
@@ -205,6 +229,9 @@ class ControlStationUI(QMainWindow):
|
||||
self.group_tab_widget.currentChanged.connect(self._on_group_tab_changed)
|
||||
right_layout.addWidget(self.group_tab_widget)
|
||||
|
||||
# 🌟 新增:保存群組面板的展開狀態
|
||||
self.group_panel_expanded = True
|
||||
|
||||
# 預設建立 Group A
|
||||
self._add_mission_group()
|
||||
|
||||
@@ -377,22 +404,39 @@ 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}")
|
||||
|
||||
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)
|
||||
future = self.monitor.arm_drone(drone_id, arm_state)
|
||||
loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {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")
|
||||
|
||||
def handle_takeoff(self, drone_id):
|
||||
loop = asyncio.get_event_loop()
|
||||
@@ -425,20 +469,53 @@ 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})")
|
||||
self.statusBar().showMessage(f"{action} 成功", 3000)
|
||||
else:
|
||||
print(f"\n❌ {action} 失敗 (result={result})")
|
||||
self.statusBar().showMessage(f"{action} 失敗", 3000)
|
||||
except asyncio.CancelledError:
|
||||
print(f"⚠️ {action} 被取消")
|
||||
except Exception as e:
|
||||
print(f"❌ {action} 錯誤: {str(e)}")
|
||||
import traceback
|
||||
traceback.print_exc()
|
||||
self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
|
||||
|
||||
def handle_arm_selected(self):
|
||||
print(f"\n📢 [GUI] handle_arm_selected 被調用")
|
||||
selected = list(self.monitor.selected_drones)
|
||||
print(f" 已選擇的無人機: {selected}")
|
||||
loop = asyncio.get_event_loop()
|
||||
for drone_id in self.monitor.selected_drones:
|
||||
future = self.monitor.arm_drone(drone_id, True)
|
||||
loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}"))
|
||||
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):
|
||||
loop = asyncio.get_event_loop()
|
||||
@@ -456,6 +533,23 @@ class ControlStationUI(QMainWindow):
|
||||
self._group_counter += 1
|
||||
return gid
|
||||
|
||||
def _toggle_group_panel(self):
|
||||
"""🌟 收起/展開任務群組面板"""
|
||||
if self.group_panel_expanded:
|
||||
# 收起
|
||||
self.group_tab_widget.setFixedHeight(0)
|
||||
self.group_tab_widget.hide()
|
||||
self.toggle_group_btn.setText("▶")
|
||||
self.toggle_group_btn.setToolTip("展開任務群組")
|
||||
self.group_panel_expanded = False
|
||||
else:
|
||||
# 展開
|
||||
self.group_tab_widget.setFixedHeight(150)
|
||||
self.group_tab_widget.show()
|
||||
self.toggle_group_btn.setText("▼")
|
||||
self.toggle_group_btn.setToolTip("收起任務群組")
|
||||
self.group_panel_expanded = True
|
||||
|
||||
def _add_mission_group(self):
|
||||
"""新增一個任務群組"""
|
||||
gid = self._next_group_id()
|
||||
@@ -475,6 +569,8 @@ class ControlStationUI(QMainWindow):
|
||||
panel.box_select_requested.connect(self._handle_box_select)
|
||||
panel.select_all_requested.connect(self._handle_select_all_for_group)
|
||||
panel.clear_group_requested.connect(self._handle_clear_group)
|
||||
panel.add_group_requested.connect(self._add_mission_group)
|
||||
panel.delete_group_requested.connect(self._handle_delete_group)
|
||||
self.group_panels[gid] = panel
|
||||
|
||||
# 用帶顏色的 tab 標題
|
||||
@@ -538,6 +634,19 @@ class ControlStationUI(QMainWindow):
|
||||
if panel:
|
||||
panel.update_drone_list()
|
||||
panel.update_status()
|
||||
|
||||
# 同步更新左側面板的 checkbox 狀態
|
||||
self.monitor.selected_drones = group.drone_ids.copy()
|
||||
for drone_id in all_ids:
|
||||
if drone_id in self.drones:
|
||||
checkbox = self.drones[drone_id].get_checkbox()
|
||||
if checkbox:
|
||||
checkbox.blockSignals(True)
|
||||
checkbox.setChecked(drone_id in group.drone_ids)
|
||||
checkbox.blockSignals(False)
|
||||
# 更新 socket 群組的 checkbox 狀態
|
||||
self.update_group_checkbox_state(self.get_socket_id(drone_id))
|
||||
|
||||
self.statusBar().showMessage(
|
||||
f"Group {group_id}: 已分配 {len(group.drone_ids)} 台無人機", 3000)
|
||||
|
||||
@@ -611,23 +720,133 @@ class ControlStationUI(QMainWindow):
|
||||
|
||||
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)
|
||||
return
|
||||
loop = asyncio.get_event_loop()
|
||||
for drone_id in group.drone_ids:
|
||||
future = self.monitor.set_mode(drone_id, mode)
|
||||
loop.create_task(self.handle_service_response(future, f"{drone_id} 切換 {mode}"))
|
||||
|
||||
if not group.drone_ids:
|
||||
print(f"⚠️ 群組中沒有無人機", flush=True)
|
||||
self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
|
||||
return
|
||||
|
||||
print(f" 準備為 {len(group.drone_ids)} 台無人機切換模式...", flush=True)
|
||||
self.statusBar().showMessage(f"正在切換模式...", 1000)
|
||||
|
||||
# 在後台線程中執行(避免阻塞 GUI)
|
||||
def do_mode_changes_threaded():
|
||||
print(f"\n 【後台線程】開始執行模式切換", flush=True)
|
||||
import sys
|
||||
import os
|
||||
import time
|
||||
|
||||
# 确保 src 在 Python 路径中
|
||||
src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
|
||||
if src_path not in sys.path:
|
||||
sys.path.insert(0, src_path)
|
||||
print(f" 【路徑】已添加: {src_path}", flush=True)
|
||||
|
||||
# 模式值映射
|
||||
MODE_MAPPING = {
|
||||
"STABILIZE": 0, "ACRO": 1, "ALT_HOLD": 2, "AUTO": 3,
|
||||
"GUIDED": 4, "LOITER": 5, "RTL": 6, "CIRCLE": 7,
|
||||
"LAND": 9, "DRIFT": 11, "SPORT": 13, "AUTOTUNE": 15,
|
||||
"POSHOLD": 16, "BRAKE": 17, "SMART_RTL": 21,
|
||||
}
|
||||
|
||||
custom_mode = MODE_MAPPING.get(mode)
|
||||
if custom_mode is None:
|
||||
msg = f"❌ 未知模式: {mode}"
|
||||
print(f" {msg}", flush=True)
|
||||
self.message_queue.put((msg, 2000))
|
||||
return
|
||||
|
||||
for drone_id in group.drone_ids:
|
||||
print(f"\n 【切換】{drone_id} → {mode} (mode={custom_mode})", flush=True)
|
||||
try:
|
||||
# 導入模式切換函數
|
||||
from fc_network_apps.changeMode import change_mode
|
||||
|
||||
# 解析 sysid(從 drone_id 的最後一個數字)
|
||||
sysid = int(drone_id.split('_')[-1])
|
||||
print(f" └─ sysid={sysid}", flush=True)
|
||||
|
||||
# 調用 change_mode(同步操作)
|
||||
try:
|
||||
result = change_mode(
|
||||
target_sysid=sysid,
|
||||
custom_mode=float(custom_mode),
|
||||
target_compid=0,
|
||||
base_mode=1.0,
|
||||
confirmation=0,
|
||||
timeout_sec=2.0,
|
||||
)
|
||||
|
||||
print(f" └─ 結果: success={result.success}", flush=True)
|
||||
|
||||
if result.success:
|
||||
msg = f"✅ {drone_id} 切換成功"
|
||||
print(f" {msg}", flush=True)
|
||||
self.message_queue.put((msg, 2000))
|
||||
else:
|
||||
msg = f"❌ {drone_id} 切換失敗: {result.message}"
|
||||
print(f" {msg}", flush=True)
|
||||
self.message_queue.put((msg, 2000))
|
||||
|
||||
except Exception as service_error:
|
||||
msg = f"❌ {drone_id} 服務調用錯誤: {str(service_error)}"
|
||||
print(f" {msg}", flush=True)
|
||||
self.message_queue.put((msg, 2000))
|
||||
|
||||
except Exception as e:
|
||||
msg = f"❌ {drone_id} 錯誤: {str(e)}"
|
||||
print(f" {msg}", flush=True)
|
||||
import traceback
|
||||
traceback.print_exc()
|
||||
self.message_queue.put((msg, 2000))
|
||||
self.message_queue.put((msg, 2000))
|
||||
|
||||
print(f"\n 【完成】模式切換任務結束\n", flush=True)
|
||||
|
||||
# 在後台線程執行
|
||||
import threading
|
||||
print(f" 【排隊】將任務提交至後台線程", flush=True)
|
||||
thread = threading.Thread(target=do_mode_changes_threaded, daemon=True)
|
||||
thread.start()
|
||||
|
||||
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")
|
||||
return
|
||||
print(f" 群組內無人機: {group.drone_ids}")
|
||||
loop = asyncio.get_event_loop()
|
||||
print(f" 事件循環: {loop}")
|
||||
|
||||
for drone_id in group.drone_ids:
|
||||
future = self.monitor.arm_drone(drone_id, True)
|
||||
loop.create_task(self.handle_service_response(future, f"解鎖 {drone_id}"))
|
||||
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(
|
||||
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):
|
||||
"""群組內所有無人機起飛"""
|
||||
@@ -663,23 +882,59 @@ class ControlStationUI(QMainWindow):
|
||||
if panel:
|
||||
panel.update_drone_list()
|
||||
panel.update_status()
|
||||
|
||||
# 同步更新左側面板的 checkbox 狀態
|
||||
self.monitor.selected_drones = group.drone_ids.copy()
|
||||
for drone_id in self.drones.keys():
|
||||
checkbox = self.drones[drone_id].get_checkbox()
|
||||
if checkbox:
|
||||
checkbox.blockSignals(True)
|
||||
checkbox.setChecked(drone_id in group.drone_ids)
|
||||
checkbox.blockSignals(False)
|
||||
self.update_group_checkbox_state(self.get_socket_id(drone_id))
|
||||
|
||||
self.statusBar().showMessage(
|
||||
f"Group {group_id}: 框選分配 {len(valid)} 台無人機", 3000)
|
||||
|
||||
def _handle_select_all_for_group(self, group_id):
|
||||
"""全選所有可用無人機,直接分配到該群組"""
|
||||
"""全選/取消全選 - Toggle 邏輯"""
|
||||
group = self.mission_groups.get(group_id)
|
||||
if not group:
|
||||
return
|
||||
|
||||
other = self._get_other_assigned(group_id)
|
||||
available = {did for did in self.drones.keys() if did not in other}
|
||||
group.drone_ids = available
|
||||
|
||||
# Toggle 邏輯:如果已全選,則清空;否則全選
|
||||
if group.drone_ids == available:
|
||||
# 已全選 → 清空
|
||||
group.drone_ids = set()
|
||||
self.monitor.selected_drones.clear()
|
||||
msg_status = "已取消全選"
|
||||
else:
|
||||
# 未全選 → 全選
|
||||
group.drone_ids = available
|
||||
self.monitor.selected_drones = group.drone_ids.copy()
|
||||
msg_status = f"全選分配 {len(available)} 台無人機"
|
||||
|
||||
panel = self.group_panels.get(group_id)
|
||||
if panel:
|
||||
panel.update_drone_list()
|
||||
panel.update_status()
|
||||
# 更新按鈕文本
|
||||
panel.set_all_select_state(group.drone_ids == available)
|
||||
|
||||
# 同步更新左側面板的 checkbox 狀態
|
||||
for drone_id in self.drones.keys():
|
||||
checkbox = self.drones[drone_id].get_checkbox()
|
||||
if checkbox:
|
||||
checkbox.blockSignals(True)
|
||||
checkbox.setChecked(drone_id in group.drone_ids)
|
||||
checkbox.blockSignals(False)
|
||||
self.update_group_checkbox_state(self.get_socket_id(drone_id))
|
||||
|
||||
self.statusBar().showMessage(
|
||||
f"Group {group_id}: 全選分配 {len(available)} 台無人機", 3000)
|
||||
f"Group {group_id}: {msg_status}", 3000)
|
||||
|
||||
def _handle_clear_group(self, group_id):
|
||||
"""清除群組的無人機分配"""
|
||||
@@ -696,9 +951,59 @@ class ControlStationUI(QMainWindow):
|
||||
panel.update_drone_list()
|
||||
panel.update_status()
|
||||
panel.clear_mission_info()
|
||||
|
||||
# 同步更新左側面板的 checkbox 狀態(全部取消勾選)
|
||||
self.monitor.selected_drones.clear()
|
||||
for drone_id in self.drones.keys():
|
||||
checkbox = self.drones[drone_id].get_checkbox()
|
||||
if checkbox:
|
||||
checkbox.blockSignals(True)
|
||||
checkbox.setChecked(False)
|
||||
checkbox.blockSignals(False)
|
||||
self.update_group_checkbox_state(self.get_socket_id(drone_id))
|
||||
|
||||
self.statusBar().showMessage(
|
||||
f"Group {group_id}: 已清除分組", 3000)
|
||||
|
||||
def _handle_delete_group(self, group_id):
|
||||
"""刪除一個任務群組"""
|
||||
if group_id not in self.mission_groups:
|
||||
self.statusBar().showMessage(f"Group {group_id} 不存在", 3000)
|
||||
return
|
||||
|
||||
# 停止群組的執行(如果有)
|
||||
group = self.mission_groups[group_id]
|
||||
if group.executor:
|
||||
group.executor.stop()
|
||||
|
||||
# 移除地圖上的任務計畫
|
||||
self.drone_map.clear_mission_plan_for_group(group_id)
|
||||
|
||||
# 移除 tab
|
||||
for i in range(self.group_tab_widget.count()):
|
||||
if f"Group {group_id}" in self.group_tab_widget.tabText(i):
|
||||
self.group_tab_widget.removeTab(i)
|
||||
break
|
||||
|
||||
# 移除資料
|
||||
del self.mission_groups[group_id]
|
||||
if group_id in self.group_panels:
|
||||
del self.group_panels[group_id]
|
||||
|
||||
# 更新 active group
|
||||
if self.active_group_id == group_id:
|
||||
if self.group_tab_widget.count() > 0:
|
||||
self.group_tab_widget.setCurrentIndex(0)
|
||||
# 更新 active_group_id 為當前 tab 的群組
|
||||
for gid, panel in self.group_panels.items():
|
||||
if panel == self.group_tab_widget.currentWidget().widget():
|
||||
self.active_group_id = gid
|
||||
break
|
||||
else:
|
||||
self.active_group_id = None
|
||||
|
||||
self.statusBar().showMessage(f"已刪除 Group {group_id}", 3000)
|
||||
|
||||
def _on_group_mission_completed(self, group_id):
|
||||
"""群組任務完成回呼"""
|
||||
panel = self.group_panels.get(group_id)
|
||||
@@ -751,12 +1056,47 @@ class ControlStationUI(QMainWindow):
|
||||
else: self.monitor.selected_drones.discard(drone_id)
|
||||
|
||||
def handle_drone_selection(self, drone_id, state):
|
||||
if state == Qt.CheckState.Checked.value:
|
||||
is_checked = state == Qt.CheckState.Checked.value
|
||||
if is_checked:
|
||||
self.monitor.selected_drones.add(drone_id)
|
||||
else:
|
||||
self.monitor.selected_drones.discard(drone_id)
|
||||
self.update_group_checkbox_state(self.get_socket_id(drone_id))
|
||||
|
||||
# 同步更新任務群組的無人機分配狀態
|
||||
# 遍歷所有任務群組,更新已分配的無人機列表顯示
|
||||
if not is_checked:
|
||||
# 取消勾選時:從所有包含該無人機的群組中移除
|
||||
for group_id, group in self.mission_groups.items():
|
||||
if drone_id in group.drone_ids:
|
||||
group.drone_ids.discard(drone_id)
|
||||
panel = self.group_panels.get(group_id)
|
||||
if panel:
|
||||
panel.update_drone_list()
|
||||
panel.update_status()
|
||||
# 更新全選按鈕狀態
|
||||
other = self._get_other_assigned(group_id)
|
||||
available = {did for did in self.drones.keys() if did not in other}
|
||||
panel.set_all_select_state(group.drone_ids == available)
|
||||
else:
|
||||
# 勾選時:檢查該無人機是否已分配給其他群組,若未分配則添加到當前活躍群組
|
||||
is_already_assigned = any(
|
||||
drone_id in group.drone_ids
|
||||
for group in self.mission_groups.values()
|
||||
)
|
||||
if not is_already_assigned and self.active_group_id:
|
||||
# 無人機未被分配給任何群組,可以添加到當前活躍群組
|
||||
group = self.mission_groups.get(self.active_group_id)
|
||||
panel = self.group_panels.get(self.active_group_id)
|
||||
if group and panel:
|
||||
group.drone_ids.add(drone_id)
|
||||
panel.update_drone_list()
|
||||
panel.update_status()
|
||||
# 更新全選按鈕狀態
|
||||
other = self._get_other_assigned(self.active_group_id)
|
||||
available = {did for did in self.drones.keys() if did not in other}
|
||||
panel.set_all_select_state(group.drone_ids == available)
|
||||
|
||||
def update_group_checkbox_state(self, socket_id):
|
||||
group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
|
||||
if not group_drones: return
|
||||
@@ -1180,7 +1520,6 @@ class ControlStationUI(QMainWindow):
|
||||
self._map_update_count += 1
|
||||
now = time.time()
|
||||
if now - self._map_update_time >= 1.0:
|
||||
print(f"[Panel/Map Update] {self._map_update_count} Hz")
|
||||
self._map_update_time = now
|
||||
self._map_update_count = 0
|
||||
|
||||
@@ -1314,27 +1653,73 @@ class ControlStationUI(QMainWindow):
|
||||
|
||||
|
||||
|
||||
def _process_message_queue(self):
|
||||
"""處理來自後台線程的消息隊列(更新 GUI 狀態欄)"""
|
||||
try:
|
||||
while True:
|
||||
try:
|
||||
message, duration = self.message_queue.get_nowait()
|
||||
self.statusBar().showMessage(message, duration)
|
||||
except:
|
||||
break
|
||||
except Exception as e:
|
||||
print(f"❌ 消息隊列處理錯誤: {e}", flush=True)
|
||||
|
||||
def _spin_asyncio(self):
|
||||
"""驅動 asyncio 事件循環,允許異步任務執行
|
||||
|
||||
關鍵修復:asyncio 事件循環不會自動運行。
|
||||
這個定時器會定期執行事件循環,讓 run_coroutine_threadsafe() 提交的任務能夠執行。
|
||||
"""
|
||||
try:
|
||||
loop = asyncio.get_event_loop()
|
||||
if loop and not loop.is_closed() and not loop.is_running():
|
||||
# 執行事件循環直到沒有掛起的任務或超時
|
||||
# 使用 run_until_complete() 配合一個快速返回的 coroutine
|
||||
loop.run_until_complete(asyncio.sleep(0))
|
||||
except Exception as e:
|
||||
# 靜默忽略任何錯誤,防止 Qt 定時器出現異常
|
||||
pass
|
||||
|
||||
def spin_ros(self):
|
||||
try:
|
||||
self.executor.spin_once(timeout_sec=0.01)
|
||||
for (drone_id, msg_type), data in self.monitor.latest_data.items():
|
||||
self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
|
||||
self.monitor.latest_data.clear()
|
||||
# 仅在 ROS2 正常工作时才尝试 spin
|
||||
if rclpy.ok():
|
||||
self.executor.spin_once(timeout_sec=0.01)
|
||||
for (drone_id, msg_type), data in self.monitor.latest_data.items():
|
||||
self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
|
||||
self.monitor.latest_data.clear()
|
||||
except RuntimeError as e:
|
||||
# ROS2 context 被破坏或不可用
|
||||
if "Context" in str(e) or "context" in str(e).lower():
|
||||
print(f"⚠️ ROS2 context 錯誤(忽略): {e}", flush=True)
|
||||
else:
|
||||
print(f"ROS spin error: {e}", flush=True)
|
||||
except Exception as e:
|
||||
print(f"ROS spin error: {e}")
|
||||
print(f"ROS spin error: {e}", flush=True)
|
||||
|
||||
def closeEvent(self, event):
|
||||
for group in self.mission_groups.values():
|
||||
if group.executor:
|
||||
group.executor.stop()
|
||||
self.command_sender.close()
|
||||
for receiver in self.udp_receivers:
|
||||
receiver.stop()
|
||||
for receiver in self.monitor.ws_receivers:
|
||||
receiver.stop()
|
||||
self.monitor.destroy_node()
|
||||
self.executor.shutdown()
|
||||
rclpy.shutdown()
|
||||
try:
|
||||
for group in self.mission_groups.values():
|
||||
if group.executor:
|
||||
group.executor.stop()
|
||||
self.command_sender.close()
|
||||
for receiver in self.udp_receivers:
|
||||
receiver.stop()
|
||||
for receiver in self.monitor.ws_receivers:
|
||||
receiver.stop()
|
||||
self.monitor.destroy_node()
|
||||
self.executor.shutdown()
|
||||
except Exception as e:
|
||||
print(f"⚠️ 清理資源時出錯: {e}", flush=True)
|
||||
|
||||
# 安全地 shutdown ROS2
|
||||
try:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
except RuntimeError as e:
|
||||
print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True)
|
||||
|
||||
event.accept()
|
||||
|
||||
|
||||
|
||||
+46
-22
@@ -146,6 +146,8 @@ class GroupPanel(QWidget):
|
||||
box_select_requested = pyqtSignal(str) # group_id — 框選直接分配
|
||||
select_all_requested = pyqtSignal(str) # group_id — 全選直接分配
|
||||
clear_group_requested = pyqtSignal(str) # group_id — 清除分組
|
||||
add_group_requested = pyqtSignal() # 新增群組
|
||||
delete_group_requested = pyqtSignal(str) # group_id — 刪除群組
|
||||
|
||||
BUTTON_STYLE = """
|
||||
QPushButton {{ background-color: {bg}; color: {fg}; border: none;
|
||||
@@ -157,6 +159,8 @@ class GroupPanel(QWidget):
|
||||
def __init__(self, group: MissionGroup, parent=None):
|
||||
super().__init__(parent)
|
||||
self.group = group
|
||||
self._is_all_selected = False # 追蹤全選狀態
|
||||
self.all_btn_ref = None # 保存全選按鈕的參考
|
||||
self._build_ui()
|
||||
|
||||
def _make_sep(self):
|
||||
@@ -296,7 +300,7 @@ class GroupPanel(QWidget):
|
||||
mid.addLayout(mid_body)
|
||||
|
||||
# ============================
|
||||
# 選取與分組(2x2 按鈕)
|
||||
# 選取與分組(3x2 按鈕)
|
||||
# ============================
|
||||
right = QVBoxLayout()
|
||||
right.setSpacing(3)
|
||||
@@ -310,9 +314,28 @@ class GroupPanel(QWidget):
|
||||
self.drone_list_label.setWordWrap(True)
|
||||
right.addWidget(self.drone_list_label)
|
||||
|
||||
# 2x2 按鈕網格
|
||||
# 3x2 按鈕網格:第一行 框選 全選 新增群組
|
||||
grid_r1 = QHBoxLayout()
|
||||
grid_r1.setSpacing(3)
|
||||
box_btn = QPushButton("框選")
|
||||
box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
|
||||
box_btn.clicked.connect(
|
||||
lambda: self.box_select_requested.emit(self.group.group_id))
|
||||
all_btn = QPushButton("全選/取消")
|
||||
all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
|
||||
all_btn.clicked.connect(self._on_all_select_clicked)
|
||||
self.all_btn_ref = all_btn # 保存按鈕參考(備用)
|
||||
add_group_btn = QPushButton("+ 新增群組")
|
||||
add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
|
||||
add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
|
||||
grid_r1.addWidget(box_btn)
|
||||
grid_r1.addWidget(all_btn)
|
||||
grid_r1.addWidget(add_group_btn)
|
||||
right.addLayout(grid_r1)
|
||||
|
||||
# 第二行 編輯分配 清除分組 刪除群組
|
||||
grid_r2 = QHBoxLayout()
|
||||
grid_r2.setSpacing(3)
|
||||
assign_btn = QPushButton("編輯分配")
|
||||
assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
|
||||
assign_btn.clicked.connect(
|
||||
@@ -321,22 +344,13 @@ class GroupPanel(QWidget):
|
||||
clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
|
||||
clear_btn.clicked.connect(
|
||||
lambda: self.clear_group_requested.emit(self.group.group_id))
|
||||
grid_r1.addWidget(assign_btn)
|
||||
grid_r1.addWidget(clear_btn)
|
||||
right.addLayout(grid_r1)
|
||||
|
||||
grid_r2 = QHBoxLayout()
|
||||
grid_r2.setSpacing(3)
|
||||
box_btn = QPushButton("框選")
|
||||
box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
|
||||
box_btn.clicked.connect(
|
||||
lambda: self.box_select_requested.emit(self.group.group_id))
|
||||
all_btn = QPushButton("全選")
|
||||
all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
|
||||
all_btn.clicked.connect(
|
||||
lambda: self.select_all_requested.emit(self.group.group_id))
|
||||
grid_r2.addWidget(box_btn)
|
||||
grid_r2.addWidget(all_btn)
|
||||
delete_group_btn = QPushButton("刪除群組")
|
||||
delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
|
||||
delete_group_btn.clicked.connect(
|
||||
lambda: self.delete_group_requested.emit(self.group.group_id))
|
||||
grid_r2.addWidget(assign_btn)
|
||||
grid_r2.addWidget(clear_btn)
|
||||
grid_r2.addWidget(delete_group_btn)
|
||||
right.addLayout(grid_r2)
|
||||
|
||||
right.addStretch()
|
||||
@@ -410,13 +424,15 @@ class GroupPanel(QWidget):
|
||||
self.type_combo.currentTextChanged.connect(self._update_param_visibility)
|
||||
|
||||
# ── 組裝四欄:控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
|
||||
cols.addLayout(left, 1)
|
||||
# 使用伸展因子 0 讓列根據內容自動調整寬度,而不是均等分配
|
||||
cols.addLayout(left, 0)
|
||||
cols.addWidget(self._make_sep())
|
||||
cols.addLayout(mid, 1)
|
||||
cols.addLayout(mid, 0)
|
||||
cols.addWidget(self._make_sep())
|
||||
cols.addLayout(param_col, 1)
|
||||
cols.addLayout(param_col, 0)
|
||||
cols.addWidget(self._make_sep())
|
||||
cols.addLayout(right, 1)
|
||||
cols.addLayout(right, 0)
|
||||
cols.addStretch() # 填充剩餘空間,使四列置左
|
||||
|
||||
layout.addLayout(cols)
|
||||
|
||||
@@ -451,6 +467,14 @@ class GroupPanel(QWidget):
|
||||
self.status_label.setStyleSheet(
|
||||
f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
|
||||
|
||||
def _on_all_select_clicked(self):
|
||||
"""全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
|
||||
self.select_all_requested.emit(self.group.group_id)
|
||||
|
||||
def set_all_select_state(self, is_selected):
|
||||
"""外部設置全選狀態(按鈕文本保持「全選/取消」)"""
|
||||
self._is_all_selected = is_selected
|
||||
|
||||
def _update_param_visibility(self, _=None):
|
||||
"""根據當前任務類型,顯示/隱藏對應的參數列"""
|
||||
mission_type = self.type_combo.currentText()
|
||||
|
||||
Reference in New Issue
Block a user