重大更新 :
1. 修正不同 socket 轉傳間效能低落問題 於 mavlinkObject.py 重構 mavlink_object 增加 async_io_manager (版本目標功能調整 寫於程式碼註解中) 2. 調整各個程式碼的檔案位置 讓測試用程式與專案程式分離
This commit is contained in:
@@ -0,0 +1,448 @@
|
||||
import os
|
||||
import sys
|
||||
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
|
||||
|
||||
# 基礎功能的 import
|
||||
import queue
|
||||
import time
|
||||
|
||||
# ROS2 的 import
|
||||
import rclpy
|
||||
|
||||
# mavlink 的 import
|
||||
from pymavlink import mavutil
|
||||
|
||||
# 自定義的 import
|
||||
from fc_network_adapter import mavlinkObject as mo
|
||||
from fc_network_adapter import mavlinkDevice as md
|
||||
|
||||
# ====================== 分割線 =====================
|
||||
|
||||
test_item = 21
|
||||
running_time = 30
|
||||
|
||||
|
||||
print('test_item : ', test_item)
|
||||
|
||||
'''
|
||||
測試項 個位數 表示分離的功能
|
||||
|
||||
測試項 1X 表示 mavlink_object 的功能 測試連線的能力
|
||||
'''
|
||||
|
||||
if test_item == 10:
|
||||
# 需要開啟一個 ardupilot 的模擬器
|
||||
# 測試 mavlink_object 放入 ring buffer 的應用
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
|
||||
# 清空 ring buffer
|
||||
mo.stream_bridge_ring.clear()
|
||||
mo.return_packet_ring.clear()
|
||||
|
||||
manager = mo.async_io_manager()
|
||||
manager.start()
|
||||
time.sleep(0.5) # 等待事件循環啟動
|
||||
|
||||
# 初始化輸入通道
|
||||
connection_string="udp:127.0.0.1:14560"
|
||||
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
|
||||
|
||||
manager.add_mavlink_object(mavlink_object1)
|
||||
|
||||
start_time = time.time()
|
||||
while (time.time() - start_time) < running_time:
|
||||
items_a = mo.stream_bridge_ring.get_all()
|
||||
items_b = mo.return_packet_ring.get_all()
|
||||
try:
|
||||
print(f"data num {len(items_a)} in return_packet_ring, first data : {items_a[0][2]}")
|
||||
except IndexError:
|
||||
print("stream_bridge_ring is empty")
|
||||
|
||||
try:
|
||||
print(f"data num {len(items_b)} in return_packet_ring, first data : {items_b[0][2]}")
|
||||
except IndexError:
|
||||
print("return_packet_ring is empty")
|
||||
time.sleep(1)
|
||||
|
||||
manager.stop()
|
||||
|
||||
print('<=== End of Program')
|
||||
|
||||
elif test_item == 11:
|
||||
# 需要開啟一個 ardupilot 的模擬器
|
||||
# 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
|
||||
# 清空 ring buffer
|
||||
mo.stream_bridge_ring.clear()
|
||||
mo.return_packet_ring.clear()
|
||||
|
||||
manager = mo.async_io_manager()
|
||||
manager.start()
|
||||
time.sleep(0.5) # 等待事件循環啟動
|
||||
|
||||
# 初始化輸入通道
|
||||
connection_string="udp:127.0.0.1:14560"
|
||||
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
|
||||
manager.add_mavlink_object(mavlink_object1)
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
# 印出目前所有 mavlink_systems 的內容
|
||||
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:
|
||||
if (time.time() - show_time) >= 2:
|
||||
# print("mark B")
|
||||
|
||||
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("===================")
|
||||
|
||||
manager.stop()
|
||||
analyzer.stop()
|
||||
|
||||
print('<=== End of Program')
|
||||
|
||||
elif test_item == 12:
|
||||
# 需要開啟一個 ardupilot 的模擬器 與 GCS
|
||||
# 這邊是測試 mavlink object 作為交換器功能的代碼
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
|
||||
# 清空 ring buffer
|
||||
mo.stream_bridge_ring.clear()
|
||||
mo.return_packet_ring.clear()
|
||||
|
||||
manager = mo.async_io_manager()
|
||||
manager.start()
|
||||
time.sleep(0.5) # 等待事件循環啟動
|
||||
|
||||
# 初始化輸入通道
|
||||
connection_string="udp:127.0.0.1:14560"
|
||||
mavlink_socket_in1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1)
|
||||
|
||||
connection_string="udp:127.0.0.1:14561"
|
||||
mavlink_socket_in2 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2)
|
||||
|
||||
# 初始化輸出通道
|
||||
connection_string="udpout:127.0.0.1:14550"
|
||||
mavlink_socket_out = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
|
||||
|
||||
manager.add_mavlink_object(mavlink_object_out)
|
||||
manager.add_mavlink_object(mavlink_object_in1)
|
||||
manager.add_mavlink_object(mavlink_object_in2)
|
||||
|
||||
mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id)
|
||||
mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id)
|
||||
|
||||
mavlink_object_in2.add_target_socket(mavlink_object_out.socket_id)
|
||||
mavlink_object_out.add_target_socket(mavlink_object_in2.socket_id)
|
||||
|
||||
start_time = time.time()
|
||||
while (time.time() - start_time) < running_time:
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
manager.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 才能使用 node
|
||||
rclpy.init()
|
||||
|
||||
# 清空 ring buffer
|
||||
mo.stream_bridge_ring.clear()
|
||||
mo.return_packet_ring.clear()
|
||||
|
||||
manager = mo.async_io_manager()
|
||||
manager.start()
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
# 關於 Node 的初始化
|
||||
show_time = time.time()
|
||||
analyzer._init_node() # 初始化 node
|
||||
print('初始化 node 完成 耗時 : ',time.time() - show_time)
|
||||
|
||||
time.sleep(0.5) # 系統 Setup 完成
|
||||
|
||||
|
||||
# 創建通道
|
||||
connection_string="udp:127.0.0.1:14560"
|
||||
mavlink_socket = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object3 = mo.mavlink_object(mavlink_socket)
|
||||
manager.add_mavlink_object(mavlink_object3)
|
||||
|
||||
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:
|
||||
# print(analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode'])
|
||||
analyzer.emit_info() # 這邊是測試 node 的運行
|
||||
time.sleep(1)
|
||||
except KeyboardInterrupt:
|
||||
break
|
||||
|
||||
|
||||
# 程式結束
|
||||
analyzer.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
# 結束程式 退出所有 thread
|
||||
manager.stop()
|
||||
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:
|
||||
|
||||
# # 晉凱的測試項目
|
||||
# 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_socket3 = mavutil.mavlink_connection(connection_string)
|
||||
# mavlink_object3 = mo.mavlink_object(mavlink_socket3)
|
||||
# # 設定通道流動
|
||||
# mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
|
||||
# mavlink_object3.multiplexingToReturn = [] #
|
||||
# # mavlink_object3.multiplexingToSwap = [] #
|
||||
# # 啟動通道
|
||||
# mavlink_object3.run()
|
||||
|
||||
|
||||
|
||||
# print('waiting for mavlink data ...')
|
||||
# time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
|
||||
|
||||
# compid = 1
|
||||
# sysid = 1
|
||||
# start_time = time.time()
|
||||
# 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])
|
||||
# end_time = time.time()
|
||||
# print(f"Execution time for create all topic: {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(0.5)
|
||||
# except KeyboardInterrupt:
|
||||
# break
|
||||
|
||||
# analyzer.destroy_node()
|
||||
# rclpy.shutdown()
|
||||
|
||||
# # 結束程式 退出所有 thread
|
||||
# mavlink_object3.stop()
|
||||
# mavlink_object3.thread.join()
|
||||
# analyzer.stop()
|
||||
# analyzer.thread.join()
|
||||
|
||||
# mavlink_socket3.close()
|
||||
# print('<=== End of Program')
|
||||
@@ -1,246 +0,0 @@
|
||||
import os
|
||||
import sys
|
||||
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
|
||||
|
||||
import time
|
||||
import threading
|
||||
import socket
|
||||
import random
|
||||
from pymavlink import mavutil
|
||||
from pymavlink.dialects.v20 import common as mavlink2
|
||||
|
||||
from fc_network_adapter.asyncioManager import AsyncIOManager, Connection
|
||||
|
||||
class MockMAVLinkSocket:
|
||||
"""模擬 pymavlink socket 行為的類，用於測試"""
|
||||
|
||||
def __init__(self, name="mock", simulate_errors=False):
|
||||
self.name = name
|
||||
self.simulate_errors = simulate_errors
|
||||
self.is_closed = False
|
||||
self.received_msgs = []
|
||||
self.seq = 0
|
||||
|
||||
def recv_msg(self):
|
||||
"""模擬接收 MAVLink 消息"""
|
||||
if self.is_closed:
|
||||
return None
|
||||
|
||||
# 隨機模擬網絡延遲
|
||||
time.sleep(random.uniform(0.01, 0.05))
|
||||
|
||||
# 偶爾模擬錯誤
|
||||
if self.simulate_errors and random.random() < 0.1:
|
||||
raise Exception("Simulated network error")
|
||||
|
||||
# 70% 的機率返回消息，30% 返回 None (模擬無數據可讀)
|
||||
if random.random() > 0.3:
|
||||
msg = self._generate_mavlink_message()
|
||||
self.received_msgs.append(msg)
|
||||
return msg
|
||||
|
||||
return None
|
||||
|
||||
def _generate_mavlink_message(self):
|
||||
"""生成模擬 MAVLink 消息"""
|
||||
# 創建 MAVLink 實例
|
||||
mav = mavlink2.MAVLink(self)
|
||||
|
||||
# 設置來源系統和組件
|
||||
mav.srcSystem = random.randint(1, 5)
|
||||
mav.srcComponent = random.randint(1, 5)
|
||||
|
||||
# 創建 HEARTBEAT 消息
|
||||
msg = mavlink2.MAVLink_heartbeat_message(
|
||||
type=mavlink2.MAV_TYPE_QUADROTOR,
|
||||
autopilot=mavlink2.MAV_AUTOPILOT_GENERIC,
|
||||
base_mode=0,
|
||||
custom_mode=0,
|
||||
system_status=0,
|
||||
mavlink_version=3
|
||||
)
|
||||
|
||||
# 打包消息
|
||||
msg.pack(mav)
|
||||
self.seq += 1
|
||||
return msg
|
||||
|
||||
def write(self, data):
|
||||
"""模擬發送數據"""
|
||||
if self.is_closed:
|
||||
raise Exception("Socket is closed")
|
||||
|
||||
# 模擬寫入操作
|
||||
time.sleep(random.uniform(0.001, 0.01))
|
||||
|
||||
# 偶爾模擬錯誤
|
||||
if self.simulate_errors and random.random() < 0.1:
|
||||
raise Exception("Simulated write error")
|
||||
|
||||
return len(data)
|
||||
|
||||
def close(self):
|
||||
"""關閉連接"""
|
||||
self.is_closed = True
|
||||
|
||||
def create_mock_mavlink_sockets(count=3):
|
||||
"""創建多個模擬 MAVLink socket"""
|
||||
sockets = []
|
||||
for i in range(count):
|
||||
# 每三個 socket 中有一個模擬錯誤
|
||||
simulate_errors = (i % 3 == 0)
|
||||
socket = MockMAVLinkSocket(f"mock_{i}", simulate_errors)
|
||||
sockets.append(socket)
|
||||
return sockets
|
||||
|
||||
def monitor_thread(io_manager):
|
||||
"""監視 AsyncIOManager 狀態的執行緒"""
|
||||
print("Starting monitoring thread...")
|
||||
|
||||
try:
|
||||
while True:
|
||||
# 獲取總體統計資訊
|
||||
stats = io_manager.get_stats()
|
||||
print(f"\n=== AsyncIOManager Stats ===")
|
||||
print(f"Total connections: {stats.get('connection_count', 0)}")
|
||||
print(f"Total messages received: {stats.get('rx_count', 0)}")
|
||||
print(f"Total bytes received: {stats.get('rx_bytes', 0)}")
|
||||
print(f"Total errors: {stats.get('errors', 0)}")
|
||||
|
||||
# 獲取每個連接的緩衝區狀態
|
||||
for socket_id in io_manager.connections:
|
||||
buffer = io_manager.get_buffer(socket_id)
|
||||
conn_stats = io_manager.get_stats(socket_id)
|
||||
|
||||
if buffer and not io_manager.connections[socket_id].is_closed:
|
||||
print(f"Connection {socket_id}: Buffer size={buffer.size()}, " +
|
||||
f"Messages={conn_stats.get('rx_count', 0)}, " +
|
||||
f"Last activity={time.time() - io_manager.connections[socket_id].last_activity:.1f}s ago")
|
||||
|
||||
# 每秒更新一次
|
||||
time.sleep(1.0)
|
||||
except KeyboardInterrupt:
|
||||
print("Monitoring thread stopped.")
|
||||
|
||||
def consumer_thread(io_manager, socket_id):
|
||||
"""從指定連接的緩衝區消費消息"""
|
||||
print(f"Starting consumer for socket {socket_id}...")
|
||||
|
||||
try:
|
||||
while True:
|
||||
# 獲取緩衝區
|
||||
buffer = io_manager.get_buffer(socket_id)
|
||||
if not buffer:
|
||||
print(f"Consumer {socket_id}: Buffer not found or connection closed")
|
||||
break
|
||||
|
||||
# 獲取所有消息
|
||||
messages = buffer.get_all()
|
||||
if messages:
|
||||
print(f"Consumer {socket_id}: Received {len(messages)} messages")
|
||||
for msg in messages:
|
||||
# 處理每條消息 (這裡只是示例，實際應用中可能有更複雜的處理)
|
||||
print(f" - Message from SysID={msg.get_srcSystem()}, CompID={msg.get_srcComponent()}, Type={msg.get_type()}")
|
||||
|
||||
# 短暫休眠，避免 CPU 空轉
|
||||
time.sleep(0.2)
|
||||
except Exception as e:
|
||||
print(f"Consumer {socket_id} stopped: {e}")
|
||||
|
||||
def send_command_thread(io_manager, socket_ids):
|
||||
"""定期向所有連接發送命令"""
|
||||
print("Starting command sender thread...")
|
||||
|
||||
# 創建一個 MAVLink 實例用於生成消息
|
||||
mav = mavlink2.MAVLink(None)
|
||||
mav.srcSystem = 255 # GCS system ID
|
||||
mav.srcComponent = 0
|
||||
|
||||
try:
|
||||
count = 0
|
||||
while True:
|
||||
# 等待 2 秒
|
||||
time.sleep(2.0)
|
||||
count += 1
|
||||
|
||||
# 創建一個命令消息
|
||||
msg = mavlink2.MAVLink_command_long_message(
|
||||
target_system=1,
|
||||
target_component=1,
|
||||
command=mavlink2.MAV_CMD_REQUEST_PROTOCOL_VERSION,
|
||||
confirmation=0,
|
||||
param1=0, param2=0, param3=0,
|
||||
param4=0, param5=0, param6=0, param7=0
|
||||
)
|
||||
|
||||
# 向所有連接發送消息
|
||||
for socket_id in socket_ids:
|
||||
if socket_id in io_manager.connections and not io_manager.connections[socket_id].is_closed:
|
||||
try:
|
||||
# 打包並發送消息
|
||||
result = io_manager.send_message(socket_id, msg)
|
||||
print(f"Sent command to socket {socket_id}: {'Success' if result else 'Failed'}")
|
||||
except Exception as e:
|
||||
print(f"Error sending to socket {socket_id}: {e}")
|
||||
|
||||
# 每 10 次迭代關閉一個連接 (用於演示關閉功能)
|
||||
if count % 10 == 0 and socket_ids:
|
||||
socket_id = socket_ids.pop(0)
|
||||
print(f"Closing connection {socket_id}...")
|
||||
io_manager.remove_connection(socket_id)
|
||||
except Exception as e:
|
||||
print(f"Command sender thread stopped: {e}")
|
||||
|
||||
def main():
|
||||
print("AsyncIOManager Example")
|
||||
print("======================")
|
||||
|
||||
# 創建 AsyncIOManager 實例
|
||||
io_manager = AsyncIOManager()
|
||||
|
||||
# 啟動 I/O 管理器
|
||||
io_manager.start()
|
||||
print("AsyncIOManager started")
|
||||
|
||||
# 創建模擬 MAVLink socket
|
||||
mock_sockets = create_mock_mavlink_sockets(5)
|
||||
socket_ids = []
|
||||
|
||||
# 註冊連接
|
||||
for i, socket in enumerate(mock_sockets):
|
||||
socket_id = io_manager.register_connection(socket)
|
||||
socket_ids.append(socket_id)
|
||||
print(f"Registered socket {i} with ID {socket_id}")
|
||||
|
||||
# 啟動監視執行緒
|
||||
monitor = threading.Thread(target=monitor_thread, args=(io_manager,))
|
||||
monitor.daemon = True
|
||||
monitor.start()
|
||||
|
||||
# 為每個連接啟動消費者執行緒
|
||||
consumers = []
|
||||
for socket_id in socket_ids:
|
||||
consumer = threading.Thread(target=consumer_thread, args=(io_manager, socket_id))
|
||||
consumer.daemon = True
|
||||
consumer.start()
|
||||
consumers.append(consumer)
|
||||
|
||||
# 啟動命令發送執行緒
|
||||
sender = threading.Thread(target=send_command_thread, args=(io_manager, socket_ids.copy()))
|
||||
sender.daemon = True
|
||||
sender.start()
|
||||
|
||||
print("\nPress Ctrl+C to exit...")
|
||||
try:
|
||||
# 運行一段時間然後退出
|
||||
time.sleep(30)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
# 清理資源
|
||||
print("\nShutting down...")
|
||||
io_manager.stop()
|
||||
print("AsyncIOManager stopped")
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,468 @@
|
||||
#!/usr/bin/env python
|
||||
"""
|
||||
測試腳本，用於測試 mavlinkObject.py 中的 mavlink_object 和 async_io_manager 類別
|
||||
"""
|
||||
|
||||
import os
|
||||
import sys
|
||||
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
|
||||
|
||||
|
||||
import unittest
|
||||
import time
|
||||
import threading
|
||||
import socket
|
||||
import asyncio
|
||||
from unittest.mock import MagicMock, patch
|
||||
|
||||
# 導入要測試的模組
|
||||
from fc_network_adapter.mavlinkObject import (
|
||||
mavlink_object,
|
||||
async_io_manager,
|
||||
MavlinkObjectState,
|
||||
stream_bridge_ring,
|
||||
return_packet_ring
|
||||
)
|
||||
|
||||
|
||||
class MockMavlinkSocket:
|
||||
"""模擬 Mavlink Socket 的類別，用於測試"""
|
||||
|
||||
def __init__(self, test_data=None):
|
||||
self.closed = False
|
||||
self.test_data = test_data or []
|
||||
self.data_index = 0
|
||||
self.written_data = []
|
||||
|
||||
def recv_msg(self):
|
||||
if not self.test_data or self.data_index >= len(self.test_data):
|
||||
return None
|
||||
|
||||
data = self.test_data[self.data_index]
|
||||
self.data_index += 1
|
||||
return data
|
||||
|
||||
def write(self, data):
|
||||
self.written_data.append(data)
|
||||
|
||||
def close(self):
|
||||
self.closed = True
|
||||
|
||||
|
||||
class MockMavlinkMessage:
|
||||
"""模擬 Mavlink 訊息的類別，用於測試"""
|
||||
|
||||
def __init__(self, msg_id, sysid, compid, seq=0):
|
||||
self.msg_id = msg_id
|
||||
self.sysid = sysid
|
||||
self.compid = compid
|
||||
self.seq = seq
|
||||
self.msg_buf = bytes([msg_id, sysid, compid, seq])
|
||||
|
||||
def get_msgId(self):
|
||||
return self.msg_id
|
||||
|
||||
def get_srcSystem(self):
|
||||
return self.sysid
|
||||
|
||||
def get_srcComponent(self):
|
||||
return self.compid
|
||||
|
||||
def get_seq(self):
|
||||
return self.seq
|
||||
|
||||
def get_msgbuf(self):
|
||||
return self.msg_buf
|
||||
|
||||
def get_type(self):
|
||||
return f"MSG_ID_{self.msg_id}"
|
||||
|
||||
|
||||
class TestMavlinkObject(unittest.TestCase):
|
||||
"""測試 mavlink_object 類別"""
|
||||
|
||||
def setUp(self):
|
||||
"""在每個測試方法執行前準備環境"""
|
||||
# 清空全局變數
|
||||
mavlink_object.mavlinkObjects = {}
|
||||
mavlink_object.socket_num = 0
|
||||
|
||||
# 清空 ring buffer
|
||||
stream_bridge_ring.clear()
|
||||
return_packet_ring.clear()
|
||||
|
||||
# 創建測試訊息
|
||||
self.heartbeat_msg = MockMavlinkMessage(msg_id=0, sysid=1, compid=1)
|
||||
self.attitude_msg = MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
|
||||
self.position_msg = MockMavlinkMessage(msg_id=32, sysid=1, compid=1)
|
||||
|
||||
# 創建模擬的 socket # 假的 Mavlink Socket
|
||||
self.mock_socket = MockMavlinkSocket([
|
||||
self.heartbeat_msg,
|
||||
self.attitude_msg,
|
||||
self.position_msg
|
||||
])
|
||||
|
||||
# 創建測試對象
|
||||
self.mavlink_obj = mavlink_object(self.mock_socket)
|
||||
|
||||
def test_initialization(self):
|
||||
"""測試 mavlink_object 初始化是否正確"""
|
||||
# print("Testing mavlink_object initialization")
|
||||
self.assertEqual(self.mavlink_obj.socket_id, 0)
|
||||
self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT)
|
||||
self.assertEqual(len(self.mavlink_obj.target_sockets), 0)
|
||||
self.assertEqual(self.mavlink_obj.bridge_msg_types, [0])
|
||||
|
||||
def test_add_remove_target_socket(self):
|
||||
"""測試添加和移除目標端口功能"""
|
||||
# 添加目標端口
|
||||
self.assertTrue(self.mavlink_obj.add_target_socket(1))
|
||||
self.assertEqual(len(self.mavlink_obj.target_sockets), 1)
|
||||
self.assertEqual(self.mavlink_obj.target_sockets[0], 1)
|
||||
self.assertTrue(self.mavlink_obj.add_target_socket(2))
|
||||
self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
|
||||
self.assertIn(2, self.mavlink_obj.target_sockets)
|
||||
|
||||
# 嘗試添加已存在的端口
|
||||
self.assertFalse(self.mavlink_obj.add_target_socket(1))
|
||||
self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
|
||||
|
||||
# 嘗試添加自己的端口
|
||||
self.assertFalse(self.mavlink_obj.add_target_socket(0))
|
||||
self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
|
||||
|
||||
# 移除端口
|
||||
self.assertTrue(self.mavlink_obj.remove_target_socket(2))
|
||||
self.assertEqual(len(self.mavlink_obj.target_sockets), 1)
|
||||
|
||||
# 嘗試移除不存在的端口
|
||||
self.assertFalse(self.mavlink_obj.remove_target_socket(2))
|
||||
|
||||
def test_set_message_types(self):
|
||||
"""測試設置訊息類型功能"""
|
||||
# 設置橋接器訊息類型
|
||||
self.assertTrue(self.mavlink_obj.set_bridge_message_types([0, 30]))
|
||||
self.assertEqual(self.mavlink_obj.bridge_msg_types, [0, 30])
|
||||
|
||||
# 設置回傳處理器訊息類型
|
||||
self.assertTrue(self.mavlink_obj.set_return_message_types([32]))
|
||||
self.assertEqual(self.mavlink_obj.return_msg_types, [32])
|
||||
|
||||
# 測試無效的訊息類型
|
||||
self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid"))
|
||||
self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"]))
|
||||
|
||||
def test_send_message(self):
|
||||
"""測試 send_message 功能"""
|
||||
# 創建一個新的 mavlink_object 實例
|
||||
mock_socket = MockMavlinkSocket()
|
||||
mavlink_obj = mavlink_object(mock_socket)
|
||||
|
||||
# 準備測試數據
|
||||
test_message1 = b"test_message_1"
|
||||
test_message2 = b"test_message_2"
|
||||
|
||||
# 測試初始狀態
|
||||
self.assertEqual(len(mock_socket.written_data), 0)
|
||||
|
||||
# 測試非運行狀態下發送消息
|
||||
self.assertFalse(mavlink_obj.send_message(test_message1))
|
||||
self.assertEqual(len(mock_socket.written_data), 0)
|
||||
|
||||
# 啟動 manager
|
||||
manager = async_io_manager()
|
||||
manager.start()
|
||||
time.sleep(0.5) # 等待事件循環啟動
|
||||
|
||||
# 添加對象到 manager
|
||||
manager.add_mavlink_object(mavlink_obj)
|
||||
time.sleep(0.1) # 等待對象啟動
|
||||
|
||||
# 確認對象狀態
|
||||
self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING)
|
||||
|
||||
# 測試發送消息
|
||||
self.assertTrue(mavlink_obj.send_message(test_message1))
|
||||
time.sleep(0.2) # 等待消息處理
|
||||
|
||||
# 確認消息已發送
|
||||
self.assertEqual(len(mock_socket.written_data), 1)
|
||||
self.assertEqual(mock_socket.written_data[0], test_message1)
|
||||
|
||||
# 測試連續發送多條消息
|
||||
self.assertTrue(mavlink_obj.send_message(test_message2))
|
||||
time.sleep(0.2) # 等待消息處理
|
||||
|
||||
# 確認兩條消息都已發送
|
||||
self.assertEqual(len(mock_socket.written_data), 2)
|
||||
self.assertEqual(mock_socket.written_data[1], test_message2)
|
||||
|
||||
# 模擬發送出錯的情況
|
||||
class ErrorWriteSocket(MockMavlinkSocket):
|
||||
def write(self, data):
|
||||
raise Exception("Write error")
|
||||
|
||||
error_socket = ErrorWriteSocket()
|
||||
error_obj = mavlink_object(error_socket)
|
||||
manager.add_mavlink_object(error_obj)
|
||||
time.sleep(0.1) # 等待對象啟動
|
||||
|
||||
# 發送消息到錯誤的 socket
|
||||
self.assertTrue(error_obj.send_message(test_message1))
|
||||
time.sleep(0.2) # 等待消息處理
|
||||
|
||||
# 即使寫入失敗，send_message 應該也返回 True
|
||||
# 因為消息已成功加入到佇列中，只是後續的實際發送失敗
|
||||
|
||||
# 停止 manager
|
||||
manager.stop()
|
||||
time.sleep(0.5) # 等待 manager 停止
|
||||
|
||||
# 測試對象已關閉後發送消息
|
||||
self.assertFalse(mavlink_obj.send_message(test_message1))
|
||||
self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加
|
||||
|
||||
class TestAsyncIOManager(unittest.TestCase):
|
||||
"""測試 async_io_manager 類別"""
|
||||
|
||||
def setUp(self):
|
||||
"""在每個測試方法執行前準備環境"""
|
||||
# 清空全局變數
|
||||
mavlink_object.mavlinkObjects = {}
|
||||
mavlink_object.socket_num = 0
|
||||
|
||||
# 清空 ring buffer
|
||||
stream_bridge_ring.clear()
|
||||
return_packet_ring.clear()
|
||||
|
||||
# 創建 async_io_manager 實例
|
||||
self.manager = async_io_manager()
|
||||
|
||||
# 模擬 mavlink 對象
|
||||
self.mock_socket1 = MockMavlinkSocket([
|
||||
MockMavlinkMessage(msg_id=0, sysid=1, compid=1),
|
||||
MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
|
||||
])
|
||||
self.mock_socket2 = MockMavlinkSocket([
|
||||
MockMavlinkMessage(msg_id=0, sysid=2, compid=1)
|
||||
])
|
||||
|
||||
self.mavlink_obj1 = mavlink_object(self.mock_socket1)
|
||||
self.mavlink_obj2 = mavlink_object(self.mock_socket2)
|
||||
|
||||
def tearDown(self):
|
||||
"""在每個測試方法執行後清理環境"""
|
||||
if self.manager.running:
|
||||
self.manager.stop()
|
||||
|
||||
def test_singleton_pattern(self):
|
||||
"""測試 async_io_manager 的單例模式 就是不會產生兩個 magager"""
|
||||
manager1 = async_io_manager()
|
||||
manager2 = async_io_manager()
|
||||
self.assertIs(manager1, manager2)
|
||||
|
||||
def test_start_stop(self):
|
||||
"""測試 async_io_manager 的啟動和停止功能"""
|
||||
# 啟動管理器
|
||||
self.manager.start()
|
||||
self.assertTrue(self.manager.running)
|
||||
self.assertIsNotNone(self.manager.thread)
|
||||
|
||||
# 再次啟動應該沒有效果
|
||||
old_thread = self.manager.thread
|
||||
self.manager.start()
|
||||
self.assertIs(self.manager.thread, old_thread)
|
||||
|
||||
# 停止管理器
|
||||
self.manager.stop()
|
||||
self.assertFalse(self.manager.running)
|
||||
|
||||
# 最多等待 5 秒讓線程結束
|
||||
start_time = time.time()
|
||||
while self.manager.thread.is_alive() and time.time() - start_time < 5:
|
||||
time.sleep(0.1)
|
||||
self.assertFalse(self.manager.thread.is_alive())
|
||||
|
||||
def test_add_remove_objects(self):
|
||||
"""測試添加和移除 mavlink_object"""
|
||||
# 啟動管理器
|
||||
self.manager.start()
|
||||
time.sleep(0.5) # 等待事件循環啟動
|
||||
|
||||
# 添加對象
|
||||
self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj1))
|
||||
self.assertEqual(len(self.manager.managed_objects), 1)
|
||||
self.assertEqual(self.mavlink_obj1.state, MavlinkObjectState.RUNNING)
|
||||
|
||||
# 添加另一個對象
|
||||
self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj2))
|
||||
self.assertEqual(len(self.manager.managed_objects), 2)
|
||||
|
||||
# 檢查受管理對象列表
|
||||
managed_objects = self.manager.get_managed_objects()
|
||||
self.assertEqual(len(managed_objects), 2)
|
||||
self.assertIn(0, managed_objects)
|
||||
self.assertIn(1, managed_objects)
|
||||
|
||||
# 移除對象
|
||||
self.assertTrue(self.manager.remove_mavlink_object(0))
|
||||
self.assertEqual(len(self.manager.managed_objects), 1)
|
||||
|
||||
# 嘗試移除不存在的對象
|
||||
self.assertFalse(self.manager.remove_mavlink_object(999))
|
||||
|
||||
# 停止管理器
|
||||
self.manager.stop()
|
||||
|
||||
def test_data_processing(self):
|
||||
"""測試數據處理"""
|
||||
# 創建用於轉發的 mavlink_objects
|
||||
mock_socket3 = MockMavlinkSocket()
|
||||
mavlink_obj3 = mavlink_object(mock_socket3)
|
||||
|
||||
# 設置轉發
|
||||
self.mavlink_obj1.add_target_socket(2) # socket1 轉發到 socket3
|
||||
|
||||
# 設置訊息類型
|
||||
self.mavlink_obj1.set_bridge_message_types([0, 30]) # HEARTBEAT, ATTITUDE
|
||||
# self.mavlink_obj1.enable_return(True)
|
||||
self.mavlink_obj1.set_return_message_types([30]) # ATTITUDE
|
||||
|
||||
# 啟動管理器並添加對象
|
||||
self.manager.start()
|
||||
time.sleep(0.5) # 等待事件循環啟動
|
||||
|
||||
self.manager.add_mavlink_object(self.mavlink_obj1)
|
||||
self.manager.add_mavlink_object(mavlink_obj3)
|
||||
|
||||
# 等待處理完成
|
||||
time.sleep(1.0)
|
||||
|
||||
# print("testing Mark A")
|
||||
|
||||
# 檢查 Ring buffer 是否有正確的數據
|
||||
self.assertEqual(stream_bridge_ring.size(), 2) # HEARTBEAT + ATTITUDE
|
||||
self.assertEqual(return_packet_ring.size(), 1) # ATTITUDE
|
||||
|
||||
a = stream_bridge_ring.get()
|
||||
print(f"stream_bridge_ring: {a}")
|
||||
|
||||
# 檢查是否正確轉發
|
||||
self.assertEqual(len(mock_socket3.written_data), 2) # HEARTBEAT + ATTITUDE
|
||||
|
||||
# print("testing Mark B")
|
||||
|
||||
# 停止管理器
|
||||
self.manager.stop()
|
||||
|
||||
def test_error_handling(self):
|
||||
"""測試錯誤處理情況"""
|
||||
print("=== mark A ===")
|
||||
# 創建一個會引發異常的 socket
|
||||
class ErrorSocket(MockMavlinkSocket):
|
||||
def recv_msg(self):
|
||||
raise Exception("Test exception")
|
||||
|
||||
error_socket = ErrorSocket()
|
||||
mavlink_obj_err = mavlink_object(error_socket)
|
||||
|
||||
# 啟動管理器並添加對象
|
||||
self.manager.start()
|
||||
time.sleep(0.5) # 等待事件循環啟動
|
||||
|
||||
self.manager.add_mavlink_object(mavlink_obj_err)
|
||||
|
||||
print("=== mark B ===")
|
||||
|
||||
# 等待錯誤處理
|
||||
time.sleep(1.0)
|
||||
|
||||
# 對象應該進入錯誤狀態，但不會崩潰
|
||||
# self.assertEqual(mavlink_obj_err.state, MavlinkObjectState.ERROR)
|
||||
|
||||
print("=== mark C ===")
|
||||
|
||||
# 管理器應該仍在運行
|
||||
self.assertTrue(self.manager.running)
|
||||
|
||||
|
||||
# 故意等一段時間 確認 socket 有被 manager 處理掉
|
||||
time.sleep(3)
|
||||
|
||||
# 停止管理器
|
||||
self.manager.stop()
|
||||
|
||||
|
||||
class TestIntegration(unittest.TestCase):
|
||||
"""整合測試，測試多個 mavlink_object 之間的互動"""
|
||||
|
||||
def test_bidirectional_forwarding(self):
|
||||
"""測試雙向轉發"""
|
||||
# 清空全局變數和 ring buffer
|
||||
mavlink_object.mavlinkObjects = {}
|
||||
mavlink_object.socket_num = 0
|
||||
stream_bridge_ring.clear()
|
||||
return_packet_ring.clear()
|
||||
|
||||
# 創建三個 mavlink 對象，模擬三個通道
|
||||
socket1 = MockMavlinkSocket([
|
||||
MockMavlinkMessage(msg_id=0, sysid=1, compid=1),
|
||||
MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
|
||||
])
|
||||
socket2 = MockMavlinkSocket([
|
||||
MockMavlinkMessage(msg_id=0, sysid=2, compid=1),
|
||||
MockMavlinkMessage(msg_id=32, sysid=2, compid=1)
|
||||
])
|
||||
socket3 = MockMavlinkSocket([
|
||||
MockMavlinkMessage(msg_id=0, sysid=3, compid=1),
|
||||
MockMavlinkMessage(msg_id=33, sysid=3, compid=1)
|
||||
])
|
||||
|
||||
obj1 = mavlink_object(socket1)
|
||||
obj2 = mavlink_object(socket2)
|
||||
obj3 = mavlink_object(socket3)
|
||||
|
||||
# 設置雙向轉發
|
||||
# obj1 <-> obj2 <-> obj3
|
||||
obj1.add_target_socket(1) # obj1 -> obj2
|
||||
obj2.add_target_socket(0) # obj2 -> obj1
|
||||
obj2.add_target_socket(2) # obj2 -> obj3
|
||||
obj3.add_target_socket(1) # obj3 -> obj2
|
||||
|
||||
# 啟動 async_io_manager
|
||||
manager = async_io_manager()
|
||||
manager.start()
|
||||
time.sleep(0.5) # 等待事件循環啟動
|
||||
|
||||
# 添加所有 mavlink_object
|
||||
manager.add_mavlink_object(obj1)
|
||||
manager.add_mavlink_object(obj2)
|
||||
manager.add_mavlink_object(obj3)
|
||||
|
||||
# 等待處理所有訊息
|
||||
time.sleep(1.5)
|
||||
|
||||
# 檢查轉發結果
|
||||
# socket1 應該收到 socket2 和 socket3 的訊息
|
||||
self.assertEqual(len(socket1.written_data), 4) # 2 from obj2, 2 from obj3 via obj2
|
||||
|
||||
# socket2 應該收到 socket1 和 socket3 的訊息
|
||||
self.assertEqual(len(socket2.written_data), 4) # 2 from obj1, 2 from obj3
|
||||
|
||||
# socket3 應該收到 socket1 和 socket2 的訊息
|
||||
self.assertEqual(len(socket3.written_data), 4) # 2 from obj1 via obj2, 2 from obj2
|
||||
|
||||
# 檢查 ring buffer 的數據
|
||||
# 假設所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0]
|
||||
# 應該有 3 個 HEARTBEAT 訊息
|
||||
self.assertEqual(stream_bridge_ring.size(), 3) # 3 HEARTBEAT
|
||||
|
||||
# 停止管理器
|
||||
manager.stop()
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main(defaultTest="TestMavlinkObject.test_send_message")
|
||||
Reference in New Issue
Block a user