@ -83,7 +83,10 @@ class JsonTelemetryProcessor:
" position " : { " lat " : 24.0 , " lon " : 120.0 } ,
" heading " : 90
}
Serial JSON uses this same shape ; only the transport / framing differs .
Serial JSON also accepts the compact UAV . py shape :
{ " s " : 1 , " m " : " GUIDED " , " a " : 1 , " b " : 85 , " h " : 10.0 ,
" v " : 4.2 , " p " : [ 24.0 , 120.0 ] , " ypr " : [ 90.0 , 0.0 , 0.0 ] ,
" g " : 3 , " d " : [ 0.8 , 1.2 ] }
"""
def _emit_json_connection_type ( self , drone_id ) :
@ -103,19 +106,21 @@ class JsonTelemetryProcessor:
if not isinstance ( data , dict ) :
return
system_id = data . get ( ' system_id ' , data . get ( ' sysid ' )
system_id = data . get ( ' system_id ' , data . get ( ' sysid ' , data . get ( ' s ' ) ))
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 ' )
mode = data . get ( ' mode ' , data . get ( ' mode_name ' , data . get ( ' m ' ) ))
state = { }
if mode is not None :
state [ ' mode ' ] = mode
if ' armed ' in data :
state [ ' armed ' ] = data . get ( ' armed ' )
elif ' a ' in data :
state [ ' armed ' ] = bool ( data . get ( ' a ' ) )
if state :
self . signals . update_signal . emit ( ' state ' , drone_id , state )
@ -144,6 +149,10 @@ class JsonTelemetryProcessor:
if ' battery_voltage ' in data :
battery_data [ ' voltage ' ] = data . get ( ' battery_voltage ' )
self . signals . update_signal . emit ( ' battery ' , drone_id , battery_data )
elif ' b ' in data :
self . signals . update_signal . emit ( ' battery ' , drone_id , {
' percentage ' : data . get ( ' b ' )
} )
pos = data . get ( ' position ' )
if isinstance ( pos , dict ) :
@ -159,6 +168,22 @@ class JsonTelemetryProcessor:
' lon ' : data . get ( ' lon ' , data . get ( ' longitude ' , 0 ) ) ,
' alt ' : data . get ( ' alt ' , data . get ( ' altitude ' , 0 ) )
} )
elif isinstance ( data . get ( ' p ' ) , ( list , tuple ) ) and len ( data . get ( ' p ' ) ) > = 2 :
dop = data . get ( ' d ' ) if isinstance ( data . get ( ' d ' ) , ( list , tuple ) ) else [ ]
gps_data = {
' lat ' : data [ ' p ' ] [ 0 ] ,
' lon ' : data [ ' p ' ] [ 1 ] ,
' alt ' : data . get ( ' h ' , 0 )
}
if ' g ' in data :
gps_data [ ' fix_type ' ] = data . get ( ' g ' )
if len ( dop ) > = 1 :
gps_data [ ' eph ' ] = dop [ 0 ]
gps_data [ ' hdop ' ] = dop [ 0 ]
if len ( dop ) > = 2 :
gps_data [ ' epv ' ] = dop [ 1 ]
gps_data [ ' vdop ' ] = dop [ 1 ]
self . signals . update_signal . emit ( ' gps ' , drone_id , gps_data )
local = data . get ( ' local_position ' , data . get ( ' local_pose ' , data . get ( ' local ' ) ) )
if isinstance ( local , dict ) :
@ -183,6 +208,29 @@ class JsonTelemetryProcessor:
self . signals . update_signal . emit ( ' altitude ' , drone_id , {
' altitude ' : alt
} )
elif ' h ' in data :
height = data . get ( ' h ' , 0.0 )
self . signals . update_signal . emit ( ' local_pose ' , drone_id , {
' x ' : 0.0 ,
' y ' : 0.0 ,
' z ' : height
} )
self . signals . update_signal . emit ( ' altitude ' , drone_id , {
' altitude ' : height
} )
elif (
isinstance ( data . get ( ' p ' ) , ( list , tuple ) )
or ' lat ' in data
or ' latitude ' in data
) :
self . signals . update_signal . emit ( ' local_pose ' , drone_id , {
' x ' : 0.0 ,
' y ' : 0.0 ,
' z ' : 0.0
} )
self . signals . update_signal . emit ( ' altitude ' , drone_id , {
' altitude ' : 0.0
} )
velocity = data . get ( ' velocity ' )
if isinstance ( velocity , dict ) :
@ -191,6 +239,12 @@ class JsonTelemetryProcessor:
' vy ' : velocity . get ( ' vy ' , velocity . get ( ' y ' , 0.0 ) ) ,
' vz ' : velocity . get ( ' vz ' , velocity . get ( ' z ' , 0.0 ) )
} )
elif ' v ' in data :
self . signals . update_signal . emit ( ' velocity ' , drone_id , {
' vx ' : data . get ( ' v ' , 0.0 ) ,
' vy ' : 0.0 ,
' vz ' : 0.0
} )
attitude = data . get ( ' attitude ' )
if isinstance ( attitude , dict ) :
@ -200,12 +254,28 @@ class JsonTelemetryProcessor:
' yaw ' : attitude . get ( ' yaw ' , 0.0 ) ,
' rates ' : attitude . get ( ' rates ' , ( 0.0 , 0.0 , 0.0 ) )
} )
elif isinstance ( data . get ( ' ypr ' ) , ( list , tuple ) ) and len ( data . get ( ' ypr ' ) ) > = 3 :
yaw , pitch , roll = data [ ' ypr ' ] [ 0 ] , data [ ' ypr ' ] [ 1 ] , data [ ' ypr ' ] [ 2 ]
self . signals . update_signal . emit ( ' attitude ' , drone_id , {
' roll ' : roll ,
' pitch ' : pitch ,
' yaw ' : yaw ,
' 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 ' )
elif ' y ' in data :
hud [ ' heading ' ] = data . get ( ' y ' )
elif isinstance ( data . get ( ' ypr ' ) , ( list , tuple ) ) and len ( data . get ( ' ypr ' ) ) > = 1 :
hud [ ' heading ' ] = data [ ' ypr ' ] [ 0 ]
if ' v ' in data and ' groundspeed ' not in hud :
hud [ ' groundspeed ' ] = data . get ( ' v ' )
if ' h ' in data and ' alt ' not in hud and ' altitude ' not in hud :
hud [ ' alt ' ] = data . get ( ' h ' )
if hud :
self . signals . update_signal . emit ( ' hud ' , drone_id , {
' heading ' : hud . get ( ' heading ' , 0.0 ) ,
@ -229,6 +299,7 @@ class UDPMavlinkReceiver(threading.Thread):
self . connection_name = connection_name
self . monitor = monitor # 保存 monitor 引用
self . socket_id = monitor . get_next_socket_id ( ) if monitor else 0
self . _socket_id_released = False
self . running = False
self . sock = None
@ -258,6 +329,7 @@ class UDPMavlinkReceiver(threading.Thread):
finally :
if self . sock :
self . sock . close ( )
self . _release_socket_id ( )
def process_mavlink_message ( self , msg ) :
""" 處理 MAVLink 訊息 """
@ -343,6 +415,12 @@ class UDPMavlinkReceiver(threading.Thread):
def stop ( self ) :
""" 停止接收器 """
self . running = False
self . _release_socket_id ( )
def _release_socket_id ( self ) :
if self . monitor and not self . _socket_id_released :
self . monitor . release_socket_id ( self . socket_id )
self . _socket_id_released = True
class SerialMavlinkReceiver ( threading . Thread , JsonTelemetryProcessor ) :
""" 串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。 """
@ -354,6 +432,7 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self . connection_name = connection_name
self . monitor = monitor # 保存 monitor 引用
self . socket_id = monitor . get_next_socket_id ( ) if monitor else 0
self . _socket_id_released = False
self . source_type = ' Serial '
self . running = False
self . serial_conn = None
@ -402,6 +481,7 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self . serial_conn . close ( )
except Exception :
pass
self . _release_socket_id ( )
def _process_mavlink_byte ( self , byte ) :
try :
@ -562,6 +642,12 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self . serial_conn . close ( )
except Exception :
pass
self . _release_socket_id ( )
def _release_socket_id ( self ) :
if self . monitor and not self . _socket_id_released :
self . monitor . release_socket_id ( self . socket_id )
self . _socket_id_released = True
class WebSocketMavlinkReceiver ( threading . Thread , JsonTelemetryProcessor ) :
""" WebSocket MAVLink 接收器 """
@ -572,6 +658,7 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self . connection_name = connection_name
self . monitor = monitor # 保存 monitor 引用
self . socket_id = monitor . get_next_socket_id ( ) if monitor else 0 # 一次性分配 socket_id
self . _socket_id_released = False
self . source_type = ' WS '
self . running = False
self . max_retries = 5
@ -629,6 +716,7 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
break
print ( f " WebSocket client { self . connection_name } stopped " )
self . _release_socket_id ( )
def process_websocket_message ( self , data ) :
""" 處理 WebSocket 訊息 """
@ -637,6 +725,12 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
def stop ( self ) :
""" 停止接收器 """
self . running = False
self . _release_socket_id ( )
def _release_socket_id ( self ) :
if self . monitor and not self . _socket_id_released :
self . monitor . release_socket_id ( self . socket_id )
self . _socket_id_released = True
class DroneMonitor ( Node ) :
# Subscribe to drone ROS2 topics
@ -675,7 +769,7 @@ class DroneMonitor(Node):
# 【新增】Socket ID 重新分配機制 (從 0 開始)
# ================================================================================
self . socket_id_mapping = { } # {原始socket_id: 重新分配的socket_id}
self . socket_id_counter = 0 # 當前分配到的最大 socket_id
self . active_socket_ids = set ( ) # 目前通訊連線使用中的 socket_id
self . socket_id_lock = Lock ( ) # 線程安全鎖
# ================================================================================
@ -708,23 +802,55 @@ class DroneMonitor(Node):
self . create_timer ( 1.0 , self . scan_topics )
def get_next_socket_id ( self ) :
""" 获取下一个可用的 socket_id（ """
""" 取得目前最小的未使用 socket_id（ """
with self . socket_id_lock :
current_id = self . socket_id_counter
self . socket_id_counter + = 1
return current_id
socket_id = 0
while socket_id in self . active_socket_ids :
socket_id + = 1
self . active_socket_ids . add ( socket_id )
return socket_id
def release_socket_id ( self , socket_id ) :
""" 釋放通訊連線使用的 socket_id， """
try :
socket_id = int ( socket_id )
except ( TypeError , ValueError ) :
return
with self . socket_id_lock :
if socket_id in getattr ( self , ' socket_id_mapping ' , { } ) . values ( ) :
return
if socket_id in getattr ( self , ' sys_to_socket_id ' , { } ) . values ( ) :
return
self . active_socket_ids . discard ( socket_id )
def get_or_assign_socket_id ( self , original_socket_id ) :
""" 根據原始 socket_id 分配或獲取對應的 socket_id（
同一個原始 socket_id 會得到同一個分配的 ID
""" ROS2 socket_id 映射。
有原始 socket_id = N 時優先使用 N ； 若 N 已被其他通訊占用 ，
才改用目前最小未使用 ID 。 同一個原始 socket_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
try :
preferred_socket_id = int ( original_socket_id )
except ( TypeError , ValueError ) :
preferred_socket_id = None
if (
preferred_socket_id is not None
and preferred_socket_id > = 0
and preferred_socket_id not in self . active_socket_ids
) :
socket_id = preferred_socket_id
else :
socket_id = 0
while socket_id in self . active_socket_ids :
socket_id + = 1
self . active_socket_ids . add ( socket_id )
self . socket_id_mapping [ original_socket_id ] = socket_id
return self . socket_id_mapping [ original_socket_id ]
@ -796,8 +922,7 @@ class DroneMonitor(Node):
# 为每个 sys_id 分配 socket_id（ ）
# 注意：如果后续 summary 提供了 socket_id，
if sys_id not in self . sys_to_socket_id :
# 暂时所有 ROS2 topic 共享同一个 socket_id = 0
self . sys_to_socket_id [ sys_id ] = 0
self . sys_to_socket_id [ sys_id ] = self . get_next_socket_id ( )
subs_attr = f ' drone_ { sys_id } _subs '
if not hasattr ( self , subs_attr ) :
@ -1065,6 +1190,39 @@ class DroneMonitor(Node):
traceback . print_exc ( )
return False
async def reboot_drone ( self , drone_id ) :
""" 使用 CommandLongClient 執行飛控重啟。 """
try :
parts = drone_id . split ( ' _ ' )
if len ( parts ) < 2 :
_log ( " ERROR " , f " [REBOOT] 無效的 drone_id 格式: { drone_id } " )
return False
sysid = int ( parts [ - 1 ] )
_log ( " INFO " , f " [REBOOT] { drone_id } -> 飛控重啟 " )
client = self . get_or_create_client ( drone_id )
if not client :
_log ( " ERROR " , " [REBOOT] CommandLongClient 無法初始化 " )
return False
result = await client . reboot_autopilot_async (
target_sysid = sysid ,
target_compid = 0 ,
timeout_sec = 5.0 ,
)
if result and result . success :
_log ( " INFO " , f " [REBOOT] { drone_id } 重啟命令已送出 " )
return True
_log ( " ERROR " , f " [REBOOT] 重啟失敗 (message= { result . message if result else ' None ' } ) " )
return False
except Exception as e :
_log ( " ERROR " , f " [REBOOT] 例外錯誤: { 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 :
@ -1161,8 +1319,12 @@ class DroneMonitor(Node):
# 從 summary 獲取原始 socket_id，
original_socket_id = data . get ( ' socket_id ' )
if original_socket_id is not None :
fallback_socket_id = self . sys_to_socket_id . pop ( sys_id , None )
if fallback_socket_id is not None :
self . release_socket_id ( fallback_socket_id )
# 使用原始 socket_id 獲取或分配統一的 socket_id
assigned_socket_id = self . get_or_assign_socket_id ( original_socket_id )
self . sys_to_socket_id [ sys_id ] = assigned_socket_id
else :
# 如果沒有 socket_id，
assigned_socket_id = self . sys_to_socket_id . get ( sys_id , 0 )
@ -1328,7 +1490,7 @@ class DroneMonitor(Node):
' ping ' : msg . data
}
def start_serial_connection ( self , port = ' /dev/ttyUSB0 ' , baudrate = 576 00) :
def start_serial_connection ( self , port = ' /dev/ttyUSB0 ' , baudrate = 1152 00) :
""" 啟動串口遙測連接（自動辨識 MAVLink / JSON） """
connection_name = f " Serial_ { port . replace ( ' / ' , ' _ ' ) } "
receiver = SerialMavlinkReceiver ( port , baudrate , self . signals , connection_name , self )
wenchun
ken910606
Chiyu Chen