MAVLink simple message receiver 接通了再來要實裝 node topic

2025-03-30 16:19:30 +08:00
parent d21bc6b069
commit b6c730f74a
2 changed files with 310 additions and 70 deletions
@@ -10,7 +10,6 @@ import time


 '''
-
 # 不同的匯流排只有單一種通訊協定 
 # 匯流排接到訊息後透過 queue stack 傳送到分析器
 # 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器
@@ -21,13 +20,14 @@ import time
 # pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面
 # 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function

-
 '''

 fixed_stream_analyzer_queue = queue.Queue()
 return_packet_processor_queue = queue.Queue()
 converte_format_queues = []

+mavlink_systems = {} # 用來記錄每個 system 的資訊
+
 class MavlinkObject():
    def __init__(self, mavlink_socket, socket_id = None):
        self.socket_id = socket_id
@@ -35,18 +35,24 @@ class MavlinkObject():
        self.msg_count = {}
        self.multiplexingList = []

+        # 關聯到全域變數
+        global mavlink_systems
+        self.mavlink_systems = mavlink_systems
+
+        # 這三個 list 用來分配不同的訊息到不同的 queue
        self.multiplexingToAnalysis = [
-            # 0, # HEARTBEAT
+            0, # HEARTBEAT
+            24, # GPS_RAW_INT
            30, # ATTITUDE
            33, # GLOBAL_POSITION_INT
-            # 147, # BATTERY_STATUS
+            74, # VFR_HUD
+            147, # BATTERY_STATUS
        ]
        self.multiplexingToReturn = [
            0, # HEARTBEAT
            # 30, # ATTITUDE
        ]
        self.multiplexingToConvert = [
-            [74, ] # VFR_HUD 
        ]
    def __del__(self):
        self.mavlink_socket.close()
@@ -66,6 +72,7 @@ class MavlinkObject():
    def _run_thread(self):
        start_time = time.time()
        while self.running:
+            timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差
            try:
                mavlinkMsg = self.mavlink_socket.recv_msg()
            except Exception as e:
@@ -75,7 +82,13 @@ class MavlinkObject():
                break

            if mavlinkMsg:
-                self.count_mavlink_type(mavlinkMsg) # 統計收到的訊息
+                # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間
+                sysid = mavlinkMsg.get_srcSystem()
+                compid = mavlinkMsg.get_srcComponent()
+                if sysid in self.mavlink_systems: #　只有當這個 system id 已經透過 HEARTBEAT 訊號被初始化過 才會記錄相關訊息
+                    # mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型
+                    mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp)
+
                # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug
                # break # Debug

@@ -83,115 +96,313 @@ class MavlinkObject():
                for i in range(len(self.multiplexingList)):
                    if mavlinkMsg.get_msgId() in self.multiplexingList[i]:
                        if i == 0:
-                            fixed_stream_analyzer_queue.put((self.socket_id,mavlinkMsg))
+                            fixed_stream_analyzer_queue.put((self.socket_id,timestamp,mavlinkMsg))
                        elif i == 1:
-                            return_packet_processor_queue.put((self.socket_id,mavlinkMsg))
+                            return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg))
                        else:
                            convert_queue = converte_format_queues[i-2]
-                            convert_queue.put((self.socket_id,mavlinkMsg))
+                            convert_queue.put((self.socket_id,timestamp,mavlinkMsg))

-            # 每秒中 統計一次 收到的訊息總量
-            elapsed_time = time.time() - start_time
-            if elapsed_time > 1:
-                print('[mavlinkObject.py] Messages Type received (in about 1 sec) :')
-                print(self.msg_count)
-                start_time = time.time()
-                self.msg_count = {}
-        
        # thread 結束
-        print('[mavlinkObject.py] End of _run_thread')
-
-    def count_mavlink_type(self, mavlinkMsg):
-        msg_type = mavlinkMsg.get_type()
-        if msg_type in self.msg_count:
-            self.msg_count[msg_type] += 1
-        else:
-            self.msg_count[msg_type] = 1
-
-    def publish_mavlink_data(self, mavlinkMsg):
-        msg = String()
-        msg.data = str(mavlinkMsg)
-        self.publisher_.publish(msg)
+        print('[mavlinkObject.py] End of MavlinkObject._run_thread')

    def updateMultiplexingList(self):
+        '''
+        更新 multiplexing list 並做簡單的檢查
+        '''
+
        # 更新 multiplexing list 
        self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert

+        # 檢查 multiplexing list 格式是否有錯誤
        check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList)
-
-        # 若有錯誤則回傳 False
        if not check:
-            print('[mavlinkObject.py] Multiplexing List Error, Please check the list')
+            print('[mavlinkObject.py] Multiplexing List Format Error, Please check the list')
+            return False # 若有錯誤則回傳 False
+
+        check = len(self.multiplexingToConvert) != len(converte_format_queues)
+        if check:
+            print('[mavlinkObject.py] Multiplexing Queue not fit List , Please check the list')
            return False
        return True

+# ===================== 分割線 =====================================
+
 class mavlink_analyzer(Node):
    '''
-    這個 class 是用來接收 mavlink 訊息 並進行分析
+    這個 class 就是 固定串流分析器
+    是用來接收 mavlink 訊息 並進行分析
+    這個地方是針對 fixed_stream_analyzer_queue 的資料做處理的
    記錄有 mavlink bus 上有那些 system id 和 component id
    為了每個 system id 都有一個對應的 device object
    並且看是否有重複 system id

-    也許可以用 node 去創建 這樣就可以直接進到 ros2 執行緒?
+    整段代碼包含兩大區塊 thread 和 node
+
+    thread 區塊內會對 fixed_stream_analyzer_queue 進行監聽 並且將收到的訊息進行處理
+    其中 HEARTBEAT 是一個特殊類別 用來初始化整個 device object 
+
+    node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關
+    藉由控制 ros2 的機制再把 device object 的資訊發送出去
+
+    ps. 我限制了這個 class 只能有一個 instance
+    
    '''
+    _instance = None 
+    _lock = threading.Lock()  # 確保多線程安全
+
+    def __new__(cls, *args, **kwargs):
+        with cls._lock:  # 確保多線程環境下只有一個實例被創建
+            if cls._instance is None:
+                cls._instance = super(mavlink_analyzer, cls).__new__(cls)
+        return cls._instance
    def __init__(self):
-        super().__init__('mavlink_analyzer')
+        if not hasattr(self, "initialized"):  # 防止重複初始化
+            self.initialized = True
+            rclpy.init()
+            super().__init__('mavlink_analyzer')

-        # self.publisher_ = self.create_publisher(String, 'mavlink_data', 10)
-        # self.mavlink_object = None
-        # self.create_mavlink_object()
+            # 關聯到全域變數
+            global mavlink_systems
+            self.mavlink_systems = mavlink_systems

-    def create_mavlink_object(self):
-        # connection_string="udp:
+            # 當 object 建立時會直接運行 thread 直到消滅
+            self.running = True
+            self.thread = threading.Thread(target=self._run_thread)
+            self.thread.start()
+        else:
+            print('[mavlinkObject.py] WARNING : mavlink_analyzer instance already exists')
+    def stop(self):
+        self.running = False
+
+    # === Thread 區塊 ===
+    def _run_thread(self):
+        start_time = time.time() # debug
+        # 從 Queue fixed_stream_analyzer_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理
+        while self.running:
+            if fixed_stream_analyzer_queue.empty():
+                continue
+            msg_pack = fixed_stream_analyzer_queue.get()
+
+            msg = msg_pack[2]
+            sysid = msg.get_srcSystem()
+
+            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
+
+                    
+                    # 初始化 emitParams 方便之後更新與利用
+                    this_component.emitParams.add_parameter('base_mode')
+                    this_component.emitParams.add_parameter('flight_mode')
+                    
+                    # TODO 這邊先不過度設計 但是也許之後要依照不同的載具類型再定義
+                    # for BATTERY_STATUS (147)
+                    this_component.emitParams.add_parameter('battery_voltage')
+                    # for GPS_RAW_INT (24)
+                    this_component.emitParams.add_parameter('gps_fix_type')
+                    # for ATTITUDE (30)
+                    this_component.emitParams.add_parameter('roll')
+                    this_component.emitParams.add_parameter('pitch')
+                    this_component.emitParams.add_parameter('yaw')
+                    # for GLOBAL_POSITION_INT (33)
+                    this_component.emitParams.add_parameter('latitude')
+                    this_component.emitParams.add_parameter('longitude')
+                    this_component.emitParams.add_parameter('altitude')
+                    # for VFR_HUD (74)
+                    this_component.emitParams.add_parameter('heading')
+
+                this_component.emitParams.base_mode = msg.base_mode    
+                this_component.emitParams.flight_mode = mavutil.mode_string_v10(msg)
+            
+            elif msg.get_msgId() == 147: # BATTERY_STATUS 處理
+                this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()]
+
+
+            # 若未定義的訊息類型則不處理 並跳出訊息
+            else:
+                print('[mavlinkObject.py] Warning This Message Type Did not define process method : ',msg.get_msgId(), get_type())
+                continue
+
+
+        print('[mavlinkObject.py] End of mavlink_analyzer._run_thread')
+
+    # === Node 區塊 ===
+    def _init_node(self):
        pass
+        # self.publisher = self.create_publisher(String, 'mavlink_analyzer', 10)
+        # self.subscription = self.create_subscription(String, 'mavlink_analyzer', self.listener_callback, 10)

+    def emit_info(self):
+        # 這邊將 mavlink_systems 內所有的 device object 內所有的 component 輪循過 
+        # 把 emitParams 的參數發送出去
+        for sysid in self.mavlink_systems:
+            for compid in self.mavlink_systems[sysid].components:
+
+                # 先抓 base_mode 驗證看看算法是否正確
+                msg_str = self.mavlink_systems[sysid].components[compid].base_mode
+                self.publisher.publish(msg_str)
+                # self.get_logger().info('Publishing: "%s"' % msg.data)
+
+
+# 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component
 class mavlink_device():
    def __init__(self):
        self.socket_id = None # 記錄來自於哪個 socket
        self.sysid = None
-        self.component_count = 1
-        self.compid_list = []
-        
+        self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object

+    def __str__(self):
+        p_str = ''
+        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'
+        return p_str
+
+    def updateComponentPacketCount(self, compid, current_seq, current_type, current_time):
+        # 這段檢查遺失封包
+        try:
+            last_seq = self.components[compid].msg_seq
+        except KeyError:
+            # 這個 component id 還不存在
+            print('[mavlinkObject.py] Warning System ID : {} This Component ID : {} Did not init yet'.format(self.sysid, compid))
+            return
+
+        if last_seq != None:
+            expected_seq = (last_seq + 1) % 256
+            diff = current_seq - expected_seq
+            # print("current last exp diff : ",current_seq, last_seq, expected_seq, diff) # debug
+            if diff < 0:
+                diff += 256
+            self.components[compid].lost_packet_count += diff
+
+        # 這段更新封包的基本資訊
+        self.components[compid].msg_seq = current_seq
+        self.components[compid].last_msg_time = current_time
+        if current_type in self.components[compid].msg_count:
+            self.components[compid].msg_count[current_type] += 1
+        else:
+            self.components[compid].msg_count[current_type] = 1
+
+    def resetComponentPacketCount(self, compid):
+        self.components[compid].msg_count = {}
+        self.components[compid].msg_seq = None
+        self.components[compid].lost_packet_count = 0
+
+    class mavlink_component():
+        # 這個 class 用來存放該 component 參數，然而這些參數是固定以一個頻率要輸出給 ros2 topic
+        class emitParamSet():
+            def __init__(self):
+                pass
+            def add_parameter(self, param_name):
+                setattr(self, param_name, None)
+
+        def __init__(self):
+            self.mav_type = None # 表示 Vehicle 或 component type (例如: 旋翼機是2, 雲台是26, GCS是6)
+            self.mav_autopilot = None # 表示 autopilot type (例如: ardupilot是3, px4是12)
+            self.mav_system_status = None # 表示 system status (例如: active是3, standby是4)
+            
+            # 紀錄從這個 component 收到最後的訊息序號和時間
+            self.msg_count = {}
+            self.msg_seq = None
+            self.lost_packet_count = 0
+            self.last_msg_time = 0
+
+            # 存放該 emit component 參數的區域
+            self.emitParams = self.emitParamSet()

 def main(args=None):

+    # print('Start of Program .Test 1')
+    # # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來
+    # connection_string="udp:127.0.0.1:14550"
+    # mavlink_socket = mavutil.mavlink_connection(connection_string)
+
+    # mavlink_object = MavlinkObject(mavlink_socket, 1)
+    # mavlink_object.multiplexingToAnalysis = [30] # only ATTITUDE
+    # mavlink_object.multiplexingToReturn = [0] # only HEARTBEAT
+    # mavlink_object.multiplexingToConvert = [[74,]] # only VFR_HUD 
+
+    # # 做一個空的 queue list 驗證 multiplexingToConvert
+    # converte_format_queues.append(queue.Queue())
+    # # 啟動連線的模組
+    # mavlink_object.run()
+
+    # # 運行幾秒並印出 queue 的資料
+    # start_time = time.time()
+    # while time.time() - start_time < 2:
+        
+    #     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_object.running = False
+    # mavlink_object.thread.join()
+    # mavlink_socket.close()
+    # print('End of Program .Test 1')
+
+    # # # ========================================================================
+    print('Start of Program .Test 2')
    connection_string="udp:127.0.0.1:14550"
    mavlink_socket = mavutil.mavlink_connection(connection_string)

-    # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來
    mavlink_object = MavlinkObject(mavlink_socket, 1)
-    mavlink_object.multiplexingToAnalysis = [30] # only ATTITUDE
-    mavlink_object.multiplexingToReturn = [0] # only HEARTBEAT
-    mavlink_object.multiplexingToConvert = [[74,]] # only VFR_HUD 
-    
-    converte_format_queues.append(queue.Queue())
+    mavlink_object.multiplexingToAnalysis = [0] # only HEARTBEAT
+    mavlink_object.multiplexingToReturn = [] # only HEARTBEAT
+    mavlink_object.multiplexingToConvert = [] #

+    # 啟動連線的模組
    mavlink_object.run()
+    # 啟動 mavlink_analyzer
+    analyzer = mavlink_analyzer()
+    compid = 1
+
+    # 運行幾秒
    start_time = time.time()
-    while time.time() - start_time < 2:
-        
-        while not fixed_stream_analyzer_queue.empty():
-            print('fixed_stream_analyzer_queue:')
-            t = fixed_stream_analyzer_queue.get()
-            print('from {} : {}'.format(t[0],t[1]))
-        while not return_packet_processor_queue.empty():
-            print('return_packet_processor_queue:')
-            t = return_packet_processor_queue.get()
-            print('from {} : {}'.format(t[0],t[1]))
-        
-        for q in converte_format_queues:
-            while not q.empty():
-                print('converte_format_queues:')
-                t = q.get()
-                print('from {} : {}'.format(t[0],t[1]))
+    show_time = time.time()
+    while time.time() - start_time < 10:
+        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.flight_mode)
+                analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)

-
-    mavlink_object.running = False
+    # 結束程式 退出所有 thread
+    mavlink_object.stop()
    mavlink_object.thread.join()
-    print('End of Program')
-
+    analyzer.stop()

+    analyzer = mavlink_analyzer() # 這邊是測試是否只有一個 instance
+    mavlink_socket.close()

 if __name__ == '__main__':
    main()