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

Upload files to 'src/fc_network_adapter/fc_network_adapter'

Browse Source
chiyu
ken910606 12 months ago
parent 4d694f28b4
commit b49a8b0314
1 changed files with 179 additions and 240 deletions
Show Stats Download Patch File Download Diff File

419
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
Unescape Escape View File

@ -1,16 +1,15 @@
'''
# 不同的匯流排只有單一種通訊協定
# 匯流排接到訊息後透過 queue stack 傳送到分析器
# 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器
# 匯流排接到訊息後透過 queue stack 傳送到橋接器
# 會有三種 Queue 分別作為 1. 固定串流橋接器 2. 回傳封包處理器 3. 轉拋串流
# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue
# 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去
# 橋接器會將解析得到的結論 再透過 ros2 的 publisher 發送出去
# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排
# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlink_object 裡面
# 這邊的 main 會是用來初始化 mavlink_object 並啟動他的 run function
'''
# 基礎功能的 import
import threading
import queue
@ -23,44 +22,38 @@ from pymavlink import mavutil
from rclpy.node import Node
# 自定義的 import
from mavlinkDevice import mavlink_device
from mavlinkDevice import mavlink_device, mavlink_systems
from mavlinkPublish import mavlink_publisher
from theLogger import setup_logger
# ====================== 分割線 =====================
logger = setup_logger("mavlinkObject.py")
fixed_stream_analyzer_queue = queue.Queue()
fixed_stream_bridge_queue = queue.Queue()
return_packet_processor_queue = queue.Queue()
converte_format_queues = []
# 用來記錄每個 system 的資訊
# 資料格式 { sysid : mavlink_device object }
mavlink_systems = {}
swap_queues = [] # 這個 list 用來存放轉拋串流的 queue # 這些 queue 同時也是各自 mavlink_object 的 output buffer
# ====================== 分割線 =====================
class mavlink_analyzer(Node, mavlink_publisher):
class mavlink_bridge(Node, mavlink_publisher):
'''
這個 class 就是 固定串流分析器
是用來接收 mavlink 訊息 並進行分析
這個地方是針對 fixed_stream_analyzer_queue 的資料做處理的
這個 class 就是 固定串流橋接器
是用來接收 mavlink 訊息 並進行橋接
這個地方是針對 fixed_stream_bridge_queue 的資料做處理的
記錄有 mavlink bus 上有那些 system id 和 component id
為了每個 system id 都有一個對應的 device object
並且看是否有重複 system id
整段代碼包含兩大區塊 thread 和 node
thread 區塊內會對 fixed_stream_analyzer_queue 進行監聽 並且將收到的訊息進行處理
thread 區塊內會對 fixed_stream_bridge_queue 進行監聽 並且將收到的訊息進行處理
其中 HEARTBEAT 是一個特殊類別 用來初始化整個 device object
node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關
藉由控制 ros2 的機制再把 device object 的資訊發送出去
ps. 我限制了這個 class 只能有一個 instance
'''
_instance = None
_lock = threading.Lock() # 確保多線程安全
@ -68,7 +61,7 @@ class mavlink_analyzer(Node, mavlink_publisher):
def __new__(cls, *args, **kwargs):
with cls._lock: # 確保多線程環境下只有一個實例被創建
if cls._instance is None:
cls._instance = super(mavlink_analyzer, cls).__new__(cls)
cls._instance = super(mavlink_bridge, cls).__new__(cls)
return cls._instance
def __init__(self):
@ -84,7 +77,7 @@ class mavlink_analyzer(Node, mavlink_publisher):
self.thread = threading.Thread(target=self._run_thread)
self.thread.start()
else:
logger.error('mavlink_analyzer instance already exists. Do not create another one.')
logger.error('mavlink_bridge instance already exists. Do not create another one.')
def stop(self):
self.running = False
@ -92,33 +85,45 @@ class mavlink_analyzer(Node, mavlink_publisher):
# === Thread 區塊 ===
def _run_thread(self):
# start_time = time.time() # debug
logger.info('Start of mavlink_analyzer._run_thread')
# 從 Queue fixed_stream_analyzer_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理
logger.info('Start of mavlink_bridge._run_thread')
# 從 Queue fixed_stream_bridge_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理
while self.running:
if fixed_stream_analyzer_queue.empty():
if fixed_stream_bridge_queue.empty():
continue
msg_pack = fixed_stream_analyzer_queue.get()
msg_pack = fixed_stream_bridge_queue.get()
msg = msg_pack[2]
sysid = msg.get_srcSystem()
compid = msg.get_srcComponent()
msg_id = msg.get_msgId()
msg_type = msg.get_type()
# 若這個 system id 還不存在 執行完整建立 device object 的流程
# 若這個 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_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]
this_component = device_object.mavlink_component() # 創建一個新的 component object
device_object.components[msg.get_srcComponent()] = this_component
this_component.mav_type = msg_type
# 若該 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_id == 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_id == 0: # HEARTBEAT 處理
this_component.emitParams['base_mode'] = msg.base_mode
this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg)
if msg_id == 0: # HEARTBEAT 處理
this_component.emitParams['heartbeat'] = msg
elif msg_id == 30: # ATTITUDE 處理
this_component.emitParams['attitude'] = msg
@ -139,15 +144,15 @@ class mavlink_analyzer(Node, mavlink_publisher):
# 若未定義的訊息類型則不處理 並跳出訊息
else:
logger.warning('This Message Type Did not define process method : {} / {}'.format(msg.get_msgId(), msg.get_type()))
logger.warning('This Message Type Did not define process method : {} / {}'.format(msg.get_msgId(), msg.get_type()))
continue
logger.info('End of mavlink_analyzer._run_thread')
logger.info('End of mavlink_bridge._run_thread')
# === Node 區塊 ===
def _init_node(self):
logger.info('Start of mavlink_analyzer._init_node')
super().__init__('mavlink_analyzer') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒
logger.info('Start of mavlink_bridge._init_node')
super().__init__('mavlink_bridge') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒
def emit_info(self):
# 這邊將 mavlink_systems 內所有的 device object 內所有的 component 輪循過
@ -173,16 +178,41 @@ class mavlink_object():
每個 mavlink bus 都會有一個 mavlink_object
其中主要是 thread 做統計封包與分流
分流表的控制在三個 list 分別為
multiplexingToAnalysis : 這個 list 是用來分流到固定串流分析器的
multiplexingToAnalysis : 這個 list 是用來分流到固定串流橋接器的
multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的
multiplexingToConvert : 這個 list 是用來分流到轉格式的
multiplexingToSwap : 這個 list 是用來分流到轉拋串流的
透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為
'''
def __init__(self, mavlink_socket, socket_id = None):
self.socket_id = socket_id
self.mavlink_socket = mavlink_socket
self.msg_count = {}
self.multiplexingList = []
mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) }
socket_num = 0 # 用來記錄目前的 socket 數量
def __new__(cls, *args, **kwargs):
# 每創建一個實例 就將其添加到 mavlinkObjects 列表中
# 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號
# 若序號無中斷 則 socket_id 往後加一 若序號有中斷 則填補最小的序號
# socket_id 從 0 開始
instance = super().__new__(cls)
socket_id = 0
for k in cls.mavlinkObjects.keys():
if k == socket_id:
socket_id += 1
else:
break
instance.socket_id = socket_id
cls.socket_num += 1
cls.mavlinkObjects[socket_id] = instance
return instance
def __init__(self, socket):
self.mavlink_socket = socket
# 這邊變數是執行的時候被使用的 不要直接寫入它
self._multiplexingList = []
# 存放要發送的訊息的 queue 或稱 buffer
self.output_buffer = queue.Queue()
if self.socket_id >= len(swap_queues):
swap_queues.append(self.output_buffer)
else:
swap_queues[self.socket_id] = self.output_buffer
# 關聯到全域變數
global mavlink_systems
@ -190,28 +220,57 @@ class mavlink_object():
# 這三個 list 用來分配不同的訊息到不同的 queue
self.multiplexingToAnalysis = [
0, # HEARTBEAT
24, # GPS_RAW_INT
30, # ATTITUDE
32, # LOCAL_POSITION_NED
33, # GLOBAL_POSITION_INT
74, # VFR_HUD
147, # BATTERY_STATUS
]
self.multiplexingToReturn = [
0, # HEARTBEAT
0, # HEARTBEAT # 挺必要的項目
# 24, # GPS_RAW_INT
# 30, # ATTITUDE
# 33, # GLOBAL_POSITION_INT
# 74, # VFR_HUD
# 147, # BATTERY_STATUS
]
self.multiplexingToConvert = [
self.multiplexingToReturn = []
self.multiplexingToSwap = [
[] for _ in range(len(swap_queues))
]
# 刷新其他 mavlink_object 的 multiplexingToSwap
for k, object in self.mavlinkObjects.items():
if (k != self.socket_id) and (len(object.multiplexingToSwap) <= self.socket_id):
object.multiplexingToSwap.append([])
object.updateMultiplexingList()
logger.info('mavlink_object instance {} created'.format(self.socket_id))
def __del__(self):
self.mavlink_socket.close()
self.stop()
# 停下自己的 thread
if self.mavlink_socket != None:
self.mavlink_socket.close()
self.stop()
# 移除其他 mavlink_object 的 multiplexingToSwap
for k, object in self.mavlinkObjects.items():
if (k != self.socket_id) and (len(object.multiplexingToSwap) > self.socket_id):
object.multiplexingToSwap[self.socket_id] = []
object.updateMultiplexingList()
# 移除自己的 swap_queues
swap_queues[self.socket_id] = None
# 處理 class 的 instance 記錄
self.socket_num -= 1
self.mavlinkObjects.pop(self.socket_id) # 刪除這個 instance
# logger 模組在這邊會一直掛掉 找不到問題 先用 print 代替
# print('mavlink_object instance {} deleted'.format(self.socket_id)) # debug
# logger.info('mavlink_object instance {} deleted'.format(self.socket_id))
# try:
# logger.info('mavlink_object instance {} deleted'.format(self.socket_id))
# except Exception as e:
# print(f"Error logging in __del__: {e}")
# if 'logger' in globals() and logger:
# logger.info('mavlink_object instance {} deleted'.format(self.socket_id))
def run(self):
if not self.socket_id:
logger.error('Please set socket id before running')
return
# TODO 檢查 socket 是否有連線
self.thread = threading.Thread(target=self._run_thread)
self.running = self.updateMultiplexingList()
self.thread.start()
@ -220,20 +279,24 @@ class mavlink_object():
self.running = False
def _run_thread(self):
logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id))
logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id))
start_time = time.time()
while self.running:
timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差
# 處理接收到的封包
try:
mavlinkMsg = self.mavlink_socket.recv_msg()
except Exception as e:
logger.critical(f"Receiving data not mavlink format. Object Delete.")
logger.critical(f"Receiving data not mavlink format. Object Delete.")
print(f"[mavlinkObject.py] Error receiving mavlink data: {e}")
print(mavlinkMsg)
self.running = False
break
if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message'
if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega. etc...'
# 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間
sysid = mavlinkMsg.get_srcSystem()
compid = mavlinkMsg.get_srcComponent()
@ -241,19 +304,36 @@ class mavlink_object():
# mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型
mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp)
# print(mavlinkMsg) # debug
# break # Debug
# break # Debug function can put here you can see the input data from mavlink
# 將訊息依照 multiplexing list 分發到不同的 queue
for i in range(len(self.multiplexingList)):
if mavlinkMsg.get_msgId() in self.multiplexingList[i]:
for i in range(len(self._multiplexingList)):
if (self._multiplexingList[i] == []):
continue
elif (mavlinkMsg.get_msgId() in self._multiplexingList[i]) or (self._multiplexingList[i][0] == -1):
if i == 0:
fixed_stream_analyzer_queue.put((self.socket_id,timestamp,mavlinkMsg))
fixed_stream_bridge_queue.put((self.socket_id,timestamp,mavlinkMsg))
elif i == 1:
return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg))
else:
convert_queue = converte_format_queues[i-2]
convert_queue.put((self.socket_id,timestamp,mavlinkMsg))
_queue = swap_queues[i-2]
# _queue.put((self.socket_id,timestamp,mavlinkMsg)) # 測試看看 也許不需要別的資訊 只需要封包
_queue.put(mavlinkMsg)
# 處理要送出的封包
# 如果 有資料在 output_buffer 中則將其取出並發送
# 沒有就略過發送
try:
mavlinkMsg_send = self.output_buffer.get(block=False)
except queue.Empty:
mavlinkMsg_send = None
# except Exception as e:
# logger.error(f"Error getting data from output_buffer: {e}")
# mavlinkMsg_send = None
if mavlinkMsg_send:
# self.mavlink_socket.mav.send(mavlinkMsg_send)
self.mavlink_socket.write(mavlinkMsg_send.get_msgbuf()) # 這邊會將封包寫入 socket 中
# thread 結束
logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id))
@ -262,20 +342,37 @@ class mavlink_object():
'''
更新 multiplexing list 並做簡單的檢查
'''
# 檢查 multiplexingToAnalysis 與 multiplexingToReturn 是否有 -1 值
check = (-1 in self.multiplexingToAnalysis) or (-1 in self.multiplexingToReturn)
if check:
logger.error('MultiplexingToAnalysis or MultiplexingToReturn NOT all type spilt. socket id : {}'.format(self.socket_id))
return False
# 更新 multiplexing list
self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert
# 檢查 multiplexingToSwap 與 swap_queues 的長度是否一致 而且 swap_queues 的長度不能為 0
check = len(self.multiplexingToSwap) != len(swap_queues) or len(swap_queues) == 0
if check:
logger.error('Multiplexing Queue not fit List , Please check the list. socket id : {}'.format(self.socket_id))
return False
# 對應到自己的 multiplexingToSwap 必需為空 避免對自己迴圈轉拋
try:
self.multiplexingToSwap[self.socket_id] = []
except IndexError:
logger.error('Multiplexing List of self socket id should be void. socket id : {}'.format(self.socket_id))
return False
# 組合 multiplexing list
multiL_tmp = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToSwap
# 檢查 multiplexing list 格式是否有錯誤
check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList)
# 檢查 multiplexing list 格式是否有錯誤 # 全部都要是 list 每個 list 裡面都要是 int
check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in multiL_tmp)
if not check:
logger.error('Multiplexing List Format Error, Please check the list')
logger.error('Multiplexing List Format Error, Please check the list. socket id : {}'.format(self.socket_id))
return False # 若有錯誤則回傳 False
check = len(self.multiplexingToConvert) != len(converte_format_queues)
if check:
logger.error('Multiplexing Queue not fit List , Please check the list')
return False
# 更新 multiplexing list
self._multiplexingList = multiL_tmp
return True
# ====================== 分割線 =====================
@ -284,164 +381,6 @@ class mavlink_object():
def main_node():
pass
# ====================== 分割線 =====================
# 測試時的程式進入點
def main_develop(args=None):
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 = mavlink_object(mavlink_socket, 1)
mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE
mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT
mavlink_object1.multiplexingToConvert = [[74,]] # only VFR_HUD
# 做一個空的 queue list 驗證 multiplexingToConvert
converte_format_queues.append(queue.Queue())
# 啟動連線的模組
mavlink_object1.run()
# 運行幾秒並印出 queue 的資料
start_time = time.time()
while True:
while not fixed_stream_analyzer_queue.empty():
print('fixed_stream_analyzer_queue:')
t = fixed_stream_analyzer_queue.get()
print('from {} : {}'.format(t[0],t[2]))
while not return_packet_processor_queue.empty():
print('return_packet_processor_queue:')
t = return_packet_processor_queue.get()
# print('from {} : {}'.format(t[0],t[2]))
print(t[2].type)
for q in converte_format_queues:
while not q.empty():
print('converte_format_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_analyzer的thread區塊 的功能
print('Start of Program .Test 2')
connection_string="udp:127.0.0.1:14550"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object2 = mavlink_object(mavlink_socket, 1)
mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT
mavlink_object2.multiplexingToReturn = [] # only HEARTBEAT
mavlink_object2.multiplexingToConvert = [] #
# 啟動 mavlink_analyzer
analyzer = mavlink_analyzer()
# 啟動連線的模組
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 mavlink_systems:
print(mavlink_systems[sysid])
# 結束程式 退出所有 thread
mavlink_object2.stop()
mavlink_object2.thread.join()
analyzer.stop()
analyzer = mavlink_analyzer() # 這邊是測試是否只有一個 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 = mavlink_object(mavlink_socket, 1)
mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
mavlink_object3.multiplexingToReturn = [] # only HEARTBEAT
mavlink_object3.multiplexingToConvert = [] #
# 啟動 mavlink_analyzer
analyzer = mavlink_analyzer()
# 關於 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])
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_flightMode: {end_time - start_time} seconds")
print("start emit info")
start_time = time.time()
show_time = time.time()
while time.time() - start_time < 200000:
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')
if __name__ == '__main__':
import rclpy # 這邊是為了測試而加的
main_develop()
pass

Write Preview
Loading…
Cancel
Save