longCommand

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
+    CommandLongClient = None
-    takeoff = 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):
-        """
+        """使用 CommandLongClient 切換無人機飛行模式"""
-        使用 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:
+            # 解析 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}")
+            print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
             
-        # 檢查 fc_network_apps 的 change_mode 函數是否可用
+            # 在線程池中調用 CommandLongClient
-        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():
+            def _call_set_mode():
-                print(f"\n   🔄 [_call_change_mode] 在線程池中調用 change_mode...")
+                client = CommandLongClient() if CommandLongClient else None
-                print(f"      ├─ 線程開始時間: {__import__('time').time()}")
+                if not client:
-                print(f"      ├─ 目標: sysid={sysid}, mode={custom_mode}\n")
+                    return False
-                result = change_mode(
+                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}")
+                client.destroy_node()
-                print(f"      └─ 線程任務完成")
+                return result.success if result else False
-                return result
             
-            print(f"   📢 [SET_MODE] 提交 change_mode 到線程池...")
+            result = await loop.run_in_executor(executor, _call_set_mode)
-            result = await loop.run_in_executor(executor, _call_change_mode)
+            if result:
-            print(f"\n   ✓ [SET_MODE] 從線程池接收到返回值")
+                print(f"✅ [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"❌ [SET_MODE] 模式切換失敗")
-                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"❌ [SET_MODE] 例外錯誤: {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):
-        """
+        """使用 CommandLongClient 執行 ARM/DISARM"""
-        使用 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
         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] 無效的 drone_id 格式: {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
             
+            action_name = "解鎖" if arm else "上鎖"
+            print(f"\n📢 [ARM] {drone_id} → {action_name}")
             
-        try:
+            # 在線程池中調用 CommandLongClient
-            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():
+            def _call_arm_disarm():
-                """直接調用 ROS2 服務"""
+                client = CommandLongClient() if CommandLongClient else None
-                import time
+                if not client:
-                print(f"\n   🔄 [_call_ros2_arm_service] 在線程池中調用 ROS2 服務...")
+                    return False
-                print(f"      ├─ 時間: {time.time()}")
+                result = client.arm_disarm(
-                print(f"      ├─ 目標: sysid={sysid}, arm={arm}")
+                    target_sysid=sysid,
-                print(f"      └─ 直接調用ROS2服務（避免rclpy.init()衝突）\n")
+                    arm=arm,
+                    target_compid=0,
+                    timeout_sec=2.0,
+                )
+                client.destroy_node()
+                return result.success if result else False
             
-                try:
+            result = await loop.run_in_executor(executor, _call_arm_disarm)
-                    # 建立 ROS2 客戶端（使用現有 context）
+            if result:
-                    client = self.create_client(MavCommandLong, "/fc_network/vehicle/send_command_long")
+                print(f"✅ [ARM] {action_name}成功")
-                    
-                    # 等待服務
-                    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"❌ [ARM] {action_name}失敗")
-                print(f"\n❌ [ARM_DISARM] 無人機{action_name}失敗！")
                 return False
-                
         except Exception as e:
-            self.get_logger().error(f"Exception during arm state change for {drone_id}: {e}")
+            print(f"❌ [ARM] 例外錯誤: {e}")
-            print(f"\n❌ [ARM_DISARM] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
     async def takeoff_drone(self, drone_id, altitude):
-        """
+        """使用 CommandLongClient 執行無人機起飛"""
-        使用 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:
+            # 解析 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 函數是否可用
+            print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
-        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:
+            # 在線程池中調用 CommandLongClient
-            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})...")
+                client = CommandLongClient() if CommandLongClient else None
-                result = takeoff(
+                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() 返回結果")
+                client.destroy_node()
-                return result
+                return result.success if result else False
             
-            print(f"   📢 [TAKEOFF] 提交 takeoff 到線程池...")
             result = await loop.run_in_executor(executor, _call_takeoff)
-            
+            if result:
-            print(f"\n📥 [TAKEOFF] 從 takeoff() 接收服務響應:")
+                print(f"✅ [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"❌ [TAKEOFF] 起飛失敗")
-                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"❌ [TAKEOFF] 例外錯誤: {e}")
-            print(f"\n❌ [TAKEOFF] 例外錯誤: {e}")
-            import traceback
             traceback.print_exc()
             return False
 

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）
+        # 使用 asyncio 執行（通過事件循環）
-        def do_mode_changes_threaded():
+        async def do_mode_changes_async():
-            print(f"\n   【後台線程】開始執行模式切換", flush=True)
+            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)
+                print(f"\n   【切換】{drone_id} → {mode}", flush=True)
                 try:
-                    # 導入模式切換函數
+                    result = await self.monitor.set_mode(drone_id, mode)
-                    from fc_network_apps.changeMode import change_mode
                     
-                    # 解析 sysid（從 drone_id 的最後一個數字）
+                    if result:
-                    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}"
+                        msg = f"❌ {drone_id} 切換失敗"
                         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)
-                        traceback.print_exc()
-                        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)
-        print(f"   【排隊】將任務提交至後台線程", flush=True)
+        loop = asyncio.get_event_loop()
-        thread = threading.Thread(target=do_mode_changes_threaded, daemon=True)
+        asyncio.run_coroutine_threadsafe(
-        thread.start()
+            do_mode_changes_async(),
+            loop
+        )
 
     def _handle_group_arm(self, group_id):
         """解鎖群組內所有無人機"""