code 沒有動只是調整版面與新增一些註解增加可讀性

2025-04-09 11:25:40 +08:00
parent bdd6cceb96
commit 5c08a961cf
1 changed files with 136 additions and 114 deletions
@@ -30,6 +30,7 @@ from theLogger import setup_logger


 # ====================== 分割線 =====================
+
 logger = setup_logger("mavlinkObject.py")

 fixed_stream_analyzer_queue = queue.Queue()
@@ -40,7 +41,133 @@ converte_format_queues = []
 # 資料格式 { sysid : mavlink_device object }
 mavlink_systems = {} 

+# ====================== 分割線 =====================
+
+class mavlink_analyzer(Node, mavlink_publisher):
+    '''
+    這個 class 就是 固定串流分析器
+    是用來接收 mavlink 訊息 並進行分析
+    這個地方是針對 fixed_stream_analyzer_queue 的資料做處理的
+    記錄有 mavlink bus 上有那些 system id 和 component id
+    為了每個 system id 都有一個對應的 device object
+    並且看是否有重複 system id
+
+    整段代碼包含兩大區塊 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):
+        if not hasattr(self, "initialized"):  # 防止重複初始化
+            self.initialized = True
+
+            # 關聯到全域變數
+            global mavlink_systems
+            self.mavlink_systems = mavlink_systems
+
+            # 當 object 建立時會直接運行 thread 直到消滅
+            self.running = True
+            self.thread = threading.Thread(target=self._run_thread)
+            self.thread.start()
+        else:
+            logger.error('mavlink_analyzer instance already exists. Do not create another one.')
+
+    def stop(self):
+        self.running = False
+
+    # === 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 進行後處理
+        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()
+
+            # ↓↓↓↓↓↓↓↓↓↓↓↓  處理不同訊息類型的功能寫在這裡 請加在這個 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)
+
+            elif msg.get_msgId() == 147: # BATTERY_STATUS 處理
+                this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()]
+
+            # ↑↑↑↑↑↑↑↑↑↑↑↑  處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↑↑↑↑↑↑↑↑↑↑↑↑
+
+            # 若未定義的訊息類型則不處理 並跳出訊息
+            else:
+                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')
+
+    # === Node 區塊 ===
+    def _init_node(self):
+        logger.info('Start of mavlink_analyzer._init_node')
+        super().__init__('mavlink_analyzer') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒
+
+    def emit_info(self):
+        # 這邊將 mavlink_systems 內所有的 device object 內所有的 component 輪循過 
+        # 把 emitParams 的參數發送出去
+        for sysid, device in self.mavlink_systems.items():
+            for compid, component in device.components.items():
+                for topic_name in component.publishers.keys():
+                    publisher = component.publishers[topic_name][0]
+                    packEmit_func = component.publishers[topic_name][1]
+                    packEmit_func(component.emitParams, publisher)
+                    
+    def _del_node(self):
+        # TODO 這邊要刪除 node 的時候要做的事情 
+        # 先註銷所有 mavlink_systems 中 component 的 publisher
+        # 再註銷所有 mavlink_systems 中的 device object
+        # 再註銷 node
+        pass
+
+# ====================== 分割線 =====================
+
 class mavlink_object():
+    '''
+    每個 mavlink bus 都會有一個 mavlink_object
+    其中主要是 thread 做統計封包與分流
+    分流表的控制在三個 list 分別為
+        multiplexingToAnalysis : 這個 list 是用來分流到固定串流分析器的
+        multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的
+        multiplexingToConvert : 這個 list 是用來分流到轉格式的
+    透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為
+    '''
    def __init__(self, mavlink_socket, socket_id = None):
        self.socket_id = socket_id
        self.mavlink_socket = mavlink_socket
@@ -66,6 +193,7 @@ class mavlink_object():
        ]
        self.multiplexingToConvert = [
        ]
+
    def __del__(self):
        self.mavlink_socket.close()
        self.stop()
@@ -140,121 +268,15 @@ class mavlink_object():
            return False
        return True

-# ===================== 分割線 =====================
+# ====================== 分割線 =====================

-class mavlink_analyzer(Node, mavlink_publisher):
-    '''
-    這個 class 就是 固定串流分析器
-    是用來接收 mavlink 訊息 並進行分析
-    這個地方是針對 fixed_stream_analyzer_queue 的資料做處理的
-    記錄有 mavlink bus 上有那些 system id 和 component id
-    為了每個 system id 都有一個對應的 device object
-    並且看是否有重複 system id
-
-    整段代碼包含兩大區塊 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):
-        if not hasattr(self, "initialized"):  # 防止重複初始化
-            self.initialized = True
-
-            # 關聯到全域變數
-            global mavlink_systems
-            self.mavlink_systems = mavlink_systems
-
-            # 當 object 建立時會直接運行 thread 直到消滅
-            self.running = True
-            self.thread = threading.Thread(target=self._run_thread)
-            self.thread.start()
-        else:
-            logger.error('mavlink_analyzer instance already exists. Do not create another one.')
-    def stop(self):
-        self.running = False
-
-    # === 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 進行後處理
-        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()
-
-            # ↓↓↓↓↓↓↓↓↓↓↓↓  處理不同訊息類型的功能寫在這裡 請加在這個 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)
-
-            elif msg.get_msgId() == 147: # BATTERY_STATUS 處理
-                this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()]
-
-            # ↑↑↑↑↑↑↑↑↑↑↑↑  處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↑↑↑↑↑↑↑↑↑↑↑↑
-
-            # 若未定義的訊息類型則不處理 並跳出訊息
-            else:
-                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')
-
-    # === Node 區塊 ===
-    def _init_node(self):
-        logger.info('Start of mavlink_analyzer._init_node')
-        super().__init__('mavlink_analyzer') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒
-
-    def emit_info(self):
-        # 這邊將 mavlink_systems 內所有的 device object 內所有的 component 輪循過 
-        # 把 emitParams 的參數發送出去
-        for sysid, device in self.mavlink_systems.items():
-            for compid, component in device.components.items():
-                for topic_name in component.publishers.keys():
-                    publisher = component.publishers[topic_name][0]
-                    packEmit_func = component.publishers[topic_name][1]
-                    packEmit_func(component.emitParams, publisher)
-                    
-    def _del_node(self):
-        # TODO 這邊要刪除 node 的時候要做的事情 
-        # 先註銷所有 mavlink_systems 中 component 的 publisher
-        # 再註銷所有 mavlink_systems 中的 device object
-        # 再註銷 node
+# 整合到 ros2 之後的程式進入點
+def main_node():
    pass

-
-def main(args=None):
+# ====================== 分割線 =====================
+# 測試時的程式進入點
+def main_develop(args=None):

    test_item = 2
    print('test_item : ', test_item)
@@ -411,4 +433,4 @@ def main(args=None):

 if __name__ == '__main__':
    import rclpy # 這邊是為了測試而加的
-    main()
+    main_develop()