@ -12,13 +12,23 @@ import socket
import sys
import os
import traceback
try :
import serial
except ImportError :
serial = None
from pymavlink import mavutil
from geometry_msgs . msg import Point , Vector3
from geometry_msgs . msg import Point , Vector3 , Vector3Stamped , PoseWithCovarianceStamped
from sensor_msgs . msg import BatteryState , NavSatFix , Imu
from std_msgs . msg import Float64 , String
from mavros_msgs . msg import State , VfrHud
from nav_msgs . msg import Odometry
from mavros_msgs . srv import CommandBool , CommandTOL
def _log ( level , message ) :
print ( f " [ { level } ] { message } " )
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
_src_path = os . path . dirname ( os . path . dirname ( os . path . abspath ( __file__ ) ) )
if _src_path not in sys . path :
@ -29,16 +39,179 @@ 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 " 完整堆栈跟踪: " )
_log ( " WARN " , " 無法導入 CommandLongClient " )
_log ( " ERROR " , f " 錯誤: { e } " )
_log ( " WARN " , " 這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整 " )
traceback . print_exc ( )
CommandLongClient = None
# 導入 fc_network_apps 的 navigation（ ）
try :
from fc_network_apps . navigation import PositionTargetGlobalIntClient
except ImportError as e :
import traceback
_log ( " WARN " , " 無法導入 PositionTargetGlobalIntClient " )
_log ( " ERROR " , f " 錯誤: { e } " )
traceback . print_exc ( )
PositionTargetGlobalIntClient = None
try :
from fc_interfaces . msg import AttitudeRaw
except ImportError as e :
_log ( " WARN " , " 無法導入 AttitudeRaw " )
_log ( " ERROR " , f " 錯誤: { e } " )
AttitudeRaw = None
class DroneSignals ( QObject ) :
update_signal = pyqtSignal ( str , str , object ) # (msg_type, drone_id, data)
class JsonTelemetryProcessor :
""" 共用 WebSocket JSON telemetry 轉換器。
Canonical JSON fields :
{
" system_id " : 1 ,
" mode " : " GUIDED " ,
" battery " : 85 ,
" position " : { " lat " : 24.0 , " lon " : 120.0 } ,
" heading " : 90
}
Serial JSON uses this same shape ; only the transport / framing differs .
"""
def _emit_json_connection_type ( self , drone_id ) :
self . signals . update_signal . emit ( ' connection_type ' , drone_id , {
' type ' : self . source_type
} )
def process_json_telemetry_message ( self , data ) :
""" 處理 WebSocket JSON 格式的遙測資料。 """
try :
if isinstance ( data , list ) :
for item in data :
if isinstance ( item , dict ) :
self . process_json_telemetry_message ( item )
return
if not isinstance ( data , dict ) :
return
system_id = data . get ( ' system_id ' , data . get ( ' sysid ' ) )
if system_id is None :
return
drone_id = f " s { self . socket_id } _ { system_id } "
self . _emit_json_connection_type ( drone_id )
mode = data . get ( ' mode ' , data . get ( ' mode_name ' ) )
state = { }
if mode is not None :
state [ ' mode ' ] = mode
if ' armed ' in data :
state [ ' armed ' ] = data . get ( ' armed ' )
if state :
self . signals . update_signal . emit ( ' state ' , drone_id , state )
if ' battery ' in data :
battery = data [ ' battery ' ]
if isinstance ( battery , dict ) :
battery_data = { }
if ' percentage ' in battery :
battery_data [ ' percentage ' ] = battery . get ( ' percentage ' )
elif ' percent ' in battery :
battery_data [ ' percentage ' ] = battery . get ( ' percent ' )
if ' voltage ' in battery :
battery_data [ ' voltage ' ] = battery . get ( ' voltage ' )
elif ' voltage_v ' in battery :
battery_data [ ' voltage ' ] = battery . get ( ' voltage_v ' )
if battery_data :
self . signals . update_signal . emit ( ' battery ' , drone_id , battery_data )
else :
self . signals . update_signal . emit ( ' battery ' , drone_id , {
' percentage ' : battery
} )
elif ' battery_percentage ' in data or ' battery_voltage ' in data :
battery_data = { }
if ' battery_percentage ' in data :
battery_data [ ' percentage ' ] = data . get ( ' battery_percentage ' )
if ' battery_voltage ' in data :
battery_data [ ' voltage ' ] = data . get ( ' battery_voltage ' )
self . signals . update_signal . emit ( ' battery ' , drone_id , battery_data )
pos = data . get ( ' position ' )
if isinstance ( pos , dict ) :
gps_data = {
' lat ' : pos . get ( ' lat ' , pos . get ( ' latitude ' , 0 ) ) ,
' lon ' : pos . get ( ' lon ' , pos . get ( ' longitude ' , 0 ) ) ,
' alt ' : pos . get ( ' alt ' , pos . get ( ' altitude ' , 0 ) )
}
self . signals . update_signal . emit ( ' gps ' , drone_id , gps_data )
elif ' lat ' in data or ' latitude ' in data :
self . signals . update_signal . emit ( ' gps ' , drone_id , {
' lat ' : data . get ( ' lat ' , data . get ( ' latitude ' , 0 ) ) ,
' lon ' : data . get ( ' lon ' , data . get ( ' longitude ' , 0 ) ) ,
' alt ' : data . get ( ' alt ' , data . get ( ' altitude ' , 0 ) )
} )
local = data . get ( ' local_position ' , data . get ( ' local_pose ' , data . get ( ' local ' ) ) )
if isinstance ( local , dict ) :
x = local . get ( ' x ' , 0.0 )
y = local . get ( ' y ' , 0.0 )
z = local . get ( ' z ' , 0.0 )
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
} )
elif isinstance ( pos , dict ) :
alt = pos . get ( ' alt ' , pos . get ( ' altitude ' , 0.0 ) )
self . signals . update_signal . emit ( ' local_pose ' , drone_id , {
' x ' : 0.0 ,
' y ' : 0.0 ,
' z ' : alt
} )
self . signals . update_signal . emit ( ' altitude ' , drone_id , {
' altitude ' : alt
} )
velocity = data . get ( ' velocity ' )
if isinstance ( velocity , dict ) :
self . signals . update_signal . emit ( ' velocity ' , drone_id , {
' vx ' : velocity . get ( ' vx ' , velocity . get ( ' x ' , 0.0 ) ) ,
' vy ' : velocity . get ( ' vy ' , velocity . get ( ' y ' , 0.0 ) ) ,
' vz ' : velocity . get ( ' vz ' , velocity . get ( ' z ' , 0.0 ) )
} )
attitude = data . get ( ' attitude ' )
if isinstance ( attitude , dict ) :
self . signals . update_signal . emit ( ' attitude ' , drone_id , {
' roll ' : attitude . get ( ' roll ' , 0.0 ) ,
' pitch ' : attitude . get ( ' pitch ' , 0.0 ) ,
' yaw ' : attitude . get ( ' yaw ' , 0.0 ) ,
' rates ' : attitude . get ( ' rates ' , ( 0.0 , 0.0 , 0.0 ) )
} )
hud = data . get ( ' hud ' , { } )
if not isinstance ( hud , dict ) :
hud = { }
if ' heading ' in data :
hud [ ' heading ' ] = data . get ( ' heading ' )
if hud :
self . signals . update_signal . emit ( ' hud ' , drone_id , {
' heading ' : hud . get ( ' heading ' , 0.0 ) ,
' groundspeed ' : hud . get ( ' groundspeed ' , 0.0 ) ,
' airspeed ' : hud . get ( ' airspeed ' , 0.0 ) ,
' throttle ' : hud . get ( ' throttle ' , 0.0 ) ,
' alt ' : hud . get ( ' alt ' , hud . get ( ' altitude ' , 0.0 ) ) ,
' climb ' : hud . get ( ' climb ' , 0.0 )
} )
except Exception as e :
print ( f " { self . source_type } JSON telemetry processing error: { e } " )
class UDPMavlinkReceiver ( threading . Thread ) :
""" UDP MAVLink 接收器 """
def __init__ ( self , ip , port , signals , connection_name , monitor = None ) :
@ -162,8 +335,8 @@ class UDPMavlinkReceiver(threading.Thread):
""" 停止接收器 """
self . running = False
class SerialMavlinkReceiver ( threading . Thread :
""" 串口 MAVLink 接收器 """
class SerialMavlinkReceiver ( threading . Thread , JsonTelemetryProcessor ):
""" 串口 遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。 """
def __init__ ( self , port , baudrate , signals , connection_name , monitor = None ) :
super ( ) . __init__ ( daemon = True )
self . port = port
@ -172,47 +345,125 @@ class SerialMavlinkReceiver(threading.Thread):
self . connection_name = connection_name
self . monitor = monitor # 保存 monitor 引用
self . socket_id = monitor . get_next_socket_id ( ) if monitor else 0
self . source_type = ' Serial '
self . running = False
self . mav = None
self . serial_conn = None
self . mav_parser = mavutil . mavlink . MAVLink ( None )
self . mav_parser . robust_parsing = True
self . json_buffer = [ ]
self . json_depth = 0
self . json_in_string = False
self . json_escape = False
self . _detected_protocols = set ( )
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 (
print ( f " Serial receiver started on { self . port } at { self . baudrate } baud (MAVLink/JSON auto detect) " )
if serial is None :
raise RuntimeError ( " pyserial 未安裝，無法啟動 Serial 連線 " )
self . serial_conn = serial . Serial (
self . port ,
baud = self . baudrate ,
source_system= 255
self . baudrate ,
timeout= 0.2
)
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 :
chunk = self . serial_conn . read ( 256 )
if not chunk :
continue
self . process_mavlink_message ( msg )
for raw_byte in chunk :
byte = bytes ( [ raw_byte ] )
self . _process_json_byte ( byte )
self . _process_mavlink_byte ( byte )
except Exception as e :
if self . running :
print ( f " Error receiving MAVLink message from serial: { e } " )
print ( f " Error receiving serial telemetry : { e } " )
except Exception as e :
print ( f " Serial receiver error: { e } " )
finally :
if self . mav :
if self . serial_conn :
try :
self . mav . close ( )
except :
self . serial_conn . close ( )
except Exception :
pass
def _process_mavlink_byte ( self , byte ) :
try :
msg = self . mav_parser . parse_char ( byte )
if msg is None :
return
if ' MAVLink ' not in self . _detected_protocols :
print ( f " Serial { self . connection_name } detected MAVLink " )
self . _detected_protocols . add ( ' MAVLink ' )
self . process_mavlink_message ( msg )
except Exception :
# MAVLink parser is deliberately fed the whole stream, including JSON bytes.
return
def _process_json_byte ( self , byte ) :
try :
char = byte . decode ( ' utf-8 ' )
except UnicodeDecodeError :
if self . json_buffer :
self . _reset_json_framing ( )
return
if not self . json_buffer :
if char . isspace ( ) :
return
if char not in ( ' { ' , ' [ ' ) :
return
self . json_depth = 1
self . json_in_string = False
self . json_escape = False
self . json_buffer = [ char ]
return
self . json_buffer . append ( char )
if self . json_in_string :
if self . json_escape :
self . json_escape = False
elif char == ' \\ ' :
self . json_escape = True
elif char == ' " ' :
self . json_in_string = False
return
if char == ' " ' :
self . json_in_string = True
elif char in ( ' { ' , ' [ ' ) :
self . json_depth + = 1
elif char in ( ' } ' , ' ] ' ) :
self . json_depth - = 1
if self . json_depth > 0 :
return
payload = ' ' . join ( self . json_buffer )
self . _reset_json_framing ( )
try :
data = json . loads ( payload )
if ' JSON ' not in self . _detected_protocols :
print ( f " Serial { self . connection_name } detected JSON " )
self . _detected_protocols . add ( ' JSON ' )
self . process_json_telemetry_message ( data )
except json . JSONDecodeError as e :
print ( f " Serial { self . connection_name } JSON decode error: { e } " )
def _reset_json_framing ( self ) :
self . json_buffer = [ ]
self . json_depth = 0
self . json_in_string = False
self . json_escape = False
def process_mavlink_message ( self , msg ) :
""" 處理 MAVLink 訊息 """
try :
@ -295,15 +546,23 @@ class SerialMavlinkReceiver(threading.Thread):
def stop ( self ) :
""" 停止接收器 """
self . running = False
if self . serial_conn :
try :
self . serial_conn . close ( )
except Exception :
pass
class WebSocketMavlinkReceiver ( threading . Thread ) :
class WebSocketMavlinkReceiver ( threading . Thread , JsonTelemetryProcessor ):
""" 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 . monitor = monitor # 保存 monitor 引用
self . socket_id = monitor . get_next_socket_id ( ) if monitor else 0 # 一次性分配 socket_id
self . source_type = ' WS '
self . running = False
self . max_retries = 5
self . base_delay = 1.0
@ -331,7 +590,8 @@ class WebSocketMavlinkReceiver(threading.Thread):
try :
data = json . loads ( message )
data [ ' _connection_source ' ] = self . connection_name
if isinstance ( data , dict ) :
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 } " )
@ -361,57 +621,7 @@ class WebSocketMavlinkReceiver(threading.Thread):
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 } " )
self . process_json_telemetry_message ( data )
def stop ( self ) :
""" 停止接收器 """
@ -467,14 +677,20 @@ class DroneMonitor(Node):
self . serial_receivers = [ ]
# ================================================================================
# 【新增】初始化 CommandLongClient （持久化，不會每次調用都創建/銷毀 ）
# 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client ）
# ================================================================================
self . command_long_client = None
try :
self . command_long_client = CommandLongClient ( )
except Exception as e :
print ( f " ⚠️ 警告: 無法初始化 CommandLongClient: { e } " )
self . command_long_client = None
# 改為為每個 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
# ================================================================================
# ================================================================================
# PositionTargetGlobalIntClient 字典（ ， ）
# ================================================================================
self . position_target_clients = { } # {drone_id: PositionTargetGlobalIntClient}
self . pos_client_counter = 0
# ================================================================================
# 主题检测定时器
@ -501,6 +717,58 @@ class DroneMonitor(Node):
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
_log ( " INFO " , f " 已為 { drone_id } 建立 CommandLongClient (node= { unique_name } ) " )
# 將新 client 添加到主執行器（這樣它的回調才能被處理）
if self . executor :
self . executor . add_node ( client )
_log ( " INFO " , f " 已將 { drone_id } 的 CommandLongClient 加入主執行器 " )
except TypeError :
# 舊版 CommandLongClient 不支持 node_name 參數，使用預設
client = CommandLongClient ( )
self . command_long_clients [ drone_id ] = client
_log ( " INFO " , f " 已為 { drone_id } 建立 CommandLongClient (使用預設名稱) " )
if self . executor :
self . executor . add_node ( client )
_log ( " INFO " , f " 已將 { drone_id } 的 CommandLongClient 加入主執行器 " )
except Exception as e :
_log ( " WARN " , f " 無法為 { drone_id } 建立 CommandLongClient: { e } " )
return None
return self . command_long_clients [ drone_id ]
def get_or_create_position_client ( self , drone_id ) :
""" 為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。 """
if PositionTargetGlobalIntClient is None :
return None
with self . client_lock :
if drone_id not in self . position_target_clients :
try :
self . pos_client_counter + = 1
unique_name = f " pos_target_client_ { drone_id } _ { self . pos_client_counter } "
client = PositionTargetGlobalIntClient ( node_name = unique_name )
self . position_target_clients [ drone_id ] = client
_log ( " INFO " , f " 已為 { drone_id } 建立 PositionTargetGlobalIntClient (node= { unique_name } ) " )
if self . executor :
self . executor . add_node ( client )
_log ( " INFO " , f " 已將 { drone_id } 的 PositionTargetGlobalIntClient 加入主執行器 " )
except Exception as e :
_log ( " WARN " , f " 無法為 { drone_id } 建立 PositionTargetGlobalIntClient: { e } " )
return None
return self . position_target_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+) ' )
@ -520,8 +788,38 @@ class DroneMonitor(Node):
# 暂时所有 ROS2 topic 共享同一个 socket_id = 0
self . sys_to_socket_id [ sys_id ] = 0
if not hasattr ( self , f ' drone_ { sys_id } _subs ' ) :
subs_attr = f ' drone_ { sys_id } _subs '
if not hasattr ( self , subs_attr ) :
self . setup_drone ( sys_id )
else :
# 檢查既有訂閱是否包含 position_ned / attitude， （ ）
subs = getattr ( self , subs_attr , { } )
if isinstance ( subs , dict ) and ' position_ned ' not in subs :
base_topic = f ' /fc_network/vehicle/ { sys_id } '
try :
pos_ned_sub = self . create_subscription (
Odometry ,
f ' { base_topic } /position_ned ' ,
lambda msg , sid = sys_id : self . position_ned_callback ( sid , msg ) ,
10
)
subs [ ' position_ned ' ] = pos_ned_sub
setattr ( self , subs_attr , subs ) # 明確保存更新後的字典
except Exception as e :
pass
if isinstance ( subs , dict ) and ' attitude ' not in subs and AttitudeRaw is not None :
base_topic = f ' /fc_network/vehicle/ { sys_id } '
try :
attitude_sub = self . create_subscription (
AttitudeRaw ,
f ' { base_topic } /attitude ' ,
lambda msg , sid = sys_id : self . attitude_callback ( sid , msg ) ,
10
)
subs [ ' attitude ' ] = attitude_sub
setattr ( self , subs_attr , subs )
except Exception :
pass
def setup_drone ( self , sys_id ) :
# sys_id 格式： sys11, sys12, ...
@ -568,8 +866,22 @@ class DroneMonitor(Node):
f ' { base_topic } /vfr_hud ' ,
lambda msg , sid = sys_id : self . hud_callback ( sid , msg ) ,
10
) ,
' position_ned ' : self . create_subscription (
Odometry ,
f ' { base_topic } /position_ned ' ,
lambda msg , sid = sys_id : self . position_ned_callback ( sid , msg ) ,
10
)
}
if AttitudeRaw is not None :
subs [ ' attitude ' ] = self . create_subscription (
AttitudeRaw ,
f ' { base_topic } /attitude ' ,
lambda msg , sid = sys_id : self . attitude_callback ( sid , msg ) ,
10
)
setattr ( self , f ' drone_ { sys_id } _subs ' , subs )
@ -607,39 +919,41 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id . split ( ' _ ' )
if len ( parts ) < 2 :
print ( f " ❌ { drone_id } " )
_log ( " ERROR " , f " { 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 " ❌ { mode_name } " )
_log ( " ERROR " , f " { mode_name } " )
return False
print ( f " \n 📢 [SET_MODE] { drone_id } → { mode_name } (custom_mode= { custom_mode } ) " )
_log ( " INFO " , f " [SET_MODE] { drone_id } -> { mode_name } (custom_mode= { custom_mode } ) " )
if not self . command_long_client :
print ( f " ❌ [SET_MODE] CommandLongClient 未初始化 " )
# 獲取或創建該 drone 專用的 client（ ）
client = self . get_or_create_client ( drone_id )
if not client :
_log ( " ERROR " , " [SET_MODE] CommandLongClient 無法初始化 " )
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
result = await self . command_long_ client. change_mode_async (
result = await . change_mode_async (
target_sysid = sysid ,
custom_mode = float ( custom_mode ) ,
target_compid = 0 ,
base_mode = 1.0 ,
timeout_sec = 2.0,
timeout_sec = 5.0, # 增加超時時間以提高多機操作的可靠性
)
if result and result . success :
print ( f " ✅ [SET_MODE] " )
_log ( " INFO " , f " [SET_MODE] { drone_id } " )
return True
else :
print ( f " ❌ { result . message if result else ' None ' } ) " )
_log ( " ERROR " , f " { result . message if result else ' None ' } ) " )
return False
except Exception as e :
print ( f " ❌ { e } " )
_log ( " ERROR " , f " { e } " )
traceback . print_exc ( )
return False
@ -649,33 +963,35 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id . split ( ' _ ' )
if len ( parts ) < 2 :
print ( f " ❌ { drone_id } " )
_log ( " ERROR " , f " { drone_id } " )
return False
sysid = int ( parts [ - 1 ] )
action_name = " 解鎖 " if arm else " 上鎖 "
print ( f " \n 📢 [ARM] { drone_id } → { action_name } " )
_log ( " INFO " , f " [ARM] { drone_id } -> { action_name } " )
if not self . command_long_client :
print ( f " ❌ [ARM] CommandLongClient 未初始化 " )
# 獲取或創建該 drone 專用的 client（ ）
client = self . get_or_create_client ( drone_id )
if not client :
_log ( " ERROR " , " [ARM] CommandLongClient 無法初始化 " )
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
result = await self . command_long_ client. arm_disarm_async (
result = await . arm_disarm_async (
target_sysid = sysid ,
arm = arm ,
target_compid = 0 ,
timeout_sec = 2.0,
timeout_sec = 5.0, # 增加超時時間以提高多機操作的可靠性
)
if result and result . success :
print ( f " ✅ [ARM] { action_name } 成功 " )
_log ( " INFO " , f " [ARM] { drone_id } { action_name } 成功 " )
return True
else :
print ( f " ❌ [ARM] { action_name } 失敗 (message= { result . message if result else ' None ' } ) " )
_log ( " ERROR " , f " [ARM] { drone_id } { action_name } 失敗 (message= { result . message if result else ' None ' } ) " )
return False
except Exception as e :
print ( f " ❌ { e } " )
_log ( " ERROR " , f " { e } " )
traceback . print_exc ( )
return False
@ -685,32 +1001,34 @@ class DroneMonitor(Node):
# 解析 drone_id 提取 sysid
parts = drone_id . split ( ' _ ' )
if len ( parts ) < 2 :
print ( f " ❌ { drone_id } " )
_log ( " ERROR " , f " { drone_id } " )
return False
sysid = int ( parts [ - 1 ] )
print ( f " \n 📢 [TAKEOFF] { drone_id } → { altitude } m) " )
_log ( " INFO " , f " [TAKEOFF] { drone_id } -> { altitude } m) " )
if not self . command_long_client :
print ( f " ❌ [TAKEOFF] CommandLongClient 未初始化 " )
# 獲取或創建該 drone 專用的 client（ ）
client = self . get_or_create_client ( drone_id )
if not client :
_log ( " ERROR " , " [TAKEOFF] CommandLongClient 無法初始化 " )
return False
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
result = await self . command_long_ client. takeoff_async (
result = await . takeoff_async (
target_sysid = sysid ,
altitude_m = float ( altitude ) ,
target_compid = 0 ,
timeout_sec = 2.0,
timeout_sec = 5.0, # 增加超時時間以提高多機操作的可靠性
)
if result and result . success :
print ( f " ✅ [TAKEOFF] " )
_log ( " INFO " , f " [TAKEOFF] { drone_id } " )
return True
else :
print ( f " ❌ { result . message if result else ' None ' } ) " )
_log ( " ERROR " , f " { result . message if result else ' None ' } ) " )
return False
except Exception as e :
print ( f " ❌ { e } " )
_log ( " ERROR " , f " { e } " )
traceback . print_exc ( )
return False
@ -742,17 +1060,42 @@ class DroneMonitor(Node):
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 attitude_callback ( self , sys_id , msg ) :
""" 處理姿態 topic， """
actual_drone_id = self . sys_to_actual_id . get ( sys_id , None )
if actual_drone_id is None :
return
try :
if hasattr ( msg , ' roll ' ) and hasattr ( msg , ' pitch ' ) and hasattr ( msg , ' yaw ' ) :
data = {
' roll ' : math . degrees ( msg . roll ) ,
' pitch ' : math . degrees ( msg . pitch ) ,
' yaw ' : math . degrees ( msg . yaw ) ,
' rates ' : (
getattr ( msg , ' rollspeed ' , 0.0 ) ,
getattr ( msg , ' pitchspeed ' , 0.0 ) ,
getattr ( msg , ' yawspeed ' , 0.0 ) ,
)
}
elif hasattr ( msg , ' orientation ' ) :
roll , pitch , yaw = self . quaternion_to_euler ( msg . orientation )
data = {
' roll ' : roll ,
' pitch ' : pitch ,
' yaw ' : yaw ,
' rates ' : (
msg . angular_velocity . x ,
msg . angular_velocity . y ,
msg . angular_velocity . z ,
)
}
else :
return
self . latest_data [ ( actual_drone_id , ' attitude ' ) ] = data
except Exception as e :
print ( f " Error parsing attitude msg for { sys_id } : { e } " )
def battery_callback ( self , sys_id , msg ) :
# 使用映射獲取實際的 drone_id
@ -805,6 +1148,7 @@ class DroneMonitor(Node):
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
_log ( " INFO " , f " summary_callback 已建立映射 { sys_id } -> { actual_drone_id } (使用 sys_num) " )
# 先發送連接類型資訊
self . signals . update_signal . emit ( ' connection_type ' , actual_drone_id , {
@ -885,6 +1229,55 @@ class DroneMonitor(Node):
' climb ' : msg . climb
}
def position_ned_callback ( self , sys_id , msg ) :
""" 處理 position_ned topic (nav_msgs/msg/Odometry)
NED 座標系中的位置 （ 在 msg . pose . pose . position ） ：
- x : 北向位移 ( m )
- y : 東向位移 ( m )
- z : 向下位移 ( m ) - 負值表示向下 ， 轉換為高度需要 * ( - 1 )
"""
try :
# 使用映射獲取實際的 drone_id
actual_drone_id = self . sys_to_actual_id . get ( sys_id , None )
# 如果還沒有從 summary 獲取到映射，則不處理
if actual_drone_id is None :
return
# 從 Odometry 消息中提取位置數據 (msg.pose.pose.position)
# Odometry 結構：
x = msg . pose . pose . position . y # NED 座標系中交換 x/y（
y = msg . pose . pose . position . x
z = - msg . pose . pose . position . z # 將向下的 NED z 轉換為向上的高度（
vx = msg . twist . twist . linear . y
vy = msg . twist . twist . linear . x
vz = - msg . twist . twist . linear . z
# 儲存高度信息
self . latest_data [ ( actual_drone_id , ' altitude ' ) ] = {
' altitude ' : z
}
# 儲存本地位置信息
self . latest_data [ ( actual_drone_id , ' local_pose ' ) ] = {
' x ' : x ,
' y ' : y ,
' z ' : z
}
# 儲存速度資訊，
self . latest_data [ ( actual_drone_id , ' velocity ' ) ] = {
' vx ' : vx ,
' vy ' : vy ,
' vz ' : vz
}
# 發送信號給 GUI 更新高度顯示
self . signals . update_signal . emit ( ' altitude ' , actual_drone_id , {
' altitude ' : z
} )
except Exception as e :
pass
def loss_rate_callback ( self , drone_id , msg ) :
self . latest_data [ ( drone_id , ' loss_rate ' ) ] = {
' loss_rate ' : msg . data
@ -896,10 +1289,10 @@ class DroneMonitor(Node):
}
def start_serial_connection ( self , port = ' /dev/ttyUSB0 ' , baudrate = 57600 ) :
""" 啟動串口 MAVLink 連接 """
""" 啟動串口 遙測連接（自動辨識 MAVLink / JSON） """
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
_log ( " INFO " , f " 已啟動 Serial 連線: { port } @ { baudrate } baud (MAVLink/JSON) " )
return receiver
lenting89
