wenchun
/
AirTrapMine
forked from chiyu1468/AirTrapMine
1
0
Fork 0
Code Pull Requests Activity

item51&54 連接多個port

Browse Source
ken910606 9 months ago
parent 94cf156187
commit 4388ebd9ed
1 changed files with 42 additions and 721 deletions
Show Stats Download Patch File Download Diff File

761
src/fc_network_adapter/fc_network_adapter/devRun.py
Unescape Escape View File

@ -18,452 +18,7 @@ test_item = 51
running_time =10000
print('test_item : ', test_item)
if test_item == 1:
# 測試 mavlink_object 中 updateMultiplexingList 的輸出
print('===> Start of Program .Test ', test_item)
mavlink_object_none = mo.mavlink_object(None)
print('=====================')
print('正常範例')
mavlink_object_none.multiplexingToAnalysis = []
mavlink_object_none.multiplexingToReturn = []
# mavlink_object_none.multiplexingToSwap = [[]]
ret = mavlink_object_none.updateMultiplexingList()
print('execute result : ', ret)
print('multiplexingList : ', mavlink_object_none._multiplexingList)
print('=====================')
print('錯誤範例 長度不一致')
mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],]
ret = mavlink_object_none.updateMultiplexingList()
print('execute result : ', ret)
print('=====================')
print('正常範例')
mo.swap_queues.append(queue.Queue())
mo.swap_queues.append(queue.Queue())
mavlink_object_none.multiplexingToAnalysis = [0, 1, 2, 3, 4, 5]
mavlink_object_none.multiplexingToReturn = [6, 7, 8, 9, 10]
mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],]
ret = mavlink_object_none.updateMultiplexingList()
print('execute result : ', ret)
print('multiplexingList : ', mavlink_object_none._multiplexingList)
print('=====================')
print('錯誤範例 multiplexingToAnalysis 不可以有 -1')
mavlink_object_none.multiplexingToAnalysis = [-1, 1, 2, 3, 4, 5]
ret = mavlink_object_none.updateMultiplexingList()
print('execute result : ', ret)
print('<=== End of Program')
elif test_item == 2:
# 測試 mavlink_object 創建時 socket_id 是否正確
# 說實在 這個測試項 似乎因為 python 的 GC 機制 會導致難以測試
print('===> Start of Program .Test ', test_item)
mavlink_object_none1 = mo.mavlink_object(None)
mavlink_object_none2 = mo.mavlink_object(None)
mavlink_object_none3 = mo.mavlink_object(None)
del mavlink_object_none2
print(len(mo.swap_queues))
mavlink_object_none2 = mo.mavlink_object(None)
print(len(mo.swap_queues))
print(mavlink_object_none1._multiplexingList)
print('<=== End of Program')
elif test_item == 10:
# 需要開啟一個 ardupilot 的模擬器
# 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來
# 只啟用了 mavlink_object 的功能
print('===> Start of Program .Test ', test_item)
# 創建一個空的通道 這個通道的 socket_id 是 0
mavlink_object_none = mo.mavlink_object(None)
# 創建另一個通道
connection_string="udp:127.0.0.1:14550"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object1 = mo.mavlink_object(mavlink_socket)
# 設定通道轉發的參數
mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE
mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT
mavlink_object1.multiplexingToSwap[0] = [74, ] # only VFR_HUD
# print(mavlink_object1.multiplexingToSwap)
# print(mo.swap_queues)
# 啟動通道
mavlink_object1.run()
# 確認轉拋的設定有沒有錯
print("_multiplexingList for mavlink object :", mavlink_object1._multiplexingList)
# 運行幾秒並印出 queue 的資料
start_time = time.time()
while time.time() - start_time < running_time:
while not mo.fixed_stream_bridge_queue.empty():
print('fixed_stream_bridge_queue:')
t = mo.fixed_stream_bridge_queue.get()
print('from {} : {}'.format(t[0],t[2]))
while not mo.return_packet_processor_queue.empty():
print('return_packet_processor_queue:')
t = mo.return_packet_processor_queue.get()
print('from {} : {}'.format(t[0],t[2]))
# print(t[2].get_msgbuf())
# print(t[2].type)
for n in range(len(mo.swap_queues)):
q = mo.swap_queues[n]
while not q.empty():
print('swap_queues:')
t = q.get()
print('{} gets : {}'.format(n,t))
# 結束程式 退出所有 thread
mavlink_object1.running = False
mavlink_object1.thread.join()
mavlink_socket.close()
print('<=== End of Program')
elif test_item == 11:
# 需要開啟一個 ardupilot 的模擬器
# 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況
# 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能
print('===> Start of Program .Test ', test_item)
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
# 創建通道
connection_string="udp:127.0.0.1:14550"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object2 = mo.mavlink_object(mavlink_socket)
# mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT
# mavlink_object2.multiplexingToReturn = []
# 啟動連線的模組
mavlink_object2.run()
# 做點延遲 讓 heartbeat 先吃進來
time.sleep(2)
print("=== connection established! ===")
# 印出目前所有 mavlink_systems 的內容
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
start_time = time.time()
show_time = time.time()
compid = 1
while time.time() - start_time < running_time:
if (time.time() - show_time) >= 1:
show_time = time.time()
for sysid in analyzer.mavlink_systems:
print("目前收到的訊息數量 : ",analyzer.mavlink_systems[sysid].components[compid].msg_count)
print("目前遺失的訊息數量 : ",analyzer.mavlink_systems[sysid].components[compid].lost_packet_count)
print("現在飛機的模式 : ",analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode'])
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
print("===================")
# 結束程式 退出所有 thread
mavlink_object2.stop()
mavlink_object2.thread.join()
analyzer.stop()
analyzer = mo.mavlink_bridge() # 這邊是測試是否只有一個 instance
analyzer.thread.join()
mavlink_socket.close()
print('<=== End of Program')
elif test_item == 12:
# 需要開啟一個 ardupilot 的模擬器 與 GCS
# 這邊是測試 mavlink object 作為交換器功能的代碼
print('===> Start of Program .Test ', test_item)
# 啟動連線的模組
analyzer = mo.mavlink_bridge()
# 初始化輸入通道
connection_string_in="udp:127.0.0.1:14552"
mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
mavlink_object_in.multiplexingToAnalysis = [0] # only HEARTBEAT
# 初始化輸出通道
connection_string_out="udpout:127.0.0.1:14553"
mavlink_socket_out = mavutil.mavlink_connection(connection_string_out)
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
mavlink_object_out.multiplexingToAnalysis = [0]
# 做一個空的通道驗證 可以拿來 debug
mavlink_object_none = mo.mavlink_object(None)
# 讓兩個通道互相傳輸
mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all
mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all
# mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all
# 啟動通道
mavlink_object_in.run()
mavlink_object_out.run()
# 做點延遲 讓 heartbeat 先吃進來
time.sleep(3)
print("=== connection established! ===")
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
# print(type(mavlink_socket_out))
# print(type(mavlink_socket_out.mav))
start_time = time.time()
show_time = time.time()
while time.time() - start_time < running_time:
try:
test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False)
print('none object : ', test)
except queue.Empty:
pass
if (time.time() - show_time) >= 2:
show_time = time.time()
for sysid in analyzer.mavlink_systems:
for compid in analyzer.mavlink_systems[sysid].components:
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
print("===================")
# 結束程式 退出所有 thread
mavlink_object_in.stop()
mavlink_object_in.thread.join()
mavlink_object_out.stop()
mavlink_object_out.thread.join()
mavlink_socket_in.close()
mavlink_socket_out.close()
analyzer.stop()
print('<=== End of Program')
elif test_item == 20:
# 這邊測試 node 生成 topic 的功能
# 利用 空的通道 發出假的 heartbeat 封包
print('===> Start of Program .Test ', test_item)
rclpy.init() # 注意要初始化 rclpy 才能使用 node
from pymavlink.dialects.v20 import common as mavlink2
sysid = 1
compid = 1
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
mav = mavlink2.MAVLink(None)
mav.srcSystem = sysid
mav.srcComponent = compid
# 這是一個 heartbeat 封包
fake_heartbeat = mavlink2.MAVLink_heartbeat_message(
type=mavlink2.MAV_TYPE_QUADROTOR,
autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode=3,
custom_mode=0,
system_status=0,
mavlink_version=3
)
mavlink_bytes = fake_heartbeat.pack(mav)
fake_heartbeat_msg = mav.parse_char(mavlink_bytes)
print(analyzer.mavlink_systems)
mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data
time.sleep(0.1)
print(analyzer.mavlink_systems)
# ROS2 初始化
analyzer._init_node()
# 創建 ROS2 topic
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid
print("topic created")
time.sleep(5)
# 丟出 topic
analyzer.emit_info()
# 結束程式
analyzer.running = False
analyzer.destroy_node()
rclpy.shutdown()
analyzer.stop()
analyzer.thread.join()
print('<=== End of Program')
elif test_item == 21:
# 需要開啟一個 ardupilot 的模擬器
# 這邊是測試代碼 引入 rclpy 來測試 node 的運行
print('===> Start of Program .Test ', test_item)
rclpy.init() # 注意要初始化 rclpy 才能使用 node
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
# 關於 Node 的初始化
show_time = time.time()
analyzer._init_node() # 初始化 node
print('初始化 node 完成 耗時 : ',time.time() - show_time)
# 創建通道
connection_string="udp:127.0.0.1:15551"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object3 = mo.mavlink_object(mavlink_socket)
# 啟動通道
mavlink_object3.run()
print('=== waiting for mavlink data ...')
time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
compid = 1
sysid = 1
start_time = time.time()
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
end_time = time.time()
print(f"Execution time for create_flightMode: {end_time - start_time} seconds")
print("start emit info")
start_time = time.time()
show_time = time.time()
while time.time() - start_time < running_time:
try:
# rclpy.spin(analyzer)
analyzer.emit_info() # 這邊是測試 node 的運行
time.sleep(1)
except KeyboardInterrupt:
break
# 程式結束
analyzer.destroy_node()
rclpy.shutdown()
# 結束程式 退出所有 thread
mavlink_object3.stop()
mavlink_object3.thread.join()
analyzer.stop()
analyzer.thread.join()
mavlink_socket.close()
print('<=== End of Program')
elif test_item == 22:
# 需要開啟一個 ardupilot 的模擬器 與 GCS
# 這邊是測試代碼 引入 rclpy 來測試 node 的運行
print('===> Start of Program .Test ', test_item)
rclpy.init() # 注意要初始化 rclpy 才能使用 node
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
# 關於 Node 的初始化
show_time = time.time()
analyzer._init_node() # 初始化 node
print('初始化 node 完成 耗時 : ',time.time() - show_time)
# 初始化兩個通道
connection_string_in="udp:127.0.0.1:15551"
mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
mavlink_object_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
connection_string_out="udpout:127.0.0.1:14553"
mavlink_socket_out = mavutil.mavlink_connection(connection_string_out)
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
mavlink_object_out.multiplexingToAnalysis = []
# 讓兩個通道互相傳輸
mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all
mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all
# 啟動通道
mavlink_object_in.run()
mavlink_object_out.run()
print('waiting for mavlink data ...')
time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
compid = 1
sysid = 1
show_time = time.time()
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
print(f"Execution time for create_flightMode: {time.time() - show_time} seconds")
print("start emit info")
start_time = time.time()
show_time = time.time()
show_time2 = time.time()
while time.time() - start_time < running_time:
if (time.time() - show_time2) >= 1:
analyzer.emit_info() # 這邊是測試 node 的運行
# sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode']
fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode']
sss = mavutil.mode_string_v10(fmsg)
# print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq()))
print("目前的飛行模式 : {}".format(sss))
show_time2 = time.time()
# if (time.time() - show_time) >= 2:
# show_time = time.time()
# for sysid in analyzer.mavlink_systems:
# for compid in analyzer.mavlink_systems[sysid].components:
# print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
# analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
# print("===================")
analyzer.destroy_node()
rclpy.shutdown()
# 結束程式 退出所有 thread
mavlink_object_in.stop()
mavlink_object_in.thread.join()
mavlink_socket_in.close()
mavlink_object_out.stop()
mavlink_object_out.thread.join()
mavlink_socket_out.close()
analyzer.stop()
analyzer.thread.join()
print('<=== End of Program')
elif test_item == 51:
if test_item == 51:
# 晉凱的測試項目 - 修改為支援多UDP連接
print('===> Start of Program .Test ', test_item)
rclpy.init() # 注意要初始化 rclpy 才能使用 node
@ -476,18 +31,18 @@ elif test_item == 51:
print('初始化 node 完成 耗時 : ',time.time() - show_time)
# === 修改：支援多個UDP連接 ===
udp_ports = [14551, 14552, 14553] # 可以根據需要調整端口
ports = ["udp:127.0.0.1:14550",
"udp:127.0.0.1:14570",
"/dev/ttyUSB0",
"/dev/ttyUSB1"] # 可以根據需要調整端口
mavlink_sockets = []
mavlink_objects = []
print(f"設定連接到 UDP 端口: {udp_ports}")
# 循環創建多個連接
for i, port in enumerate(udp_ports):
for i, port in enumerate(ports):
try:
print(f"連接到 UDP:{port}")
connection_string = f"udp:127.0.0.1:{port}"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_socket = mavutil.mavlink_connection(port)
mavlink_object = mo.mavlink_object(mavlink_socket)
# 設定通道流動（所有連接使用相同參數）
@ -529,13 +84,6 @@ elif test_item == 51:
print(f"為系統 {sysid}, 組件 {compid} 創建topics...")
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_attitude(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_local_position_pose(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_local_position_velocity(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_global_global(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_vfr_hud(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_battery(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid])
topics_created += 1
print(f"系統 {sysid}_{compid} topics創建完成")
@ -563,7 +111,7 @@ elif test_item == 51:
loop_count += 1
if loop_count % 10 == 0: # 每5秒顯示一次（0.5s * 10）
print(f"\n=== 狀態更新 (第 {loop_count} 次循環) ===")
print(f"連接的UDP端口: {udp_ports[:len(mavlink_sockets)]}")
#print(f"連接的端口: {udp_ports[:len(mavlink_sockets)]}")
print(f"檢測到的系統數: {len(analyzer.mavlink_systems)}")
for sysid in analyzer.mavlink_systems:
@ -626,7 +174,7 @@ elif test_item == 51:
# 清理所有mavlink objects
for i, mavlink_obj in enumerate(mavlink_objects):
try:
print(f"停止 mavlink_object {i+1} (UDP:{udp_ports[i]}, socket_id: {mavlink_obj.socket_id})")
print(f"停止 mavlink_object {i+1} (UDP:{ports[i]}, socket_id: {mavlink_obj.socket_id})")
mavlink_obj.stop()
mavlink_obj.thread.join()
print(f"mavlink_object {i+1} 已停止")
@ -636,10 +184,10 @@ elif test_item == 51:
# 關閉所有mavlink sockets
for i, mavlink_sock in enumerate(mavlink_sockets):
try:
print(f"關閉 UDP 連接 {udp_ports[i]}")
print(f"關閉 UDP 連接 {ports[i]}")
mavlink_sock.close()
except Exception as e:
print(f"關閉 UDP 連接 {udp_ports[i]} 失敗: {e}")
print(f"關閉 UDP 連接 {ports[i]} 失敗: {e}")
# 清理analyzer
try:
@ -653,221 +201,6 @@ elif test_item == 51:
print(f"清理完成，共處理了 {len(mavlink_sockets)} 個 UDP 連接")
print('<=== End of Program')
elif test_item == 52:
# 文鈞的測試項目
# 需要開啟一個 ardupilot 的模擬器 與 GCS
# 這邊是測試 mavlink object 作為交換器功能的代碼
print('===> Start of Program .Test ', test_item)
# 啟動連線的模組
analyzer = mo.mavlink_bridge()
# 共用的輸出連接 (連接到GCS)
common_out = "udpout:127.0.0.1:14553"
mavlink_socket_out = mavutil.mavlink_connection(common_out)
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
mavlink_object_out.multiplexingToAnalysis = [0]
# 設定多台無人機的輸入連接
drone_inputs = [
#"udp:127.0.0.1:14550", # 無人機1
#"udp:127.0.0.1:14550", # 無人機2
"udp:127.0.0.1:14550", # 無人機3
"udp:127.0.0.1:14554" # 無人機4
]
# 建立輸入連接
mavlink_objects_in = []
mavlink_sockets_in = []
for i, input_conn in enumerate(drone_inputs):
print(f"連接無人機 {i+1} 輸入: {input_conn}")
mavlink_in = mavutil.mavlink_connection(input_conn)
obj_in = mo.mavlink_object(mavlink_in)
obj_in.multiplexingToAnalysis = [0] # 只分析心跳訊息
mavlink_objects_in.append(obj_in)
mavlink_sockets_in.append(mavlink_in)
# 設定轉發關係
obj_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # 所有訊息
mavlink_object_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # 所有訊息
# 做一個空的通道驗證 可以拿來 debug
mavlink_object_none = mo.mavlink_object(None)
# 啟動所有通道
for obj in mavlink_objects_in:
obj.run()
mavlink_object_out.run()
# 做點延遲 讓 heartbeat 先吃進來
time.sleep(3)
print("=== connection established! ===")
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
start_time = time.time()
show_time = time.time()
while time.time() - start_time < running_time:
try:
test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False)
print('none object : ', test)
except queue.Empty:
pass
if (time.time() - show_time) >= 8:
show_time = time.time()
for sysid in analyzer.mavlink_systems:
for compid in analyzer.mavlink_systems[sysid].components:
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
print("===================")
# 結束程式 退出所有 thread
for i, obj in enumerate(mavlink_objects_in):
obj.stop()
obj.thread.join()
mavlink_sockets_in[i].close()
mavlink_object_out.stop()
mavlink_object_out.thread.join()
mavlink_socket_out.close()
analyzer.stop()
print('<=== End of Program')
elif test_item == 53:
# 文鈞的測試項目 - 雙輸出版本
# 需要開啟一個 ardupilot 的模擬器 與 兩個 GCS
# 這邊是測試 mavlink object 作為交換器功能的代碼，支援雙輸出
print('===> Start of Program .Test ', test_item)
# 啟動連線的模組
analyzer = mo.mavlink_bridge()
# 雙輸出連接設定 (連接到兩個不同的GCS)
gcs_outputs = [
"udpout:127.0.0.1:14560", # GCS 1
"udpout:127.0.0.1:14570" # GCS 2
]
# 建立輸出連接物件
mavlink_objects_out = []
mavlink_sockets_out = []
for i, output_conn in enumerate(gcs_outputs):
print(f"建立 GCS {i+1} 輸出連接: {output_conn}")
mavlink_out = mavutil.mavlink_connection(output_conn)
obj_out = mo.mavlink_object(mavlink_out)
obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息
mavlink_objects_out.append(obj_out)
mavlink_sockets_out.append(mavlink_out)
# 設定多台無人機的輸入連接
drone_inputs = [
"udp:127.0.0.1:14551", # 無人機3
"udp:127.0.0.1:14552" # 無人機4
]
# 建立輸入連接
mavlink_objects_in = []
mavlink_sockets_in = []
for i, input_conn in enumerate(drone_inputs):
print(f"連接無人機 {i+1} 輸入: {input_conn}")
mavlink_in = mavutil.mavlink_connection(input_conn)
obj_in = mo.mavlink_object(mavlink_in)
obj_in.multiplexingToAnalysis = [0] # 只分析心跳訊息
mavlink_objects_in.append(obj_in)
mavlink_sockets_in.append(mavlink_in)
# 設定無人機到所有GCS的轉發關係
for obj_out in mavlink_objects_out:
obj_in.multiplexingToSwap[obj_out.socket_id] = [-1, ] # 無人機→所有GCS
print(f"設定無人機 {i+1} (socket_id: {obj_in.socket_id}) → GCS (socket_id: {obj_out.socket_id}) 轉發")
# 設定GCS到所有無人機的轉發關係
for i, obj_out in enumerate(mavlink_objects_out):
for j, obj_in in enumerate(mavlink_objects_in):
obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有無人機
print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 無人機 {j+1} (socket_id: {obj_in.socket_id}) 轉發")
# 做一個空的通道驗證 可以拿來 debug
mavlink_object_none = mo.mavlink_object(None)
# 啟動所有輸入通道
for obj in mavlink_objects_in:
obj.run()
# 啟動所有輸出通道
for obj in mavlink_objects_out:
obj.run()
# 做點延遲 讓 heartbeat 先吃進來
time.sleep(3)
print("=== connection established! ===")
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
# 顯示轉發設定摘要
print("\n=== 轉發設定摘要 ===")
print(f"無人機數量: {len(mavlink_objects_in)}")
print(f"GCS數量: {len(mavlink_objects_out)}")
print("轉發規則:")
print(" - 每台無人機的所有訊息 → 所有GCS")
print(" - 每個GCS的所有訊息 → 所有無人機")
print("===================\n")
start_time = time.time()
show_time = time.time()
while time.time() - start_time < running_time:
try:
test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False)
print('none object 收到訊息: ', test)
except queue.Empty:
pass
if (time.time() - show_time) >= 5: # 改為每5秒顯示一次，更即時監控
show_time = time.time()
print(f"\n=== 系統狀態報告 (運行時間: {int(time.time() - start_time)}秒) ===")
for sysid in analyzer.mavlink_systems:
for compid in analyzer.mavlink_systems[sysid].components:
msg_count = analyzer.mavlink_systems[sysid].components[compid].msg_count
print(f"系統ID: {sysid}, 元件ID: {compid}, 收到訊息數量: {msg_count}")
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
# 顯示各通道的狀態
print(f"輸入通道數量: {len(mavlink_objects_in)} (運行中)")
print(f"輸出通道數量: {len(mavlink_objects_out)} (運行中)")
print("===================\n")
# 結束程式 退出所有 thread
print("正在關閉所有連接...")
# 關閉輸入通道
for i, obj in enumerate(mavlink_objects_in):
print(f"關閉無人機 {i+1} 輸入通道")
obj.stop()
obj.thread.join()
mavlink_sockets_in[i].close()
# 關閉輸出通道
for i, obj in enumerate(mavlink_objects_out):
print(f"關閉 GCS {i+1} 輸出通道")
obj.stop()
obj.thread.join()
mavlink_sockets_out[i].close()
analyzer.stop()
print('<=== End of Program')
elif test_item == 54:
# 文鈞的測試項目 - 5輸入2輸出版本 + 結合test51的ROS2功能
# 加入詳細調試信息
@ -915,19 +248,43 @@ elif test_item == 54:
print("ROS2 bridge 初始化完成")
# 雙輸出連接設定 (連接到兩個不同的GCS)
gcs_outputs = [
"udpout:127.0.0.1:14500", # GCS 1
"udpout:127.0.0.1:14600" # GCS 2
]
# 建立輸出連接物件
mavlink_objects_out = []
mavlink_sockets_out = []
# 設定5個輸入連接（修改為實際測試可用的端口）
device_inputs = [
"udp:127.0.0.1:14551", # 無人機1 (UDP)
"udp:127.0.0.1:14552", # 無人機2 (UDP)
"udp:127.0.0.1:14553", # 無人機3 (UDP)
"udp:127.0.0.1:14554", # 無人機4 (UDP)
"udp:127.0.0.1:14555", # 無人機5 (UDP)
"udp:127.0.0.1:14550", # 無人機1 (UDP)
"udp:127.0.0.1:14570", # 無人機2 (UDP)
"/dev/ttyUSB0", # 無人機3 (UDP)
"/dev/ttyUSB1", # 無人機4 (UDP)
"/dev/ttyUSB2", # 無人機5 (UDP)
]
# 建立輸入連接
mavlink_objects_in = []
mavlink_sockets_in = []
for i, output_conn in enumerate(gcs_outputs):
print(f"建立 GCS {i+1} 輸出連接: {output_conn}")
mavlink_out = mavutil.mavlink_connection(output_conn)
obj_out = mo.mavlink_object(mavlink_out)
obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息
mavlink_objects_out.append(obj_out)
mavlink_sockets_out.append(mavlink_out)
# 設定GCS到所有設備的轉發關係
for i, obj_out in enumerate(mavlink_objects_out):
for j, obj_in in enumerate(mavlink_objects_in):
obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有設備
print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 設備 {j+1} (socket_id: {obj_in.socket_id}) 轉發")
for i, input_conn in enumerate(device_inputs):
print(f"連接設備 {i+1} 輸入: {input_conn}")
try:
@ -954,30 +311,6 @@ elif test_item == 54:
print(f"跳過設備 {i+1}")
continue
# 雙輸出連接設定 (連接到兩個不同的GCS)
gcs_outputs = [
"udpout:127.0.0.1:14560", # GCS 1
"udpout:127.0.0.1:14570" # GCS 2
]
# 建立輸出連接物件
mavlink_objects_out = []
mavlink_sockets_out = []
for i, output_conn in enumerate(gcs_outputs):
print(f"建立 GCS {i+1} 輸出連接: {output_conn}")
mavlink_out = mavutil.mavlink_connection(output_conn)
obj_out = mo.mavlink_object(mavlink_out)
obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息
mavlink_objects_out.append(obj_out)
mavlink_sockets_out.append(mavlink_out)
# 設定GCS到所有設備的轉發關係
for i, obj_out in enumerate(mavlink_objects_out):
for j, obj_in in enumerate(mavlink_objects_in):
obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有設備
print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 設備 {j+1} (socket_id: {obj_in.socket_id}) 轉發")
# 做一個空的通道驗證 可以拿來 debug
mavlink_object_none = mo.mavlink_object(None)
@ -1056,7 +389,6 @@ elif test_item == 54:
try:
while not mo.fixed_stream_bridge_queue.empty():
msg = mo.fixed_stream_bridge_queue.get(block=False)
print(f'[DEBUG] fixed_stream_bridge_queue: from socket {msg[0]}: {msg[2].get_type()}')
debug_counters['analysis_messages'] += 1
message_count += 1
except queue.Empty:
@ -1066,7 +398,6 @@ elif test_item == 54:
try:
while not mo.return_packet_processor_queue.empty():
msg = mo.return_packet_processor_queue.get(block=False)
print(f'[DEBUG] return_packet_processor_queue: from socket {msg[0]}: {msg[2].get_type()}')
debug_counters['return_messages'] += 1
message_count += 1
except queue.Empty:
@ -1083,8 +414,6 @@ elif test_item == 54:
except Exception as e:
print(f"發送 TIMESYNC 到設備 {i+1} 失敗: {e}")
if timesync_sent > 0:
print(f"[DEBUG] 發送 TIMESYNC 到 {timesync_sent} 個設備")
last_timesync = now
# ROS2 發布 (來自test51新版本)
@ -1098,10 +427,9 @@ elif test_item == 54:
# 狀態報告 (更頻繁的調試輸出)
if (time.time() - show_time) >= 2: # 每2秒顯示一次狀態
show_time = time.time()
print(f"\n=== 調試狀態報告 (運行時間: {int(now - time.time() + running_time)}秒) ===")
print(f"\n=== 調試狀態報告===")
# 顯示消息統計
print(f"消息計數器: {debug_counters}")
print(f"總消息數: {message_count}, ROS2發布次數: {ros2_publish_count}")
# 重置調試計數器
@ -1115,13 +443,6 @@ elif test_item == 54:
for compid in system.components:
component = system.components[compid]
msg_count = component.msg_count
print(f" 元件 {compid}: 收到訊息數量={msg_count}")
# 顯示 emitParams 內容
if hasattr(component, 'emitParams') and component.emitParams:
print(f" emitParams keys: {list(component.emitParams.keys())}")
else:
print(" emitParams: 空")
system.resetComponentPacketCount(compid)
else:

Write Preview
Loading…
Cancel
Save