From 4b8efcc5f9b9cde3518a155749d195608523f7e7 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:47:00 +0800 Subject: [PATCH] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/devRun.py | 532 ++++++++++++++++++ 1 file changed, 532 insertions(+) create mode 100644 src/fc_network_adapter/fc_network_adapter/devRun.py diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py new file mode 100644 index 0000000..60d54b9 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -0,0 +1,532 @@ +# 基礎功能的 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:14550" + 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_state(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() + T = True + while T: + 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') \ No newline at end of file