|
|
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() |