From 4388ebd9ede6d0098f0403b0edb3168e0cc223c0 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 14 Aug 2025 17:17:04 +0800 Subject: [PATCH] =?UTF-8?q?item51&54=20=E9=80=A3=E6=8E=A5=E5=A4=9A?= =?UTF-8?q?=E5=80=8Bport?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/devRun.py | 763 +----------------- 1 file changed, 42 insertions(+), 721 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 133e56d..78abb8f 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -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,14 +443,7 @@ 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: print("目前沒有檢測到任何MAVLink系統") @@ -1184,4 +505,4 @@ elif test_item == 54: # 關閉分析器 bridge.stop() bridge.thread.join() - print('<=== End of Program') + print('<=== End of Program') \ No newline at end of file