#!/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 ..fc_network_adapter.mavlinkObject import ( mavlink_object, async_io_manager, MavlinkObjectState, stream_bridge_ring, return_packet_ring ) # 預先定義好的真實 MAVLink heartbeat 封包 (MAVLink 1.0 格式) # Format: STX(0xFE) + LEN + SEQ + SYS + COMP + MSG_ID + PAYLOAD(9 bytes for heartbeat) + CRC(2 bytes) HEARTBEAT_PACKET_1 = bytes([ 0xFE, # STX (MAVLink 1.0) 0x09, # payload length (9 bytes) 0x00, # sequence 0x01, # system ID = 1 0x01, # component ID = 1 0x00, # message ID (HEARTBEAT = 0) # Payload (9 bytes): custom_mode(4), type(1), autopilot(1), base_mode(1), system_status(1), mavlink_version(1) 0x00, 0x00, 0x00, 0x00, # custom_mode = 0 0x02, # type = MAV_TYPE_QUADROTOR (2) 0x03, # autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA (3) 0x40, # base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (64) 0x03, # system_status = MAV_STATE_STANDBY (3) 0x03, # mavlink_version = 3 0x62, 0x8E # CRC (simplified placeholder) ]) HEARTBEAT_PACKET_2 = bytes([ 0xFE, # STX 0x09, # payload length 0x01, # sequence (增加) 0x01, # system ID = 1 0x01, # component ID = 1 0x00, # message ID (HEARTBEAT = 0) 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x41, 0x03, 0x03, 0x33, 0xEC ]) HEARTBEAT_PACKET_3 = bytes([ 0xFE, # STX 0x09, # payload length 0x02, # sequence 0x02, # system ID = 2 0x01, # component ID = 1 0x00, # message ID (HEARTBEAT = 0) 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x42, 0x03, 0x03, 0x37, 0x44 ]) class MockMavlinkSocket: """模擬 Mavlink Socket 的類別，用於測試 使用真實的 MAVLink 封包，而不是模擬的訊息對象 """ def __init__(self, test_packets=None): """ Args: test_packets: list of bytes，每個元素都是完整的 MAVLink 封包 """ self.closed = False self.test_packets = test_packets or [] self.packet_index = 0 self.written_data = [] # 使用 pymavlink 來解析封包 from pymavlink import mavutil self.mav_parser = mavutil.mavlink.MAVLink(self) def recv_msg(self): """返回解析後的 MAVLink 訊息對象""" if not self.test_packets or self.packet_index >= len(self.test_packets): return None packet = self.test_packets[self.packet_index] self.packet_index += 1 # 使用 pymavlink 解析封包 try: for byte in packet: msg = self.mav_parser.parse_char(bytes([byte])) if msg: return msg except Exception as e: print(f"Error parsing packet: {e}") return None return None def write(self, data): """寫入數據（用於檢查轉發）""" self.written_data.append(data) def close(self): """關閉 socket""" self.closed = True 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() # 創建模擬的 socket，使用真實封包 self.mock_socket = MockMavlinkSocket([HEARTBEAT_PACKET_1]) # 創建測試對象 self.mavlink_obj = mavlink_object(self.mock_socket) def test_initialization(self): """測試 mavlink_object 初始化是否正確""" 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]) self.assertEqual(self.mavlink_obj.return_msg_types, []) 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_validation(self): """測試 send_message 的數據驗證功能（不需要啟動 manager）""" # 測試非運行狀態下發送消息 self.assertFalse(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1)) # 測試無效的數據類型 self.mavlink_obj.state = MavlinkObjectState.RUNNING # 臨時設置狀態 self.assertFalse(self.mavlink_obj.send_message("invalid")) self.assertFalse(self.mavlink_obj.send_message(123)) # 測試太短的封包 self.assertFalse(self.mavlink_obj.send_message(bytes([0xFE, 0x00]))) # 測試無效的起始標記 invalid_packet = bytes([0xFF, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00]) self.assertFalse(self.mavlink_obj.send_message(invalid_packet)) # 測試有效的封包可以加入佇列 self.assertTrue(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1)) self.assertEqual(len(self.mavlink_obj.outgoing_msgs), 1) self.mavlink_obj.state = MavlinkObjectState.INIT # 恢復狀態 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([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2]) self.mock_socket2 = MockMavlinkSocket([HEARTBEAT_PACKET_3]) 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.shutdown() def test_singleton_pattern(self): """測試 async_io_manager 的單例模式""" 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.shutdown() 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.shutdown() class TestIntegration(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() # 創建 async_io_manager 實例 self.manager = async_io_manager() def tearDown(self): """在每個測試方法執行後清理環境""" if self.manager.running: self.manager.shutdown() def test_send_message_with_manager(self): """測試透過 async_io_manager 發送訊息的完整流程""" # 創建一個新的 mavlink_object 實例 mock_socket = MockMavlinkSocket() mavlink_obj = mavlink_object(mock_socket) # 測試初始狀態 self.assertEqual(len(mock_socket.written_data), 0) # 測試非運行狀態下發送消息 self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) self.assertEqual(len(mock_socket.written_data), 0) # 啟動 manager self.manager.start() time.sleep(0.5) # 等待事件循環啟動 # 添加對象到 manager self.manager.add_mavlink_object(mavlink_obj) time.sleep(0.1) # 等待對象啟動 # 確認對象狀態 self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING) # 測試發送消息 self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) time.sleep(0.2) # 等待消息處理 # 確認消息已發送 self.assertEqual(len(mock_socket.written_data), 1) self.assertEqual(mock_socket.written_data[0], HEARTBEAT_PACKET_1) # 測試連續發送多條消息 self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_2)) time.sleep(0.2) # 等待消息處理 # 確認兩條消息都已發送 self.assertEqual(len(mock_socket.written_data), 2) self.assertEqual(mock_socket.written_data[1], HEARTBEAT_PACKET_2) # 停止 manager self.manager.shutdown() time.sleep(0.5) # 等待 manager 停止 # 測試對象已關閉後發送消息 self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加 def test_data_processing_and_forwarding(self): """測試數據處理與轉發流程""" # 創建用於轉發的 mavlink_objects mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2,]) mock_socket3 = MockMavlinkSocket() mavlink_obj1 = mavlink_object(mock_socket1) mavlink_obj3 = mavlink_object(mock_socket3) # 設置訊息類型 mavlink_obj1.set_bridge_message_types([0]) # 只處理 HEARTBEAT # 設置轉發: obj1 -> obj3 mavlink_obj1.add_target_socket(mavlink_obj3.socket_id) # socket1 轉發到 socket3 (socket_id=1) # 啟動管理器並添加對象 self.manager.start() time.sleep(0.5) # 等待事件循環啟動 """ 這邊出現很奇怪的狀況 應該說 設計時沒有考量 但是實測會發現 mavlink_obj3 是接收端 必需要被優先加入 manager 才能正確接收來自 mavlink_obj1 的轉發封包 若先把 mavlink_ojb1 加入 manger 則可能會導致前面幾個封包丟失 """ self.manager.add_mavlink_object(mavlink_obj3) self.manager.add_mavlink_object(mavlink_obj1) # 等待處理完成 time.sleep(0.5) # 檢查 Ring buffer 是否有正確的數據 self.assertGreaterEqual(stream_bridge_ring.size(), 2) # 至少 2 個 HEARTBEAT # 檢查是否正確轉發 self.assertGreaterEqual(len(mock_socket3.written_data), 2) # 至少 2 個 HEARTBEAT # 停止管理器 self.manager.shutdown() 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() socket2 = MockMavlinkSocket() socket3 = MockMavlinkSocket() 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 self.manager.start() time.sleep(0.5) # 等待事件循環啟動 # 添加所有 mavlink_object self.manager.add_mavlink_object(obj1) self.manager.add_mavlink_object(obj2) self.manager.add_mavlink_object(obj3) # 對三個對象添加數據 socket1.test_packets.append(HEARTBEAT_PACKET_1) socket2.test_packets.append(HEARTBEAT_PACKET_2) socket3.test_packets.append(HEARTBEAT_PACKET_3) # 等待處理所有訊息 time.sleep(1.0) # 檢查轉發結果 # socket1 應該收到 socket2 的訊息 self.assertGreaterEqual(len(socket1.written_data), 1) # socket2 應該收到 socket1 和 socket3 的訊息 self.assertGreaterEqual(len(socket2.written_data), 2) # socket3 應該收到 socket2 的訊息 self.assertGreaterEqual(len(socket3.written_data), 1) # 檢查 ring buffer 的數據 # 所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0] self.assertGreaterEqual(stream_bridge_ring.size(), 3) # 至少 3 個 HEARTBEAT # 停止管理器 self.manager.shutdown() if __name__ == "__main__": # 可以指定要運行的測試 # unittest.main(defaultTest="TestMavlinkObject.test_send_message_validation") # unittest.main(defaultTest="TestAsyncIOManager.test_add_remove_objects") unittest.main(defaultTest="TestIntegration.test_bidirectional_forwarding") unittest.main()