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

longCommand

Browse Source
chiyu
ken910606 1 month ago
parent 989d3ad2d2
commit f34693a400
2 changed files with 89 additions and 333 deletions
Show Stats Download Patch File Download Diff File

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

@ -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}")
# 獲取模式對應的 custom_mode 值
custom_mode = self.MODE_MAPPING.get(mode_name)
if custom_mode is None:
print(f"❌ [SET_MODE] 未知模式: {mode_name}")
return False
# 檢查 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
print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
# 使用 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 函數
# 在線程池中調用 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)}")
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")
client.destroy_node()
return result.success if result else False
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
action_name = "解鎖" if arm else "上鎖"
print(f"\n📢 [ARM] {drone_id} → {action_name}")
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
# 在線程池中調用 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
print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
try:
print(f"\n📢 [TAKEOFF] 開始起飛無人機")
print(f" Drone ID: {drone_id}")
print(f" ROS2 服務呼叫: target_sysid={sysid}, altitude_m={altitude}")
# 在線程池中運行同步的 takeoff 函數
# 在線程池中調用 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

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

@ -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,70 +737,22 @@ 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
# 解析 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))
result = await self.monitor.set_mode(drone_id, mode)
except Exception as service_error:
msg = f"❌ {drone_id} 服務調用錯誤: {str(service_error)}"
if result:
msg = f"✅ {drone_id} 切換成功"
print(f" {msg}", flush=True)
self.message_queue.put((msg, 2000))
else:
msg = f"❌ {drone_id} 切換失敗"
print(f" {msg}", flush=True)
traceback.print_exc()
self.message_queue.put((msg, 2000))
except Exception as e:
@ -808,15 +760,16 @@ class ControlStationUI(QMainWindow):
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):
"""解鎖群組內所有無人機"""

Write Preview
Loading…
Cancel
Save