Merge branch 'chiyu' into wenchun
commit
cd3c0f5bee
@ -1,531 +0,0 @@
|
||||
# 基礎功能的 import
|
||||
import queue
|
||||
import time
|
||||
|
||||
# ROS2 的 import
|
||||
import rclpy
|
||||
|
||||
# mavlink 的 import
|
||||
from pymavlink import mavutil
|
||||
|
||||
# 自定義的 import
|
||||
import mavlinkObject as mo
|
||||
import mavlinkDevice as md
|
||||
|
||||
# ====================== 分割線 =====================
|
||||
|
||||
test_item = 51
|
||||
running_time = 30
|
||||
print('test_item : ', test_item)
|
||||
|
||||
if test_item == 1:
|
||||
# 測試 mavlink_object 中 updateMultiplexingList 的輸出
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
mavlink_object_none = mo.mavlink_object(None)
|
||||
|
||||
print('=====================')
|
||||
print('正常範例')
|
||||
mavlink_object_none.multiplexingToAnalysis = []
|
||||
mavlink_object_none.multiplexingToReturn = []
|
||||
# mavlink_object_none.multiplexingToSwap = [[]]
|
||||
ret = mavlink_object_none.updateMultiplexingList()
|
||||
print('execute result : ', ret)
|
||||
print('multiplexingList : ', mavlink_object_none._multiplexingList)
|
||||
|
||||
print('=====================')
|
||||
print('錯誤範例 長度不一致')
|
||||
mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],]
|
||||
ret = mavlink_object_none.updateMultiplexingList()
|
||||
print('execute result : ', ret)
|
||||
|
||||
print('=====================')
|
||||
print('正常範例')
|
||||
mo.swap_queues.append(queue.Queue())
|
||||
mo.swap_queues.append(queue.Queue())
|
||||
mavlink_object_none.multiplexingToAnalysis = [0, 1, 2, 3, 4, 5]
|
||||
mavlink_object_none.multiplexingToReturn = [6, 7, 8, 9, 10]
|
||||
mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],]
|
||||
ret = mavlink_object_none.updateMultiplexingList()
|
||||
print('execute result : ', ret)
|
||||
print('multiplexingList : ', mavlink_object_none._multiplexingList)
|
||||
|
||||
print('=====================')
|
||||
print('錯誤範例 multiplexingToAnalysis 不可以有 -1')
|
||||
mavlink_object_none.multiplexingToAnalysis = [-1, 1, 2, 3, 4, 5]
|
||||
ret = mavlink_object_none.updateMultiplexingList()
|
||||
print('execute result : ', ret)
|
||||
|
||||
print('<=== End of Program')
|
||||
|
||||
elif test_item == 2:
|
||||
# 測試 mavlink_object 創建時 socket_id 是否正確
|
||||
# 說實在 這個測試項 似乎因為 python 的 GC 機制 會導致難以測試
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
mavlink_object_none1 = mo.mavlink_object(None)
|
||||
mavlink_object_none2 = mo.mavlink_object(None)
|
||||
mavlink_object_none3 = mo.mavlink_object(None)
|
||||
del mavlink_object_none2
|
||||
|
||||
print(len(mo.swap_queues))
|
||||
|
||||
mavlink_object_none2 = mo.mavlink_object(None)
|
||||
print(len(mo.swap_queues))
|
||||
|
||||
print(mavlink_object_none1._multiplexingList)
|
||||
|
||||
print('<=== End of Program')
|
||||
|
||||
elif test_item == 10:
|
||||
# 需要開啟一個 ardupilot 的模擬器
|
||||
# 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來
|
||||
# 只啟用了 mavlink_object 的功能
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
|
||||
# 創建一個空的通道 這個通道的 socket_id 是 0
|
||||
mavlink_object_none = mo.mavlink_object(None)
|
||||
|
||||
# 創建另一個通道
|
||||
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.multiplexingToSwap[0] = [74, ] # only VFR_HUD
|
||||
|
||||
# print(mavlink_object1.multiplexingToSwap)
|
||||
# print(mo.swap_queues)
|
||||
|
||||
# 啟動通道
|
||||
mavlink_object1.run()
|
||||
|
||||
# 確認轉拋的設定有沒有錯
|
||||
print("_multiplexingList for mavlink object :", mavlink_object1._multiplexingList)
|
||||
|
||||
# 運行幾秒並印出 queue 的資料
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < running_time:
|
||||
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].get_msgbuf())
|
||||
# print(t[2].type)
|
||||
|
||||
for n in range(len(mo.swap_queues)):
|
||||
q = mo.swap_queues[n]
|
||||
while not q.empty():
|
||||
print('swap_queues:')
|
||||
t = q.get()
|
||||
print('{} gets : {}'.format(n,t))
|
||||
|
||||
|
||||
# 結束程式 退出所有 thread
|
||||
mavlink_object1.running = False
|
||||
mavlink_object1.thread.join()
|
||||
mavlink_socket.close()
|
||||
print('<=== End of Program')
|
||||
|
||||
elif test_item == 11:
|
||||
# 需要開啟一個 ardupilot 的模擬器
|
||||
# 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況
|
||||
# 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
|
||||
# 創建通道
|
||||
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 = []
|
||||
|
||||
# 啟動連線的模組
|
||||
mavlink_object2.run()
|
||||
|
||||
# 做點延遲 讓 heartbeat 先吃進來
|
||||
time.sleep(2)
|
||||
print("=== connection established! ===")
|
||||
|
||||
# 印出目前所有 mavlink_systems 的內容
|
||||
print('目前所有的系統 : ')
|
||||
for sysid in analyzer.mavlink_systems:
|
||||
print(analyzer.mavlink_systems[sysid])
|
||||
|
||||
start_time = time.time()
|
||||
show_time = time.time()
|
||||
compid = 1
|
||||
while time.time() - start_time < running_time:
|
||||
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("===================")
|
||||
|
||||
# 結束程式 退出所有 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')
|
||||
|
||||
elif test_item == 12:
|
||||
# 需要開啟一個 ardupilot 的模擬器 與 GCS
|
||||
# 這邊是測試 mavlink object 作為交換器功能的代碼
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
|
||||
# 啟動連線的模組
|
||||
analyzer = mo.mavlink_bridge()
|
||||
|
||||
# 初始化輸入通道
|
||||
connection_string_in="udp:127.0.0.1:15551"
|
||||
mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
|
||||
mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
|
||||
mavlink_object_in.multiplexingToAnalysis = [0] # only HEARTBEAT
|
||||
|
||||
# 初始化輸出通道
|
||||
connection_string_out="udpout:127.0.0.1:14553"
|
||||
mavlink_socket_out = mavutil.mavlink_connection(connection_string_out)
|
||||
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
|
||||
mavlink_object_out.multiplexingToAnalysis = [0]
|
||||
|
||||
# 做一個空的通道驗證 可以拿來 debug
|
||||
mavlink_object_none = mo.mavlink_object(None)
|
||||
|
||||
# 讓兩個通道互相傳輸
|
||||
mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all
|
||||
mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all
|
||||
# mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all
|
||||
|
||||
# 啟動通道
|
||||
mavlink_object_in.run()
|
||||
mavlink_object_out.run()
|
||||
|
||||
# 做點延遲 讓 heartbeat 先吃進來
|
||||
time.sleep(3)
|
||||
print("=== connection established! ===")
|
||||
|
||||
print('目前所有的系統 : ')
|
||||
for sysid in analyzer.mavlink_systems:
|
||||
print(analyzer.mavlink_systems[sysid])
|
||||
|
||||
# print(type(mavlink_socket_out))
|
||||
# print(type(mavlink_socket_out.mav))
|
||||
|
||||
start_time = time.time()
|
||||
show_time = time.time()
|
||||
while time.time() - start_time < running_time:
|
||||
try:
|
||||
test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False)
|
||||
print('none object : ', test)
|
||||
except queue.Empty:
|
||||
pass
|
||||
|
||||
if (time.time() - show_time) >= 2:
|
||||
show_time = time.time()
|
||||
for sysid in analyzer.mavlink_systems:
|
||||
for compid in analyzer.mavlink_systems[sysid].components:
|
||||
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
|
||||
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
|
||||
print("===================")
|
||||
|
||||
|
||||
|
||||
# 結束程式 退出所有 thread
|
||||
mavlink_object_in.stop()
|
||||
mavlink_object_in.thread.join()
|
||||
mavlink_object_out.stop()
|
||||
mavlink_object_out.thread.join()
|
||||
mavlink_socket_in.close()
|
||||
mavlink_socket_out.close()
|
||||
|
||||
analyzer.stop()
|
||||
print('<=== End of Program')
|
||||
|
||||
elif test_item == 20:
|
||||
# 這邊測試 node 生成 topic 的功能
|
||||
# 利用 空的通道 發出假的 heartbeat 封包
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
rclpy.init() # 注意要初始化 rclpy 才能使用 node
|
||||
|
||||
from pymavlink.dialects.v20 import common as mavlink2
|
||||
|
||||
sysid = 1
|
||||
compid = 1
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
|
||||
mav = mavlink2.MAVLink(None)
|
||||
mav.srcSystem = sysid
|
||||
mav.srcComponent = compid
|
||||
|
||||
# 這是一個 heartbeat 封包
|
||||
fake_heartbeat = mavlink2.MAVLink_heartbeat_message(
|
||||
type=mavlink2.MAV_TYPE_QUADROTOR,
|
||||
autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA,
|
||||
base_mode=3,
|
||||
custom_mode=0,
|
||||
system_status=0,
|
||||
mavlink_version=3
|
||||
)
|
||||
mavlink_bytes = fake_heartbeat.pack(mav)
|
||||
fake_heartbeat_msg = mav.parse_char(mavlink_bytes)
|
||||
|
||||
print(analyzer.mavlink_systems)
|
||||
|
||||
mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data
|
||||
time.sleep(0.1)
|
||||
print(analyzer.mavlink_systems)
|
||||
|
||||
# ROS2 初始化
|
||||
analyzer._init_node()
|
||||
|
||||
# 創建 ROS2 topic
|
||||
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid
|
||||
print("topic created")
|
||||
time.sleep(5)
|
||||
|
||||
# 丟出 topic
|
||||
analyzer.emit_info()
|
||||
|
||||
# 結束程式
|
||||
analyzer.running = False
|
||||
analyzer.destroy_node()
|
||||
|
||||
rclpy.shutdown()
|
||||
analyzer.stop()
|
||||
analyzer.thread.join()
|
||||
print('<=== End of Program')
|
||||
|
||||
elif test_item == 21:
|
||||
# 需要開啟一個 ardupilot 的模擬器
|
||||
# 這邊是測試代碼 引入 rclpy 來測試 node 的運行
|
||||
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
rclpy.init() # 注意要初始化 rclpy 才能使用 node
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
# 關於 Node 的初始化
|
||||
show_time = time.time()
|
||||
analyzer._init_node() # 初始化 node
|
||||
print('初始化 node 完成 耗時 : ',time.time() - show_time)
|
||||
|
||||
# 創建通道
|
||||
connection_string="udp:127.0.0.1:15551"
|
||||
mavlink_socket = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object3 = mo.mavlink_object(mavlink_socket)
|
||||
# 啟動通道
|
||||
mavlink_object3.run()
|
||||
|
||||
|
||||
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:
|
||||
# rclpy.spin(analyzer)
|
||||
analyzer.emit_info() # 這邊是測試 node 的運行
|
||||
time.sleep(1)
|
||||
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')
|
||||
|
||||
elif test_item == 22:
|
||||
# 需要開啟一個 ardupilot 的模擬器 與 GCS
|
||||
# 這邊是測試代碼 引入 rclpy 來測試 node 的運行
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
rclpy.init() # 注意要初始化 rclpy 才能使用 node
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
# 關於 Node 的初始化
|
||||
show_time = time.time()
|
||||
analyzer._init_node() # 初始化 node
|
||||
print('初始化 node 完成 耗時 : ',time.time() - show_time)
|
||||
|
||||
# 初始化兩個通道
|
||||
connection_string_in="udp:127.0.0.1:15551"
|
||||
mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
|
||||
mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
|
||||
mavlink_object_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
|
||||
|
||||
|
||||
connection_string_out="udpout:127.0.0.1:14553"
|
||||
mavlink_socket_out = mavutil.mavlink_connection(connection_string_out)
|
||||
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
|
||||
mavlink_object_out.multiplexingToAnalysis = []
|
||||
|
||||
|
||||
|
||||
# 讓兩個通道互相傳輸
|
||||
mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all
|
||||
mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all
|
||||
|
||||
# 啟動通道
|
||||
mavlink_object_in.run()
|
||||
mavlink_object_out.run()
|
||||
|
||||
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
|
||||
show_time = time.time()
|
||||
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
print(f"Execution time for create_flightMode: {time.time() - show_time} seconds")
|
||||
|
||||
print("start emit info")
|
||||
|
||||
start_time = time.time()
|
||||
show_time = time.time()
|
||||
show_time2 = time.time()
|
||||
|
||||
while time.time() - start_time < running_time:
|
||||
if (time.time() - show_time2) >= 1:
|
||||
analyzer.emit_info() # 這邊是測試 node 的運行
|
||||
# sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode']
|
||||
fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode']
|
||||
sss = mavutil.mode_string_v10(fmsg)
|
||||
# print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq()))
|
||||
print("目前的飛行模式 : {}".format(sss))
|
||||
show_time2 = time.time()
|
||||
|
||||
# if (time.time() - show_time) >= 2:
|
||||
# show_time = time.time()
|
||||
# for sysid in analyzer.mavlink_systems:
|
||||
# for compid in analyzer.mavlink_systems[sysid].components:
|
||||
# print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
|
||||
# analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
|
||||
# print("===================")
|
||||
|
||||
|
||||
|
||||
analyzer.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
# 結束程式 退出所有 thread
|
||||
mavlink_object_in.stop()
|
||||
mavlink_object_in.thread.join()
|
||||
mavlink_socket_in.close()
|
||||
mavlink_object_out.stop()
|
||||
mavlink_object_out.thread.join()
|
||||
mavlink_socket_out.close()
|
||||
analyzer.stop()
|
||||
analyzer.thread.join()
|
||||
|
||||
|
||||
print('<=== End of Program')
|
||||
|
||||
elif test_item == 51:
|
||||
|
||||
# 晉凱的測試項目
|
||||
print('===> Start of Program .Test ', test_item)
|
||||
rclpy.init() # 注意要初始化 rclpy 才能使用 node
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
# 關於 Node 的初始化
|
||||
show_time = time.time()
|
||||
analyzer._init_node() # 初始化 node
|
||||
print('初始化 node 完成 耗時 : ',time.time() - show_time)
|
||||
|
||||
# 創建通道
|
||||
connection_string="udp:127.0.0.1:15551"
|
||||
mavlink_socket3 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object3 = mo.mavlink_object(mavlink_socket3)
|
||||
# 設定通道流動
|
||||
mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
|
||||
mavlink_object3.multiplexingToReturn = [] #
|
||||
# mavlink_object3.multiplexingToSwap = [] #
|
||||
# 啟動通道
|
||||
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])
|
||||
analyzer.create_attitude(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
analyzer.create_local_position_pose(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
analyzer.create_local_position_velocity(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
analyzer.create_global_global(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
analyzer.create_vfr_hud(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
analyzer.create_battery(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
analyzer.create_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
end_time = time.time()
|
||||
print(f"Execution time for create all topic: {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:
|
||||
# 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_socket3.close()
|
||||
print('<=== End of Program')
|
||||
@ -0,0 +1,448 @@
|
||||
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 mavlinkDevice as md
|
||||
|
||||
# ====================== 分割線 =====================
|
||||
|
||||
test_item = 21
|
||||
running_time = 30
|
||||
|
||||
|
||||
print('test_item : ', test_item)
|
||||
|
||||
'''
|
||||
測試項 個位數 表示分離的功能
|
||||
|
||||
測試項 1X 表示 mavlink_object 的功能 測試連線的能力
|
||||
'''
|
||||
|
||||
if 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.0.1:14560"
|
||||
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
|
||||
|
||||
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.stop()
|
||||
|
||||
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:14560"
|
||||
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
|
||||
manager.add_mavlink_object(mavlink_object1)
|
||||
|
||||
# 啟動 mavlink_bridge
|
||||
analyzer = mo.mavlink_bridge()
|
||||
|
||||
time.sleep(3)
|
||||
|
||||
# 印出目前所有 mavlink_systems 的內容
|
||||
print('目前所有的系統 : ')
|
||||
for sysid in analyzer.mavlink_systems:
|
||||
print(analyzer.mavlink_systems[sysid])
|
||||
|
||||
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 in analyzer.mavlink_systems:
|
||||
for compid in analyzer.mavlink_systems[sysid].components:
|
||||
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
|
||||
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
|
||||
print("===================")
|
||||
|
||||
manager.stop()
|
||||
analyzer.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:14560"
|
||||
mavlink_socket_in1 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1)
|
||||
|
||||
connection_string="udp:127.0.0.1:14561"
|
||||
mavlink_socket_in2 = mavutil.mavlink_connection(connection_string)
|
||||
mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2)
|
||||
|
||||
# 初始化輸出通道
|
||||
connection_string="udpout:127.0.0.1:14550"
|
||||
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)
|
||||
|
||||
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.stop()
|
||||
|
||||
print('<=== End of Program')
|
||||
|
||||
|
||||
# elif test_item == 20:
|
||||
# # 這邊測試 node 生成 topic 的功能
|
||||
# # 利用 空的通道 發出假的 heartbeat 封包
|
||||
# print('===> Start of Program .Test ', test_item)
|
||||
# rclpy.init() # 注意要初始化 rclpy 才能使用 node
|
||||
|
||||
# from pymavlink.dialects.v20 import common as mavlink2
|
||||
|
||||
# sysid = 1
|
||||
# compid = 1
|
||||
|
||||
# # 啟動 mavlink_bridge
|
||||
# analyzer = mo.mavlink_bridge()
|
||||
|
||||
# mav = mavlink2.MAVLink(None)
|
||||
# mav.srcSystem = sysid
|
||||
# mav.srcComponent = compid
|
||||
|
||||
# # 這是一個 heartbeat 封包
|
||||
# fake_heartbeat = mavlink2.MAVLink_heartbeat_message(
|
||||
# type=mavlink2.MAV_TYPE_QUADROTOR,
|
||||
# autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA,
|
||||
# base_mode=3,
|
||||
# custom_mode=0,
|
||||
# system_status=0,
|
||||
# mavlink_version=3
|
||||
# )
|
||||
# mavlink_bytes = fake_heartbeat.pack(mav)
|
||||
# fake_heartbeat_msg = mav.parse_char(mavlink_bytes)
|
||||
|
||||
# print(analyzer.mavlink_systems)
|
||||
|
||||
# mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data
|
||||
# time.sleep(0.1)
|
||||
# print(analyzer.mavlink_systems)
|
||||
|
||||
# # ROS2 初始化
|
||||
# analyzer._init_node()
|
||||
|
||||
# # 創建 ROS2 topic
|
||||
# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid
|
||||
# print("topic created")
|
||||
# time.sleep(5)
|
||||
|
||||
# # 丟出 topic
|
||||
# analyzer.emit_info()
|
||||
|
||||
# # 結束程式
|
||||
# analyzer.running = False
|
||||
# analyzer.destroy_node()
|
||||
|
||||
# rclpy.shutdown()
|
||||
# analyzer.stop()
|
||||
# analyzer.thread.join()
|
||||
# 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 == 22:
|
||||
# # 需要開啟一個 ardupilot 的模擬器 與 GCS
|
||||
# # 這邊是測試代碼 引入 rclpy 來測試 node 的運行
|
||||
# print('===> Start of Program .Test ', test_item)
|
||||
# rclpy.init() # 注意要初始化 rclpy 才能使用 node
|
||||
|
||||
# # 啟動 mavlink_bridge
|
||||
# analyzer = mo.mavlink_bridge()
|
||||
# # 關於 Node 的初始化
|
||||
# show_time = time.time()
|
||||
# analyzer._init_node() # 初始化 node
|
||||
# print('初始化 node 完成 耗時 : ',time.time() - show_time)
|
||||
|
||||
# # 初始化兩個通道
|
||||
# connection_string_in="udp:127.0.0.1:15551"
|
||||
# mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
|
||||
# mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
|
||||
# mavlink_object_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
|
||||
|
||||
|
||||
# connection_string_out="udpout:127.0.0.1:14553"
|
||||
# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out)
|
||||
# mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
|
||||
# mavlink_object_out.multiplexingToAnalysis = []
|
||||
|
||||
|
||||
|
||||
# # 讓兩個通道互相傳輸
|
||||
# mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all
|
||||
# mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all
|
||||
|
||||
# # 啟動通道
|
||||
# mavlink_object_in.run()
|
||||
# mavlink_object_out.run()
|
||||
|
||||
# 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
|
||||
# show_time = time.time()
|
||||
# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
# print(f"Execution time for create_flightMode: {time.time() - show_time} seconds")
|
||||
|
||||
# print("start emit info")
|
||||
|
||||
# start_time = time.time()
|
||||
# show_time = time.time()
|
||||
# show_time2 = time.time()
|
||||
|
||||
# while time.time() - start_time < running_time:
|
||||
# if (time.time() - show_time2) >= 1:
|
||||
# analyzer.emit_info() # 這邊是測試 node 的運行
|
||||
# # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode']
|
||||
# fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode']
|
||||
# sss = mavutil.mode_string_v10(fmsg)
|
||||
# # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq()))
|
||||
# print("目前的飛行模式 : {}".format(sss))
|
||||
# show_time2 = time.time()
|
||||
|
||||
# # if (time.time() - show_time) >= 2:
|
||||
# # show_time = time.time()
|
||||
# # for sysid in analyzer.mavlink_systems:
|
||||
# # for compid in analyzer.mavlink_systems[sysid].components:
|
||||
# # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
|
||||
# # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
|
||||
# # print("===================")
|
||||
|
||||
|
||||
|
||||
# analyzer.destroy_node()
|
||||
# rclpy.shutdown()
|
||||
|
||||
|
||||
# # 結束程式 退出所有 thread
|
||||
# mavlink_object_in.stop()
|
||||
# mavlink_object_in.thread.join()
|
||||
# mavlink_socket_in.close()
|
||||
# mavlink_object_out.stop()
|
||||
# mavlink_object_out.thread.join()
|
||||
# mavlink_socket_out.close()
|
||||
# analyzer.stop()
|
||||
# analyzer.thread.join()
|
||||
|
||||
|
||||
# print('<=== End of Program')
|
||||
|
||||
# elif test_item == 51:
|
||||
|
||||
# # 晉凱的測試項目
|
||||
# print('===> Start of Program .Test ', test_item)
|
||||
# rclpy.init() # 注意要初始化 rclpy 才能使用 node
|
||||
|
||||
# # 啟動 mavlink_bridge
|
||||
# analyzer = mo.mavlink_bridge()
|
||||
# # 關於 Node 的初始化
|
||||
# show_time = time.time()
|
||||
# analyzer._init_node() # 初始化 node
|
||||
# print('初始化 node 完成 耗時 : ',time.time() - show_time)
|
||||
|
||||
# # 創建通道
|
||||
# connection_string="udp:127.0.0.1:15551"
|
||||
# mavlink_socket3 = mavutil.mavlink_connection(connection_string)
|
||||
# mavlink_object3 = mo.mavlink_object(mavlink_socket3)
|
||||
# # 設定通道流動
|
||||
# mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
|
||||
# mavlink_object3.multiplexingToReturn = [] #
|
||||
# # mavlink_object3.multiplexingToSwap = [] #
|
||||
# # 啟動通道
|
||||
# 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])
|
||||
# analyzer.create_attitude(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
# analyzer.create_local_position_pose(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
# analyzer.create_local_position_velocity(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
# analyzer.create_global_global(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
# analyzer.create_vfr_hud(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
# analyzer.create_battery(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
# analyzer.create_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid])
|
||||
# end_time = time.time()
|
||||
# print(f"Execution time for create all topic: {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:
|
||||
# # 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_socket3.close()
|
||||
# print('<=== End of Program')
|
||||
@ -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!")
|
||||
@ -1,33 +0,0 @@
|
||||
import socket
|
||||
import os
|
||||
|
||||
# socket file path
|
||||
SOCKET_PATH = "/tmp/unix_socket_example"
|
||||
|
||||
# 若檔案存在就先刪除
|
||||
if os.path.exists(SOCKET_PATH):
|
||||
os.remove(SOCKET_PATH)
|
||||
|
||||
# 建立 socket
|
||||
server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
||||
|
||||
# 綁定 socket 到檔案
|
||||
server.bind(SOCKET_PATH)
|
||||
server.listen(1)
|
||||
|
||||
print("🔌 Server waiting for connection...")
|
||||
|
||||
# 等待 client 連線
|
||||
conn, _ = server.accept()
|
||||
print("✅ Client connected.")
|
||||
|
||||
while True:
|
||||
data = conn.recv(1024)
|
||||
if not data:
|
||||
break
|
||||
print("📥 Received:", data.decode())
|
||||
conn.sendall(b"Echo: " + data)
|
||||
|
||||
conn.close()
|
||||
server.close()
|
||||
os.remove(SOCKET_PATH)
|
||||
@ -0,0 +1,94 @@
|
||||
import socket
|
||||
import os
|
||||
|
||||
# # socket file path
|
||||
# SOCKET_PATH = "/tmp/unix_socket_example"
|
||||
|
||||
# # 若檔案存在就先刪除
|
||||
# if os.path.exists(SOCKET_PATH):
|
||||
# os.remove(SOCKET_PATH)
|
||||
|
||||
# # 建立 socket
|
||||
# server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
||||
|
||||
# # 綁定 socket 到檔案
|
||||
# server.bind(SOCKET_PATH)
|
||||
# server.listen(1)
|
||||
|
||||
# print("🔌 Server waiting for connection...")
|
||||
|
||||
# # 等待 client 連線
|
||||
# conn, _ = server.accept()
|
||||
# print("✅ Client connected.")
|
||||
|
||||
# while True:
|
||||
# data = conn.recv(1024)
|
||||
# if not data:
|
||||
# break
|
||||
# print("📥 Received:", data.decode())
|
||||
# conn.sendall(b"Echo: " + data)
|
||||
|
||||
# conn.close()
|
||||
# server.close()
|
||||
# os.remove(SOCKET_PATH)
|
||||
|
||||
|
||||
# =====================
|
||||
|
||||
# from pymavlink import mavutil
|
||||
|
||||
|
||||
# def create_unix_socket_connection():
|
||||
# # 建立一個 mavtcpin 實例
|
||||
# mav_conn = mavutil.mavtcpin("127.0.0.250:9999", source_system=1, source_component=1)
|
||||
|
||||
# # 替換底層 socket
|
||||
# # 關閉原有的 socket
|
||||
# mav_conn.listen.close()
|
||||
|
||||
# # 創建 Unix socket 並替換
|
||||
# unix_socket = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
|
||||
# unix_socket_path = "/tmp/unix_socket_mavlink.sock"
|
||||
|
||||
# # 確保先前的 unix socket 已被移除
|
||||
# if os.path.exists(unix_socket_path):
|
||||
# os.remove(unix_socket_path)
|
||||
|
||||
# # 綁定並設定 Unix socket
|
||||
# unix_socket.bind(unix_socket_path)
|
||||
# unix_socket.listen(1)
|
||||
# unix_socket.setblocking(0)
|
||||
# mavutil.set_close_on_exec(unix_socket.fileno())
|
||||
|
||||
# # 替換 listen socket
|
||||
# mav_conn.listen = unix_socket
|
||||
# mav_conn.fd = unix_socket.fileno()
|
||||
|
||||
# # mav_conn.port = unix_socket
|
||||
|
||||
# return mav_conn
|
||||
|
||||
|
||||
# a = create_unix_socket_connection()
|
||||
|
||||
# # print(a.port)
|
||||
|
||||
|
||||
|
||||
|
||||
# ===============================
|
||||
|
||||
import mavunixin
|
||||
mav_conn = mavunixin.mavunixin("unix:/tmp/unix_socket_mavlink.sock", source_system=1, source_component=1)
|
||||
|
||||
import time
|
||||
print("🔌 Server waiting for connection...")
|
||||
while True:
|
||||
time.sleep(1)
|
||||
mavlinkMsg = mav_conn.recv_msg()
|
||||
if mavlinkMsg is not None:
|
||||
print("📥 Received:", mavlinkMsg)
|
||||
# a.send(mavlinkMsg)
|
||||
# print("📤 Server replied:", data.decode())
|
||||
else:
|
||||
print("No message received.")
|
||||
Loading…
Reference in New Issue