From f9080a5015b3bbd2cd1ae6ee24736e205a5cff52 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Fri, 16 May 2025 22:37:40 +0800 Subject: [PATCH] =?UTF-8?q?1.=20=E4=BF=AE=E6=94=B9=20mavlinkObject.py=20?= =?UTF-8?q?=E7=84=A1=E6=B3=95=E9=A0=86=E5=88=A9=E8=AD=98=E5=88=A5=E5=A4=9A?= =?UTF-8?q?=E5=8F=B0=20vehicle=20=E7=9A=84=E9=87=8D=E5=A4=A7bug=202.=20?= =?UTF-8?q?=E9=87=8D=E6=96=B0=E7=B7=A8=E5=AF=AB=20devRun.py=20=E8=AE=93?= =?UTF-8?q?=E5=85=B6=E6=9B=B4=E5=A5=BD=E9=96=B1=E8=AE=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/devRun.py | 151 ++++++++++-------- .../fc_network_adapter/mavlinkDevice.py | 5 +- .../fc_network_adapter/mavlinkObject.py | 43 +++-- src/unitdev02/unitdev02/test01.py | 3 + 4 files changed, 116 insertions(+), 86 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 6736ae7..b0e83e2 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -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() - start_time = time.time() - show_time = time.time() - print("connection established!") + # 做點延遲 讓 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)) + # print(type(mavlink_socket_out)) + # print(type(mavlink_socket_out.mav)) - while time.time() - start_time < 100: + 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) @@ -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: - sysid = 1 - compid = 1 - # 這邊測試 node 生成 topic 的功能 + # 利用 空的通道 發出假的 heartbeat 封包 + print('===> Start of Program .Test ', test_item) rclpy.init() # 注意要初始化 rclpy 才能使用 node - # mavlink_object_none = mo.mavlink_object(None) + + from pymavlink.dialects.v20 import common as mavlink2 + + sysid = 1 + compid = 1 # 啟動 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() diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py index d8c6e04..f090b41 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py @@ -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): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index e7223b0..9130a15 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -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: diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py index e00d946..5beda2e 100644 --- a/src/unitdev02/unitdev02/test01.py +++ b/src/unitdev02/unitdev02/test01.py @@ -257,3 +257,6 @@ print('Start') simple_getMavlink() + + +# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out, source_system=17, source_component=200)