|
|
from rclpy.node import Node
|
|
|
from PyQt6.QtCore import QObject, pyqtSignal
|
|
|
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
|
|
|
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 的 longCommand（統一的 MAV_CMD_* API）
|
|
|
try:
|
|
|
from fc_network_apps.longCommand import CommandLongClient
|
|
|
except ImportError as e:
|
|
|
import traceback
|
|
|
print(f"⚠️ 警告: 無法導入 CommandLongClient")
|
|
|
print(f" 错误: {e}")
|
|
|
print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
|
|
|
print(f" 完整堆栈跟踪:")
|
|
|
traceback.print_exc()
|
|
|
CommandLongClient = None
|
|
|
|
|
|
class DroneSignals(QObject):
|
|
|
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
|
|
|
|
|
|
class UDPMavlinkReceiver(threading.Thread):
|
|
|
"""UDP MAVLink 接收器"""
|
|
|
def __init__(self, ip, port, signals, connection_name, monitor=None):
|
|
|
super().__init__(daemon=True)
|
|
|
self.ip = ip
|
|
|
self.port = port
|
|
|
self.signals = signals
|
|
|
self.connection_name = connection_name
|
|
|
self.monitor = monitor # 保存 monitor 引用
|
|
|
self.socket_id = monitor.get_next_socket_id() if monitor else 0
|
|
|
self.running = False
|
|
|
self.sock = None
|
|
|
|
|
|
def run(self):
|
|
|
"""執行 UDP 接收循環"""
|
|
|
self.running = True
|
|
|
try:
|
|
|
print(f"UDP MAVLink receiver started on {self.ip}:{self.port}")
|
|
|
|
|
|
# 創建 MAVLink 連接
|
|
|
mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}')
|
|
|
while self.running:
|
|
|
try:
|
|
|
msg = mav.recv_match(blocking=True, timeout=1.0)
|
|
|
if msg is None:
|
|
|
continue
|
|
|
|
|
|
self.process_mavlink_message(msg)
|
|
|
|
|
|
except socket.timeout:
|
|
|
continue
|
|
|
except Exception as e:
|
|
|
print(f"Error receiving MAVLink message: {e}")
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"UDP receiver error: {e}")
|
|
|
finally:
|
|
|
if self.sock:
|
|
|
self.sock.close()
|
|
|
|
|
|
def process_mavlink_message(self, msg):
|
|
|
"""處理 MAVLink 訊息"""
|
|
|
try:
|
|
|
msg_type = msg.get_type()
|
|
|
system_id = msg.get_srcSystem()
|
|
|
drone_id = f"s{self.socket_id}_{system_id}"
|
|
|
|
|
|
if msg_type == "HEARTBEAT":
|
|
|
# 先發送連接類型資訊
|
|
|
self.signals.update_signal.emit('connection_type', drone_id, {
|
|
|
'type': 'UDP'
|
|
|
})
|
|
|
mode = mavutil.mode_string_v10(msg)
|
|
|
armed = bool(msg.base_mode & 128)
|
|
|
self.signals.update_signal.emit('state', drone_id, {
|
|
|
'mode': mode,
|
|
|
'armed': armed
|
|
|
})
|
|
|
|
|
|
elif msg_type == "BATTERY_STATUS":
|
|
|
voltage = msg.voltages[0] / 1000
|
|
|
self.signals.update_signal.emit('battery', drone_id, {
|
|
|
'voltage': voltage
|
|
|
})
|
|
|
|
|
|
elif msg_type == "GLOBAL_POSITION_INT":
|
|
|
latitude = msg.lat / 1e7
|
|
|
longitude = msg.lon / 1e7
|
|
|
self.signals.update_signal.emit('gps', drone_id, {
|
|
|
'lat': latitude,
|
|
|
'lon': longitude
|
|
|
})
|
|
|
|
|
|
elif msg_type == "GPS_RAW_INT":
|
|
|
fix_type = msg.fix_type
|
|
|
|
|
|
elif msg_type == "LOCAL_POSITION_NED":
|
|
|
x = msg.y
|
|
|
y = msg.x
|
|
|
z = -msg.z
|
|
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
|
|
'x': x,
|
|
|
'y': y,
|
|
|
'z': z
|
|
|
})
|
|
|
self.signals.update_signal.emit('altitude', drone_id, {
|
|
|
'altitude': z
|
|
|
})
|
|
|
self.signals.update_signal.emit('velocity', drone_id, {
|
|
|
'vx': msg.vx,
|
|
|
'vy': msg.vy,
|
|
|
'vz': msg.vz
|
|
|
})
|
|
|
|
|
|
elif msg_type == "ATTITUDE":
|
|
|
# 從 MAVLink 訊息中提取並轉為角度
|
|
|
pitch = math.degrees(msg.pitch)
|
|
|
roll = math.degrees(msg.roll)
|
|
|
yaw = math.degrees(msg.yaw)
|
|
|
self.signals.update_signal.emit('attitude', drone_id, {
|
|
|
'pitch': pitch,
|
|
|
'roll': roll,
|
|
|
'yaw': yaw,
|
|
|
'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
|
|
|
})
|
|
|
|
|
|
elif msg_type == "VFR_HUD":
|
|
|
self.signals.update_signal.emit('hud', drone_id, {
|
|
|
'airspeed': msg.airspeed,
|
|
|
'groundspeed': msg.groundspeed,
|
|
|
'heading': msg.heading,
|
|
|
'throttle': msg.throttle,
|
|
|
'alt': msg.alt,
|
|
|
'climb': msg.climb
|
|
|
})
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"Error processing MAVLink message: {e}")
|
|
|
|
|
|
def stop(self):
|
|
|
"""停止接收器"""
|
|
|
self.running = False
|
|
|
|
|
|
class SerialMavlinkReceiver(threading.Thread):
|
|
|
"""串口 MAVLink 接收器"""
|
|
|
def __init__(self, port, baudrate, signals, connection_name, monitor=None):
|
|
|
super().__init__(daemon=True)
|
|
|
self.port = port
|
|
|
self.baudrate = baudrate
|
|
|
self.signals = signals
|
|
|
self.connection_name = connection_name
|
|
|
self.monitor = monitor # 保存 monitor 引用
|
|
|
self.socket_id = monitor.get_next_socket_id() if monitor else 0
|
|
|
self.running = False
|
|
|
self.mav = None
|
|
|
|
|
|
def run(self):
|
|
|
"""執行串口接收循環"""
|
|
|
self.running = True
|
|
|
try:
|
|
|
print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
|
|
|
|
|
|
# 創建 MAVLink 串口連接
|
|
|
self.mav = mavutil.mavlink_connection(
|
|
|
self.port,
|
|
|
baud=self.baudrate,
|
|
|
source_system=255
|
|
|
)
|
|
|
|
|
|
print(f"Waiting for heartbeat from {self.port}...")
|
|
|
self.mav.wait_heartbeat()
|
|
|
print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
|
|
|
|
|
|
while self.running:
|
|
|
try:
|
|
|
msg = self.mav.recv_match(blocking=True, timeout=1.0)
|
|
|
if msg is None:
|
|
|
continue
|
|
|
|
|
|
self.process_mavlink_message(msg)
|
|
|
|
|
|
except Exception as e:
|
|
|
if self.running:
|
|
|
print(f"Error receiving MAVLink message from serial: {e}")
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"Serial receiver error: {e}")
|
|
|
finally:
|
|
|
if self.mav:
|
|
|
try:
|
|
|
self.mav.close()
|
|
|
except:
|
|
|
pass
|
|
|
|
|
|
def process_mavlink_message(self, msg):
|
|
|
"""處理 MAVLink 訊息"""
|
|
|
try:
|
|
|
msg_type = msg.get_type()
|
|
|
system_id = msg.get_srcSystem()
|
|
|
drone_id = f"s{self.socket_id}_{system_id}"
|
|
|
|
|
|
if msg_type == "HEARTBEAT":
|
|
|
# 先發送連接類型資訊
|
|
|
self.signals.update_signal.emit('connection_type', drone_id, {
|
|
|
'type': 'Serial'
|
|
|
})
|
|
|
mode = mavutil.mode_string_v10(msg)
|
|
|
armed = bool(msg.base_mode & 128)
|
|
|
self.signals.update_signal.emit('state', drone_id, {
|
|
|
'mode': mode,
|
|
|
'armed': armed
|
|
|
})
|
|
|
|
|
|
elif msg_type == "BATTERY_STATUS":
|
|
|
voltage = msg.voltages[0] / 1000
|
|
|
self.signals.update_signal.emit('battery', drone_id, {
|
|
|
'voltage': voltage
|
|
|
})
|
|
|
|
|
|
elif msg_type == "GLOBAL_POSITION_INT":
|
|
|
latitude = msg.lat / 1e7
|
|
|
longitude = msg.lon / 1e7
|
|
|
self.signals.update_signal.emit('gps', drone_id, {
|
|
|
'lat': latitude,
|
|
|
'lon': longitude
|
|
|
})
|
|
|
|
|
|
elif msg_type == "GPS_RAW_INT":
|
|
|
fix_type = msg.fix_type
|
|
|
|
|
|
elif msg_type == "LOCAL_POSITION_NED":
|
|
|
x = msg.y
|
|
|
y = msg.x
|
|
|
z = -msg.z
|
|
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
|
|
'x': x,
|
|
|
'y': y,
|
|
|
'z': z
|
|
|
})
|
|
|
self.signals.update_signal.emit('altitude', drone_id, {
|
|
|
'altitude': z
|
|
|
})
|
|
|
self.signals.update_signal.emit('velocity', drone_id, {
|
|
|
'vx': msg.vx,
|
|
|
'vy': msg.vy,
|
|
|
'vz': msg.vz
|
|
|
})
|
|
|
|
|
|
elif msg_type == "ATTITUDE":
|
|
|
# 從 MAVLink 訊息中提取並轉為角度
|
|
|
pitch = math.degrees(msg.pitch)
|
|
|
roll = math.degrees(msg.roll)
|
|
|
yaw = math.degrees(msg.yaw)
|
|
|
self.signals.update_signal.emit('attitude', drone_id, {
|
|
|
'pitch': pitch,
|
|
|
'roll': roll,
|
|
|
'yaw': yaw,
|
|
|
'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
|
|
|
})
|
|
|
|
|
|
elif msg_type == "VFR_HUD":
|
|
|
self.signals.update_signal.emit('hud', drone_id, {
|
|
|
'airspeed': msg.airspeed,
|
|
|
'groundspeed': msg.groundspeed,
|
|
|
'heading': msg.heading,
|
|
|
'throttle': msg.throttle,
|
|
|
'alt': msg.alt,
|
|
|
'climb': msg.climb
|
|
|
})
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"Error processing MAVLink message from serial: {e}")
|
|
|
|
|
|
def stop(self):
|
|
|
"""停止接收器"""
|
|
|
self.running = False
|
|
|
|
|
|
class WebSocketMavlinkReceiver(threading.Thread):
|
|
|
"""WebSocket MAVLink 接收器"""
|
|
|
def __init__(self, url, signals, connection_name, monitor=None):
|
|
|
super().__init__(daemon=True)
|
|
|
self.url = url
|
|
|
self.signals = signals
|
|
|
self.connection_name = connection_name
|
|
|
self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False
|
|
|
self.max_retries = 5
|
|
|
self.base_delay = 1.0
|
|
|
|
|
|
def run(self):
|
|
|
"""執行 WebSocket 接收循環"""
|
|
|
self.running = True
|
|
|
asyncio.set_event_loop(asyncio.new_event_loop())
|
|
|
asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
|
|
|
|
|
|
async def ws_client_loop(self):
|
|
|
"""WebSocket 連接的主循環"""
|
|
|
retry_count = 0
|
|
|
|
|
|
print(f"Starting WebSocket client for {self.connection_name} at {self.url}")
|
|
|
|
|
|
while self.running and retry_count < self.max_retries:
|
|
|
try:
|
|
|
async with websockets.connect(self.url) as websocket:
|
|
|
print(f"WebSocket {self.connection_name} connected to {self.url}")
|
|
|
retry_count = 0 # 重置重試計數
|
|
|
|
|
|
async for message in websocket:
|
|
|
if not self.running:
|
|
|
break
|
|
|
|
|
|
try:
|
|
|
data = json.loads(message)
|
|
|
data['_connection_source'] = self.connection_name
|
|
|
self.process_websocket_message(data)
|
|
|
except json.JSONDecodeError as e:
|
|
|
print(f"WebSocket {self.connection_name} JSON decode error: {e}")
|
|
|
except Exception as e:
|
|
|
print(f"WebSocket {self.connection_name} message processing error: {e}")
|
|
|
|
|
|
except websockets.exceptions.ConnectionClosedError:
|
|
|
print(f"WebSocket {self.connection_name} connection closed")
|
|
|
if self.running:
|
|
|
retry_count += 1
|
|
|
if retry_count < self.max_retries:
|
|
|
delay = self.base_delay * (2 ** min(retry_count, 4))
|
|
|
print(f"Reconnecting in {delay}s...")
|
|
|
await asyncio.sleep(delay)
|
|
|
else:
|
|
|
break
|
|
|
except Exception as e:
|
|
|
retry_count += 1
|
|
|
if retry_count < self.max_retries and self.running:
|
|
|
delay = self.base_delay * (2 ** min(retry_count, 4))
|
|
|
print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})")
|
|
|
await asyncio.sleep(delay)
|
|
|
else:
|
|
|
break
|
|
|
|
|
|
print(f"WebSocket client {self.connection_name} stopped")
|
|
|
|
|
|
def process_websocket_message(self, data):
|
|
|
"""處理 WebSocket 訊息"""
|
|
|
try:
|
|
|
system_id = data.get('system_id')
|
|
|
if not system_id:
|
|
|
return
|
|
|
drone_id = f"s{self.socket_id}_{system_id}"
|
|
|
|
|
|
# 模式
|
|
|
if 'mode' in data:
|
|
|
# 先發送連接類型資訊
|
|
|
self.signals.update_signal.emit('connection_type', drone_id, {
|
|
|
'type': 'WS'
|
|
|
})
|
|
|
self.signals.update_signal.emit('state', drone_id, {
|
|
|
'mode': data['mode'],
|
|
|
})
|
|
|
|
|
|
# 電池
|
|
|
if 'battery' in data:
|
|
|
self.signals.update_signal.emit('battery', drone_id, {
|
|
|
'percentage': data['battery']
|
|
|
})
|
|
|
|
|
|
# 位置
|
|
|
if 'position' in data:
|
|
|
pos = data['position']
|
|
|
self.signals.update_signal.emit('gps', drone_id, {
|
|
|
'lat': pos.get('lat', 0),
|
|
|
'lon': pos.get('lon', 0)
|
|
|
})
|
|
|
|
|
|
# Local position - 設定 x, y 為 0.0
|
|
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
|
|
'x': 0.0,
|
|
|
'y': 0.0,
|
|
|
'z': 0.0
|
|
|
})
|
|
|
|
|
|
# Altitude - 設定為 0.0
|
|
|
self.signals.update_signal.emit('altitude', drone_id, {
|
|
|
'altitude': 0.0
|
|
|
})
|
|
|
|
|
|
# 航向
|
|
|
if 'heading' in data:
|
|
|
self.signals.update_signal.emit('hud', drone_id, {
|
|
|
'heading': data['heading'],
|
|
|
'groundspeed': 0.0
|
|
|
})
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"WebSocket message processing error: {e}")
|
|
|
|
|
|
def stop(self):
|
|
|
"""停止接收器"""
|
|
|
self.running = False
|
|
|
|
|
|
class DroneMonitor(Node):
|
|
|
# Subscribe to drone ROS2 topics
|
|
|
_instance = None # Singleton pattern to prevent duplicate nodes
|
|
|
|
|
|
def __init__(self):
|
|
|
# Use a unique node name with timestamp to avoid conflicts on restart
|
|
|
import time
|
|
|
node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
|
|
|
super().__init__(node_name)
|
|
|
self.signals = DroneSignals()
|
|
|
self.drone_topics = {}
|
|
|
self.lock = Lock()
|
|
|
|
|
|
self.arm_clients = {}
|
|
|
self.takeoff_clients = {}
|
|
|
self.setpoint_pubs = {}
|
|
|
self.selected_drones = set()
|
|
|
self.latest_data = {}
|
|
|
|
|
|
# 定義需要過濾的模式
|
|
|
self.filtered_modes = ['Mode(0x000000c0)']
|
|
|
|
|
|
# WebSocket 接收器列表
|
|
|
self.ws_receivers = []
|
|
|
|
|
|
# 串口接收器列表
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】儲存 GPS 資料的字典
|
|
|
# ================================================================================
|
|
|
self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
|
|
|
# ================================================================================
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】Socket ID 重新分配機制 (從 0 開始)
|
|
|
# ================================================================================
|
|
|
self.socket_id_mapping = {} # {原始socket_id: 重新分配的socket_id}
|
|
|
self.socket_id_counter = 0 # 當前分配到的最大socket_id
|
|
|
self.socket_id_lock = Lock() # 線程安全鎖
|
|
|
# ================================================================================
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取)
|
|
|
# ================================================================================
|
|
|
self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'}
|
|
|
self.sys_to_socket_id = {} # {sys_id: assigned_socket_id} e.g. {'sys11': 0}
|
|
|
# ================================================================================
|
|
|
self.serial_receivers = []
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client）
|
|
|
# ================================================================================
|
|
|
# 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件
|
|
|
self.command_long_clients = {} # {drone_id: CommandLongClient}
|
|
|
self.client_lock = Lock() # 保護 clients 字典的訪問
|
|
|
self.client_counter = 0 # 用於生成唯一的 client 節點名稱
|
|
|
self.executor = None # 將在 gui.py 中設置，用於添加新的 clients
|
|
|
# ================================================================================
|
|
|
|
|
|
# 主题检测定时器
|
|
|
self.create_timer(1.0, self.scan_topics)
|
|
|
|
|
|
def get_next_socket_id(self):
|
|
|
"""获取下一个可用的 socket_id（从 0 开始连续分配）"""
|
|
|
with self.socket_id_lock:
|
|
|
current_id = self.socket_id_counter
|
|
|
self.socket_id_counter += 1
|
|
|
return current_id
|
|
|
|
|
|
def get_or_assign_socket_id(self, original_socket_id):
|
|
|
"""根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配）
|
|
|
同一個原始 socket_id 會得到同一個分配的 ID
|
|
|
"""
|
|
|
original_socket_id = str(original_socket_id)
|
|
|
|
|
|
with self.socket_id_lock:
|
|
|
if original_socket_id not in self.socket_id_mapping:
|
|
|
# 分配新的 socket_id
|
|
|
self.socket_id_mapping[original_socket_id] = self.socket_id_counter
|
|
|
self.socket_id_counter += 1
|
|
|
|
|
|
return self.socket_id_mapping[original_socket_id]
|
|
|
|
|
|
def get_or_create_client(self, drone_id):
|
|
|
"""為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件"""
|
|
|
with self.client_lock:
|
|
|
if drone_id not in self.command_long_clients:
|
|
|
try:
|
|
|
# 生成唯一的 client 節點名稱
|
|
|
self.client_counter += 1
|
|
|
unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
|
|
|
client = CommandLongClient(node_name=unique_name)
|
|
|
self.command_long_clients[drone_id] = client
|
|
|
print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
|
|
|
|
|
|
# 將新 client 添加到主執行器（這樣它的回調才能被處理）
|
|
|
if self.executor:
|
|
|
self.executor.add_node(client)
|
|
|
print(f" ✓ 將 {drone_id} 的 client 添加到主執行器")
|
|
|
|
|
|
except TypeError:
|
|
|
# 舊版 CommandLongClient 不支持 node_name 參數，使用預設
|
|
|
client = CommandLongClient()
|
|
|
self.command_long_clients[drone_id] = client
|
|
|
print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
|
|
|
|
|
|
if self.executor:
|
|
|
self.executor.add_node(client)
|
|
|
print(f" ✓ 將 {drone_id} 的 client 添加到主執行器")
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"⚠️ 無法為 {drone_id} 創建 CommandLongClient: {e}")
|
|
|
return None
|
|
|
return self.command_long_clients[drone_id]
|
|
|
|
|
|
def scan_topics(self):
|
|
|
topics = self.get_topic_names_and_types()
|
|
|
drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
|
|
|
|
|
|
found_drones = set()
|
|
|
for topic_name, _ in topics:
|
|
|
if match := drone_pattern.match(topic_name):
|
|
|
sys_id, topic_type = match.groups()
|
|
|
found_drones.add(sys_id)
|
|
|
with self.lock:
|
|
|
self.drone_topics.setdefault(sys_id, set()).add(topic_type)
|
|
|
|
|
|
for sys_id in found_drones:
|
|
|
# 为每个 sys_id 分配 socket_id（如果还没有分配）
|
|
|
# 注意：如果后续 summary 提供了 socket_id，会使用 summary 的映射覆盖
|
|
|
if sys_id not in self.sys_to_socket_id:
|
|
|
# 暂时所有 ROS2 topic 共享同一个 socket_id = 0
|
|
|
self.sys_to_socket_id[sys_id] = 0
|
|
|
|
|
|
if not hasattr(self, f'drone_{sys_id}_subs'):
|
|
|
self.setup_drone(sys_id)
|
|
|
|
|
|
def setup_drone(self, sys_id):
|
|
|
# sys_id 格式： sys11, sys12, ...
|
|
|
base_topic = f'/fc_network/vehicle/{sys_id}'
|
|
|
|
|
|
# Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務)
|
|
|
self.arm_clients[sys_id] = self.create_client(
|
|
|
CommandBool,
|
|
|
f'{base_topic}/cmd/arming'
|
|
|
)
|
|
|
self.takeoff_clients[sys_id] = self.create_client(
|
|
|
CommandTOL,
|
|
|
f'{base_topic}/cmd/takeoff'
|
|
|
)
|
|
|
|
|
|
# Add setpoint publisher
|
|
|
self.setpoint_pubs[sys_id] = self.create_publisher(
|
|
|
Point,
|
|
|
f'{base_topic}/setpoint_position/local',
|
|
|
10
|
|
|
)
|
|
|
|
|
|
subs = {
|
|
|
'battery': self.create_subscription(
|
|
|
BatteryState,
|
|
|
f'{base_topic}/battery',
|
|
|
lambda msg, sid=sys_id: self.battery_callback(sid, msg),
|
|
|
10
|
|
|
),
|
|
|
'position': self.create_subscription(
|
|
|
NavSatFix,
|
|
|
f'{base_topic}/position',
|
|
|
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
|
|
|
10
|
|
|
),
|
|
|
'summary': self.create_subscription(
|
|
|
String,
|
|
|
f'{base_topic}/summary',
|
|
|
lambda msg, sid=sys_id: self.summary_callback(sid, msg),
|
|
|
10
|
|
|
),
|
|
|
'vfr_hud': self.create_subscription(
|
|
|
VfrHud,
|
|
|
f'{base_topic}/vfr_hud',
|
|
|
lambda msg, sid=sys_id: self.hud_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
}
|
|
|
|
|
|
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):
|
|
|
"""使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
|
|
|
try:
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
parts = drone_id.split('_')
|
|
|
if len(parts) < 2:
|
|
|
print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
|
|
|
return False
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
# 獲取模式對應的 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})")
|
|
|
|
|
|
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
if not client:
|
|
|
print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
|
|
|
return False
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
result = await client.change_mode_async(
|
|
|
target_sysid=sysid,
|
|
|
custom_mode=float(custom_mode),
|
|
|
target_compid=0,
|
|
|
base_mode=1.0,
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
)
|
|
|
|
|
|
if result and result.success:
|
|
|
print(f"✅ [SET_MODE] 模式切換成功")
|
|
|
return True
|
|
|
else:
|
|
|
print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
|
|
|
return False
|
|
|
except Exception as e:
|
|
|
print(f"❌ [SET_MODE] 例外錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
return False
|
|
|
|
|
|
async def arm_drone(self, drone_id, arm):
|
|
|
"""使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
|
|
|
try:
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
parts = drone_id.split('_')
|
|
|
if len(parts) < 2:
|
|
|
print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
|
|
|
return False
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
action_name = "解鎖" if arm else "上鎖"
|
|
|
print(f"\n📢 [ARM] {drone_id} → {action_name}")
|
|
|
|
|
|
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
if not client:
|
|
|
print(f"❌ [ARM] CommandLongClient 無法初始化")
|
|
|
return False
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
result = await client.arm_disarm_async(
|
|
|
target_sysid=sysid,
|
|
|
arm=arm,
|
|
|
target_compid=0,
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
)
|
|
|
|
|
|
if result and result.success:
|
|
|
print(f"✅ [ARM] {action_name}成功")
|
|
|
return True
|
|
|
else:
|
|
|
print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
|
|
|
return False
|
|
|
except Exception as e:
|
|
|
print(f"❌ [ARM] 例外錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
return False
|
|
|
|
|
|
async def takeoff_drone(self, drone_id, altitude):
|
|
|
"""使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
|
|
|
try:
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
parts = drone_id.split('_')
|
|
|
if len(parts) < 2:
|
|
|
print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
|
|
|
return False
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
|
|
|
|
|
|
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
if not client:
|
|
|
print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
|
|
|
return False
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
result = await client.takeoff_async(
|
|
|
target_sysid=sysid,
|
|
|
altitude_m=float(altitude),
|
|
|
target_compid=0,
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
)
|
|
|
|
|
|
if result and result.success:
|
|
|
print(f"✅ [TAKEOFF] 起飛成功")
|
|
|
return True
|
|
|
else:
|
|
|
print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
|
|
|
return False
|
|
|
except Exception as e:
|
|
|
print(f"❌ [TAKEOFF] 例外錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
return False
|
|
|
|
|
|
def send_setpoint(self, drone_id, x, y, z):
|
|
|
"""Send setpoint position command"""
|
|
|
if drone_id not in self.setpoint_pubs:
|
|
|
return False
|
|
|
|
|
|
msg = Point()
|
|
|
msg.x = float(x)
|
|
|
msg.y = float(y)
|
|
|
msg.z = float(z)
|
|
|
|
|
|
self.setpoint_pubs[drone_id].publish(msg)
|
|
|
return True
|
|
|
|
|
|
def quaternion_to_euler(self, q):
|
|
|
sinr_cosp = 2 * (q.w * q.x + q.y * q.z)
|
|
|
cosr_cosp = 1 - 2 * (q.x**2 + q.y**2)
|
|
|
roll = math.atan2(sinr_cosp, cosr_cosp)
|
|
|
|
|
|
sinp = 2 * (q.w * q.y - q.z * q.x)
|
|
|
pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp)
|
|
|
|
|
|
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
|
|
|
cosy_cosp = 1 - 2 * (q.y**2 + q.z**2)
|
|
|
yaw = math.atan2(siny_cosp, cosy_cosp)
|
|
|
|
|
|
return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
|
|
|
|
|
|
# callbacks
|
|
|
def attitude_callback(self, drone_id, msg):
|
|
|
if hasattr(msg, 'orientation'):
|
|
|
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
|
|
|
self.latest_data[(drone_id, 'attitude')] = {
|
|
|
'roll': roll,
|
|
|
'pitch': pitch,
|
|
|
'yaw': yaw,
|
|
|
'rates': (msg.angular_velocity.x,
|
|
|
msg.angular_velocity.y,
|
|
|
msg.angular_velocity.z)
|
|
|
}
|
|
|
|
|
|
def battery_callback(self, sys_id, msg):
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
|
|
|
# 如果還沒有從 summary 獲取到映射，則不處理
|
|
|
if actual_drone_id is None:
|
|
|
return
|
|
|
|
|
|
self.latest_data[(actual_drone_id, 'battery')] = {
|
|
|
'voltage': msg.voltage
|
|
|
}
|
|
|
|
|
|
def state_callback(self, drone_id, msg):
|
|
|
mode = msg.mode
|
|
|
if mode in self.filtered_modes:
|
|
|
return
|
|
|
self.latest_data[(drone_id, 'state')] = {
|
|
|
'mode': msg.mode,
|
|
|
'armed': msg.armed
|
|
|
}
|
|
|
|
|
|
def summary_callback(self, sys_id, msg):
|
|
|
"""處理 summary topic (JSON 格式)"""
|
|
|
try:
|
|
|
data = json.loads(msg.data)
|
|
|
mode = data.get('mode_name', 'UNKNOWN')
|
|
|
if mode in self.filtered_modes:
|
|
|
return
|
|
|
|
|
|
# 從 summary 獲取原始 socket_id，並映射到分配的 socket_id
|
|
|
original_socket_id = data.get('socket_id')
|
|
|
if original_socket_id is not None:
|
|
|
# 使用原始 socket_id 獲取或分配統一的 socket_id
|
|
|
assigned_socket_id = self.get_or_assign_socket_id(original_socket_id)
|
|
|
else:
|
|
|
# 如果沒有 socket_id，使用 sys_to_socket_id 映射
|
|
|
assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0)
|
|
|
|
|
|
sysid = data.get('sysid')
|
|
|
if sysid is not None:
|
|
|
actual_drone_id = f's{assigned_socket_id}_{sysid}'
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【關鍵】保存 sys_id 到 actual_drone_id 的映射
|
|
|
# ================================================================================
|
|
|
self.sys_to_actual_id[sys_id] = actual_drone_id
|
|
|
# ================================================================================
|
|
|
else:
|
|
|
# 如果沒有 sysid，使用 sys_id 中的數字
|
|
|
sys_num = sys_id.replace('sys', '')
|
|
|
actual_drone_id = f's{assigned_socket_id}_{sys_num}'
|
|
|
self.sys_to_actual_id[sys_id] = actual_drone_id
|
|
|
|
|
|
# 先發送連接類型資訊
|
|
|
self.signals.update_signal.emit('connection_type', actual_drone_id, {
|
|
|
'type': 'ROS2'
|
|
|
})
|
|
|
|
|
|
self.latest_data[(actual_drone_id, 'state')] = {
|
|
|
'mode': mode,
|
|
|
'armed': data.get('armed', False),
|
|
|
'socket_id': original_socket_id,
|
|
|
'sysid': sysid,
|
|
|
'vehicle_type': data.get('vehicle_type'),
|
|
|
'autopilot': data.get('autopilot'),
|
|
|
'gps_fix': data.get('gps_fix'),
|
|
|
'gps_fix_type': data.get('gps_fix'),
|
|
|
'connected': data.get('connected')
|
|
|
}
|
|
|
except json.JSONDecodeError as e:
|
|
|
print(f"Error parsing summary JSON for {sys_id}: {e}")
|
|
|
except Exception as e:
|
|
|
print(f"Error in summary_callback for {sys_id}: {e}")
|
|
|
|
|
|
def gps_callback(self, sys_id, msg):
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
|
|
|
# 如果還沒有從 summary 獲取到映射，則不處理
|
|
|
if actual_drone_id is None:
|
|
|
return
|
|
|
|
|
|
self.latest_data[(actual_drone_id, 'gps')] = {
|
|
|
'lat': msg.latitude,
|
|
|
'lon': msg.longitude,
|
|
|
'alt': msg.altitude
|
|
|
}
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】儲存 GPS 資料到 drone_gps 字典
|
|
|
# ================================================================================
|
|
|
self.drone_gps[actual_drone_id] = {
|
|
|
'lat': msg.latitude,
|
|
|
'lon': msg.longitude,
|
|
|
'alt': msg.altitude
|
|
|
}
|
|
|
# ================================================================================
|
|
|
|
|
|
def local_vel_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'velocity')] = {
|
|
|
'vx': msg.x,
|
|
|
'vy': msg.y,
|
|
|
'vz': msg.z
|
|
|
}
|
|
|
|
|
|
def altitude_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'altitude')] = {
|
|
|
'altitude': msg.data
|
|
|
}
|
|
|
|
|
|
def local_pose_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'local_pose')] = {
|
|
|
'x': msg.x,
|
|
|
'y': msg.y,
|
|
|
'z': msg.z
|
|
|
}
|
|
|
|
|
|
def hud_callback(self, sys_id, msg):
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
|
|
|
# 如果還沒有從 summary 獲取到映射，則不處理
|
|
|
if actual_drone_id is None:
|
|
|
return
|
|
|
|
|
|
self.latest_data[(actual_drone_id, 'hud')] = {
|
|
|
'airspeed': msg.airspeed,
|
|
|
'groundspeed': msg.groundspeed,
|
|
|
'heading': msg.heading,
|
|
|
'throttle': msg.throttle,
|
|
|
'alt': msg.altitude,
|
|
|
'climb': msg.climb
|
|
|
}
|
|
|
|
|
|
def loss_rate_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'loss_rate')] = {
|
|
|
'loss_rate': msg.data
|
|
|
}
|
|
|
|
|
|
def ping_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'ping')] = {
|
|
|
'ping': msg.data
|
|
|
}
|
|
|
|
|
|
def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
|
|
|
"""啟動串口 MAVLink 連接"""
|
|
|
connection_name = f"Serial_{port.replace('/', '_')}"
|
|
|
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
|
|
|
receiver.start()
|
|
|
self.serial_receivers.append(receiver)
|
|
|
print(f"Started serial connection on {port} at {baudrate} baud")
|
|
|
return receiver |