Update GUI 2.0.0 from local
This commit is contained in:
+366
-29
@@ -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)
|
||||
|
||||
async def arm_drone(self, drone_id, arm):
|
||||
if drone_id not in self.arm_clients:
|
||||
return False
|
||||
# ================================================================================
|
||||
# 【新增】模式名稱到 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 函數切換無人機飛行模式
|
||||
|
||||
client = self.arm_clients[drone_id]
|
||||
if not client.wait_for_service(timeout_sec=1.0):
|
||||
return False
|
||||
|
||||
request = CommandBool.Request()
|
||||
request.value = arm
|
||||
參數:
|
||||
drone_id: 無人機ID (格式: "s{socket_id}_{sysid}")
|
||||
mode_name: 模式名稱 (例如: "GUIDED", "AUTO", "LOITER")
|
||||
|
||||
future = client.call_async(request)
|
||||
返回:
|
||||
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:
|
||||
response = await future
|
||||
return response.success
|
||||
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'Arm service call failed: {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 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
|
||||
async def arm_drone(self, drone_id, arm):
|
||||
"""
|
||||
使用 fc_network_apps 的 arm_disarm 函數上鎖/解鎖無人機
|
||||
|
||||
future = client.call_async(request)
|
||||
參數:
|
||||
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:
|
||||
response = await future
|
||||
return response.success
|
||||
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
|
||||
|
||||
|
||||
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
|
||||
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):
|
||||
|
||||
Reference in New Issue
Block a user