1. (中繼的存檔) 準備對 mavlinkObject.py 的 mavlink_object 重構
2. 新增 ringBuffer.py 作為 queue 取代 3. 新增 tests 資料夾分離程式與測試檔案 4. asyncioManager.py 是參考用的範例
This commit is contained in:
@@ -0,0 +1,246 @@
|
||||
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,152 @@
|
||||
import os
|
||||
import sys
|
||||
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
|
||||
|
||||
import time
|
||||
import threading
|
||||
|
||||
from fc_network_adapter.ringBuffer import RingBuffer
|
||||
|
||||
|
||||
def producer(buffer, count, interval=0.01):
|
||||
"""生產者：向緩衝區添加資料"""
|
||||
print(f"Producer started (thread {threading.get_ident()})")
|
||||
for i in range(count):
|
||||
# 嘗試寫入數據，直到成功
|
||||
while not buffer.put(f"Item-{i}"):
|
||||
print(f"Buffer full, producer waiting... (thread {threading.get_ident()})")
|
||||
time.sleep(0.1)
|
||||
|
||||
print(f"Produced: Item-{i}, buffer size: {buffer.size()}")
|
||||
time.sleep(interval) # 模擬生產過程
|
||||
|
||||
print(f"Producer finished (thread {threading.get_ident()})")
|
||||
|
||||
def consumer(buffer, max_items, interval=0.05):
|
||||
"""消費者：從緩衝區讀取資料"""
|
||||
print(f"Consumer started (thread {threading.get_ident()})")
|
||||
items_consumed = 0
|
||||
|
||||
while items_consumed < max_items:
|
||||
# 嘗試讀取數據
|
||||
item = buffer.get()
|
||||
if item:
|
||||
print(f"Consumed: {item}, buffer size: {buffer.size()}")
|
||||
items_consumed += 1
|
||||
else:
|
||||
print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})")
|
||||
|
||||
time.sleep(interval) # 模擬消費過程
|
||||
|
||||
print(f"Consumer finished (thread {threading.get_ident()})")
|
||||
|
||||
def batch_consumer(buffer, interval=0.2):
|
||||
"""批量消費者：一次性讀取緩衝區中的所有資料"""
|
||||
print(f"Batch consumer started (thread {threading.get_ident()})")
|
||||
|
||||
for _ in range(5): # 執行5次批量讀取
|
||||
time.sleep(interval) # 等待緩衝區積累數據
|
||||
items = buffer.get_all()
|
||||
if items:
|
||||
print(f"Batch consumed {len(items)} items: {items}")
|
||||
else:
|
||||
print("Buffer empty for batch consumer")
|
||||
|
||||
print(f"Batch consumer finished (thread {threading.get_ident()})")
|
||||
|
||||
def demonstrate_multi_writer():
|
||||
"""示範多個寫入執行緒同時操作緩衝區"""
|
||||
print("\n=== Demonstrating Multiple Writers ===")
|
||||
buffer = RingBuffer(capacity=80)
|
||||
|
||||
# 創建多個生產者執行緒
|
||||
threads = []
|
||||
for i in range(3):
|
||||
thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1)))
|
||||
threads.append(thread)
|
||||
thread.start()
|
||||
|
||||
# 等待所有執行緒完成
|
||||
for thread in threads:
|
||||
thread.join()
|
||||
|
||||
buffer.print_stats() # 印出統計資訊
|
||||
|
||||
# 讀出所有剩餘資料
|
||||
remaining = buffer.get_all()
|
||||
print(f"Remaining items in buffer after multiple writers: {remaining}")
|
||||
|
||||
def demonstrate_basic_usage():
|
||||
"""示範基本使用方式"""
|
||||
print("\n=== Demonstrating Basic Usage ===")
|
||||
# 創建緩衝區
|
||||
buffer = RingBuffer(capacity=20, buffer_id=7)
|
||||
|
||||
# 檢查初始狀態
|
||||
print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
|
||||
|
||||
# 添加幾個項目
|
||||
for i in range(5):
|
||||
buffer.put(f"Test-{i}")
|
||||
|
||||
# 檢查狀態
|
||||
print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
|
||||
|
||||
# 讀取一個項目
|
||||
item = buffer.get()
|
||||
print(f"Read item: {item}")
|
||||
print(f"After reading 1 item - Content Size: {buffer.size()}")
|
||||
|
||||
# 添加更多項目直到滿
|
||||
items_added = 0
|
||||
while not buffer.is_full():
|
||||
buffer.put(f"Fill-{items_added}")
|
||||
items_added += 1
|
||||
|
||||
print(f"Added {items_added} more items until full")
|
||||
print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
|
||||
|
||||
# 嘗試添加到已滿的緩衝區
|
||||
result = buffer.put("Overflow")
|
||||
print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}")
|
||||
|
||||
# 獲取所有項目
|
||||
all_items = buffer.get_all()
|
||||
print(f"All items in buffer: {all_items}")
|
||||
print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}")
|
||||
|
||||
# 印出統計資訊
|
||||
buffer.print_stats()
|
||||
|
||||
def demonstrate_producer_consumer():
|
||||
"""示範生產者-消費者模式"""
|
||||
print("\n=== Demonstrating Producer-Consumer Pattern ===")
|
||||
buffer = RingBuffer(capacity=16)
|
||||
|
||||
# 創建生產者和消費者執行緒
|
||||
producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1))
|
||||
consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2))
|
||||
batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5))
|
||||
|
||||
# 啟動執行緒
|
||||
producer_thread.start()
|
||||
consumer_thread.start()
|
||||
batch_thread.start()
|
||||
|
||||
# 等待執行緒完成
|
||||
producer_thread.join()
|
||||
consumer_thread.join()
|
||||
batch_thread.join()
|
||||
|
||||
# 檢查最終狀態
|
||||
print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}")
|
||||
|
||||
buffer.print_stats()
|
||||
|
||||
if __name__ == "__main__":
|
||||
# 展示各種使用場景
|
||||
# demonstrate_basic_usage()
|
||||
# demonstrate_producer_consumer()
|
||||
demonstrate_multi_writer()
|
||||
|
||||
print("\nAll demonstrations completed!")
|
||||
Reference in New Issue
Block a user