chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

1. 修改 mavlinkObject.py 無法順利識別多台 vehicle 的重大bug

Browse Source 
2. 重新編寫 devRun.py 讓其更好閱讀
This commit is contained in:
chiyu1468
2025-05-16 22:37:40 +08:00
parent 84119b788e
commit f9080a5015
4 changed files with 116 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_adapter/fc_network_adapter/devRun.py
+83 -68
View File
@@ -10,14 +10,16 @@ from pymavlink import mavutil
# 自定義的 import
import mavlinkObject as mo
import mavlinkDevice as md
# ====================== 分割線 =====================
test_item = 22
running_time = 20
print('test_item : ', test_item)
if test_item == 1:
# 測試 updateMultiplexingList 的輸出
# 測試 mavlink_object 中 updateMultiplexingList 的輸出
print('===> Start of Program .Test ', test_item)
mavlink_object_none = mo.mavlink_object(None)
@@ -55,9 +57,9 @@ if test_item == 1:
print('End of Program')
elif test_item == 2:
# 測試 updateMultiplexingList 的輸出
# 測試 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)
@@ -73,33 +75,36 @@ elif test_item == 2:
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)
print(mavlink_object1.multiplexingToSwap)
print(mo.swap_queues)
# 設定通道轉發的參數
mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE
mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT
mavlink_object1.multiplexingToSwap[0] = [74, ] # only VFR_HUD
# mavlink_object1.multiplexingToSwap[0] = [-1, ] # all
# 啟動連線的模組
# print(mavlink_object1.multiplexingToSwap)
# print(mo.swap_queues)
# 啟動通道
mavlink_object1.run()
print(mavlink_object1._multiplexingList)
# 確認轉拋的設定有沒有錯
print("_multiplexingList for mavlink object :", mavlink_object1._multiplexingList)
# 運行幾秒並印出 queue 的資料
start_time = time.time()
while time.time() - start_time < 3:
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()
@@ -125,28 +130,38 @@ elif test_item == 10:
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)
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_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 < 5:
while time.time() - start_time < running_time:
if (time.time() - show_time) >= 1:
show_time = time.time()
for sysid in analyzer.mavlink_systems:
@@ -156,14 +171,6 @@ elif test_item == 11:
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()
@@ -175,46 +182,51 @@ elif test_item == 11:
print('End of Program')
elif test_item == 12:
# 我這邊會測試 mavlink object 作為交換器的功能
# 需要開啟一個 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, source_system=17, source_component=200)
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_none = mo.mavlink_object(None)
# mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all
# 啟動連線的模組
analyzer = mo.mavlink_bridge()
# 啟動通道
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()
print("connection established!")
print(type(mavlink_socket_out))
print(type(mavlink_socket_out.mav))
while time.time() - start_time < 100:
while time.time() - start_time < running_time:
try:
test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False)
print('none object : ', test)
@@ -226,14 +238,9 @@ elif test_item == 12:
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("===================")
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
# 結束程式 退出所有 thread
@@ -248,18 +255,19 @@ elif test_item == 12:
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
# 這邊測試 node 生成 topic 的功能
rclpy.init() # 注意要初始化 rclpy 才能使用 node
# mavlink_object_none = mo.mavlink_object(None)
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
from pymavlink.dialects.v20 import common as mavlink2
mav = mavlink2.MAVLink(None)
mav.srcSystem = sysid
mav.srcComponent = compid
@@ -282,12 +290,15 @@ elif test_item == 20:
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()
# 結束程式
@@ -299,9 +310,10 @@ elif test_item == 20:
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
@@ -316,11 +328,11 @@ elif test_item == 21:
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 ...')
print('=== waiting for mavlink data ...')
time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
print('目前所有的系統 : ')
@@ -338,7 +350,7 @@ elif test_item == 21:
start_time = time.time()
show_time = time.time()
while time.time() - start_time < 10:
while time.time() - start_time < running_time:
try:
# rclpy.spin(analyzer)
analyzer.emit_info() # 這邊是測試 node 的運行
@@ -346,9 +358,10 @@ elif test_item == 21:
except KeyboardInterrupt:
break
# 程式結束
analyzer.destroy_node()
rclpy.shutdown()
rclpy.shutdown()
# 結束程式 退出所有 thread
mavlink_object3.stop()
@@ -359,8 +372,8 @@ elif test_item == 21:
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
@@ -377,16 +390,17 @@ elif test_item == 22:
mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
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_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_in.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ]
# 啟動通道
mavlink_object_in.run()
@@ -411,13 +425,14 @@ elif test_item == 22:
show_time = time.time()
show_time2 = time.time()
while time.time() - start_time < 100:
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("目前的飛行模式 : ", sss)
# print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq()))
print("目前的飛行模式 : {}".format(sss))
show_time2 = time.time()
# if (time.time() - show_time) >= 2:
@@ -428,7 +443,7 @@ elif test_item == 22:
# analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
# print("===================")
time.sleep(1)
analyzer.destroy_node()
rclpy.shutdown()
src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py
+3 -2
View File
@@ -20,14 +20,15 @@ class mavlink_device():
self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object
def __str__(self):
p_str = ''
p_str = '=====================\n'
p_str += f'object id : {id(self)}\n' # debug
p_str += f'socket_id : {self.socket_id}\n'
p_str += f'sysid : {self.sysid}\n'
p_str += 'has components : \n'
for compid in self.components:
p_str += f'compid : {compid}\n'
p_str += f'mav_type : {self.components[compid].mav_type}\n'
p_str += '=====================\n'
# p_str += '=====================\n'
return p_str
def updateComponentPacketCount(self, compid, current_seq, current_type, current_time):
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
+27 -16
View File
@@ -55,7 +55,6 @@ class mavlink_bridge(Node, mavlink_publisher):
藉由控制 ros2 的機制再把 device object 的資訊發送出去
ps. 我限制了這個 class 只能有一個 instance
'''
_instance = None
_lock = threading.Lock() # 確保多線程安全
@@ -96,28 +95,41 @@ class mavlink_bridge(Node, mavlink_publisher):
msg = msg_pack[2]
sysid = msg.get_srcSystem()
compid = msg.get_srcComponent()
# 若這個 system id 還不存在 則建立 device object
if not sysid in self.mavlink_systems:
this_device = mavlink_device() # 創建一個新的 device object
self.mavlink_systems[sysid] = this_device
this_device.socket_id = msg_pack[0]
this_device.sysid = sysid
else:
this_device = self.mavlink_systems[sysid]
# 若該 component id 存在 則直接使用該 component object
# 若該 component id 不存在 則利用 heartbeat 創建一個新的 component object
# 若該 component id 不存在 又不是 heartbeat 則不處理
if compid in self.mavlink_systems[sysid].components:
this_component = self.mavlink_systems[sysid].components[compid]
elif msg.get_msgId() == 0:
# 只有透過 heartbeat 可以創建一個新的 component object
this_component = this_device.mavlink_component()
this_device.components[msg.get_srcComponent()] = this_component
this_component.mav_type = msg.type
this_component.mav_autopilot = msg.autopilot
else:
continue
# ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓
if msg.get_msgId() == 0: # HEARTBEAT 處理
# 若這個 system id 還不存在 執行完整建立 device object 的流程
if not sysid in self.mavlink_systems:
device_object = mavlink_device() # 創建一個新的 device object
self.mavlink_systems[sysid] = device_object
device_object.socket_id = msg_pack[0]
device_object.sysid = sysid
this_component = device_object.mavlink_component() # 創建一個新的 component object
device_object.components[msg.get_srcComponent()] = this_component
this_component.mav_type = msg.type
this_component.mav_autopilot = msg.autopilot
this_component.emitParams['base_mode'] = msg.base_mode
this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg)
this_component.emitParams['flightMode'] = msg # debug
# print("mav_type : ", msg.type) # debug
# print("get mode :", mavutil.mode_string_v10(msg)) # debug
# print("record mode :", this_component.emitParams['flightMode_mode']) # debug
this_component.emitParams['flightMode'] = msg # debug
elif msg.get_msgId() == 147: # BATTERY_STATUS 處理
this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()]
@@ -303,7 +315,6 @@ class mavlink_object():
# 處理要送出的封包
# 如果 有資料在 output_buffer 中則將其取出並發送
# 沒有就略過發送
try:
mavlinkMsg_send = self.output_buffer.get(block=False)
except queue.Empty: