diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py
index 835bfae..133e56d 100644
--- a/src/fc_network_adapter/fc_network_adapter/devRun.py
+++ b/src/fc_network_adapter/fc_network_adapter/devRun.py
@@ -15,7 +15,7 @@ import mavlinkDevice as md
 # ====================== 分割線 =====================
 
 test_item = 51
-running_time = 30
+running_time =10000
 print('test_item : ', test_item)
 
 if test_item == 1:
@@ -190,7 +190,7 @@ elif test_item == 12:
     analyzer = mo.mavlink_bridge()
 
     # 初始化輸入通道
-    connection_string_in="udp:127.0.0.1:15551"
+    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
@@ -464,47 +464,724 @@ elif test_item == 22:
     print('<=== End of Program')
 
 elif test_item == 51:
-    print("===> Start of Program .Test", test_item)
+    # 晉凱的測試項目 - 修改為支援多UDP連接
+    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)
+
+    # === 修改：支援多個UDP連接 ===
+    udp_ports = [14551, 14552, 14553]  # 可以根據需要調整端口
+    mavlink_sockets = []
+    mavlink_objects = []
+    
+    print(f"設定連接到 UDP 端口: {udp_ports}")
+    
+    # 循環創建多個連接
+    for i, port in enumerate(udp_ports):
+        try:
+            print(f"連接到 UDP:{port}")
+            connection_string = f"udp:127.0.0.1:{port}"
+            mavlink_socket = mavutil.mavlink_connection(connection_string)
+            mavlink_object = mo.mavlink_object(mavlink_socket)
+            
+            # 設定通道流動（所有連接使用相同參數）
+            mavlink_object.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
+            mavlink_object.multiplexingToReturn = [] 
+            
+            # 啟動通道
+            mavlink_object.run()
+            
+            # 保存連接引用
+            mavlink_sockets.append(mavlink_socket)
+            mavlink_objects.append(mavlink_object)
+            
+            print(f"UDP:{port} 連接成功，socket_id: {mavlink_object.socket_id}")
+            
+        except Exception as e:
+            print(f"連接 UDP:{port} 失敗: {e}")
+            continue
+    
+    print(f"成功建立 {len(mavlink_sockets)} 個 UDP 連接")
+
+    print('waiting for mavlink data ...')
+    time.sleep(3) # 等待足夠時間讓所有device object收到MAVLink訊息
+
+    # 顯示檢測到的系統
+    print('目前所有的系統：')
+    for sysid in analyzer.mavlink_systems:
+        system = analyzer.mavlink_systems[sysid]
+        socket_id = getattr(system, 'socket_id', 'Unknown')
+        print(f"  系統ID: {sysid}, socket_id: {socket_id}")
+
+    # === 為所有檢測到的系統創建topics ===
+    topic_creation_start = time.time()
+    topics_created = 0
+    
+    for sysid in analyzer.mavlink_systems:
+        for compid in analyzer.mavlink_systems[sysid].components:
+            try:
+                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創建完成")
+                
+            except Exception as e:
+                print(f"為系統 {sysid}_{compid} 創建topics失敗: {e}")
+                continue
+    
+    topic_creation_end = time.time()
+    print(f"總共為 {topics_created} 個系統創建topics，耗時: {topic_creation_end - topic_creation_start:.2f} 秒")
+
+    print("start emit info")
+
+    # === 主循環：支援多連接 ===
+    start_time = time.time()
+    loop_count = 0
+    
+    # while time.time() - start_time < running_time:
+    while True:
+        try:
+            # ROS2 發布
+            analyzer.emit_info() # 這邊是測試 node 的運行
+            
+            # 每10次循環顯示一次詳細狀態
+            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"檢測到的系統數: {len(analyzer.mavlink_systems)}")
+                
+                for sysid in analyzer.mavlink_systems:
+                    system = analyzer.mavlink_systems[sysid]
+                    socket_id = getattr(system, 'socket_id', 'Unknown')
+                    print(f"  系統 {sysid}: socket_id={socket_id}")
+                    
+                    for compid in system.components:
+                        component = system.components[compid]
+                        
+                        # 安全地獲取msg_count
+                        msg_count = getattr(component, 'msg_count', 0)
+                        if isinstance(msg_count, dict):
+                            total_count = sum(msg_count.values()) if msg_count else 0
+                        else:
+                            total_count = msg_count if msg_count else 0
+                            
+                        print(f"    組件 {compid}: 收到 {total_count} 條消息")
+                        
+                        # 顯示emitParams狀態
+                        if hasattr(component, 'emitParams') and component.emitParams:
+                            param_count = len(component.emitParams)
+                            print(f"      emitParams: {param_count} 項數據")
+                        else:
+                            print(f"      emitParams: 無數據")
+                        
+                        # 重置消息計數
+                        try:
+                            system.resetComponentPacketCount(compid)
+                        except Exception as e:
+                            print(f"    重置計數失敗: {e}")
+                
+                print("===============================")
+            
+            time.sleep(0.5)
+            
+        except KeyboardInterrupt:
+            print("\n使用者中斷程序...")
+            break
+        except Exception as e:
+            print(f"主循環錯誤: {e}")
+            break
+    
+    # === 清理資源 ===
+    print("開始清理資源...")
+    
+    # 清理ROS2
+    try:
+        analyzer.destroy_node()
+        print("ROS2 node 已清理")
+    except Exception as e:
+        print(f"清理ROS2 node失敗: {e}")
+    
+    try:
+        rclpy.shutdown()
+        print("ROS2 已關閉")
+    except Exception as e:
+        print(f"關閉ROS2失敗: {e}")
+    
+    # 清理所有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})")
+            mavlink_obj.stop()
+            mavlink_obj.thread.join()
+            print(f"mavlink_object {i+1} 已停止")
+        except Exception as e:
+            print(f"停止 mavlink_object {i+1} 失敗: {e}")
+    
+    # 關閉所有mavlink sockets
+    for i, mavlink_sock in enumerate(mavlink_sockets):
+        try:
+            print(f"關閉 UDP 連接 {udp_ports[i]}")
+            mavlink_sock.close()
+        except Exception as e:
+            print(f"關閉 UDP 連接 {udp_ports[i]} 失敗: {e}")
+    
+    # 清理analyzer
+    try:
+        print("停止 analyzer")
+        analyzer.stop()
+        analyzer.thread.join()
+        print("analyzer 已停止")
+    except Exception as e:
+        print(f"停止 analyzer 失敗: {e}")
+
+    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功能
+    # 加入詳細調試信息
+    print('===> Start of Program .Test ', test_item)
+
+    # === ROS2 初始化 (來自test51新版本) ===
     rclpy.init()
+    print("ROS2 初始化完成")
 
     # 1) 啟動 bridge（它已自動建立所有 publisher）
     bridge = mo.mavlink_bridge()
-    bridge._init_node()
+    
+    try:
+        bridge._init_node()
+        
+        # 添加Node初始化檢查和修復
+        if not hasattr(bridge, '_default_callback_group'):
+            print("警告：Node 初始化不完整，嘗試修復...")
+            from rclpy.node import Node
+            # 強制重新初始化為正確的 Node
+            Node.__init__(bridge, 'mavlink_bridge_fixed')
+            print("Node 重新初始化完成")
+        else:
+            print("Node 初始化成功")
+            
+    except Exception as e:
+        print(f"Node初始化失敗: {e}")
+        print("嘗試備用初始化方法...")
+        
+        # 備用方法：創建一個新的Node實例
+        from rclpy.node import Node
+        
+        class BackupNode(Node):
+            def __init__(self):
+                super().__init__('mavlink_bridge_backup')
+                
+        backup_node = BackupNode()
+        
+        # 將備用node的方法附加到bridge對象
+        bridge.create_publisher = backup_node.create_publisher
+        bridge.destroy_node = backup_node.destroy_node
+        bridge._backup_node = backup_node
+        
+        print("備用Node創建完成")
+    
+    print("ROS2 bridge 初始化完成")
+
+    # 設定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)
+    ]
+    
+    # 建立輸入連接
+    mavlink_objects_in = []
+    mavlink_sockets_in = []
+    
+    for i, input_conn in enumerate(device_inputs):
+        print(f"連接設備 {i+1} 輸入: {input_conn}")
+        try:
+            # UDP連接
+            mavlink_in = mavutil.mavlink_connection(input_conn)
+            print(f"  UDP連接 {input_conn}")
+                
+            obj_in = mo.mavlink_object(mavlink_in)
+            
+            # === 設置消息分析類型 (來自test51新版本) ===
+            obj_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 111, 147]  # HEARTBEAT + 常用訊息 + TIMESYNC
+            print(f"  設備 {i+1} 設置分析消息類型: {obj_in.multiplexingToAnalysis}")
+            
+            mavlink_objects_in.append(obj_in)
+            mavlink_sockets_in.append(mavlink_in)
+            
+            # 設定設備到所有GCS的轉發關係
+            for j, obj_out in enumerate(mavlink_objects_out):
+                obj_in.multiplexingToSwap[obj_out.socket_id] = [-1, ]  # 設備→所有GCS
+                print(f"設定設備 {i+1} (socket_id: {obj_in.socket_id}) → GCS {j+1} (socket_id: {obj_out.socket_id}) 轉發")
+                
+        except Exception as e:
+            print(f"警告：無法建立連接 {input_conn}，錯誤：{e}")
+            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}) 轉發")
 
-    # 2) 建立一條 UDP 通道並啟動 mavlink_object
-    mav_sock = mavutil.mavlink_connection("udp:127.0.0.1:14550")
-    mobj     = mo.mavlink_object(mav_sock)           # 自動分配 socket_id
-    mobj.multiplexingToAnalysis = [0, 30, 32, 33, 74, 111, 147]  # HEARTBEAT + 常用訊息
-    mobj.run()
+    # 做一個空的通道驗證 可以拿來 debug
+    mavlink_object_none = mo.mavlink_object(None)
 
-    print("waiting for mavlink data ...")
-    time.sleep(2)   # 給一點時間收 HEARTBEAT
+    print(f"\n成功建立 {len(mavlink_objects_in)} 個輸入連接")
+    print(f"成功建立 {len(mavlink_objects_out)} 個輸出連接")
 
-    # 3) 顯示目前偵測到的 sysid 清單
-    print("Current sysid list:", list(bridge.mavlink_systems.keys()))
+    # 啟動所有輸入通道
+    for i, obj in enumerate(mavlink_objects_in):
+        obj.run()
+        print(f"啟動輸入通道 {i+1}")
+        
+    # 啟動所有輸出通道  
+    for i, obj in enumerate(mavlink_objects_out):
+        obj.run()
+        print(f"啟動輸出通道 {i+1}")
+
+    # === 等待MAVLink數據 (來自test51新版本) ===
+    print("waiting for mavlink data...")
+    time.sleep(3)   # 增加等待時間
+    print("=== connection established! ===")
 
-    # 4) 主迴圈：持續將資料 emit 出去
+    # 顯示目前偵測到的 sysid 清單
+    print("Current sysid list:", list(bridge.mavlink_systems.keys()))
+    for sysid in bridge.mavlink_systems:
+        print(bridge.mavlink_systems[sysid])
+
+    # 顯示轉發設定摘要
+    print("\n=== 系統配置摘要 ===")
+    print(f"輸入設備數量: {len(mavlink_objects_in)}")
+    print("輸入設備類型:")
+    for i, input_conn in enumerate(device_inputs[:len(mavlink_objects_in)]):
+        device_type = "串口" if input_conn.startswith("/dev/tty") else "UDP"
+        print(f"  設備 {i+1}: {input_conn} ({device_type})")
+    print(f"GCS數量: {len(mavlink_objects_out)}")
+    print("輸出GCS:")
+    for i, output_conn in enumerate(gcs_outputs):
+        print(f"  GCS {i+1}: {output_conn}")
+    print("轉發規則:")
+    print("  - 每個設備的所有訊息 → 所有GCS")
+    print("  - 每個GCS的所有訊息 → 所有設備")
+    print("MAVLink分析:")
+    print("  - 消息類型: [0, 30, 32, 33, 74, 111, 147] (HEARTBEAT, ATTITUDE, LOCAL_POSITION_NED, GLOBAL_POSITION_INT, VFR_HUD, TIMESYNC, BATTERY_STATUS)")
+    print("ROS2 Topics: 已自動建立所有publisher")
+    print("===================\n")
+
+    # === 主運行循環 (來自test51新版本) + 調試信息 ===
+    print("開始ROS2 topics發布...")
+    
     try:
         last_timesync = time.time()
-        while rclpy.ok():
+        show_time = time.time()
+        message_count = 0
+        ros2_publish_count = 0
+        
+        # 調試用計數器
+        debug_counters = {
+            'swap_messages': 0,
+            'analysis_messages': 0,
+            'return_messages': 0,
+            'ros2_publishes': 0
+        }
+        
+        while rclpy.ok() and time.time() - last_timesync < running_time:
             now = time.time()
 
-            # 每秒一次
+            # === 調試：檢查各種queue的消息 ===
+            # 檢查 swap_queues
+            try:
+                test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False)
+                print('none object 收到訊息: ', test)
+                debug_counters['swap_messages'] += 1
+            except queue.Empty:
+                pass
+            
+            # 檢查 fixed_stream_bridge_queue
+            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:
+                pass
+            
+            # 檢查 return_packet_processor_queue  
+            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:
+                pass
+
+            # 每秒發送 TIMESYNC (來自test51新版本)
             if now - last_timesync >= 1.0:
-                # 發送 TIMESYNC request
-                mav_sock.mav.timesync_send(0, int(now * 1e9))
+                timesync_sent = 0
+                # 對每個輸入設備發送 TIMESYNC request
+                for i, mavlink_socket in enumerate(mavlink_sockets_in):
+                    try:
+                        mavlink_socket.mav.timesync_send(0, int(now * 1e9))
+                        timesync_sent += 1
+                    except Exception as e:
+                        print(f"發送 TIMESYNC 到設備 {i+1} 失敗: {e}")
+                
+                if timesync_sent > 0:
+                    print(f"[DEBUG] 發送 TIMESYNC 到 {timesync_sent} 個設備")
                 last_timesync = now
                 
-            bridge.emit_info()     # 將所有 emitParams 發布到 ROS topic
-            time.sleep(0.5)
+            # ROS2 發布 (來自test51新版本)
+            try:
+                bridge.emit_info()     # 將所有 emitParams 發布到 ROS topic
+                debug_counters['ros2_publishes'] += 1
+                ros2_publish_count += 1
+            except Exception as e:
+                print(f"[ERROR] ROS2 發布失敗: {e}")
+
+            # 狀態報告 (更頻繁的調試輸出)
+            if (time.time() - show_time) >= 2:  # 每2秒顯示一次狀態
+                show_time = time.time()
+                print(f"\n=== 調試狀態報告 (運行時間: {int(now - time.time() + running_time)}秒) ===")
+                
+                # 顯示消息統計
+                print(f"消息計數器: {debug_counters}")
+                print(f"總消息數: {message_count}, ROS2發布次數: {ros2_publish_count}")
+                
+                # 重置調試計數器
+                debug_counters = {k: 0 for k in debug_counters}
+                
+                if len(bridge.mavlink_systems) > 0:
+                    for sysid in bridge.mavlink_systems:
+                        system = bridge.mavlink_systems[sysid]
+                        print(f"系統 {sysid}: socket_id={getattr(system, 'socket_id', 'N/A')}")
+                        
+                        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系統")
+                
+                # 顯示各通道的狀態
+                print(f"輸入通道數量: {len(mavlink_objects_in)} (運行中)")
+                print(f"輸出通道數量: {len(mavlink_objects_out)} (運行中)")
+                print("ROS2發布狀態: 運行中")
+                
+                # 顯示queue狀態
+                print(f"Queue 狀態:")
+                print(f"  fixed_stream_bridge_queue: {mo.fixed_stream_bridge_queue.qsize()}")
+                print(f"  return_packet_processor_queue: {mo.return_packet_processor_queue.qsize()}")
+                for i, q in enumerate(mo.swap_queues):
+                    print(f"  swap_queue[{i}]: {q.qsize()}")
+                
+                print("===================\n")
+            
+            time.sleep(0.1)  # 更快的循環，更及時的調試信息
+            
     except KeyboardInterrupt:
+        print("\n使用者中斷程序...")
         pass
 
-    # -------- 清理 --------
-    bridge.destroy_node()
+    # === 程序結束清理 (來自test51新版本) ===
+    print("正在關閉所有連接...")
+    
+    # -------- 清理 ROS2 --------
+    try:
+        bridge.destroy_node()
+    except Exception as e:
+        print(f"清理主Node失敗: {e}")
+    
+    # 清理備用node（如果存在）
+    if hasattr(bridge, '_backup_node'):
+        try:
+            bridge._backup_node.destroy_node()
+            print("備用Node已清理")
+        except Exception as e:
+            print(f"清理備用Node失敗: {e}")
+    
     rclpy.shutdown()
-
-    mobj.stop();     mobj.thread.join()
-    bridge.stop();   bridge.thread.join()
-    mav_sock.close()
-    print("<=== End of Program")
+    
+    # 關閉輸入通道
+    for i, obj in enumerate(mavlink_objects_in):
+        device_type = "UDP"
+        print(f"關閉設備 {i+1} ({device_type}) 輸入通道")
+        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()
+
+    # 關閉分析器
+    bridge.stop()
+    bridge.thread.join()
+    print('<=== End of Program')