parent
5c08a961cf
commit
0af71f8c8e
@ -0,0 +1,166 @@
|
||||
# 基礎功能的 import
|
||||
import queue
|
||||
import time
|
||||
|
||||
# ROS2 的 import
|
||||
import rclpy
|
||||
|
||||
# mavlink 的 import
|
||||
from pymavlink import mavutil
|
||||
|
||||
# 自定義的 import
|
||||
import mavlinkObject as mo
|
||||
|
||||
# ====================== 分割線 =====================
|
||||
|
||||
test_item = 3
|
||||
print('test_item : ', test_item)
|
||||
|
||||
if test_item == 1:
|
||||
# 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來
|
||||
# 只啟用了 mavlink_object 的功能
|
||||
print('Start of Program .Test 1')
|
||||
connection_string="udp:127.0.0.1:14550"
|
||||
mavlink_socket = mavutil.mavlink_connection(connection_string)
|
||||
|
||||
mavlink_object1 = mo.mavlink_object(mavlink_socket)
|
||||
mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE
|
||||
mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT
|
||||
mavlink_object1.multiplexingToConvert = [[74,]] # only VFR_HUD
|
||||
|
||||
# 做一個空的 queue list 驗證 multiplexingToConvert
|
||||
mo.swap_queues.append(queue.Queue())
|
||||
# 啟動連線的模組
|
||||
mavlink_object1.run()
|
||||
|
||||
# 運行幾秒並印出 queue 的資料
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < 2:
|
||||
while not mo.fixed_stream_bridge_queue.empty():
|
||||
print('fixed_stream_bridge_queue:')
|
||||
t = mo.fixed_stream_bridge_queue.get()
|
||||
print('from {} : {}'.format(t[0],t[2]))
|
||||
while not mo.return_packet_processor_queue.empty():
|
||||
print('return_packet_processor_queue:')
|
||||
t = mo.return_packet_processor_queue.get()
|
||||
# print('from {} : {}'.format(t[0],t[2]))
|
||||
print(t[2].type)
|
||||
|
||||
for q in mo.swap_queues:
|
||||
while not q.empty():
|
||||
print('swap_queues:')
|
||||
t = q.get()
|
||||
print('from {} : {}'.format(t[0],t[2]))
|
||||
|
||||
# 結束程式 退出所有 thread
|
||||
mavlink_object1.running = False
|
||||
mavlink_object1.thread.join()
|
||||
mavlink_socket.close()
|
||||
print('End of Program .Test 1')
|
||||
|
||||
elif test_item == 2:
|
||||
# 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況
|
||||
# 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能
|
||||
print('Start of Program .Test 2')
|
||||
connection_string="udp:127.0.0.1:14550"
|
||||
mavlink_socket = mavutil.mavlink_connection(connection_string)
|
||||
|
||||
mavlink_object2 = mo.mavlink_object(mavlink_socket)
|
||||
mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT
|
||||
mavlink_object2.multiplexingToReturn = [] # only HEARTBEAT
|
||||
mavlink_object2.multiplexingToConvert = [] #
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
|
||||
# 啟動連線的模組
|
||||
mavlink_object2.run()
|
||||
|
||||
start_time = time.time()
|
||||
show_time = time.time()
|
||||
compid = 1
|
||||
while time.time() - start_time < 5:
|
||||
if (time.time() - show_time) >= 1:
|
||||
show_time = time.time()
|
||||
for sysid in analyzer.mavlink_systems:
|
||||
print("目前收到的訊息數量 : ",analyzer.mavlink_systems[sysid].components[compid].msg_count)
|
||||
print("目前遺失的訊息數量 : ",analyzer.mavlink_systems[sysid].components[compid].lost_packet_count)
|
||||
print("現在飛機的模式 : ",analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode'])
|
||||
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
|
||||
print("===================")
|
||||
|
||||
# 印出目前所有 mavlink_systems 的內容
|
||||
print('目前所有的系統 : ')
|
||||
for sysid in analyzer.mavlink_systems:
|
||||
print(analyzer.mavlink_systems[sysid])
|
||||
|
||||
|
||||
|
||||
|
||||
# 結束程式 退出所有 thread
|
||||
mavlink_object2.stop()
|
||||
mavlink_object2.thread.join()
|
||||
analyzer.stop()
|
||||
|
||||
analyzer = mo.mavlink_bridge() # 這邊是測試是否只有一個 instance
|
||||
analyzer.thread.join()
|
||||
mavlink_socket.close()
|
||||
print('End of Program .Test 2')
|
||||
|
||||
elif test_item == 3:
|
||||
# 這邊是測試代碼 引入 rclpy 來測試 node 的運行
|
||||
print('Start of Program .Test 3')
|
||||
rclpy.init() # 注意要初始化 rclpy 才能使用 node
|
||||
connection_string="udp:127.0.0.1:14550"
|
||||
mavlink_socket = mavutil.mavlink_connection(connection_string)
|
||||
|
||||
mavlink_object3 = mo.mavlink_object(mavlink_socket)
|
||||
mavlink_object3.multiplexingToAnalysis = [0] # only HEARTBEAT
|
||||
mavlink_object3.multiplexingToReturn = [] # only HEARTBEAT
|
||||
mavlink_object3.multiplexingToConvert = [] #
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
|
||||
# 關於 Node 的初始化
|
||||
show_time = time.time()
|
||||
analyzer._init_node() # 初始化 node
|
||||
print('初始化 node 完成 耗時 : ',time.time() - show_time)
|
||||
|
||||
# 啟動連線的模組
|
||||
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])
|
||||
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 < 10:
|
||||
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_socket.close()
|
||||
print('End of Program .Test 3')
|
||||
Loading…
Reference in New Issue