diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 0168dfa..caa480c 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -4,12 +4,14 @@ import math import re import threading from threading import Lock +from concurrent.futures import ThreadPoolExecutor import asyncio import websockets import json import socket import sys import os +import traceback from pymavlink import mavutil from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import BatteryState, NavSatFix, Imu @@ -22,18 +24,17 @@ _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 的函數 +# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API） try: - from fc_network_apps import change_mode, takeoff + from fc_network_apps.longCommand import CommandLongClient except ImportError as e: import traceback - print(f"⚠️ 警告: 無法導入 fc_network_apps") + print(f"⚠️ 警告: 無法導入 CommandLongClient") print(f" 错误: {e}") print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装") print(f" 完整堆栈跟踪:") traceback.print_exc() - change_mode = None - takeoff = None + CommandLongClient = None class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -585,331 +586,133 @@ class DroneMonitor(Node): # ================================================================================ 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") + """使用 CommandLongClient 切換無人機飛行模式""" try: + # 解析 drone_id 提取 sysid 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 函數 + # 獲取模式對應的 custom_mode 值 + custom_mode = self.MODE_MAPPING.get(mode_name) + if custom_mode is None: + print(f"❌ [SET_MODE] 未知模式: {mode_name}") + return False + + print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})") + + # 在線程池中調用 CommandLongClient 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( + def _call_set_mode(): + client = CommandLongClient() if CommandLongClient else None + if not client: + return False + result = client.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)}") + client.destroy_node() + return result.success if result else False - 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") + result = await loop.run_in_executor(executor, _call_set_mode) + if result: + print(f"✅ [SET_MODE] 模式切換成功") 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") + print(f"❌ [SET_MODE] 模式切換失敗") 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 + print(f"❌ [SET_MODE] 例外錯誤: {e}") traceback.print_exc() - print(f" 返回: False (异常)") return False async def arm_drone(self, drone_id, arm): - """ - 使用 fc_network_apps 的 arm_disarm 函數上鎖/解鎖無人機 - - 參數: - drone_id: 無人機ID (格式: "s{socket_id}_{sysid}") - arm: True 為上鎖, False 為解鎖 - - 返回: - bool: 操作是否成功 - """ - import asyncio - from concurrent.futures import ThreadPoolExecutor - - 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 + """使用 CommandLongClient 執行 ARM/DISARM""" try: + # 解析 drone_id 提取 sysid 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}") + print(f"❌ [ARM] 無效的 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 - - - try: - 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 + action_name = "解鎖" if arm else "上鎖" + print(f"\n📢 [ARM] {drone_id} → {action_name}") + + # 在線程池中調用 CommandLongClient 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) + def _call_arm_disarm(): + client = CommandLongClient() if CommandLongClient else None + if not client: + return False + result = client.arm_disarm( + target_sysid=sysid, + arm=arm, + target_compid=0, + timeout_sec=2.0, + ) + client.destroy_node() + return result.success if result else False - if result_dict['success']: - self.get_logger().info(f"Arm state change successful for {drone_id}") - print(f"\n✅ [ARM_DISARM] 無人機{action_name}成功！") + result = await loop.run_in_executor(executor, _call_arm_disarm) + if result: + print(f"✅ [ARM] {action_name}成功") return True else: - self.get_logger().warning(f"Arm state change failed for {drone_id}") - print(f"\n❌ [ARM_DISARM] 無人機{action_name}失敗！") + print(f"❌ [ARM] {action_name}失敗") return False - except Exception as e: - self.get_logger().error(f"Exception during arm state change for {drone_id}: {e}") - print(f"\n❌ [ARM_DISARM] 例外錯誤: {e}") + print(f"❌ [ARM] 例外錯誤: {e}") + traceback.print_exc() 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 + """使用 CommandLongClient 執行無人機起飛""" try: + # 解析 drone_id 提取 sysid 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 函數 + print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)") + + # 在線程池中調用 CommandLongClient loop = asyncio.get_event_loop() executor = ThreadPoolExecutor(max_workers=1) def _call_takeoff(): - print(f"\n 🔄 [_call_takeoff] 在線程池中調用 takeoff(altitude={altitude})...") - result = takeoff( + client = CommandLongClient() if CommandLongClient else None + if not client: + return False + result = client.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 + client.destroy_node() + return result.success if result else False - 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}") + if result: + print(f"✅ [TAKEOFF] 起飛成功") 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}") + print(f"❌ [TAKEOFF] 起飛失敗") 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 + print(f"❌ [TAKEOFF] 例外錯誤: {e}") traceback.print_exc() return False diff --git a/src/GUI/gui.py b/src/GUI/gui.py index e6f6051..4784545 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -33,7 +33,7 @@ from mission_group import ( # ================================================================================ class ControlStationUI(QMainWindow): - VERSION = '2.0.2' + VERSION = '2.0.3' def __init__(self): super().__init__() @@ -737,86 +737,39 @@ class ControlStationUI(QMainWindow): 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 + # 使用 asyncio 執行（通過事件循環） + async def do_mode_changes_async(): + print(f"\n 【異步任務】開始執行模式切換", flush=True) for drone_id in group.drone_ids: - print(f"\n 【切換】{drone_id} → {mode} (mode={custom_mode})", flush=True) + print(f"\n 【切換】{drone_id} → {mode}", flush=True) try: - # 導入模式切換函數 - from fc_network_apps.changeMode import change_mode + result = await self.monitor.set_mode(drone_id, 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)}" + if result: + msg = f"✅ {drone_id} 切換成功" print(f" {msg}", flush=True) - traceback.print_exc() self.message_queue.put((msg, 2000)) - + else: + msg = f"❌ {drone_id} 切換失敗" + 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) 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() + # 通過事件循環提交異步任務 + print(f" 【排隊】將任務提交至事件循環", flush=True) + loop = asyncio.get_event_loop() + asyncio.run_coroutine_threadsafe( + do_mode_changes_async(), + loop + ) def _handle_group_arm(self, group_id): """解鎖群組內所有無人機"""