Bring fc_network_adapter from master
This commit is contained in:
@@ -0,0 +1,277 @@
|
||||
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 mavlinkVehicleView as mvv
|
||||
# from ..fc_network_adapter import mavlinkDevice as md
|
||||
|
||||
# ====================== 分割線 =====================
|
||||
|
||||
test_item = 1
|
||||
running_time = 3
|
||||
|
||||
|
||||
print('test_item : ', test_item)
|
||||
|
||||
'''
|
||||
測試項 個位數 表示分離的功能
|
||||
|
||||
測試項 1X 表示 mavlink_object 的功能 測試連線的能力
|
||||
'''
|
||||
|
||||
if test_item == 1:
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
|
||||
connection_string="udp:127.0.0.1:14591"
|
||||
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
|
||||
# mavlink_object1 = mo.mavlink_object(mavlink_socket1)
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
print("mark A")
|
||||
|
||||
# print("Socket IP:", mavlink_socket1.target_system)
|
||||
print("Socket port:", mavlink_socket1.port.getsockname())
|
||||
|
||||
# print("=== ", dir(mavlink_socket1.port))
|
||||
|
||||
|
||||
elif 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..1:14571"
|
||||
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
|
||||
|
||||
sock = mavlink_socket1.port
|
||||
print("Socket port:", sock)
|
||||
|
||||
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.shutdown()
|
||||
|
||||
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:14571"
|
||||
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
|
||||
manager.add_mavlink_object(mavlink_object1)
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
bridge = mo.mavlink_bridge()
|
||||
bridge.start()
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
# 印出目前所有 mavlink_systems 的內容
|
||||
print('目前所有的系統 : ')
|
||||
all_vehicles = mvv.vehicle_registry.get_all()
|
||||
for sysid, vehicle in all_vehicles.items():
|
||||
print(f" System {sysid}: {vehicle}")
|
||||
|
||||
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, vehicle in all_vehicles.items():
|
||||
for compid in vehicle.components:
|
||||
comp = vehicle.get_component(compid)
|
||||
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, comp.packet_stats.received_count))
|
||||
comp.reset_packet_stats()
|
||||
print("===================")
|
||||
|
||||
manager.shutdown()
|
||||
bridge.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:14571"
|
||||
mavlink_socket_in1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1)
|
||||
|
||||
connection_string="udp:127.0.0.1:14571"
|
||||
mavlink_socket_in2 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2)
|
||||
|
||||
# 初始化輸出通道
|
||||
connection_string="udpout:127.0.0.1:14551"
|
||||
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)
|
||||
|
||||
time.sleep(1) # 等待通道啟動
|
||||
|
||||
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.shutdown()
|
||||
|
||||
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 == 52:
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
|
||||
manager = mo.async_io_manager()
|
||||
manager.start()
|
||||
|
||||
# print(manager.thread.is_alive())
|
||||
|
||||
manager.shutdown()
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
print('manager stopped')
|
||||
|
||||
Reference in New Issue
Block a user