|
|
|
|
@ -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()
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
bridge._init_node()
|
|
|
|
|
|
|
|
|
|
# 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()
|
|
|
|
|
# 添加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}) 轉發")
|
|
|
|
|
|
|
|
|
|
# 做一個空的通道驗證 可以拿來 debug
|
|
|
|
|
mavlink_object_none = mo.mavlink_object(None)
|
|
|
|
|
|
|
|
|
|
print(f"\n成功建立 {len(mavlink_objects_in)} 個輸入連接")
|
|
|
|
|
print(f"成功建立 {len(mavlink_objects_out)} 個輸出連接")
|
|
|
|
|
|
|
|
|
|
print("waiting for mavlink data ...")
|
|
|
|
|
time.sleep(2) # 給一點時間收 HEARTBEAT
|
|
|
|
|
# 啟動所有輸入通道
|
|
|
|
|
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! ===")
|
|
|
|
|
|
|
|
|
|
# 3) 顯示目前偵測到的 sysid 清單
|
|
|
|
|
# 顯示目前偵測到的 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發布...")
|
|
|
|
|
|
|
|
|
|
# 4) 主迴圈：持續將資料 emit 出去
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
# ROS2 發布 (來自test51新版本)
|
|
|
|
|
try:
|
|
|
|
|
bridge.emit_info() # 將所有 emitParams 發布到 ROS topic
|
|
|
|
|
time.sleep(0.5)
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
# -------- 清理 --------
|
|
|
|
|
# === 程序結束清理 (來自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')
|
|
|
|
|
|
