From b49a8b0314a0e7e487a2e7a3a85fac6377887e77 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:46:33 +0800 Subject: [PATCH 01/33] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkObject.py | 419 ++++++++---------- 1 file changed, 179 insertions(+), 240 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index d3f2214..0a219be 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -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 From 1ff5df87c1688a824b848a84513433e3db10a26d Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:46:50 +0800 Subject: [PATCH 02/33] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkPublish.py | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index 028e3ee..b328f81 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -9,6 +9,8 @@ publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> ''' +from pymavlink import mavutil + import std_msgs.msg import sensor_msgs.msg import geometry_msgs.msg @@ -24,13 +26,13 @@ class mavlink_publisher(): prefix_path = 'MavLinkBus' - def create_flightMode(self, sysid, component_obj): + def create_state(self, sysid, component_obj): # target topic name # 請跟這個 method 的名稱保持一致 - target_topic = 'flightMode' + target_topic = 'state' # 這邊要檢查 flight_mode 是否存在 try: - _ = component_obj.emitParams['flightMode_mode'] + _ = component_obj.emitParams['heartbeat'] except KeyError: # 這個 component id 還不存在 logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) @@ -38,15 +40,16 @@ class mavlink_publisher(): # 若存在則 建立 publisher object 並回傳 true topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode] + publisher_ = self.create_publisher(mavros_msgs.msg.State, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_state] return True - def packEmit_flightMode(cls, emitParams, publisher): - msg_str = emitParams['flightMode_mode'] - msg = std_msgs.msg.String() - msg.data = msg_str + def packEmit_state(cls, emitParams, publisher): + mav_msg = emitParams['heartbeat'] + msg = mavros_msgs.msg.State() + msg.mode = mavutil.mode_string_v10(mav_msg) + msg.armed = (mav_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 publisher.publish(msg) pass From 4b8efcc5f9b9cde3518a155749d195608523f7e7 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:47:00 +0800 Subject: [PATCH 03/33] 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 From fa7689eaf560829514eeabd4603e8c447e8f965b Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:47:19 +0800 Subject: [PATCH 04/33] Delete 'src/fc_network_adapter/fc_network_adapter/socketManager.py' --- .../fc_network_adapter/socketManager.py | 14 -------------- 1 file changed, 14 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/socketManager.py diff --git a/src/fc_network_adapter/fc_network_adapter/socketManager.py b/src/fc_network_adapter/fc_network_adapter/socketManager.py deleted file mode 100644 index 52e8681..0000000 --- a/src/fc_network_adapter/fc_network_adapter/socketManager.py +++ /dev/null @@ -1,14 +0,0 @@ - -''' - -透過某個地方 得到 udp 或 uart 接口 -對於每個接口 視為一個獨立的物件 - -物件對於不同的接口 是為不同類型的物件 - -每個類型的物件 創建一個獨立的執行緒 來處理資料 -關於執行緒的實作 是寫在另一個模組 - -物件之間 也可以做資料轉換/轉拋 - -''' \ No newline at end of file From d32610d700670c37b048d05e0a98c5e108d4218e Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 21:37:15 +0800 Subject: [PATCH 05/33] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 35 ++++++++++++++++++---------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index a81dbb2..fbe30d8 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -4,8 +4,7 @@ from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QApplication) -from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal, QUrl -from PyQt6.QtGui import QColor +from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal from PyQt6.QtWebEngineWidgets import QWebEngineView import math import re @@ -14,8 +13,8 @@ import sys from threading import Lock from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import BatteryState, NavSatFix, Imu -from std_msgs.msg import String, Float64 -from mavros_msgs.msg import VfrHud +from std_msgs.msg import Float64 +from mavros_msgs.msg import VfrHud, State class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -62,10 +61,10 @@ class DroneMonitor(Node): lambda msg, did=drone_id: self.battery_callback(did, msg), 10 ), - 'flightMode': self.create_subscription( - String, - f'{base_topic}/flightMode', - lambda msg, did=drone_id: self.mode_callback(did, msg), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), 10 ), 'global': self.create_subscription( @@ -135,14 +134,17 @@ class DroneMonitor(Node): 'voltage': msg.voltage }) - def mode_callback(self, drone_id, msg): - self.signals.update_signal.emit('mode', drone_id, msg.data) + def state_callback(self, drone_id, msg): + self.signals.update_signal.emit('state', drone_id, { + 'mode': msg.mode, + 'armed': msg.armed + }) def gps_callback(self, drone_id, msg): self.signals.update_signal.emit('gps', drone_id, { 'lat': msg.latitude, - 'lon': msg.longitude - ,'alt': msg.altitude + 'lon': msg.longitude, + 'alt': msg.altitude }) def local_vel_callback(self, drone_id, msg): @@ -236,7 +238,7 @@ class ControlStationUI(QMainWindow):
@@ -365,8 +367,9 @@ class ControlStationUI(QMainWindow): if not (panel := self.drones.get(drone_id)): return - if msg_type == 'mode': - self.update_field(panel, drone_id, 'mode', f"{data}", + if msg_type == 'state': + mode = data['mode'] + self.update_field(panel, drone_id, 'mode', f"{mode}", '#FF5555' if '返航' in data else '#55FF55') elif msg_type == 'battery': From 800617dd3c20330fe1ed3198df3fbc474291ff8c Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 21 May 2025 15:19:54 +0800 Subject: [PATCH 06/33] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 85 +++++++++++++++++++++++++++------- 1 file changed, 68 insertions(+), 17 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index fbe30d8..9d8651a 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -240,29 +240,63 @@ class ControlStationUI(QMainWindow): @@ -421,12 +455,29 @@ class ControlStationUI(QMainWindow): label.setText(text) if color: label.setStyleSheet(f"color: {color};") - + ''' def add_drone(self, drone_id): if drone_id not in self.drones: panel = self.create_drone_panel(drone_id) self.info_layout.addWidget(panel) self.drones[drone_id] = panel + ''' + def add_drone(self, drone_id): + if drone_id not in self.drones: + panel = self.create_drone_panel(drone_id) + self.info_layout.addWidget(panel) + self.drones[drone_id] = panel + + # 清空原有 layout + for i in reversed(range(self.info_layout.count())): + widget = self.info_layout.itemAt(i).widget() + if widget: + self.info_layout.removeWidget(widget) + widget.setParent(None) + + # 根據 key 排序後重新加入 + for sorted_id in sorted(self.drones, key=lambda x: int(''.join(filter(str.isdigit, x)) or 0)): + self.info_layout.addWidget(self.drones[sorted_id]) def spin_ros(self): try: From 68af81ac682be58b301db37df45828a15d42cf91 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Fri, 13 Jun 2025 18:45:16 +0800 Subject: [PATCH 09/33] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 580 +++++++++++++++++++++++++++------ 1 file changed, 489 insertions(+), 91 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index 9d8651a..6636fa3 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -3,18 +3,21 @@ import rclpy from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, - QSizePolicy, QApplication) + QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, + QHeaderView, QPushButton, QCheckBox) from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal from PyQt6.QtWebEngineWidgets import QWebEngineView import math import re -import os import sys from threading import Lock from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import BatteryState, NavSatFix, Imu from std_msgs.msg import Float64 -from mavros_msgs.msg import VfrHud, State +from mavros_msgs.msg import State, VfrHud +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +import asyncio +from functools import partial class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -26,6 +29,10 @@ class DroneMonitor(Node): self.drone_topics = {} self.lock = Lock() + self.arm_clients = {} + self.takeoff_clients = {} + self.selected_drones = set() + # 主题检测定时器 self.create_timer(1.0, self.scan_topics) @@ -43,11 +50,22 @@ class DroneMonitor(Node): for drone_id in found_drones: if not hasattr(self, f'drone_{drone_id}_subs'): - self.setup_drone_subs(drone_id) + self.setup_drone(drone_id) - def setup_drone_subs(self, drone_id): + def setup_drone(self, drone_id): base_topic = f'/MavLinkBus/{drone_id}' + + # Add service clients + self.arm_clients[drone_id] = self.create_client( + CommandBool, + f'{base_topic}/cmd/arming' + ) + self.takeoff_clients[drone_id] = self.create_client( + CommandTOL, + f'{base_topic}/cmd/takeoff' + ) + subs = { 'attitude': self.create_subscription( Imu, @@ -61,12 +79,6 @@ class DroneMonitor(Node): lambda msg, did=drone_id: self.battery_callback(did, msg), 10 ), - 'state': self.create_subscription( - State, - f'{base_topic}/state', - lambda msg, did=drone_id: self.state_callback(did, msg), - 10 - ), 'global': self.create_subscription( NavSatFix, f'{base_topic}/global_position/global', @@ -91,6 +103,24 @@ class DroneMonitor(Node): lambda msg, did=drone_id: self.local_vel_callback(did, msg), 10 ), + 'loss_rate': self.create_subscription( + Float64, + f'{base_topic}/packet_loss_rate', + lambda msg, did=drone_id: self.loss_rate_callback(did, msg), + 10 + ), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), + 10 + ), + 'ping': self.create_subscription( + Float64, + f'{base_topic}/ping', + lambda msg, did=drone_id: self.ping_callback(did, msg), + 10 + ), 'vfr_hud': self.create_subscription( VfrHud, f'{base_topic}/vfr_hud', @@ -102,6 +132,46 @@ class DroneMonitor(Node): setattr(self, f'drone_{drone_id}_subs', subs) self.signals.update_signal.emit('new_drone', drone_id, None) + async def arm_drone(self, drone_id, arm): + if drone_id not in self.arm_clients: + return False + + client = self.arm_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandBool.Request() + request.value = arm + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Arm service call failed: {e}') + return False + + async def takeoff_drone(self, drone_id, altitude=10.0): + if drone_id not in self.takeoff_clients: + return False + + client = self.takeoff_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandTOL.Request() + request.altitude = altitude + request.min_pitch = 0.0 + request.yaw = 0.0 + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Takeoff service call failed: {e}') + return False + def quaternion_to_euler(self, q): sinr_cosp = 2 * (q.w * q.x + q.y * q.z) cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) @@ -116,7 +186,7 @@ class DroneMonitor(Node): return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) - # 回调函数 + # callbacks def attitude_callback(self, drone_id, msg): if hasattr(msg, 'orientation'): roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) @@ -176,13 +246,22 @@ class DroneMonitor(Node): 'climb': msg.climb }) + def loss_rate_callback(self, drone_id, msg): + self.signals.update_signal.emit('loss_rate', drone_id, { + 'loss_rate': msg.data + }) + + def ping_callback(self, drone_id, msg): + self.signals.update_signal.emit('ping', drone_id, { + 'ping': msg.data + }) + class ControlStationUI(QMainWindow): def __init__(self): super().__init__() self.setWindowTitle('GCS') self.resize(1400, 900) - self.drones = {} - + # 初始化ROS2 rclpy.init() self.monitor = DroneMonitor() @@ -193,7 +272,26 @@ class ControlStationUI(QMainWindow): self.executor.add_node(self.monitor) # 初始化UI + self.drones = {} + self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向", + "Roll", "Pitch", "Yaw", "丟包", "延遲"] + self.info_type_map = { + "mode": 0, + "armed": 1, + "battery": 2, + "altitude": 3, + "local": 4, + "velocity": 5, + "groundspeed": 6, + "heading": 7, + "roll": 8, + "pitch": 9, + "yaw": 10, + "loss_rate": 11, + "ping": 12 + } self.drone_positions = {} + self.drone_headings = {} self.map_loaded = False self.init_ui() @@ -203,23 +301,95 @@ class ControlStationUI(QMainWindow): self.timer.start(50) def init_ui(self): + + # 只呼叫一次 main_splitter = QSplitter(Qt.Orientation.Horizontal) + + # 左側 TabWidget + self.left_tab = QTabWidget() + + # — 分頁 1：Drone Panel + self.drone_panel_container = QWidget() + self.drone_panel_layout = QVBoxLayout(self.drone_panel_container) + self.drone_panel_layout.setAlignment(Qt.AlignmentFlag.AlignTop) + self.drone_panel_layout.setSpacing(0) + self.drone_panel_layout.setContentsMargins(10, 10, 10, 10) + + # Wrap drone panel in scroll area + scroll = QScrollArea() + scroll.setWidget(self.drone_panel_container) + scroll.setWidgetResizable(True) + self.left_tab.addTab(scroll, "無人機") + + # 底部控制按鈕區域 + bottom_control = QWidget() + bottom_layout = QHBoxLayout(bottom_control) + + select_all_btn = QPushButton("全選") + select_all_btn.clicked.connect(self.handle_select_all) + select_all_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) - # 左侧信息面板 - left_panel = QWidget() - left_layout = QVBoxLayout(left_panel) - left_layout.setContentsMargins(5, 5, 5, 5) - - # 信息滚动区域 - scroll_area = QScrollArea() - scroll_area.setWidgetResizable(True) - self.info_container = QWidget() - self.info_layout = QVBoxLayout(self.info_container) - self.info_layout.setAlignment(Qt.AlignmentFlag.AlignTop) - scroll_area.setWidget(self.info_container) + arm_all_btn = QPushButton("批次解鎖") + arm_all_btn.clicked.connect(self.handle_arm_selected) + arm_all_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) - left_layout.addWidget(scroll_area) + takeoff_all_btn = QPushButton("批次起飛") + takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) + takeoff_all_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + bottom_layout.addWidget(select_all_btn) + bottom_layout.addWidget(arm_all_btn) + bottom_layout.addWidget(takeoff_all_btn) + bottom_layout.addStretch() + + # — 分頁 2：Overview Table + self.overview_table = QTableWidget() + # 一開始只有一欄 + self.overview_table.setColumnCount(1) + self.overview_table.setRowCount(len(self.info_types)) + self.overview_table.setHorizontalHeaderLabels(["資訊"]) + header = self.overview_table.horizontalHeader() + # 只有第一欄自動收縮到內容寬度，其它欄不動 + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.overview_table.verticalHeader().setVisible(False) + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(i, 0, item) + + self.left_tab.addTab(self.overview_table, "總覽") + # 右侧地图区域 self.map_view = QWebEngineView() @@ -305,7 +475,17 @@ class ControlStationUI(QMainWindow): self.map_view.setHtml(inline_html) self.map_view.loadFinished.connect(self.on_map_loaded) - main_splitter.addWidget(left_panel) + # Create left container and its layout + left_container = QWidget() + left_layout = QVBoxLayout(left_container) + left_layout.setContentsMargins(0, 0, 0, 0) + + # Add tab widget and bottom control to left container + left_layout.addWidget(self.left_tab) + left_layout.addWidget(bottom_control) + + # Add widgets to splitter + main_splitter.addWidget(left_container) main_splitter.addWidget(self.map_view) main_splitter.setSizes([400, 1000]) @@ -344,6 +524,14 @@ class ControlStationUI(QMainWindow): header_layout = QHBoxLayout(header) header_layout.setContentsMargins(0, 0, 0, 0) + # 在標題列加入勾選框 + checkbox = QCheckBox() + checkbox.setObjectName(f"{drone_id}_checkbox") + checkbox.setStyleSheet("QCheckBox { color: #DDD; }") + checkbox.stateChanged.connect(lambda state: self.handle_drone_selection(drone_id, state)) + + header_layout.insertWidget(0, checkbox) # 插入到最前面 + # ID显示 id_label = QLabel(drone_id) id_label.setStyleSheet(""" @@ -365,14 +553,79 @@ class ControlStationUI(QMainWindow): # 数据字段（带标签） self.create_data_row(layout, "模式:", f"{drone_id}_mode", "--") - self.create_data_row(layout, "電壓:", f"{drone_id}_battery", "--") - self.create_data_row(layout, "GPS:", f"{drone_id}_gps", "--") + self.create_data_row(layout, "ARM:", f"{drone_id}_armed", "--") + self.create_data_row(layout, "電池:", f"{drone_id}_battery", "--") self.create_data_row(layout, "高度:", f"{drone_id}_altitude", "--") self.create_data_row(layout, "Local:", f"{drone_id}_local", "--") - self.create_data_row(layout, "HUD:", f"{drone_id}_hud", "--") + self.create_data_row(layout, "地速:", f"{drone_id}_groundspeed", "--") + self.create_data_row(layout, "航向:", f"{drone_id}_heading", "--") + + # Add control buttons + control_widget = QWidget() + control_layout = QHBoxLayout(control_widget) + control_layout.setContentsMargins(0, 0, 0, 0) + + arm_btn = QPushButton("解鎖") + arm_btn.setObjectName(f"{drone_id}_arm_btn") + arm_btn.clicked.connect(lambda: self.handle_arm(drone_id)) + arm_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + } + QPushButton:hover { + background-color: #555; + } + """) + + takeoff_btn = QPushButton("起飛") + takeoff_btn.setObjectName(f"{drone_id}_takeoff_btn") + takeoff_btn.clicked.connect(lambda: self.handle_takeoff(drone_id)) + takeoff_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + } + QPushButton:hover { + background-color: #555; + } + """) + + control_layout.addWidget(arm_btn) + control_layout.addWidget(takeoff_btn) + control_layout.addStretch() + + layout.addWidget(control_widget) return panel + def handle_arm(self, drone_id): + loop = asyncio.get_event_loop() + arm_state = not self.monitor.get_arm_state(drone_id) # Toggle arm state + future = self.monitor.arm_drone(drone_id, arm_state) + loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}")) + + def handle_takeoff(self, drone_id): + loop = asyncio.get_event_loop() + future = self.monitor.takeoff_drone(drone_id, 10.0) # 10 meters default + loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}")) + + async def handle_service_response(self, future, action): + try: + result = await future + if result: + self.statusBar().showMessage(f"{action} 成功", 3000) + else: + self.statusBar().showMessage(f"{action} 失敗", 3000) + except Exception as e: + self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) + def create_data_row(self, layout, title, object_name, default): row = QWidget() hbox = QHBoxLayout(row) @@ -394,90 +647,235 @@ class ControlStationUI(QMainWindow): layout.addWidget(row) def update_ui(self, msg_type, drone_id, data): + # 檢查是否為新無人機，若是則加入無人機面板 if msg_type == 'new_drone': self.add_drone(drone_id) return - + + # 確認無人機面板存在 if not (panel := self.drones.get(drone_id)): return - + + # 更新特定欄位並在總覽頁面更新 if msg_type == 'state': - mode = data['mode'] - self.update_field(panel, drone_id, 'mode', f"{mode}", - '#FF5555' if '返航' in data else '#55FF55') - + mode = data.get('mode', 'UNKNOWN') + armed = data.get('armed', False) + mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' + arm_text = "ARMED" if armed else "DISARMED" + arm_color = '#55FF55' if armed else '#FF5555' + + # 更新無人機面板欄位 + self.update_field(panel, drone_id, 'mode', mode, mode_color) + self.update_field(panel, drone_id, 'armed', arm_text, arm_color) + + # 更新總覽頁面欄位 + self.update_overview_table(drone_id, 'mode', mode) + self.update_overview_table(drone_id, 'armed', arm_text) + elif msg_type == 'battery': voltage = data.get('voltage', 0) - self.update_field(panel, drone_id, 'battery', - f"{voltage:.1f} V", - '#FF6464' if voltage < 12 else '#FFFFFF') - + voltage = 14.8 + + # 使用標準電壓判斷電池節數 + cells_by_max = voltage / 4.2 + cells_by_min = voltage / 3.7 + + # 檢查是否為合理的電池組 + is_valid_cells = abs(round(cells_by_max) - cells_by_min) < 0.5 + num_cells = round(cells_by_max) + + if not is_valid_cells or num_cells < 2: + # 電壓異常，顯示錯誤 + text = f"{voltage:.1f}V (電壓異常!)" + voltage_color = '#FF0000' # 紅色 + else: + # 正常電壓計算 + cell_voltage = voltage / num_cells + cell_max = 4.2 + cell_min = 3.7 + + # 計算電量百分比 + percentage = max(0, min(100, ((cell_voltage - cell_min) / (cell_max - cell_min)) * 100)) + + # 根據百分比設置顏色 + if percentage < 20: + voltage_color = '#FF6464' # 紅色 (低電量) + elif percentage < 40: + voltage_color = '#FFA500' # 橘色 (中低電量) + else: + voltage_color = '#FFFFFF' # 白色 (正常電量) + + # 顯示總電壓、電池節數、單節電壓和百分比 + text = f"{voltage:.1f}V ({cell_voltage:.2f}V / {percentage:.0f}%)" + + self.update_field(panel, drone_id, 'battery', text, voltage_color) + self.update_overview_table(drone_id, 'battery', text) + elif msg_type == 'gps': - lat, lon = data['lat'], data['lon'] + lat, lon = data.get('lat', 0), data.get('lon', 0) self.drone_positions[drone_id] = (lat, lon) - text = (f"緯度: {lat:.6f}°\n" - f"經度: {lon:.6f}°") + text = f"{lat:.6f}°, {lon:.6f}°" self.update_field(panel, drone_id, 'gps', text) + self.update_overview_table(drone_id, 'gps', text) elif msg_type == 'altitude': - text = (f"{data['altitude']:.1f} m") + altitude = data.get('altitude', 0) + text = f"{altitude:.1f} m" self.update_field(panel, drone_id, 'altitude', text) - + self.update_overview_table(drone_id, 'altitude', text) + elif msg_type == 'local_pose': - text = (f"({data['x']:.1f}, {data['y']:.1f}, {data['z']:.1f})") + text = f"{data['x']:.1f}, {data['y']:.1f}" self.update_field(panel, drone_id, 'local', text) - + self.update_overview_table(drone_id, 'local', text) + elif msg_type == 'hud': - heading = data['heading'] - text = (f"地速: {data['groundspeed']:.1f} m/s\n" - f"航向: {heading:.1f}°") - self.update_field(panel, drone_id, 'hud', text) - - if self.map_loaded and drone_id in self.drone_positions: - lat, lon = self.drone_positions[drone_id] - js = f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});" - self.map_view.page().runJavaScript(js) - ''' + heading = data.get('heading', 0) + self.drone_headings[drone_id] = heading + groundspeed = data.get('groundspeed', 0) + heading_text = f"{heading:.1f}°" + groundspeed_text = f"{groundspeed:.1f} m/s" + self.update_field(panel, drone_id, 'heading', heading_text) + self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) + self.update_overview_table(drone_id, 'heading', heading_text) + self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) + + elif msg_type == 'loss_rate': + loss_rate = data.get('loss_rate', 0) + text = f"{loss_rate:.1f}%" + self.update_field(panel, drone_id, 'loss_rate', text) + self.update_overview_table(drone_id, 'loss_rate', text) + + elif msg_type == 'ping': + ping = data.get('ping', 0) + text = f"{ping:.1f} ms" + self.update_field(panel, drone_id, 'ping', text) + self.update_overview_table(drone_id, 'ping', text) + elif msg_type == 'velocity': - text = (f"VX: {data['vx']:.1f} m/s\n" - f"VY: {data['vy']:.1f} m/s\n" - f"VZ: {data['vz']:.1f} m/s") - self.update_field(panel, drone_id, 'velocity', text) - + text = f"{data['vx']:.1f}, {data['vy']:.1f}" + self.update_overview_table(drone_id, 'velocity', text) + elif msg_type == 'attitude': - text = (f"Roll: {data['roll']:.1f}°\n" - f"Pitch: {data['pitch']:.1f}°\n" - f"Yaw: {data['yaw']:.1f}°") - self.update_field(panel, drone_id, 'attitude', text) - ''' + roll = data.get('roll', 0) + pitch = data.get('pitch', 0) + yaw = data.get('yaw', 0) + roll_text = f"{roll:.1f}°" + pitch_text = f"{pitch:.1f}°" + yaw_text = f"{yaw:.1f}°" + self.update_overview_table(drone_id, 'roll', roll_text) + self.update_overview_table(drone_id, 'pitch', pitch_text) + self.update_overview_table(drone_id, 'yaw', yaw_text) + + # 如果地圖已經載入，則更新地圖顯示無人機位置 + if self.map_loaded and drone_id in self.drone_positions and drone_id in self.drone_headings: + lat, lon = self.drone_positions[drone_id] + heading = self.drone_headings[drone_id] + js = f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});" + self.map_view.page().runJavaScript(js) + + + def handle_drone_selection(self, drone_id, state): + """處理個別無人機勾選狀態變化""" + if state == Qt.CheckState.Checked.value: + self.monitor.selected_drones.add(drone_id) + else: + self.monitor.selected_drones.discard(drone_id) + + def handle_select_all(self): + """處理全選按鈕 - 再次點擊時取消全選""" + # 檢查是否所有無人機都已被選中 + all_selected = all( + self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox").isChecked() + for drone_id in self.drones + ) + + # 如果全部已選中，則取消全選；否則全選 + new_state = not all_selected + + # 更新所有勾選框狀態 + for drone_id in self.drones: + checkbox = self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox") + if checkbox: + checkbox.setChecked(new_state) + + def handle_arm_selected(self): + """處理批次解鎖""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.arm_drone(drone_id, True) + loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}")) + + def handle_takeoff_selected(self): + """處理批次起飛""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.takeoff_drone(drone_id, 10.0) + loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) + def update_field(self, panel, drone_id, field, text, color=None): + """Update a specific field in the panel.""" if label := panel.findChild(QLabel, f"{drone_id}_{field}"): label.setText(text) if color: label.setStyleSheet(f"color: {color};") - ''' - def add_drone(self, drone_id): - if drone_id not in self.drones: - panel = self.create_drone_panel(drone_id) - self.info_layout.addWidget(panel) - self.drones[drone_id] = panel - ''' + + def update_overview_table(self, drone_id=None, field=None, value=None): + # Ensure the widget is available + if not hasattr(self, 'overview_table') or self.overview_table is None: + return + + # Update a specific cell in the overview table + if drone_id and field and value: + col = 1 + list(self.drones.keys()).index(drone_id) + row = self.info_type_map.get(field, -1) # Get row from field mapping + + if row == -1: + return # Invalid field, exit + + item = self.overview_table.item(row, col) + if not item: + item = QTableWidgetItem() + self.overview_table.setItem(row, col, item) + + item.setText(value) # Update the cell's text + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) # Center the text + + # If no specific update, refresh entire table + if drone_id is None: + cols = 1 + len(self.drones) + self.overview_table.setColumnCount(cols) + headers = ["資訊"] + list(self.drones.keys()) + self.overview_table.setHorizontalHeaderLabels(headers) + + for col, did in enumerate(self.drones, start=1): + panel = self.drones[did] + for field, row in self.info_type_map.items(): + lbl = panel.findChild(QLabel, f"{did}_{field}") + val = lbl.text() if lbl else "--" + val_item = QTableWidgetItem(val) + val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(row, col, val_item) + def add_drone(self, drone_id): - if drone_id not in self.drones: - panel = self.create_drone_panel(drone_id) - self.info_layout.addWidget(panel) - self.drones[drone_id] = panel - - # 清空原有 layout - for i in reversed(range(self.info_layout.count())): - widget = self.info_layout.itemAt(i).widget() - if widget: - self.info_layout.removeWidget(widget) - widget.setParent(None) - - # 根據 key 排序後重新加入 - for sorted_id in sorted(self.drones, key=lambda x: int(''.join(filter(str.isdigit, x)) or 0)): - self.info_layout.addWidget(self.drones[sorted_id]) + """只這一個 add_drone，不要再重複定義。""" + if drone_id in self.drones: + return + panel = self.create_drone_panel(drone_id) + self.drones[drone_id] = panel + + # 重新排序並顯示所有 panel + # 先清空 + while self.drone_panel_layout.count(): + w = self.drone_panel_layout.takeAt(0).widget() + if w: + w.setParent(None) + # 再依照數字排序加入 + for did in sorted(self.drones, key=lambda x: int(x[1:])): + self.drone_panel_layout.addWidget(self.drones[did]) + + # 更新總覽表 + self.update_overview_table() def spin_ros(self): try: @@ -495,4 +893,4 @@ if __name__ == '__main__': app = QApplication(sys.argv) station = ControlStationUI() station.show() - app.exec(app.exec()) \ No newline at end of file + app.exec() \ No newline at end of file From a55b76534fe22896c1772cdf7299ba8fd6ae0df6 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 30 Jun 2025 19:43:36 +0800 Subject: [PATCH 10/33] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/devRun.py | 104 +++++++----------- 1 file changed, 41 insertions(+), 63 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 60d54b9..835bfae 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -464,69 +464,47 @@ elif test_item == 22: 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 的運行 + print("===> Start of Program .Test", test_item) + rclpy.init() + + # 1) 啟動 bridge（它已自動建立所有 publisher） + bridge = mo.mavlink_bridge() + bridge._init_node() + + # 2) 建立一條 UDP 通道並啟動 mavlink_object + mav_sock = mavutil.mavlink_connection("udp:127.0.0.1:14550") + mobj = mo.mavlink_object(mav_sock) # 自動分配 socket_id + mobj.multiplexingToAnalysis = [0, 30, 32, 33, 74, 111, 147] # HEARTBEAT + 常用訊息 + mobj.run() + + print("waiting for mavlink data ...") + time.sleep(2) # 給一點時間收 HEARTBEAT + + # 3) 顯示目前偵測到的 sysid 清單 + print("Current sysid list:", list(bridge.mavlink_systems.keys())) + + # 4) 主迴圈：持續將資料 emit 出去 + try: + last_timesync = time.time() + while rclpy.ok(): + now = time.time() + + # 每秒一次 + if now - last_timesync >= 1.0: + # 發送 TIMESYNC request + mav_sock.mav.timesync_send(0, int(now * 1e9)) + last_timesync = now + + bridge.emit_info() # 將所有 emitParams 發布到 ROS topic time.sleep(0.5) - except KeyboardInterrupt: - break - - analyzer.destroy_node() + except KeyboardInterrupt: + pass + + # -------- 清理 -------- + bridge.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 + mobj.stop(); mobj.thread.join() + bridge.stop(); bridge.thread.join() + mav_sock.close() + print("<=== End of Program") From 6bbf2c450fb32ff48bc10abfa67a7ef02fd86c92 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 30 Jun 2025 19:43:54 +0800 Subject: [PATCH 11/33] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkObject.py | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 0a219be..88046b8 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -20,6 +20,7 @@ from pymavlink import mavutil # ROS2 的 import from rclpy.node import Node +import std_msgs.msg # 自定義的 import from mavlinkDevice import mavlink_device, mavlink_systems @@ -117,6 +118,8 @@ class mavlink_bridge(Node, mavlink_publisher): this_device.components[msg.get_srcComponent()] = this_component this_component.mav_type = msg.type this_component.mav_autopilot = msg.autopilot + this_component.sysid = sysid # ★ 新增 + this_component.compid = compid else: continue @@ -124,6 +127,7 @@ class mavlink_bridge(Node, mavlink_publisher): if msg_id == 0: # HEARTBEAT 處理 this_component.emitParams['heartbeat'] = msg + self.ensure_all_publishers(sysid, this_component) elif msg_id == 30: # ATTITUDE 處理 this_component.emitParams['attitude'] = msg @@ -137,6 +141,12 @@ class mavlink_bridge(Node, mavlink_publisher): elif msg_id == 74: # VFR_HUD 處理 this_component.emitParams['vfr_hud'] = msg + elif msg_id == 111: #TIME_SYNC + round_trip_time = std_msgs.msg.Float64() + round_trip_time.data = (int(time.time() * 1e9) - msg.ts1) / 1e6 + if(round_trip_time.data < 1e6): + this_component.emitParams['ping'] = round_trip_time + elif msg_id == 147: # BATTERY_STATUS 處理 this_component.emitParams['battery'] = msg @@ -281,7 +291,6 @@ class mavlink_object(): 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() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 @@ -300,9 +309,20 @@ class mavlink_object(): # 統計收到的訊息 & 透過 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 類型 + device = self.mavlink_systems[sysid] mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) + + comp = device.components.get(compid) + if comp and comp.loss_rate_publisher: + if timestamp - comp.last_loss_publish_time >= 1.0: + loss_rate = comp.loss_rate # loss_rate 是在 updateComponentPacketCount 中計算並儲存的欄位 + msg = std_msgs.msg.Float64() + msg.data = float(loss_rate) + comp.loss_rate_publisher.publish(msg) + comp.last_loss_publish_time = timestamp # break # Debug function can put here you can see the input data from mavlink From 55b77f592046fa960af39df40ec80d8bdbd8058b Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 30 Jun 2025 19:44:04 +0800 Subject: [PATCH 12/33] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkPublish.py | 104 ++++++++++-------- 1 file changed, 56 insertions(+), 48 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index b328f81..a0ce087 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -19,26 +19,43 @@ import mavros_msgs.msg from theLogger import setup_logger import math - logger = setup_logger("mavlinkPublish.py") class mavlink_publisher(): prefix_path = 'MavLinkBus' + TOPIC_CREATORS = ( + "create_state", + "create_attitude", + "create_local_position_pose", + "create_local_position_velocity", + "create_global_global", + "create_global_rel", + "create_vfr_hud", + "create_battery", + "create_ping" + ) + + def ensure_all_publishers(self, sysid: int, component): + """若還沒替這個 component 建立過 publisher，則一次補齊。""" + if getattr(component, "_pub_ready", False): + return # 已做過就跳過 + for fn in self.TOPIC_CREATORS: + getattr(self, fn)(sysid, component) + # 發布丟包率 + if not hasattr(component, "loss_rate_publisher") or component.loss_rate_publisher is None: + target_topic = 'packet_loss_rate' + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + component.loss_rate_publisher = self.create_publisher(std_msgs.msg.Float64, topic_name, 10) + component.loss_rate_topic_name = topic_name + + component._pub_ready = True # 打個旗標以免重複 + logger.info("✔︎ sysid=%d compid=%d → 所有 topic 已註冊", sysid, component.compid) + def create_state(self, sysid, component_obj): # target topic name # 請跟這個 method 的名稱保持一致 target_topic = 'state' - - # 這邊要檢查 flight_mode 是否存在 - try: - _ = component_obj.emitParams['heartbeat'] - except KeyError: - # 這個 component id 還不存在 - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - - # 若存在則 建立 publisher object 並回傳 true topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(mavros_msgs.msg.State, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_state] @@ -46,6 +63,8 @@ class mavlink_publisher(): return True def packEmit_state(cls, emitParams, publisher): + if 'heartbeat' not in emitParams: # ← 沒資料就直接返回 + return mav_msg = emitParams['heartbeat'] msg = mavros_msgs.msg.State() msg.mode = mavutil.mode_string_v10(mav_msg) @@ -63,18 +82,13 @@ class mavlink_publisher(): def create_attitude(self, sysid, component_obj): target_topic = 'attitude' - - try: - _ = component_obj.emitParams['attitude'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] def packEmit_attitude(self, emitParams, publisher): + if 'attitude' not in emitParams: # ← 沒資料就直接返回 + return mav_msg = emitParams['attitude'] msg = sensor_msgs.msg.Imu() x, y, z, w = self.euler_to_quaternion(mav_msg.roll, mav_msg.pitch, mav_msg.yaw) @@ -90,16 +104,13 @@ class mavlink_publisher(): def create_local_position_pose(self, sysid, component_obj): target_topic = 'local_position/pose' - try: - _ = component_obj.emitParams['local_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose] def packEmit_local_pose(cls, emitParams, publisher): + if 'local_position' not in emitParams: + return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Point() msg.x = mav_msg.x @@ -110,16 +121,13 @@ class mavlink_publisher(): def create_local_position_velocity(self, sysid, component_obj): target_topic = 'local_position/velocity' - try: - _ = component_obj.emitParams['local_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel] def packEmit_local_vel(cls, emitParams, publisher): + if 'local_position' not in emitParams: + return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Vector3() msg.x = mav_msg.vx @@ -130,16 +138,13 @@ class mavlink_publisher(): def create_global_global(self, sysid, component_obj): target_topic = 'global_position/global' - try: - _ = component_obj.emitParams['global_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global] def packEmit_global_global(cls, emitParams, publisher): + if 'global_position' not in emitParams: + return mav_msg = emitParams['global_position'] msg = sensor_msgs.msg.NavSatFix() msg.latitude = mav_msg.lat/1e7 @@ -150,16 +155,13 @@ class mavlink_publisher(): def create_global_rel(self, sysid, component_obj): target_topic = 'global_position/rel_alt' - try: - _ = component_obj.emitParams['global_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel] def packEmit_global_rel(cls, emitParams, publisher): + if 'global_position' not in emitParams: + return mav_msg = emitParams['global_position'] msg = std_msgs.msg.Float64() msg.data = float(mav_msg.relative_alt/1e3) @@ -168,16 +170,13 @@ class mavlink_publisher(): def create_vfr_hud(self, sysid, component_obj): target_topic = 'vfr_hud' - try: - _ = component_obj.emitParams['vfr_hud'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud] def packEmit_vfr_hud(cls, emitParams, publisher): + if 'vfr_hud' not in emitParams: + return mav_msg = emitParams['vfr_hud'] msg = mavros_msgs.msg.VfrHud() msg.airspeed = mav_msg.airspeed @@ -191,21 +190,30 @@ class mavlink_publisher(): def create_battery(self, sysid, component_obj): target_topic = 'battery' - try: - _ = component_obj.emitParams['battery'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery] def packEmit_battery(cls, emitParams, publisher): + if 'battery' not in emitParams: + return mav_msg = emitParams['battery'] msg = sensor_msgs.msg.BatteryState() msg.voltage = mav_msg.voltages[0]/1e3 publisher.publish(msg) pass + + def create_ping(self, sysid, component_obj): + target_topic = 'ping' + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_ping] + def packEmit_ping(cls, emitParams, publisher): + if 'ping' not in emitParams: + return + mav_msg = emitParams['ping'] + publisher.publish(mav_msg) + pass # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file From 2783e4ba9314377c21d2727227861329a2c9664f Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 16 Jul 2025 17:04:49 +0800 Subject: [PATCH 13/33] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/mavlink.py | 100 +++++++++++++++++++++-------- 1 file changed, 72 insertions(+), 28 deletions(-) diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py index b047a0e..9bb6329 100644 --- a/src/unitdev01/unitdev01/mavlink.py +++ b/src/unitdev01/unitdev01/mavlink.py @@ -21,13 +21,15 @@ class MAVLinkWorker(QThread): param_signal = pyqtSignal(str, int) kbps_signal = pyqtSignal(str, float) - def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): + def __init__(self, connection_string="udp:127.0.0.1:14560 ", usb='/dev/ttyUSB0', acm ='/dev/ttyACM0'): super().__init__() - self.namespaces = ['UAV1', 'UAV2', 'UAV3'] - self.connection = mavutil.mavlink_connection(usb, baud=57600) + self.sysids = [1, 5, 10] + self.namespaces = {} + self.namespaces = [f"UAV{sysid}" for sysid in self.sysids] + + self.connection = mavutil.mavlink_connection(connection_string, baud=115200) self.connection.wait_heartbeat() - for sysid in self.namespaces: - sysid = int(sysid.replace('UAV', '')) + for sysid in self.sysids: self.set_sr_params(sysid) self.running = True @@ -42,8 +44,8 @@ class MAVLinkWorker(QThread): self.total_bytes_received = {} self.throughput_KBps = {} - for namespace in self.namespaces: - self.request_param(namespace, "SR1_EXTRA1") + for sysid in self.sysids: + self.request_param(sysid, "SR1_EXTRA1") self.connection.mav.timesync_send( 0, #tc1 int(time.time() * 1e9) #ts1 @@ -52,7 +54,7 @@ class MAVLinkWorker(QThread): def run(self): while self.running: try: - msg = self.connection.recv_msg() + msg = self.connection.recv_msg() # Debugging line to see received messages current_time = time.time() if not msg: continue @@ -64,7 +66,6 @@ class MAVLinkWorker(QThread): if msg_type =="BAD_DATA": continue - if sysid not in self.total_bytes_received: self.total_bytes_received[sysid] = 0 self.throughput_KBps[sysid] = 0 @@ -162,7 +163,7 @@ class MAVLinkWorker(QThread): elif msg_type == 'PARAM_VALUE': param_name = "SR1_EXTRA1" if msg.param_id == param_name: - self.param_signal.emit(namespace, msg.param_value) + self.param_signal.emit(namespace, int(msg.param_value)) except Exception as e: print(f"Error reading message: {e}") @@ -171,8 +172,11 @@ class MAVLinkWorker(QThread): self.running = False self.connection.close() + def get_sysid(self, namespace): + return int(namespace.replace('UAV', '')) + def set_mode(self, namespace, mode): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) if mode == 'STABILIZE': mode = 0 elif mode == 'AUTO': @@ -188,7 +192,7 @@ class MAVLinkWorker(QThread): ) def arm(self, namespace, arm): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -199,7 +203,7 @@ class MAVLinkWorker(QThread): ) def takeoff(self, namespace, altitude): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -210,7 +214,7 @@ class MAVLinkWorker(QThread): ) def land(self, namespace): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -221,7 +225,7 @@ class MAVLinkWorker(QThread): ) def set_local_position(self, namespace, x, y, z): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.set_position_target_local_ned_send( 0, sysid, 1, # time_boot_ms, sysid, compid mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame @@ -233,7 +237,7 @@ class MAVLinkWorker(QThread): ) def reboot_drone(self, namespace): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # target_component (一般為1) @@ -244,7 +248,7 @@ class MAVLinkWorker(QThread): ) def set_param(self, namespace, param_name, value): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) try: self.connection.mav.param_set_send( sysid, @@ -256,8 +260,7 @@ class MAVLinkWorker(QThread): except Exception as e: print(f"Failed to set parameter {param_name}: {e}") - def request_param(self, namespace, param_name): - sysid = int(namespace.replace('UAV', '')) + def request_param(self, sysid, param_name): try: self.connection.mav.param_request_read_send( sysid, # 系統 ID @@ -268,43 +271,57 @@ class MAVLinkWorker(QThread): except Exception as e: print(f"Failed to set parameter {param_name}: {e}") - ''' + def set_sr_params(self, sysid): """ 直接設置 MAVLink 訊息頻率 """ - freqs = [0, 2, 4] + freqs = [0, 1, 4] params = { "SR1_ADSB": freqs[0], "SR1_EXT_STAT": freqs[1], "SR1_EXTRA1": freqs[2], - "SR1_EXTRA2": freqs[2], + "SR1_EXTRA2": freqs[1], "SR1_EXTRA3": freqs[1], - "SR1_POSITION": freqs[1], + "SR1_POSITION": freqs[2], "SR1_RAW_SENS": freqs[1], "SR1_RC_CHAN": freqs[1] } for param, value in params.items(): self.set_param(f"UAV{sysid}", param, value) - ''' + + ''' + def set_sr_params(self, sysid): + """ 直接設置 MAVLink 訊息頻率 """ + params = { + "SERIAL1_BAUD": 38 + } + for param, value in params.items(): + self.set_param(f"UAV{sysid}", param, value) + def set_sr_params(self, sysid): """ 直接設置 MAVLink 訊息頻率 """ - freqs = [0, 1, 1] + freqs = [0, 3, 3] params = { "SR1_ADSB": freqs[0], - "SR1_EXT_STAT": freqs[0], + "SR1_EXT_STAT": freqs[1], "SR1_EXTRA1": freqs[2], "SR1_EXTRA2": freqs[2], - "SR1_EXTRA3": freqs[0], + "SR1_EXTRA3": freqs[1], "SR1_POSITION": freqs[1], "SR1_RAW_SENS": freqs[1], - "SR1_RC_CHAN": freqs[0] + "SR1_RC_CHAN": freqs[1] } for param, value in params.items(): self.set_param(f"UAV{sysid}", param, value) + ''' + def init_drone(self, namespace): + sysid = self.get_sysid(namespace) + self.set_local_position(sysid, 0, 0, 0) class DroneControlApp(QMainWindow): def __init__(self): super().__init__() self.workers = MAVLinkWorker() + self.sysids = self.workers.sysids self.namespaces = self.workers.namespaces self.initUI() @@ -438,6 +455,15 @@ class DroneControlApp(QMainWindow): self.rebootButton.clicked.connect(self.reboot_drone) main_layout.addWidget(self.rebootButton) + mission_layout = QHBoxLayout() + self.initButton = QPushButton("重設位置") + self.initButton.clicked.connect(self.init_drone) + self.startButton = QPushButton("開始任務") + self.startButton.clicked.connect(self.start_mission) + mission_layout.addWidget(self.initButton) + mission_layout.addWidget(self.startButton) + main_layout.addLayout(mission_layout) + # 設置主佈局 central_widget = QWidget(self) central_widget.setLayout(main_layout) @@ -563,6 +589,24 @@ class DroneControlApp(QMainWindow): except ValueError: QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") + def init_drone(self): + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.init_drone(namespace) + print(f"重設位置：{namespace}") + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"重設位置失敗：{e}") + + def start_mission(self): + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.start_mission(namespace) + print(f"開始任務：{namespace}") + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"開始任務失敗：{e}") + def main(): app = QtWidgets.QApplication(sys.argv) window = DroneControlApp() From 47a681e27d8ec34c77d6a8b069ceb16a347569ac Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 16 Jul 2025 17:05:29 +0800 Subject: [PATCH 14/33] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 765 ++++++++++++++++++++++++++------- 1 file changed, 621 insertions(+), 144 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index 6636fa3..5bc7213 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -4,7 +4,7 @@ from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, - QHeaderView, QPushButton, QCheckBox) + QHeaderView, QPushButton, QCheckBox, QLineEdit) from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal from PyQt6.QtWebEngineWidgets import QWebEngineView import math @@ -18,6 +18,9 @@ from mavros_msgs.msg import State, VfrHud from mavros_msgs.srv import CommandBool, CommandTOL, SetMode import asyncio from functools import partial +import threading +import websockets +import json class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -31,14 +34,99 @@ class DroneMonitor(Node): self.arm_clients = {} self.takeoff_clients = {} + self.setpoint_pubs = {} self.selected_drones = set() + self.latest_data = {} + + # 啟動 WebSocket client 執行緒 + threading.Thread(target=self.start_ws_client, daemon=True).start() # 主题检测定时器 self.create_timer(1.0, self.scan_topics) + + def start_ws_client(self): + asyncio.set_event_loop(asyncio.new_event_loop()) + asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + + async def ws_client_loop(self): + retry_count = 0 + max_retries = 5 + base_delay = 1.0 + local = "ws://0.0.0.0:8765" # 本地 WebSocket 地址 + remote = "ws://140.120.31.123:8765" + while retry_count < max_retries: + try: + async with websockets.connect(local) as websocket: + print("WebSocket connected") + retry_count = 0 # 重置重試計數 + + async for message in websocket: + try: + data = json.loads(message) + print(f"📡 Received: {data}") + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket JSON decode error: {e}") + except Exception as e: + print(f"WebSocket message processing error: {e}") + + except websockets.exceptions.ConnectionClosedError: + print("WebSocket connection closed") + break + except Exception as e: + retry_count += 1 + delay = base_delay * (2 ** min(retry_count, 4)) # 指數退避 + print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})") + await asyncio.sleep(delay) + print("WebSocket client stopped after maximum retries") + + + def process_websocket_message(self, data): + try: + drone_id = data.get('system_id') + drone_id = f"s9_{drone_id}" if drone_id else None + if not drone_id: + return + + # 模式 + if 'mode' in data: + self.signals.update_signal.emit('state', drone_id, { + 'mode': data['mode'], + }) + + # 電池 + if 'battery' in data: + # 這裡假設 battery 是百分比 + self.signals.update_signal.emit('battery', drone_id, { + 'percentage': data['battery'] + }) + + # 位置 + if 'position' in data: + pos = data['position'] + self.signals.update_signal.emit('gps', drone_id, { + 'lat': pos.get('lat', 0), + 'lon': pos.get('lon', 0) + }) + + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': round(pos.get('lat', 0), 6), + 'y': round(pos.get('lon', 0), 6), + }) + + # 航向 + if 'heading' in data: + self.signals.update_signal.emit('hud', drone_id, { + 'heading': data['heading'], + }) + + except Exception as e: + print(f"WebSocket message processing error: {e}") + def scan_topics(self): topics = self.get_topic_names_and_types() - drone_pattern = re.compile(r'/MavLinkBus/(s\d+)/(\w+)') + drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') found_drones = set() for topic_name, _ in topics: @@ -55,7 +143,6 @@ class DroneMonitor(Node): def setup_drone(self, drone_id): base_topic = f'/MavLinkBus/{drone_id}' - # Add service clients self.arm_clients[drone_id] = self.create_client( CommandBool, @@ -65,7 +152,14 @@ class DroneMonitor(Node): CommandTOL, f'{base_topic}/cmd/takeoff' ) - + + # Add setpoint publisher + self.setpoint_pubs[drone_id] = self.create_publisher( + Point, + f'{base_topic}/setpoint_position/local', + 10 + ) + subs = { 'attitude': self.create_subscription( Imu, @@ -130,7 +224,6 @@ class DroneMonitor(Node): } setattr(self, f'drone_{drone_id}_subs', subs) - self.signals.update_signal.emit('new_drone', drone_id, None) async def arm_drone(self, drone_id, arm): if drone_id not in self.arm_clients: @@ -171,7 +264,20 @@ class DroneMonitor(Node): except Exception as e: self.get_logger().error(f'Takeoff service call failed: {e}') return False + + def send_setpoint(self, drone_id, x, y, z): + """Send setpoint position command""" + if drone_id not in self.setpoint_pubs: + return False + + msg = Point() + msg.x = float(x) + msg.y = float(y) + msg.z = float(z) + self.setpoint_pubs[drone_id].publish(msg) + return True + def quaternion_to_euler(self, q): sinr_cosp = 2 * (q.w * q.x + q.y * q.z) cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) @@ -190,71 +296,71 @@ class DroneMonitor(Node): def attitude_callback(self, drone_id, msg): if hasattr(msg, 'orientation'): roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) - self.signals.update_signal.emit('attitude', drone_id, { + self.latest_data[(drone_id, 'attitude')] = { 'roll': roll, 'pitch': pitch, 'yaw': yaw, 'rates': (msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z) - }) + } def battery_callback(self, drone_id, msg): - self.signals.update_signal.emit('battery', drone_id, { + self.latest_data[(drone_id, 'battery')] = { 'voltage': msg.voltage - }) + } def state_callback(self, drone_id, msg): - self.signals.update_signal.emit('state', drone_id, { + self.latest_data[(drone_id, 'state')] = { 'mode': msg.mode, 'armed': msg.armed - }) + } def gps_callback(self, drone_id, msg): - self.signals.update_signal.emit('gps', drone_id, { + self.latest_data[(drone_id, 'gps')] = { 'lat': msg.latitude, 'lon': msg.longitude, 'alt': msg.altitude - }) + } def local_vel_callback(self, drone_id, msg): - self.signals.update_signal.emit('velocity', drone_id, { + self.latest_data[(drone_id, 'velocity')] = { 'vx': msg.x, 'vy': msg.y, 'vz': msg.z - }) + } def altitude_callback(self, drone_id, msg): - self.signals.update_signal.emit('altitude', drone_id, { + self.latest_data[(drone_id, 'altitude')] = { 'altitude': msg.data - }) + } def local_pose_callback(self, drone_id, msg): - self.signals.update_signal.emit('local_pose', drone_id, { + self.latest_data[(drone_id, 'local_pose')] = { 'x': msg.x, 'y': msg.y, 'z': msg.z - }) + } def hud_callback(self, drone_id, msg): - self.signals.update_signal.emit('hud', drone_id, { + self.latest_data[(drone_id, 'hud')] = { 'airspeed': msg.airspeed, 'groundspeed': msg.groundspeed, 'heading': msg.heading, 'throttle': msg.throttle, 'alt': msg.altitude, 'climb': msg.climb - }) + } def loss_rate_callback(self, drone_id, msg): - self.signals.update_signal.emit('loss_rate', drone_id, { + self.latest_data[(drone_id, 'loss_rate')] = { 'loss_rate': msg.data - }) + } def ping_callback(self, drone_id, msg): - self.signals.update_signal.emit('ping', drone_id, { + self.latest_data[(drone_id, 'ping')] = { 'ping': msg.data - }) + } class ControlStationUI(QMainWindow): def __init__(self): @@ -271,8 +377,14 @@ class ControlStationUI(QMainWindow): self.executor = rclpy.executors.SingleThreadedExecutor() self.executor.add_node(self.monitor) + # 定时处理ROS事件 + self.timer = QTimer() + self.timer.timeout.connect(self.spin_ros) + self.timer.start(10) + # 初始化UI self.drones = {} + self.socket_groups = {} # 新增：用於儲存 socket 分組 self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向", "Roll", "Pitch", "Yaw", "丟包", "延遲"] self.info_type_map = { @@ -293,12 +405,15 @@ class ControlStationUI(QMainWindow): self.drone_positions = {} self.drone_headings = {} self.map_loaded = False + # 添加地圖更新節流定時器 + self.map_update_timer = QTimer() + self.map_update_timer.timeout.connect(self.update_map_positions) + self.map_update_timer.start(200) # 每 200ms 更新一次地圖 + # 添加待更新的無人機位置緩存 + self.pending_map_updates = {} + self.init_ui() - # 定时处理ROS事件 - self.timer = QTimer() - self.timer.timeout.connect(self.spin_ros) - self.timer.start(50) def init_ui(self): @@ -323,7 +438,7 @@ class ControlStationUI(QMainWindow): # 底部控制按鈕區域 bottom_control = QWidget() - bottom_layout = QHBoxLayout(bottom_control) + bottom_layout = QVBoxLayout(bottom_control) select_all_btn = QPushButton("全選") select_all_btn.clicked.connect(self.handle_select_all) @@ -367,10 +482,82 @@ class ControlStationUI(QMainWindow): QPushButton:hover { background-color: #555; } """) - bottom_layout.addWidget(select_all_btn) - bottom_layout.addWidget(arm_all_btn) - bottom_layout.addWidget(takeoff_all_btn) - bottom_layout.addStretch() + # Add coordinate inputs + coord_widget = QWidget() + coord_layout = QHBoxLayout(coord_widget) + coord_layout.setContentsMargins(0, 0, 0, 0) + + self.x_input = QLineEdit() + self.y_input = QLineEdit() + self.z_input = QLineEdit() + + for input_field in [self.x_input, self.y_input, self.z_input]: + input_field.setFixedWidth(60) + input_field.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 3px; + } + """) + + coord_layout.addWidget(QLabel("X:")) + coord_layout.addWidget(self.x_input) + coord_layout.addWidget(QLabel("Y:")) + coord_layout.addWidget(self.y_input) + coord_layout.addWidget(QLabel("Z:")) + coord_layout.addWidget(self.z_input) + + # Add buttons to control layout + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.clicked.connect(self.handle_setpoint_selected) + setpoint_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + + # 上方按鈕區域 + upper_buttons = QHBoxLayout() + select_all_btn = QPushButton("全選") + select_all_btn.clicked.connect(self.handle_select_all) + arm_all_btn = QPushButton("批次解鎖") + arm_all_btn.clicked.connect(self.handle_arm_selected) + takeoff_all_btn = QPushButton("批次起飛") + takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) + for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]: + btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + upper_buttons.addWidget(btn) + upper_buttons.addStretch() + + # 下方座標輸入區域 + lower_control = QHBoxLayout() + lower_control.addWidget(coord_widget) + lower_control.addWidget(setpoint_btn) + lower_control.addStretch() + + + # 將兩個區域加入底部控制區 + bottom_layout.addLayout(upper_buttons) + bottom_layout.addLayout(lower_control) # — 分頁 2：Overview Table self.overview_table = QTableWidget() @@ -408,9 +595,9 @@ class ControlStationUI(QMainWindow):
+
+ +
+ @@ -1167,18 +1260,16 @@ class ControlStationUI(QMainWindow): elif msg_type == 'battery': voltage = data.get('voltage', 16) - # 使用標準電壓判斷電池節數 - cell_max = 4.2 + # 判斷電池節數 cells = round(voltage / 3.95) - max = cell_max * cells # 計算電量百分比 - percentage = voltage / max * 100 + percentage = (voltage / cells - 3.7) / 0.5 * 100 # 根據百分比設置顏色 if percentage < 20: voltage_color = '#FF6464' # 紅色 (低電量) - elif percentage < 40: + elif percentage < 50: voltage_color = '#FFA500' # 橘色 (中低電量) else: voltage_color = '#FFFFFF' # 白色 (正常電量) From 4388ebd9ede6d0098f0403b0edb3168e0cc223c0 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 14 Aug 2025 17:17:04 +0800 Subject: [PATCH 20/33] =?UTF-8?q?item51&54=20=E9=80=A3=E6=8E=A5=E5=A4=9A?= =?UTF-8?q?=E5=80=8Bport?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/devRun.py | 763 +----------------- 1 file changed, 42 insertions(+), 721 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 133e56d..78abb8f 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -18,452 +18,7 @@ test_item = 51 running_time =10000 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:14552" - 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: +if test_item == 51: # 晉凱的測試項目 - 修改為支援多UDP連接 print('===> Start of Program .Test ', test_item) rclpy.init() # 注意要初始化 rclpy 才能使用 node @@ -476,18 +31,18 @@ elif test_item == 51: print('初始化 node 完成 耗時 : ',time.time() - show_time) # === 修改：支援多個UDP連接 === - udp_ports = [14551, 14552, 14553] # 可以根據需要調整端口 + ports = ["udp:127.0.0.1:14550", + "udp:127.0.0.1:14570", + "/dev/ttyUSB0", + "/dev/ttyUSB1"] # 可以根據需要調整端口 mavlink_sockets = [] mavlink_objects = [] - print(f"設定連接到 UDP 端口: {udp_ports}") - # 循環創建多個連接 - for i, port in enumerate(udp_ports): + for i, port in enumerate(ports): try: print(f"連接到 UDP:{port}") - connection_string = f"udp:127.0.0.1:{port}" - mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_socket = mavutil.mavlink_connection(port) mavlink_object = mo.mavlink_object(mavlink_socket) # 設定通道流動（所有連接使用相同參數） @@ -529,13 +84,6 @@ elif test_item == 51: print(f"為系統 {sysid}, 組件 {compid} 創建topics...") 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]) topics_created += 1 print(f"系統 {sysid}_{compid} topics創建完成") @@ -563,7 +111,7 @@ elif test_item == 51: loop_count += 1 if loop_count % 10 == 0: # 每5秒顯示一次（0.5s * 10） print(f"\n=== 狀態更新 (第 {loop_count} 次循環) ===") - print(f"連接的UDP端口: {udp_ports[:len(mavlink_sockets)]}") + #print(f"連接的端口: {udp_ports[:len(mavlink_sockets)]}") print(f"檢測到的系統數: {len(analyzer.mavlink_systems)}") for sysid in analyzer.mavlink_systems: @@ -626,7 +174,7 @@ elif test_item == 51: # 清理所有mavlink objects for i, mavlink_obj in enumerate(mavlink_objects): try: - print(f"停止 mavlink_object {i+1} (UDP:{udp_ports[i]}, socket_id: {mavlink_obj.socket_id})") + print(f"停止 mavlink_object {i+1} (UDP:{ports[i]}, socket_id: {mavlink_obj.socket_id})") mavlink_obj.stop() mavlink_obj.thread.join() print(f"mavlink_object {i+1} 已停止") @@ -636,10 +184,10 @@ elif test_item == 51: # 關閉所有mavlink sockets for i, mavlink_sock in enumerate(mavlink_sockets): try: - print(f"關閉 UDP 連接 {udp_ports[i]}") + print(f"關閉 UDP 連接 {ports[i]}") mavlink_sock.close() except Exception as e: - print(f"關閉 UDP 連接 {udp_ports[i]} 失敗: {e}") + print(f"關閉 UDP 連接 {ports[i]} 失敗: {e}") # 清理analyzer try: @@ -653,221 +201,6 @@ elif test_item == 51: print(f"清理完成，共處理了 {len(mavlink_sockets)} 個 UDP 連接") print('<=== End of Program') -elif test_item == 52: - # 文鈞的測試項目 - # 需要開啟一個 ardupilot 的模擬器 與 GCS - # 這邊是測試 mavlink object 作為交換器功能的代碼 - print('===> Start of Program .Test ', test_item) - - # 啟動連線的模組 - analyzer = mo.mavlink_bridge() - - # 共用的輸出連接 (連接到GCS) - common_out = "udpout:127.0.0.1:14553" - mavlink_socket_out = mavutil.mavlink_connection(common_out) - mavlink_object_out = mo.mavlink_object(mavlink_socket_out) - mavlink_object_out.multiplexingToAnalysis = [0] - - # 設定多台無人機的輸入連接 - drone_inputs = [ - #"udp:127.0.0.1:14550", # 無人機1 - #"udp:127.0.0.1:14550", # 無人機2 - "udp:127.0.0.1:14550", # 無人機3 - "udp:127.0.0.1:14554" # 無人機4 - ] - - # 建立輸入連接 - mavlink_objects_in = [] - mavlink_sockets_in = [] - - for i, input_conn in enumerate(drone_inputs): - print(f"連接無人機 {i+1} 輸入: {input_conn}") - mavlink_in = mavutil.mavlink_connection(input_conn) - obj_in = mo.mavlink_object(mavlink_in) - obj_in.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_in.append(obj_in) - mavlink_sockets_in.append(mavlink_in) - - # 設定轉發關係 - obj_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # 所有訊息 - mavlink_object_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # 所有訊息 - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - # 啟動所有通道 - for obj in mavlink_objects_in: - obj.run() - mavlink_object_out.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(3) - print("=== connection established! ===") - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - 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) >= 8: - 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 - for i, obj in enumerate(mavlink_objects_in): - obj.stop() - obj.thread.join() - mavlink_sockets_in[i].close() - - mavlink_object_out.stop() - mavlink_object_out.thread.join() - mavlink_socket_out.close() - - analyzer.stop() - print('<=== End of Program') - -elif test_item == 53: - # 文鈞的測試項目 - 雙輸出版本 - # 需要開啟一個 ardupilot 的模擬器 與 兩個 GCS - # 這邊是測試 mavlink object 作為交換器功能的代碼，支援雙輸出 - print('===> Start of Program .Test ', test_item) - - # 啟動連線的模組 - analyzer = mo.mavlink_bridge() - - # 雙輸出連接設定 (連接到兩個不同的GCS) - gcs_outputs = [ - "udpout:127.0.0.1:14560", # GCS 1 - "udpout:127.0.0.1:14570" # GCS 2 - ] - - # 建立輸出連接物件 - mavlink_objects_out = [] - mavlink_sockets_out = [] - - for i, output_conn in enumerate(gcs_outputs): - print(f"建立 GCS {i+1} 輸出連接: {output_conn}") - mavlink_out = mavutil.mavlink_connection(output_conn) - obj_out = mo.mavlink_object(mavlink_out) - obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_out.append(obj_out) - mavlink_sockets_out.append(mavlink_out) - - # 設定多台無人機的輸入連接 - drone_inputs = [ - "udp:127.0.0.1:14551", # 無人機3 - "udp:127.0.0.1:14552" # 無人機4 - ] - - # 建立輸入連接 - mavlink_objects_in = [] - mavlink_sockets_in = [] - - for i, input_conn in enumerate(drone_inputs): - print(f"連接無人機 {i+1} 輸入: {input_conn}") - mavlink_in = mavutil.mavlink_connection(input_conn) - obj_in = mo.mavlink_object(mavlink_in) - obj_in.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_in.append(obj_in) - mavlink_sockets_in.append(mavlink_in) - - # 設定無人機到所有GCS的轉發關係 - for obj_out in mavlink_objects_out: - obj_in.multiplexingToSwap[obj_out.socket_id] = [-1, ] # 無人機→所有GCS - print(f"設定無人機 {i+1} (socket_id: {obj_in.socket_id}) → GCS (socket_id: {obj_out.socket_id}) 轉發") - - # 設定GCS到所有無人機的轉發關係 - for i, obj_out in enumerate(mavlink_objects_out): - for j, obj_in in enumerate(mavlink_objects_in): - obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有無人機 - print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 無人機 {j+1} (socket_id: {obj_in.socket_id}) 轉發") - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - # 啟動所有輸入通道 - for obj in mavlink_objects_in: - obj.run() - - # 啟動所有輸出通道 - for obj in mavlink_objects_out: - obj.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(3) - print("=== connection established! ===") - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - # 顯示轉發設定摘要 - print("\n=== 轉發設定摘要 ===") - print(f"無人機數量: {len(mavlink_objects_in)}") - print(f"GCS數量: {len(mavlink_objects_out)}") - print("轉發規則:") - print(" - 每台無人機的所有訊息 → 所有GCS") - print(" - 每個GCS的所有訊息 → 所有無人機") - print("===================\n") - - 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) >= 5: # 改為每5秒顯示一次，更即時監控 - show_time = time.time() - print(f"\n=== 系統狀態報告 (運行時間: {int(time.time() - start_time)}秒) ===") - - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - msg_count = analyzer.mavlink_systems[sysid].components[compid].msg_count - print(f"系統ID: {sysid}, 元件ID: {compid}, 收到訊息數量: {msg_count}") - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - - # 顯示各通道的狀態 - print(f"輸入通道數量: {len(mavlink_objects_in)} (運行中)") - print(f"輸出通道數量: {len(mavlink_objects_out)} (運行中)") - print("===================\n") - - # 結束程式 退出所有 thread - print("正在關閉所有連接...") - - # 關閉輸入通道 - for i, obj in enumerate(mavlink_objects_in): - print(f"關閉無人機 {i+1} 輸入通道") - obj.stop() - obj.thread.join() - mavlink_sockets_in[i].close() - - # 關閉輸出通道 - for i, obj in enumerate(mavlink_objects_out): - print(f"關閉 GCS {i+1} 輸出通道") - obj.stop() - obj.thread.join() - mavlink_sockets_out[i].close() - - analyzer.stop() - print('<=== End of Program') - - elif test_item == 54: # 文鈞的測試項目 - 5輸入2輸出版本 + 結合test51的ROS2功能 # 加入詳細調試信息 @@ -915,19 +248,43 @@ elif test_item == 54: print("ROS2 bridge 初始化完成") + # 雙輸出連接設定 (連接到兩個不同的GCS) + gcs_outputs = [ + "udpout:127.0.0.1:14500", # GCS 1 + "udpout:127.0.0.1:14600" # GCS 2 + ] + + # 建立輸出連接物件 + mavlink_objects_out = [] + mavlink_sockets_out = [] + # 設定5個輸入連接（修改為實際測試可用的端口） device_inputs = [ - "udp:127.0.0.1:14551", # 無人機1 (UDP) - "udp:127.0.0.1:14552", # 無人機2 (UDP) - "udp:127.0.0.1:14553", # 無人機3 (UDP) - "udp:127.0.0.1:14554", # 無人機4 (UDP) - "udp:127.0.0.1:14555", # 無人機5 (UDP) + "udp:127.0.0.1:14550", # 無人機1 (UDP) + "udp:127.0.0.1:14570", # 無人機2 (UDP) + "/dev/ttyUSB0", # 無人機3 (UDP) + "/dev/ttyUSB1", # 無人機4 (UDP) + "/dev/ttyUSB2", # 無人機5 (UDP) ] # 建立輸入連接 mavlink_objects_in = [] mavlink_sockets_in = [] + for i, output_conn in enumerate(gcs_outputs): + print(f"建立 GCS {i+1} 輸出連接: {output_conn}") + mavlink_out = mavutil.mavlink_connection(output_conn) + obj_out = mo.mavlink_object(mavlink_out) + obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息 + mavlink_objects_out.append(obj_out) + mavlink_sockets_out.append(mavlink_out) + + # 設定GCS到所有設備的轉發關係 + for i, obj_out in enumerate(mavlink_objects_out): + for j, obj_in in enumerate(mavlink_objects_in): + obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有設備 + print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 設備 {j+1} (socket_id: {obj_in.socket_id}) 轉發") + for i, input_conn in enumerate(device_inputs): print(f"連接設備 {i+1} 輸入: {input_conn}") try: @@ -954,30 +311,6 @@ elif test_item == 54: print(f"跳過設備 {i+1}") continue - # 雙輸出連接設定 (連接到兩個不同的GCS) - gcs_outputs = [ - "udpout:127.0.0.1:14560", # GCS 1 - "udpout:127.0.0.1:14570" # GCS 2 - ] - - # 建立輸出連接物件 - mavlink_objects_out = [] - mavlink_sockets_out = [] - - for i, output_conn in enumerate(gcs_outputs): - print(f"建立 GCS {i+1} 輸出連接: {output_conn}") - mavlink_out = mavutil.mavlink_connection(output_conn) - obj_out = mo.mavlink_object(mavlink_out) - obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_out.append(obj_out) - mavlink_sockets_out.append(mavlink_out) - - # 設定GCS到所有設備的轉發關係 - for i, obj_out in enumerate(mavlink_objects_out): - for j, obj_in in enumerate(mavlink_objects_in): - obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有設備 - print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 設備 {j+1} (socket_id: {obj_in.socket_id}) 轉發") - # 做一個空的通道驗證 可以拿來 debug mavlink_object_none = mo.mavlink_object(None) @@ -1056,7 +389,6 @@ elif test_item == 54: try: while not mo.fixed_stream_bridge_queue.empty(): msg = mo.fixed_stream_bridge_queue.get(block=False) - print(f'[DEBUG] fixed_stream_bridge_queue: from socket {msg[0]}: {msg[2].get_type()}') debug_counters['analysis_messages'] += 1 message_count += 1 except queue.Empty: @@ -1066,7 +398,6 @@ elif test_item == 54: try: while not mo.return_packet_processor_queue.empty(): msg = mo.return_packet_processor_queue.get(block=False) - print(f'[DEBUG] return_packet_processor_queue: from socket {msg[0]}: {msg[2].get_type()}') debug_counters['return_messages'] += 1 message_count += 1 except queue.Empty: @@ -1083,8 +414,6 @@ elif test_item == 54: except Exception as e: print(f"發送 TIMESYNC 到設備 {i+1} 失敗: {e}") - if timesync_sent > 0: - print(f"[DEBUG] 發送 TIMESYNC 到 {timesync_sent} 個設備") last_timesync = now # ROS2 發布 (來自test51新版本) @@ -1098,10 +427,9 @@ elif test_item == 54: # 狀態報告 (更頻繁的調試輸出) if (time.time() - show_time) >= 2: # 每2秒顯示一次狀態 show_time = time.time() - print(f"\n=== 調試狀態報告 (運行時間: {int(now - time.time() + running_time)}秒) ===") + print(f"\n=== 調試狀態報告===") # 顯示消息統計 - print(f"消息計數器: {debug_counters}") print(f"總消息數: {message_count}, ROS2發布次數: {ros2_publish_count}") # 重置調試計數器 @@ -1115,14 +443,7 @@ elif test_item == 54: for compid in system.components: component = system.components[compid] msg_count = component.msg_count - print(f" 元件 {compid}: 收到訊息數量={msg_count}") - # 顯示 emitParams 內容 - if hasattr(component, 'emitParams') and component.emitParams: - print(f" emitParams keys: {list(component.emitParams.keys())}") - else: - print(" emitParams: 空") - system.resetComponentPacketCount(compid) else: print("目前沒有檢測到任何MAVLink系統") @@ -1184,4 +505,4 @@ elif test_item == 54: # 關閉分析器 bridge.stop() bridge.thread.join() - print('<=== End of Program') + print('<=== End of Program') \ No newline at end of file From 14b70f6e2e96b34c1b833fdde8229358866156b1 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 30 Dec 2025 17:25:06 +0800 Subject: [PATCH 21/33] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/communication.py | 511 +++++++++++ src/unitdev01/unitdev01/gui.py | 1022 ++++++++++++---------- 2 files changed, 1073 insertions(+), 460 deletions(-) create mode 100644 src/unitdev01/unitdev01/communication.py diff --git a/src/unitdev01/unitdev01/communication.py b/src/unitdev01/unitdev01/communication.py new file mode 100644 index 0000000..9584a62 --- /dev/null +++ b/src/unitdev01/unitdev01/communication.py @@ -0,0 +1,511 @@ +from rclpy.node import Node +from PyQt6.QtCore import QObject, pyqtSignal +import math +import re +import threading +from threading import Lock +import asyncio +import websockets +import json +import socket +from pymavlink import mavutil +from geometry_msgs.msg import Point, Vector3 +from sensor_msgs.msg import BatteryState, NavSatFix, Imu +from std_msgs.msg import Float64 +from mavros_msgs.msg import State, VfrHud +from mavros_msgs.srv import CommandBool, CommandTOL + +class DroneSignals(QObject): + update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) + +class UDPMavlinkReceiver(threading.Thread): + """UDP MAVLink 接收器""" + def __init__(self, ip, port, signals, connection_name): + super().__init__(daemon=True) + self.ip = ip + self.port = port + self.signals = signals + self.connection_name = connection_name + self.running = False + self.sock = None + + def run(self): + """執行 UDP 接收循環""" + self.running = True + try: + print(f"UDP MAVLink receiver started on {self.ip}:{self.port}") + + # 創建 MAVLink 連接 + mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}') + while self.running: + try: + msg = mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except socket.timeout: + continue + except Exception as e: + print(f"Error receiving MAVLink message: {e}") + + except Exception as e: + print(f"UDP receiver error: {e}") + finally: + if self.sock: + self.sock.close() + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class WebSocketMavlinkReceiver(threading.Thread): + """WebSocket MAVLink 接收器""" + def __init__(self, url, signals, connection_name): + super().__init__(daemon=True) + self.url = url + self.signals = signals + self.connection_name = connection_name + self.running = False + self.max_retries = 5 + self.base_delay = 1.0 + + def run(self): + """執行 WebSocket 接收循環""" + self.running = True + asyncio.set_event_loop(asyncio.new_event_loop()) + asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + + async def ws_client_loop(self): + """WebSocket 連接的主循環""" + retry_count = 0 + + print(f"Starting WebSocket client for {self.connection_name} at {self.url}") + + while self.running and retry_count < self.max_retries: + try: + async with websockets.connect(self.url) as websocket: + print(f"WebSocket {self.connection_name} connected to {self.url}") + retry_count = 0 # 重置重試計數 + + async for message in websocket: + if not self.running: + break + + try: + data = json.loads(message) + data['_connection_source'] = self.connection_name + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket {self.connection_name} JSON decode error: {e}") + except Exception as e: + print(f"WebSocket {self.connection_name} message processing error: {e}") + + except websockets.exceptions.ConnectionClosedError: + print(f"WebSocket {self.connection_name} connection closed") + if self.running: + retry_count += 1 + if retry_count < self.max_retries: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"Reconnecting in {delay}s...") + await asyncio.sleep(delay) + else: + break + except Exception as e: + retry_count += 1 + if retry_count < self.max_retries and self.running: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})") + await asyncio.sleep(delay) + else: + break + + print(f"WebSocket client {self.connection_name} stopped") + + def process_websocket_message(self, data): + """處理 WebSocket 訊息""" + try: + drone_id = data.get('system_id') + drone_id = f"s9_{drone_id}" if drone_id else None + if not drone_id: + return + + # 模式 + if 'mode' in data: + self.signals.update_signal.emit('state', drone_id, { + 'mode': data['mode'], + }) + + # 電池 + if 'battery' in data: + self.signals.update_signal.emit('battery', drone_id, { + 'percentage': data['battery'] + }) + + # 位置 + if 'position' in data: + pos = data['position'] + self.signals.update_signal.emit('gps', drone_id, { + 'lat': pos.get('lat', 0), + 'lon': pos.get('lon', 0) + }) + + # Local position - 設定 x, y 為 0.0 + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': 0.0, + 'y': 0.0, + 'z': 0.0 + }) + + # Altitude - 設定為 0.0 + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': 0.0 + }) + + # 航向 + if 'heading' in data: + self.signals.update_signal.emit('hud', drone_id, { + 'heading': data['heading'], + 'groundspeed': 0.0 + }) + + except Exception as e: + print(f"WebSocket message processing error: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class DroneMonitor(Node): + def __init__(self): + super().__init__('drone_monitor') + self.signals = DroneSignals() + self.drone_topics = {} + self.lock = Lock() + + self.arm_clients = {} + self.takeoff_clients = {} + self.setpoint_pubs = {} + self.selected_drones = set() + self.latest_data = {} + + # 定義需要過濾的模式 + self.filtered_modes = ['Mode(0x000000c0)'] + + # WebSocket 接收器列表 + self.ws_receivers = [] + + # 主题检测定时器 + self.create_timer(1.0, self.scan_topics) + + def scan_topics(self): + topics = self.get_topic_names_and_types() + drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') + + found_drones = set() + for topic_name, _ in topics: + if match := drone_pattern.match(topic_name): + drone_id, topic_type = match.groups() + found_drones.add(drone_id) + with self.lock: + self.drone_topics.setdefault(drone_id, set()).add(topic_type) + + for drone_id in found_drones: + if not hasattr(self, f'drone_{drone_id}_subs'): + self.setup_drone(drone_id) + + def setup_drone(self, drone_id): + base_topic = f'/MavLinkBus/{drone_id}' + + # Add service clients + self.arm_clients[drone_id] = self.create_client( + CommandBool, + f'{base_topic}/cmd/arming' + ) + self.takeoff_clients[drone_id] = self.create_client( + CommandTOL, + f'{base_topic}/cmd/takeoff' + ) + + # Add setpoint publisher + self.setpoint_pubs[drone_id] = self.create_publisher( + Point, + f'{base_topic}/setpoint_position/local', + 10 + ) + + subs = { + 'attitude': self.create_subscription( + Imu, + f'{base_topic}/attitude', + lambda msg, did=drone_id: self.attitude_callback(did, msg), + 10 + ), + 'battery': self.create_subscription( + BatteryState, + f'{base_topic}/battery', + lambda msg, did=drone_id: self.battery_callback(did, msg), + 10 + ), + 'global': self.create_subscription( + NavSatFix, + f'{base_topic}/global_position/global', + lambda msg, did=drone_id: self.gps_callback(did, msg), + 10 + ), + 'rel_alt': self.create_subscription( + Float64, + f'{base_topic}/global_position/rel_alt', + lambda msg, did=drone_id: self.altitude_callback(did, msg), + 10 + ), + 'local_pose': self.create_subscription( + Point, + f'{base_topic}/local_position/pose', + lambda msg, did=drone_id: self.local_pose_callback(did, msg), + 10 + ), + 'local_vel': self.create_subscription( + Vector3, + f'{base_topic}/local_position/velocity', + lambda msg, did=drone_id: self.local_vel_callback(did, msg), + 10 + ), + 'loss_rate': self.create_subscription( + Float64, + f'{base_topic}/packet_loss_rate', + lambda msg, did=drone_id: self.loss_rate_callback(did, msg), + 10 + ), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), + 10 + ), + 'ping': self.create_subscription( + Float64, + f'{base_topic}/ping', + lambda msg, did=drone_id: self.ping_callback(did, msg), + 10 + ), + 'vfr_hud': self.create_subscription( + VfrHud, + f'{base_topic}/vfr_hud', + lambda msg, did=drone_id: self.hud_callback(did, msg), + 10 + ) + } + + setattr(self, f'drone_{drone_id}_subs', subs) + + async def arm_drone(self, drone_id, arm): + if drone_id not in self.arm_clients: + return False + + client = self.arm_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandBool.Request() + request.value = arm + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Arm service call failed: {e}') + return False + + async def takeoff_drone(self, drone_id, altitude=10.0): + if drone_id not in self.takeoff_clients: + return False + + client = self.takeoff_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandTOL.Request() + request.altitude = altitude + request.min_pitch = 0.0 + request.yaw = 0.0 + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Takeoff service call failed: {e}') + return False + + def send_setpoint(self, drone_id, x, y, z): + """Send setpoint position command""" + if drone_id not in self.setpoint_pubs: + return False + + msg = Point() + msg.x = float(x) + msg.y = float(y) + msg.z = float(z) + + self.setpoint_pubs[drone_id].publish(msg) + return True + + def quaternion_to_euler(self, q): + sinr_cosp = 2 * (q.w * q.x + q.y * q.z) + cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) + roll = math.atan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (q.w * q.y - q.z * q.x) + pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp) + + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y**2 + q.z**2) + yaw = math.atan2(siny_cosp, cosy_cosp) + + return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) + + # callbacks + def attitude_callback(self, drone_id, msg): + if hasattr(msg, 'orientation'): + roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) + self.latest_data[(drone_id, 'attitude')] = { + 'roll': roll, + 'pitch': pitch, + 'yaw': yaw, + 'rates': (msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z) + } + + def battery_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'battery')] = { + 'voltage': msg.voltage + } + + def state_callback(self, drone_id, msg): + mode = msg.mode + if mode in self.filtered_modes: + return + self.latest_data[(drone_id, 'state')] = { + 'mode': msg.mode, + 'armed': msg.armed + } + + def gps_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'gps')] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + + def local_vel_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'velocity')] = { + 'vx': msg.x, + 'vy': msg.y, + 'vz': msg.z + } + + def altitude_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'altitude')] = { + 'altitude': msg.data + } + + def local_pose_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'local_pose')] = { + 'x': msg.x, + 'y': msg.y, + 'z': msg.z + } + + def hud_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'hud')] = { + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.altitude, + 'climb': msg.climb + } + + def loss_rate_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'loss_rate')] = { + 'loss_rate': msg.data + } + + def ping_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'ping')] = { + 'ping': msg.data + } \ No newline at end of file diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index a227321..5eac6a3 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -1,405 +1,16 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, QHeaderView, QPushButton, QCheckBox, QLineEdit) -from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal +from PyQt6.QtCore import Qt, QTimer from PyQt6.QtWebEngineWidgets import QWebEngineView -import math -import re import sys -from threading import Lock -from geometry_msgs.msg import Point, Vector3 -from sensor_msgs.msg import BatteryState, NavSatFix, Imu -from std_msgs.msg import Float64 -from mavros_msgs.msg import State, VfrHud -from mavros_msgs.srv import CommandBool, CommandTOL, SetMode import asyncio -from functools import partial -import threading -import websockets -import json -class DroneSignals(QObject): - update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) - -class DroneMonitor(Node): - def __init__(self): - super().__init__('drone_monitor') - self.signals = DroneSignals() - self.drone_topics = {} - self.lock = Lock() - - self.arm_clients = {} - self.takeoff_clients = {} - self.setpoint_pubs = {} - self.selected_drones = set() - self.latest_data = {} - - # 定義需要過濾的模式 - self.filtered_modes = ['Mode(0x000000c0)'] - - # 定義多個 WebSocket 連接配置 - self.websocket_connections = [ - { - 'name': 'local_1', - 'url': 'ws://192.168.137.1:5163', - 'enabled': True - }, - { - 'name': 'local_2', - 'url': 'ws://0.0.0.0:8756', - 'enabled': True # 新增的端口 - }, - { - 'name': 'remote_8756', - 'url': 'ws://192.168.50.48:8756', - 'enabled': False # 可選擇啟用 - } - ] - - # 啟動多個 WebSocket client 執行緒 - for connection in self.websocket_connections: - if connection['enabled']: - threading.Thread( - target=self.start_ws_client, - args=(connection,), - daemon=True, - name=f"WebSocket-{connection['name']}" - ).start() - - # 主题检测定时器 - self.create_timer(1.0, self.scan_topics) - - def start_ws_client(self, connection_config): - """啟動單個 WebSocket 客戶端""" - asyncio.set_event_loop(asyncio.new_event_loop()) - asyncio.get_event_loop().run_until_complete( - self.ws_client_loop(connection_config) - ) - - async def ws_client_loop(self, connection_config): - """單個 WebSocket 連接的主循環""" - retry_count = 0 - max_retries = 5 - base_delay = 1.0 - connection_name = connection_config['name'] - url = connection_config['url'] - - print(f"Starting WebSocket client for {connection_name} at {url}") - - while retry_count < max_retries: - try: - async with websockets.connect(url) as websocket: - print(f"WebSocket {connection_name} connected to {url}") - retry_count = 0 # 重置重試計數 - - async for message in websocket: - try: - data = json.loads(message) - # 添加連接來源信息 - data['_connection_source'] = connection_name - self.process_websocket_message(data) - except json.JSONDecodeError as e: - print(f"WebSocket {connection_name} JSON decode error: {e}") - except Exception as e: - print(f"WebSocket {connection_name} message processing error: {e}") - - except websockets.exceptions.ConnectionClosedError: - print(f"WebSocket {connection_name} connection closed") - break - except Exception as e: - retry_count += 1 - delay = base_delay * (2 ** min(retry_count, 4)) # 指數退避 - print(f"WebSocket {connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})") - await asyncio.sleep(delay) - - print(f"WebSocket client {connection_name} stopped after maximum retries") - - def process_websocket_message(self, data): - try: - drone_id = data.get('system_id') - drone_id = f"s9_{drone_id}" if drone_id else None - if not drone_id: - return - - # 模式 - if 'mode' in data: - self.signals.update_signal.emit('state', drone_id, { - 'mode': data['mode'], - }) - - # 電池 - if 'battery' in data: - # 這裡假設 battery 是百分比 - self.signals.update_signal.emit('battery', drone_id, { - 'percentage': data['battery'] - }) - - # 位置 - if 'position' in data: - pos = data['position'] - self.signals.update_signal.emit('gps', drone_id, { - 'lat': pos.get('lat', 0), - 'lon': pos.get('lon', 0) - }) - - self.signals.update_signal.emit('local_pose', drone_id, { - 'x': round(pos.get('lat', 0), 6), - 'y': round(pos.get('lon', 0), 6), - }) - - # 航向 - if 'heading' in data: - self.signals.update_signal.emit('hud', drone_id, { - 'heading': data['heading'], - }) - - except Exception as e: - print(f"WebSocket message processing error: {e}") - - def scan_topics(self): - topics = self.get_topic_names_and_types() - drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') - - found_drones = set() - for topic_name, _ in topics: - if match := drone_pattern.match(topic_name): - drone_id, topic_type = match.groups() - found_drones.add(drone_id) - with self.lock: - self.drone_topics.setdefault(drone_id, set()).add(topic_type) - - for drone_id in found_drones: - if not hasattr(self, f'drone_{drone_id}_subs'): - self.setup_drone(drone_id) - - def setup_drone(self, drone_id): - base_topic = f'/MavLinkBus/{drone_id}' - - # Add service clients - self.arm_clients[drone_id] = self.create_client( - CommandBool, - f'{base_topic}/cmd/arming' - ) - self.takeoff_clients[drone_id] = self.create_client( - CommandTOL, - f'{base_topic}/cmd/takeoff' - ) - - # Add setpoint publisher - self.setpoint_pubs[drone_id] = self.create_publisher( - Point, - f'{base_topic}/setpoint_position/local', - 10 - ) - - subs = { - 'attitude': self.create_subscription( - Imu, - f'{base_topic}/attitude', - lambda msg, did=drone_id: self.attitude_callback(did, msg), - 10 - ), - 'battery': self.create_subscription( - BatteryState, - f'{base_topic}/battery', - lambda msg, did=drone_id: self.battery_callback(did, msg), - 10 - ), - 'global': self.create_subscription( - NavSatFix, - f'{base_topic}/global_position/global', - lambda msg, did=drone_id: self.gps_callback(did, msg), - 10 - ), - 'rel_alt': self.create_subscription( - Float64, - f'{base_topic}/global_position/rel_alt', - lambda msg, did=drone_id: self.altitude_callback(did, msg), - 10 - ), - 'local_pose': self.create_subscription( - Point, - f'{base_topic}/local_position/pose', - lambda msg, did=drone_id: self.local_pose_callback(did, msg), - 10 - ), - 'local_vel': self.create_subscription( - Vector3, - f'{base_topic}/local_position/velocity', - lambda msg, did=drone_id: self.local_vel_callback(did, msg), - 10 - ), - 'loss_rate': self.create_subscription( - Float64, - f'{base_topic}/packet_loss_rate', - lambda msg, did=drone_id: self.loss_rate_callback(did, msg), - 10 - ), - 'state': self.create_subscription( - State, - f'{base_topic}/state', - lambda msg, did=drone_id: self.state_callback(did, msg), - 10 - ), - 'ping': self.create_subscription( - Float64, - f'{base_topic}/ping', - lambda msg, did=drone_id: self.ping_callback(did, msg), - 10 - ), - 'vfr_hud': self.create_subscription( - VfrHud, - f'{base_topic}/vfr_hud', - lambda msg, did=drone_id: self.hud_callback(did, msg), - 10 - ) - } - - setattr(self, f'drone_{drone_id}_subs', subs) - - async def arm_drone(self, drone_id, arm): - if drone_id not in self.arm_clients: - return False - - client = self.arm_clients[drone_id] - if not client.wait_for_service(timeout_sec=1.0): - return False - - request = CommandBool.Request() - request.value = arm - - future = client.call_async(request) - try: - response = await future - return response.success - except Exception as e: - self.get_logger().error(f'Arm service call failed: {e}') - return False - - async def takeoff_drone(self, drone_id, altitude=10.0): - if drone_id not in self.takeoff_clients: - return False - - client = self.takeoff_clients[drone_id] - if not client.wait_for_service(timeout_sec=1.0): - return False - - request = CommandTOL.Request() - request.altitude = altitude - request.min_pitch = 0.0 - request.yaw = 0.0 - - future = client.call_async(request) - try: - response = await future - return response.success - except Exception as e: - self.get_logger().error(f'Takeoff service call failed: {e}') - return False - - def send_setpoint(self, drone_id, x, y, z): - """Send setpoint position command""" - if drone_id not in self.setpoint_pubs: - return False - - msg = Point() - msg.x = float(x) - msg.y = float(y) - msg.z = float(z) - - self.setpoint_pubs[drone_id].publish(msg) - return True - - def quaternion_to_euler(self, q): - sinr_cosp = 2 * (q.w * q.x + q.y * q.z) - cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) - roll = math.atan2(sinr_cosp, cosr_cosp) - - sinp = 2 * (q.w * q.y - q.z * q.x) - pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp) - - siny_cosp = 2 * (q.w * q.z + q.x * q.y) - cosy_cosp = 1 - 2 * (q.y**2 + q.z**2) - yaw = math.atan2(siny_cosp, cosy_cosp) - - return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) - - # callbacks - def attitude_callback(self, drone_id, msg): - if hasattr(msg, 'orientation'): - roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) - self.latest_data[(drone_id, 'attitude')] = { - 'roll': roll, - 'pitch': pitch, - 'yaw': yaw, - 'rates': (msg.angular_velocity.x, - msg.angular_velocity.y, - msg.angular_velocity.z) - } - - def battery_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'battery')] = { - 'voltage': msg.voltage - } - - def state_callback(self, drone_id, msg): - mode = msg.mode - if mode in self.filtered_modes: - return - self.latest_data[(drone_id, 'state')] = { - 'mode': msg.mode, - 'armed': msg.armed - } - - def gps_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'gps')] = { - 'lat': msg.latitude, - 'lon': msg.longitude, - 'alt': msg.altitude - } - - def local_vel_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'velocity')] = { - 'vx': msg.x, - 'vy': msg.y, - 'vz': msg.z - } - - def altitude_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'altitude')] = { - 'altitude': msg.data - } - - def local_pose_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'local_pose')] = { - 'x': msg.x, - 'y': msg.y, - 'z': msg.z - } - - def hud_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'hud')] = { - 'airspeed': msg.airspeed, - 'groundspeed': msg.groundspeed, - 'heading': msg.heading, - 'throttle': msg.throttle, - 'alt': msg.altitude, - 'climb': msg.climb - } - - def loss_rate_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'loss_rate')] = { - 'loss_rate': msg.data - } - - def ping_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'ping')] = { - 'ping': msg.data - } +# 從 communication.py 導入分離的類別 +from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver class ControlStationUI(QMainWindow): def __init__(self): @@ -423,7 +34,7 @@ class ControlStationUI(QMainWindow): # 初始化UI self.drones = {} - self.socket_groups = {} # 新增：用於儲存 socket 分組 + self.socket_groups = {} self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向", "Roll", "Pitch", "Yaw", "丟包", "延遲"] self.info_type_map = { @@ -442,27 +53,29 @@ class ControlStationUI(QMainWindow): "ping": 12 } self.socket_colors = { - '0': '#00BFFF', # DeepSkyBlue - '1': '#FFD700', # Gold - '2': '#FF69B4', # HotPink - '9': '#7CFC00', # LawnGreen + '0': '#00BFFF', + '1': '#FFD700', + '2': "#FF6969", + '8': '#7CFC00', + '9': '#FF8C00', 'default': '#AAAAAA' } self.drone_positions = {} self.drone_headings = {} self.map_loaded = False - # 添加地圖更新節流定時器 self.map_update_timer = QTimer() self.map_update_timer.timeout.connect(self.update_map_positions) - self.map_update_timer.start(200) # 每 200ms 更新一次地圖 - # 添加待更新的無人機位置緩存 + self.map_update_timer.start(200) self.pending_map_updates = {} - self.init_ui() + # 初始化連接列表 + self.udp_receivers = [] + self.udp_connections = [] + self.ws_connections = [] + self.init_ui() def init_ui(self): - # 只呼叫一次 main_splitter = QSplitter(Qt.Orientation.Horizontal) @@ -482,17 +95,193 @@ class ControlStationUI(QMainWindow): scroll.setWidgetResizable(True) self.left_tab.addTab(scroll, "無人載具") - # 底部控制按鈕區域 - bottom_control = QWidget() - bottom_layout = QVBoxLayout(bottom_control) + # — 分頁 2：Overview Table + self.overview_table = QTableWidget() + self.overview_table.setColumnCount(1) + self.overview_table.setRowCount(len(self.info_types)) + self.overview_table.setHorizontalHeaderLabels(["資訊"]) + header = self.overview_table.horizontalHeader() + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.overview_table.verticalHeader().setVisible(False) + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(i, 0, item) + + self.left_tab.addTab(self.overview_table, "總覽") + + # — 分頁 3：通訊設定 + self.comm_tab = QWidget() + comm_layout = QVBoxLayout(self.comm_tab) + comm_layout.setContentsMargins(10, 10, 10, 10) + comm_layout.setSpacing(10) + + # ========== UDP MAVLink 區域 ========== + udp_title = QLabel("UDP MAVLink") + udp_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + comm_layout.addWidget(udp_title) + + # UDP 連接列表容器 + self.udp_list_widget = QWidget() + self.udp_list_layout = QVBoxLayout(self.udp_list_widget) + self.udp_list_layout.setContentsMargins(0, 0, 0, 0) + self.udp_list_layout.setSpacing(5) + comm_layout.addWidget(self.udp_list_widget) + + # UDP 添加新連接區域 + add_udp_widget = QWidget() + add_udp_layout = QHBoxLayout(add_udp_widget) + add_udp_layout.setContentsMargins(0, 0, 0, 0) + + self.udp_ip_input = QLineEdit() + self.udp_ip_input.setText("127.0.0.1") + self.udp_ip_input.setPlaceholderText("IP") + self.udp_ip_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + self.udp_port_input = QLineEdit() + self.udp_port_input.setText("14540") + self.udp_port_input.setPlaceholderText("Port") + self.udp_port_input.setFixedWidth(80) + self.udp_port_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_udp_btn = QPushButton("添加") + add_udp_btn.clicked.connect(self.handle_add_udp_connection) + add_udp_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_ip_input) + add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_port_input) + add_udp_layout.addWidget(add_udp_btn) + + comm_layout.addWidget(add_udp_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + comm_layout.addWidget(separator) + + # ========== WebSocket 區域 ========== + ws_title = QLabel("WebSocket") + ws_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + comm_layout.addWidget(ws_title) + + # WebSocket 連接列表容器 + self.ws_list_widget = QWidget() + self.ws_list_layout = QVBoxLayout(self.ws_list_widget) + self.ws_list_layout.setContentsMargins(0, 0, 0, 0) + self.ws_list_layout.setSpacing(5) + comm_layout.addWidget(self.ws_list_widget) + + # WebSocket 添加新連接區域 + add_ws_widget = QWidget() + add_ws_layout = QHBoxLayout(add_ws_widget) + add_ws_layout.setContentsMargins(0, 0, 0, 0) + + self.ws_url_input = QLineEdit() + self.ws_url_input.setPlaceholderText("host") + self.ws_url_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_ws_btn = QPushButton("添加") + add_ws_btn.clicked.connect(self.handle_add_ws_connection) + add_ws_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) + add_ws_layout.addWidget(self.ws_url_input) + add_ws_layout.addWidget(add_ws_btn) + + comm_layout.addWidget(add_ws_widget) + comm_layout.addStretch() + + self.left_tab.addTab(self.comm_tab, "通訊") + + # 右侧容器 + right_container = QWidget() + right_layout = QVBoxLayout(right_container) + right_layout.setContentsMargins(10, 10, 10, 10) + right_layout.setSpacing(10) + # ========== 批次控制區域（直接使用 layout） ========== + batch_control_layout = QHBoxLayout() + + # 添加批次操作標題 + batch_title = QLabel("批次操作") + batch_title.setStyleSheet(""" + color: #DDD; + font-size: 16px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + batch_control_layout.addWidget(batch_title) + # 上方按鈕區域 upper_buttons = QHBoxLayout() select_all_btn = QPushButton("全選") select_all_btn.clicked.connect(self.handle_select_all) - arm_all_btn = QPushButton("批次解鎖") + arm_all_btn = QPushButton("解鎖") arm_all_btn.clicked.connect(self.handle_arm_selected) - takeoff_all_btn = QPushButton("批次起飛") + takeoff_all_btn = QPushButton("起飛") takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]: btn.setStyleSheet(""" @@ -500,7 +289,7 @@ class ControlStationUI(QMainWindow): background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } @@ -509,29 +298,27 @@ class ControlStationUI(QMainWindow): upper_buttons.addWidget(btn) upper_buttons.addStretch() - # --- 模式切換區域 --- + # 模式切換區域 mode_layout = QHBoxLayout() - mode_layout.setContentsMargins(0, 0, 0, 0) - mode_layout.setSpacing(8) mode_label = QLabel("模式：") mode_label.setStyleSheet("color: #DDD; min-width: 40px;") from PyQt6.QtWidgets import QComboBox self.mode_combo = QComboBox() self.mode_combo.addItems(["AUTO", "GUIDED", "LOITER", "LAND"]) - self.mode_combo.setCurrentIndex(1) # 預設 GUIDED + self.mode_combo.setCurrentIndex(1) self.mode_combo.setStyleSheet(""" QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} """) - batch_mode_btn = QPushButton("批次切換模式") + batch_mode_btn = QPushButton("切換") batch_mode_btn.clicked.connect(self.handle_batch_mode_change) batch_mode_btn.setStyleSheet(""" QPushButton { background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } @@ -542,10 +329,9 @@ class ControlStationUI(QMainWindow): mode_layout.addWidget(batch_mode_btn) mode_layout.addStretch() - # Add coordinate inputs + # 座標輸入區域 coord_widget = QWidget() coord_layout = QHBoxLayout(coord_widget) - coord_layout.setContentsMargins(0, 0, 0, 0) self.x_input = QLineEdit() self.y_input = QLineEdit() @@ -563,14 +349,13 @@ class ControlStationUI(QMainWindow): } """) - coord_layout.addWidget(QLabel("X:")) + coord_layout.addWidget(QLabel("X:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.x_input) - coord_layout.addWidget(QLabel("Y:")) + coord_layout.addWidget(QLabel("Y:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.y_input) - coord_layout.addWidget(QLabel("Z:")) + coord_layout.addWidget(QLabel("Z:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.z_input) - # Add buttons to control layout setpoint_btn = QPushButton("Setpoint") setpoint_btn.clicked.connect(self.handle_setpoint_selected) setpoint_btn.setStyleSheet(""" @@ -578,44 +363,27 @@ class ControlStationUI(QMainWindow): background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } QPushButton:hover { background-color: #555; } """) - # 下方座標輸入區域 lower_control = QHBoxLayout() lower_control.addWidget(coord_widget) lower_control.addWidget(setpoint_btn) lower_control.addStretch() - - # 加入底部控制區 - bottom_layout.addLayout(upper_buttons) - bottom_layout.addLayout(mode_layout) - bottom_layout.addLayout(lower_control) - - # — 分頁 2：Overview Table - self.overview_table = QTableWidget() - # 一開始只有一欄 - self.overview_table.setColumnCount(1) - self.overview_table.setRowCount(len(self.info_types)) - self.overview_table.setHorizontalHeaderLabels(["資訊"]) - header = self.overview_table.horizontalHeader() - # 只有第一欄自動收縮到內容寬度，其它欄不動 - header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) - self.overview_table.verticalHeader().setVisible(False) - for i, txt in enumerate(self.info_types): - item = QTableWidgetItem(txt) - item.setFlags(Qt.ItemFlag.ItemIsEnabled) - item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) - self.overview_table.setItem(i, 0, item) - - self.left_tab.addTab(self.overview_table, "總覽") + # 組裝批次控制 layout + batch_control_layout.addLayout(upper_buttons) + batch_control_layout.addLayout(mode_layout) + batch_control_layout.addLayout(lower_control) - # 右侧地图区域 + # 將批次控制 layout 添加到右側容器 + right_layout.addLayout(batch_control_layout) + + # 添加地圖 self.map_view = QWebEngineView() inline_html = ''' @@ -671,8 +439,9 @@ class ControlStationUI(QMainWindow): function getColorBySocketId(id) { if (id.startsWith("s0_")) return "#00BFFF"; if (id.startsWith("s1_")) return "#FFD700"; - if (id.startsWith("s2_")) return "#FF69B4"; - if (id.startsWith("s9_")) return "#7CFC00"; + if (id.startsWith("s2_")) return "#FF6969"; + if (id.startsWith("s8_")) return "#7CFC00"; + if (id.startsWith("s9_")) return "#FF8C00"; return "#AAAAAA"; } @@ -803,23 +572,15 @@ class ControlStationUI(QMainWindow): ''' self.map_view.setHtml(inline_html) self.map_view.loadFinished.connect(self.on_map_loaded) - - # Create left container and its layout - left_container = QWidget() - left_layout = QVBoxLayout(left_container) - left_layout.setContentsMargins(0, 0, 0, 0) - - # Add tab widget and bottom control to left container - left_layout.addWidget(self.left_tab) - left_layout.addWidget(bottom_control) + right_layout.addWidget(self.map_view) + # Add widgets to splitter - main_splitter.addWidget(left_container) - main_splitter.addWidget(self.map_view) + main_splitter.addWidget(self.left_tab) + main_splitter.addWidget(right_container) main_splitter.setSizes([400, 1000]) self.setCentralWidget(main_splitter) - def on_map_loaded(self, ok: bool): if ok: self.map_loaded = True @@ -1079,6 +840,339 @@ class ControlStationUI(QMainWindow): main_layout.addWidget(control_widget) return panel + + def create_udp_connection_panel(self, conn): + """創建 UDP 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.toggle_udp_connection(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.remove_udp_connection(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def create_ws_connection_panel(self, conn): + """創建 WebSocket 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['url']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.toggle_ws_connection(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.remove_ws_connection(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def handle_add_ws_connection(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.statusBar().showMessage("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴（如果用戶沒有輸入協議前綴） + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + # 檢查是否包含 host:port 格式 + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.statusBar().showMessage("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.statusBar().showMessage("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.statusBar().showMessage("連接已存在", 3000) + return + + # 創建新連接 + new_conn = { + 'name': f'WS {len(self.ws_connections) + 1}', + 'url': url, + 'enabled': True + } + + # 啟動接收器 + receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.ws_connections.append(new_conn) + + # 添加到 UI + conn_panel = self.create_ws_connection_panel(new_conn) + self.ws_list_layout.addWidget(conn_panel) + + # 清空輸入框 + self.ws_url_input.clear() + + self.statusBar().showMessage(f"已添加 WebSocket 連接: {url}", 3000) + print(f"WebSocket receiver added and started: {url}") + + def toggle_ws_connection(self, conn, btn, status_label): + """切換 WebSocket 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000) + else: + # 啟動連接 + receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) + + def remove_ws_connection(self, conn, panel): + """移除 WebSocket 連接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.monitor.ws_receivers: + self.monitor.ws_receivers.remove(conn['receiver']) + + # 從列表移除 + if conn in self.ws_connections: + self.ws_connections.remove(conn) + + # 從 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 WebSocket 連接: {conn['url']}", 3000) + + def handle_add_udp_connection(self): + """處理添加 UDP 連接""" + ip = self.udp_ip_input.text().strip() + port_text = self.udp_port_input.text().strip() + + if not ip or not port_text: + self.statusBar().showMessage("請輸入 IP 和 Port", 3000) + return + + try: + port = int(port_text) + if port < 1 or port > 65535: + raise ValueError("Port 超出範圍") + except ValueError: + self.statusBar().showMessage("Port 必須是 1-65535 的數字", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.udp_connections: + if conn['ip'] == ip and conn['port'] == port: + self.statusBar().showMessage("連接已存在", 3000) + return + + # 創建新連接 + new_conn = { + 'name': f'UDP {len(self.udp_connections) + 1}', + 'ip': ip, + 'port': port, + 'enabled': True + } + + # 啟動接收器 + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.udp_connections.append(new_conn) + + # 添加到 UI + conn_panel = self.create_udp_connection_panel(new_conn) + self.udp_list_layout.addWidget(conn_panel) + + # 清空輸入框 + self.udp_ip_input.clear() + self.udp_port_input.clear() + + self.statusBar().showMessage(f"已添加 UDP 連接: {ip}:{port}", 3000) + print(f"UDP MAVLink receiver added and started on {ip}:{port}") + + def toggle_udp_connection(self, conn, btn, status_label): + """切換 UDP 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + else: + # 啟動連接 + receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + + def remove_udp_connection(self, conn, panel): + """移除 UDP 連接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.udp_receivers: + self.udp_receivers.remove(conn['receiver']) + + # 從列表移除 + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + # 從 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 UDP 連接: {conn['ip']}:{conn['port']}", 3000) def create_socket_group_panel(self, socket_id): """創建 socket 分組面板""" @@ -1593,6 +1687,14 @@ class ControlStationUI(QMainWindow): print(f"ROS spin error: {e}") def closeEvent(self, event): + # 停止 UDP 接收器 + for receiver in self.udp_receivers: + receiver.stop() + + # 停止 WebSocket 接收器 + for receiver in self.monitor.ws_receivers: + receiver.stop() + self.monitor.destroy_node() self.executor.shutdown() rclpy.shutdown() From 00eb6b512d052f849035b2868e842583c1bd9ece Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 8 Jan 2026 17:14:01 +0800 Subject: [PATCH 22/33] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/comm_panel.py | 398 +++++++++ src/unitdev01/unitdev01/communication.py | 137 ++- src/unitdev01/unitdev01/drone_panel.py | 378 ++++++++ src/unitdev01/unitdev01/gui.py | 1043 ++-------------------- src/unitdev01/unitdev01/map_layout.py | 292 ++++++ 5 files changed, 1303 insertions(+), 945 deletions(-) create mode 100644 src/unitdev01/unitdev01/comm_panel.py create mode 100644 src/unitdev01/unitdev01/drone_panel.py create mode 100644 src/unitdev01/unitdev01/map_layout.py diff --git a/src/unitdev01/unitdev01/comm_panel.py b/src/unitdev01/unitdev01/comm_panel.py new file mode 100644 index 0000000..6259c8d --- /dev/null +++ b/src/unitdev01/unitdev01/comm_panel.py @@ -0,0 +1,398 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QLineEdit) +from PyQt6.QtCore import pyqtSignal + +class CommPanel(QWidget): + """通讯设置面板类""" + + # 定义信号 + udp_connection_added = pyqtSignal(str, int) # ip, port + udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + ws_connection_added = pyqtSignal(str) # url + ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + status_message = pyqtSignal(str, int) # message, timeout + + def __init__(self, parent=None): + super().__init__(parent) + self.udp_connections = [] + self.ws_connections = [] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(10) + + # ========== UDP MAVLink 區域 ========== + udp_title = QLabel("UDP") + udp_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(udp_title) + + # UDP 連接列表容器 + self.udp_list_widget = QWidget() + self.udp_list_layout = QVBoxLayout(self.udp_list_widget) + self.udp_list_layout.setContentsMargins(0, 0, 0, 0) + self.udp_list_layout.setSpacing(5) + layout.addWidget(self.udp_list_widget) + + # UDP 添加新連接區域 + add_udp_widget = QWidget() + add_udp_layout = QHBoxLayout(add_udp_widget) + add_udp_layout.setContentsMargins(0, 0, 0, 0) + + self.udp_ip_input = QLineEdit() + self.udp_ip_input.setText("127.0.0.1") + self.udp_ip_input.setPlaceholderText("IP") + self.udp_ip_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + self.udp_port_input = QLineEdit() + self.udp_port_input.setText("14540") + self.udp_port_input.setPlaceholderText("Port") + self.udp_port_input.setFixedWidth(80) + self.udp_port_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_udp_btn = QPushButton("添加") + add_udp_btn.clicked.connect(self._handle_add_udp) + add_udp_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_ip_input) + add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_port_input) + add_udp_layout.addWidget(add_udp_btn) + + layout.addWidget(add_udp_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + layout.addWidget(separator) + + # ========== WebSocket 區域 ========== + ws_title = QLabel("WebSocket") + ws_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(ws_title) + + # WebSocket 連接列表容器 + self.ws_list_widget = QWidget() + self.ws_list_layout = QVBoxLayout(self.ws_list_widget) + self.ws_list_layout.setContentsMargins(0, 0, 0, 0) + self.ws_list_layout.setSpacing(5) + layout.addWidget(self.ws_list_widget) + + # WebSocket 添加新連接區域 + add_ws_widget = QWidget() + add_ws_layout = QHBoxLayout(add_ws_widget) + add_ws_layout.setContentsMargins(0, 0, 0, 0) + + self.ws_url_input = QLineEdit() + self.ws_url_input.setPlaceholderText("host") + self.ws_url_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_ws_btn = QPushButton("添加") + add_ws_btn.clicked.connect(self._handle_add_ws) + add_ws_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) + add_ws_layout.addWidget(self.ws_url_input) + add_ws_layout.addWidget(add_ws_btn) + + layout.addWidget(add_ws_widget) + layout.addStretch() + + def _handle_add_udp(self): + """處理添加 UDP 連接""" + ip = self.udp_ip_input.text().strip() + port_text = self.udp_port_input.text().strip() + + if not ip or not port_text: + self.status_message.emit("請輸入 IP 和 Port", 3000) + return + + try: + port = int(port_text) + if port < 1 or port > 65535: + raise ValueError("Port 超出範圍") + except ValueError: + self.status_message.emit("Port 必須是 1-65535 的數字", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.udp_connections: + if conn['ip'] == ip and conn['port'] == port: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.udp_connection_added.emit(ip, port) + + # 清空輸入框 + self.udp_ip_input.clear() + self.udp_port_input.clear() + + def _handle_add_ws(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.status_message.emit("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴 + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.status_message.emit("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.ws_connection_added.emit(url) + + # 清空輸入框 + self.ws_url_input.clear() + + def create_udp_connection_panel(self, conn): + """創建 UDP 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def create_ws_connection_panel(self, conn): + """創建 WebSocket 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['url']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def add_udp_panel(self, conn): + """添加 UDP 連接面板到列表""" + panel = self.create_udp_connection_panel(conn) + self.udp_list_layout.addWidget(panel) + self.udp_connections.append(conn) + return panel + + def add_ws_panel(self, conn): + """添加 WebSocket 連接面板到列表""" + panel = self.create_ws_connection_panel(conn) + self.ws_list_layout.addWidget(panel) + self.ws_connections.append(conn) + return panel + + def remove_udp_connection_from_list(self, conn): + """從列表中移除 UDP 連接""" + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + def remove_ws_connection_from_list(self, conn): + """從列表中移除 WebSocket 連接""" + if conn in self.ws_connections: + self.ws_connections.remove(conn) \ No newline at end of file diff --git a/src/unitdev01/unitdev01/communication.py b/src/unitdev01/unitdev01/communication.py index 9584a62..2db56c1 100644 --- a/src/unitdev01/unitdev01/communication.py +++ b/src/unitdev01/unitdev01/communication.py @@ -130,6 +130,129 @@ class UDPMavlinkReceiver(threading.Thread): """停止接收器""" self.running = False +class SerialMavlinkReceiver(threading.Thread): + """串口 MAVLink 接收器""" + def __init__(self, port, baudrate, signals, connection_name): + super().__init__(daemon=True) + self.port = port + self.baudrate = baudrate + self.signals = signals + self.connection_name = connection_name + self.running = False + self.mav = None + + def run(self): + """執行串口接收循環""" + self.running = True + try: + print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud") + + # 創建 MAVLink 串口連接 + self.mav = mavutil.mavlink_connection( + self.port, + baud=self.baudrate, + source_system=255 + ) + + print(f"Waiting for heartbeat from {self.port}...") + self.mav.wait_heartbeat() + print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}") + + while self.running: + try: + msg = self.mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except Exception as e: + if self.running: + print(f"Error receiving MAVLink message from serial: {e}") + + except Exception as e: + print(f"Serial receiver error: {e}") + finally: + if self.mav: + try: + self.mav.close() + except: + pass + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message from serial: {e}") + + def stop(self): + """停止接收器""" + self.running = False + class WebSocketMavlinkReceiver(threading.Thread): """WebSocket MAVLink 接收器""" def __init__(self, url, signals, connection_name): @@ -266,6 +389,9 @@ class DroneMonitor(Node): # WebSocket 接收器列表 self.ws_receivers = [] + # 串口接收器列表 + self.serial_receivers = [] + # 主题检测定时器 self.create_timer(1.0, self.scan_topics) @@ -508,4 +634,13 @@ class DroneMonitor(Node): def ping_callback(self, drone_id, msg): self.latest_data[(drone_id, 'ping')] = { 'ping': msg.data - } \ No newline at end of file + } + + def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): + """啟動串口 MAVLink 連接""" + connection_name = f"Serial_{port.replace('/', '_')}" + receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name) + receiver.start() + self.serial_receivers.append(receiver) + print(f"Started serial connection on {port} at {baudrate} baud") + return receiver \ No newline at end of file diff --git a/src/unitdev01/unitdev01/drone_panel.py b/src/unitdev01/unitdev01/drone_panel.py new file mode 100644 index 0000000..a5a7bf5 --- /dev/null +++ b/src/unitdev01/unitdev01/drone_panel.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QSizePolicy, QCheckBox) +from PyQt6.QtCore import pyqtSignal + +class DronePanel(QWidget): + """單個無人機面板類別""" + + # 定義信號 + mode_change_requested = pyqtSignal(str) # drone_id + arm_requested = pyqtSignal(str) # drone_id + takeoff_requested = pyqtSignal(str) # drone_id + setpoint_requested = pyqtSignal(str) # drone_id + selection_changed = pyqtSignal(str, int) # drone_id, state + + def __init__(self, drone_id, parent=None): + super().__init__(parent) + self.drone_id = drone_id + self.display_id = 's' + drone_id.split('_')[1] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"panel_{self.drone_id}") + self.setFixedHeight(140) + self.setStyleSheet(""" + background-color: #2A2A2A; + border-radius: 8px; + """) + + # 主佈局 + main_layout = QHBoxLayout(self) + main_layout.setContentsMargins(8, 8, 8, 8) + main_layout.setSpacing(0) + + # 創建內容容器（包含 info 和 control） + content_widget = QWidget() + content_widget.setStyleSheet("background-color: #333; border-radius: 6px;") + content_layout = QHBoxLayout(content_widget) + content_layout.setContentsMargins(8, 8, 8, 8) + content_layout.setSpacing(8) + + # 左側資訊區域 + info_widget = self._create_info_section() + + # 右側控制按鈕區域 + control_widget = self._create_control_section() + + # 將 info 和 control 加入內容容器 + content_layout.addWidget(info_widget) + content_layout.addWidget(control_widget) + + # 將內容容器加入主佈局 + main_layout.addWidget(content_widget) + + def _create_info_section(self): + """創建資訊顯示區域""" + info_widget = QWidget() + info_layout = QVBoxLayout(info_widget) + info_layout.setContentsMargins(0, 0, 0, 0) + info_layout.setSpacing(4) + + # 頂部標題欄 + header = QWidget() + header_layout = QHBoxLayout(header) + header_layout.setContentsMargins(0, 0, 0, 0) + + # 勾選框 + self.checkbox = QCheckBox() + self.checkbox.setObjectName(f"{self.drone_id}_checkbox") + self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }") + self.checkbox.stateChanged.connect( + lambda state: self.selection_changed.emit(self.drone_id, state) + ) + + # ID 顯示 + id_label = QLabel(self.display_id) + id_label.setStyleSheet(""" + font-weight: bold; + font-size: 14px; + color: #7FFFD4; + min-width: 80px; + """) + + header_layout.addWidget(self.checkbox) + header_layout.addWidget(id_label) + header_layout.addStretch() + + info_layout.addWidget(header) + + # 第一行：狀態 (模式 + ARM狀態) + status_row = self._create_status_row() + info_layout.addWidget(status_row) + + # 第二行：電池 + battery_row = self._create_battery_row() + info_layout.addWidget(battery_row) + + # 第三行：位置 + 高度 + position_row = self._create_position_row() + info_layout.addWidget(position_row) + + # 第四行：航向 + 速度 + nav_row = self._create_nav_row() + info_layout.addWidget(nav_row) + + return info_widget + + def _create_status_row(self): + """創建狀態行""" + status_row = QWidget() + status_layout = QHBoxLayout(status_row) + status_layout.setContentsMargins(0, 0, 0, 0) + + status_title = QLabel("狀態：") + status_title.setStyleSheet("color: #888; min-width: 50px;") + + self.mode_label = QLabel("--") + self.mode_label.setObjectName(f"{self.drone_id}_mode") + self.mode_label.setStyleSheet("color: #DDD;") + + self.armed_label = QLabel("--") + self.armed_label.setObjectName(f"{self.drone_id}_armed") + self.armed_label.setStyleSheet("color: #DDD;") + + status_layout.addWidget(status_title) + status_layout.addWidget(self.mode_label) + status_layout.addWidget(self.armed_label) + status_layout.addStretch() + + return status_row + + def _create_battery_row(self): + """創建電池行""" + battery_row = QWidget() + battery_layout = QHBoxLayout(battery_row) + battery_layout.setContentsMargins(0, 0, 0, 0) + + battery_title = QLabel("電池：") + battery_title.setStyleSheet("color: #888; min-width: 50px;") + + self.battery_label = QLabel("--") + self.battery_label.setObjectName(f"{self.drone_id}_battery") + self.battery_label.setStyleSheet("color: #DDD;") + + battery_layout.addWidget(battery_title) + battery_layout.addWidget(self.battery_label) + battery_layout.addStretch() + + return battery_row + + def _create_position_row(self): + """創建位置行""" + position_row = QWidget() + position_layout = QHBoxLayout(position_row) + position_layout.setContentsMargins(0, 0, 0, 0) + + position_title = QLabel("位置：") + position_title.setStyleSheet("color: #888; min-width: 50px;") + + self.local_label = QLabel("--") + self.local_label.setObjectName(f"{self.drone_id}_local") + self.local_label.setStyleSheet("color: #DDD;") + + altitude_title = QLabel("高度：") + altitude_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.altitude_label = QLabel("--") + self.altitude_label.setObjectName(f"{self.drone_id}_altitude") + self.altitude_label.setStyleSheet("color: #DDD;") + + position_layout.addWidget(position_title) + position_layout.addWidget(self.local_label) + position_layout.addWidget(altitude_title) + position_layout.addWidget(self.altitude_label) + position_layout.addStretch() + + return position_row + + def _create_nav_row(self): + """創建導航行""" + nav_row = QWidget() + nav_layout = QHBoxLayout(nav_row) + nav_layout.setContentsMargins(0, 0, 0, 0) + + heading_title = QLabel("航向：") + heading_title.setStyleSheet("color: #888; min-width: 50px;") + + self.heading_label = QLabel("--") + self.heading_label.setObjectName(f"{self.drone_id}_heading") + self.heading_label.setStyleSheet("color: #DDD;") + + speed_title = QLabel("速度：") + speed_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.groundspeed_label = QLabel("--") + self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed") + self.groundspeed_label.setStyleSheet("color: #DDD;") + + nav_layout.addWidget(heading_title) + nav_layout.addWidget(self.heading_label) + nav_layout.addWidget(speed_title) + nav_layout.addWidget(self.groundspeed_label) + nav_layout.addStretch() + + return nav_row + + def _create_control_section(self): + """創建控制按鈕區域""" + control_widget = QWidget() + control_layout = QVBoxLayout(control_widget) + control_layout.setContentsMargins(0, 0, 0, 0) + control_layout.setSpacing(6) + + control_widget.setFixedWidth(80) + + btn_style = """ + QPushButton { + background-color: #444; + color: #DDD; + border: none; + border-radius: 4px; + font-size: 11px; + } + QPushButton:hover { + background-color: #555; + } + """ + # 模式切換按鈕 + mode_btn = QPushButton("切換模式") + mode_btn.setStyleSheet(btn_style) + mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id)) + + # 解鎖按鈕 + arm_btn = QPushButton("解鎖") + arm_btn.setStyleSheet(btn_style) + arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id)) + + # 起飛按鈕 + takeoff_btn = QPushButton("起飛") + takeoff_btn.setStyleSheet(btn_style) + takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id)) + + # Setpoint 按鈕 + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.setStyleSheet(btn_style) + setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id)) + + control_layout.addWidget(mode_btn) + control_layout.addWidget(arm_btn) + control_layout.addWidget(takeoff_btn) + control_layout.addWidget(setpoint_btn) + + return control_widget + + def update_field(self, field, text, color=None): + """更新指定欄位的值""" + label = self.findChild(QLabel, f"{self.drone_id}_{field}") + if label and label.text() != text: + label.setText(text) + if color: + label.setStyleSheet(f"color: {color};") + + def get_checkbox(self): + """獲取勾選框""" + return self.checkbox + + def set_checked(self, checked): + """設置勾選狀態""" + self.checkbox.setChecked(checked) + + def is_checked(self): + """獲取勾選狀態""" + return self.checkbox.isChecked() + +class SocketGroupPanel(QWidget): + # 定義信號 + group_selection_changed = pyqtSignal(str, int) # socket_id, state + + def __init__(self, socket_id, color='#AAAAAA', parent=None): + super().__init__(parent) + self.socket_id = socket_id + self.color = color + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"socket_group_{self.socket_id}") + self.setStyleSheet(""" + background-color: #1E1E1E; + border-radius: 12px; + """) + + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(6) + + # Socket 分組標題行 - 包含勾選框 + title_row = QWidget() + title_layout = QHBoxLayout(title_row) + title_layout.setContentsMargins(0, 0, 0, 0) + + # 分組勾選框 + self.group_checkbox = QCheckBox() + self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox") + self.group_checkbox.setStyleSheet(f""" + QCheckBox {{ color: #DDD; }} + QCheckBox::indicator {{ + width: 14px; + height: 14px; + border: 2px solid #888888; + border-radius: 3px; + background: transparent; + }} + QCheckBox::indicator:checked {{ + background-color: {self.color}; + border: 2px solid #888888; + }} + QCheckBox::indicator:indeterminate {{ + background-color: #666; + border: 2px solid #888888; + }} + """) + self.group_checkbox.stateChanged.connect( + lambda state: self.group_selection_changed.emit(self.socket_id, state) + ) + + # Socket 分組標題 + title_label = QLabel(f"Socket {self.socket_id}") + title_label.setStyleSheet(f""" + font-weight: bold; + font-size: 16px; + color: {self.color}; + margin-bottom: 8px; + padding: 4px 8px; + border-radius: 6px; + """) + + title_layout.addWidget(self.group_checkbox) + title_layout.addWidget(title_label) + title_layout.addStretch() + + layout.addWidget(title_row) + + # 創建子容器用於放置該 socket 下的所有無人機面板 + self.drones_container = QWidget() + self.drones_layout = QVBoxLayout(self.drones_container) + self.drones_layout.setContentsMargins(0, 0, 0, 0) + self.drones_layout.setSpacing(4) + + layout.addWidget(self.drones_container) + + def add_drone_panel(self, panel): + """添加無人機面板到分組""" + self.drones_layout.addWidget(panel) + + def clear_drones(self): + """清空所有無人機面板""" + while self.drones_layout.count(): + item = self.drones_layout.takeAt(0) + if item.widget(): + item.widget().setParent(None) + + def get_checkbox(self): + """獲取分組勾選框""" + return self.group_checkbox + + def set_checked(self, checked): + """設置分組勾選狀態""" + self.group_checkbox.setChecked(checked) + + def set_check_state(self, state): + """設置分組勾選狀態（支持半選）""" + self.group_checkbox.setCheckState(state) \ No newline at end of file diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index 5eac6a3..615e9ff 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -5,12 +5,14 @@ from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, QHeaderView, QPushButton, QCheckBox, QLineEdit) from PyQt6.QtCore import Qt, QTimer -from PyQt6.QtWebEngineWidgets import QWebEngineView import sys import asyncio -# 從 communication.py 導入分離的類別 +# 導入分離的類別 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver +from map_layout import DroneMap +from drone_panel import DronePanel, SocketGroupPanel +from comm_panel import CommPanel class ControlStationUI(QMainWindow): def __init__(self): @@ -52,26 +54,30 @@ class ControlStationUI(QMainWindow): "loss_rate": 11, "ping": 12 } + self.socket_colors = { - '0': '#00BFFF', - '1': '#FFD700', - '2': "#FF6969", - '8': '#7CFC00', - '9': '#FF8C00', - 'default': '#AAAAAA' + '0': '#00BFFF', # 天藍色 (DeepSkyBlue) + '1': '#FFD700', # 金色 (Gold) + '2': '#FF6969', # 淺紅色 (Light Red) + '3': '#FF69B4', # 熱粉紅 (HotPink) + '4': '#00FA9A', # 中春綠 (MediumSpringGreen) + '5': '#9370DB', # 中紫色 (MediumPurple) - 串口 + '6': '#FFA500', # 橙色 (Orange) + '7': '#20B2AA', # 淺海綠 (LightSeaGreen) + '8': '#7CFC00', # 草綠色 (LawnGreen) + '9': '#FF8C00', # 深橙色 (DarkOrange) + 'default': '#AAAAAA' # 灰色 } + self.drone_positions = {} self.drone_headings = {} - self.map_loaded = False - self.map_update_timer = QTimer() - self.map_update_timer.timeout.connect(self.update_map_positions) - self.map_update_timer.start(200) - self.pending_map_updates = {} - + # 初始化地圖 + self.drone_map = DroneMap() # 初始化連接列表 self.udp_receivers = [] self.udp_connections = [] self.ws_connections = [] + self.monitor.start_serial_connection('/dev/ttyUSB0', 57600) self.init_ui() @@ -112,147 +118,16 @@ class ControlStationUI(QMainWindow): self.left_tab.addTab(self.overview_table, "總覽") # — 分頁 3：通訊設定 - self.comm_tab = QWidget() - comm_layout = QVBoxLayout(self.comm_tab) - comm_layout.setContentsMargins(10, 10, 10, 10) - comm_layout.setSpacing(10) - - # ========== UDP MAVLink 區域 ========== - udp_title = QLabel("UDP MAVLink") - udp_title.setStyleSheet(""" - color: #DDD; - font-size: 14px; - font-weight: bold; - padding: 5px; - background-color: #333; - border-radius: 4px; - """) - comm_layout.addWidget(udp_title) - - # UDP 連接列表容器 - self.udp_list_widget = QWidget() - self.udp_list_layout = QVBoxLayout(self.udp_list_widget) - self.udp_list_layout.setContentsMargins(0, 0, 0, 0) - self.udp_list_layout.setSpacing(5) - comm_layout.addWidget(self.udp_list_widget) - - # UDP 添加新連接區域 - add_udp_widget = QWidget() - add_udp_layout = QHBoxLayout(add_udp_widget) - add_udp_layout.setContentsMargins(0, 0, 0, 0) - - self.udp_ip_input = QLineEdit() - self.udp_ip_input.setText("127.0.0.1") - self.udp_ip_input.setPlaceholderText("IP") - self.udp_ip_input.setStyleSheet(""" - QLineEdit { - background-color: #333; - color: #DDD; - border: 1px solid #555; - border-radius: 4px; - padding: 5px; - } - """) + self.comm_panel = CommPanel() + self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) + self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) + self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection) + self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection) + self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection) + self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection) + self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout)) - self.udp_port_input = QLineEdit() - self.udp_port_input.setText("14540") - self.udp_port_input.setPlaceholderText("Port") - self.udp_port_input.setFixedWidth(80) - self.udp_port_input.setStyleSheet(""" - QLineEdit { - background-color: #333; - color: #DDD; - border: 1px solid #555; - border-radius: 4px; - padding: 5px; - } - """) - - add_udp_btn = QPushButton("添加") - add_udp_btn.clicked.connect(self.handle_add_udp_connection) - add_udp_btn.setStyleSheet(""" - QPushButton { - background-color: #4CAF50; - color: white; - border: none; - padding: 6px 12px; - border-radius: 4px; - min-width: 30px; - } - QPushButton:hover { background-color: #45a049; } - """) - - add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) - add_udp_layout.addWidget(self.udp_ip_input) - add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) - add_udp_layout.addWidget(self.udp_port_input) - add_udp_layout.addWidget(add_udp_btn) - - comm_layout.addWidget(add_udp_widget) - - # 分隔線 - separator = QWidget() - separator.setFixedHeight(20) - comm_layout.addWidget(separator) - - # ========== WebSocket 區域 ========== - ws_title = QLabel("WebSocket") - ws_title.setStyleSheet(""" - color: #DDD; - font-size: 14px; - font-weight: bold; - padding: 5px; - background-color: #333; - border-radius: 4px; - """) - comm_layout.addWidget(ws_title) - - # WebSocket 連接列表容器 - self.ws_list_widget = QWidget() - self.ws_list_layout = QVBoxLayout(self.ws_list_widget) - self.ws_list_layout.setContentsMargins(0, 0, 0, 0) - self.ws_list_layout.setSpacing(5) - comm_layout.addWidget(self.ws_list_widget) - - # WebSocket 添加新連接區域 - add_ws_widget = QWidget() - add_ws_layout = QHBoxLayout(add_ws_widget) - add_ws_layout.setContentsMargins(0, 0, 0, 0) - - self.ws_url_input = QLineEdit() - self.ws_url_input.setPlaceholderText("host") - self.ws_url_input.setStyleSheet(""" - QLineEdit { - background-color: #333; - color: #DDD; - border: 1px solid #555; - border-radius: 4px; - padding: 5px; - } - """) - - add_ws_btn = QPushButton("添加") - add_ws_btn.clicked.connect(self.handle_add_ws_connection) - add_ws_btn.setStyleSheet(""" - QPushButton { - background-color: #4CAF50; - color: white; - border: none; - padding: 6px 12px; - border-radius: 4px; - min-width: 30px; - } - QPushButton:hover { background-color: #45a049; } - """) - - add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) - add_ws_layout.addWidget(self.ws_url_input) - add_ws_layout.addWidget(add_ws_btn) - - comm_layout.addWidget(add_ws_widget) - comm_layout.addStretch() - - self.left_tab.addTab(self.comm_tab, "通訊") + self.left_tab.addTab(self.comm_panel, "通訊") # 右侧容器 right_container = QWidget() @@ -384,647 +259,54 @@ class ControlStationUI(QMainWindow): right_layout.addLayout(batch_control_layout) # 添加地圖 - self.map_view = QWebEngineView() - - inline_html = ''' - - - - - - - - - - -
-
- -
- - - - - ''' - self.map_view.setHtml(inline_html) - self.map_view.loadFinished.connect(self.on_map_loaded) - - right_layout.addWidget(self.map_view) + right_layout.addWidget(self.drone_map.get_widget()) + self.drone_map.get_gps_signal().connect(self.handle_map_click) + # Add widgets to splitter main_splitter.addWidget(self.left_tab) main_splitter.addWidget(right_container) main_splitter.setSizes([400, 1000]) self.setCentralWidget(main_splitter) - def on_map_loaded(self, ok: bool): - if ok: - self.map_loaded = True - else: - print("⚠️ 地图页加载失败") - - # 修改2: 添加地圖更新節流方法 - def update_map_positions(self): - """批量更新地圖上的無人機位置""" - if not self.map_loaded or not self.pending_map_updates: - return - - # 批量執行所有待更新的位置 - js_commands = [] - for drone_id, (lat, lon, heading) in self.pending_map_updates.items(): - js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});") - - if js_commands: - combined_js = "\n".join(js_commands) - self.map_view.page().runJavaScript(combined_js) - - # 清空待更新緩存 - self.pending_map_updates.clear() - - def create_drone_panel(self, drone_id): - panel = QWidget() - panel.setObjectName(f"panel_{drone_id}") - panel.setFixedHeight(140) # 根據需要調整高度 - panel.setStyleSheet(""" - QWidget#panel_%s { - background-color: #2A2A2A; - border-radius: 8px; - margin: 6px; - padding: 10px; - border: 1px solid #444; - } - QLabel { - color: #DDD; - font-size: 12px; - padding: 2px; - } - """ % drone_id) - - # 主佈局改為水平佈局 - main_layout = QHBoxLayout(panel) - main_layout.setContentsMargins(8, 8, 8, 8) - main_layout.setSpacing(8) - - # 左側資訊區域 - info_widget = QWidget() - info_layout = QVBoxLayout(info_widget) - info_layout.setContentsMargins(0, 0, 0, 0) - info_layout.setSpacing(4) - - # 顶部标题栏 - header = QWidget() - header_layout = QHBoxLayout(header) - header_layout.setContentsMargins(0, 0, 0, 0) - - # 在標題列加入勾選框 - checkbox = QCheckBox() - checkbox.setObjectName(f"{drone_id}_checkbox") - checkbox.setStyleSheet("QCheckBox { color: #DDD; }") - checkbox.stateChanged.connect(lambda state: self.handle_drone_selection(drone_id, state)) - - header_layout.insertWidget(0, checkbox) # 插入到最前面 - - # ID显示 - 修改這裡：將 s0_4 格式改為 s4 格式 - display_id = 's' + drone_id.split('_')[1] - id_label = QLabel(display_id) - id_label.setStyleSheet(""" - font-weight: bold; - font-size: 14px; - color: #7FFFD4; - min-width: 80px; - """) - - header_layout.addWidget(id_label) - header_layout.addStretch() - - info_layout.addWidget(header) - - # 第一行：狀態 (模式 + ARM狀態) - status_row = QWidget() - status_layout = QHBoxLayout(status_row) - status_layout.setContentsMargins(0, 0, 0, 0) - - status_title = QLabel("狀態：") - status_title.setStyleSheet("color: #888; min-width: 50px;") - - mode_label = QLabel("--") - mode_label.setObjectName(f"{drone_id}_mode") - - armed_label = QLabel("--") - armed_label.setObjectName(f"{drone_id}_armed") - - status_layout.addWidget(status_title) - status_layout.addWidget(mode_label) - status_layout.addWidget(armed_label) - status_layout.addStretch() - - info_layout.addWidget(status_row) - - # 第二行：電池 - battery_row = QWidget() - battery_layout = QHBoxLayout(battery_row) - battery_layout.setContentsMargins(0, 0, 0, 0) - - battery_title = QLabel("電池：") - battery_title.setStyleSheet("color: #888; min-width: 50px;") - - battery_label = QLabel("--") - battery_label.setObjectName(f"{drone_id}_battery") - - battery_layout.addWidget(battery_title) - battery_layout.addWidget(battery_label) - battery_layout.addStretch() - - info_layout.addWidget(battery_row) - - # 第三行：位置 + 高度 - position_row = QWidget() - position_layout = QHBoxLayout(position_row) - position_layout.setContentsMargins(0, 0, 0, 0) - - position_title = QLabel("位置：") - position_title.setStyleSheet("color: #888; min-width: 50px;") - - local_label = QLabel("--") - local_label.setObjectName(f"{drone_id}_local") - - altitude_title = QLabel("高度：") - altitude_title.setStyleSheet("color: #888; margin-left: 10px;") - - altitude_label = QLabel("--") - altitude_label.setObjectName(f"{drone_id}_altitude") - - position_layout.addWidget(position_title) - position_layout.addWidget(local_label) - position_layout.addWidget(altitude_title) - position_layout.addWidget(altitude_label) - position_layout.addStretch() - - info_layout.addWidget(position_row) - - # 第四行：航向 + 速度 - nav_row = QWidget() - nav_layout = QHBoxLayout(nav_row) - nav_layout.setContentsMargins(0, 0, 0, 0) - - heading_title = QLabel("航向：") - heading_title.setStyleSheet("color: #888; min-width: 50px;") - - heading_label = QLabel("--") - heading_label.setObjectName(f"{drone_id}_heading") - - speed_title = QLabel("速度：") - speed_title.setStyleSheet("color: #888; margin-left: 10px;") - - groundspeed_label = QLabel("--") - groundspeed_label.setObjectName(f"{drone_id}_groundspeed") - - nav_layout.addWidget(heading_title) - nav_layout.addWidget(heading_label) - nav_layout.addWidget(speed_title) - nav_layout.addWidget(groundspeed_label) - nav_layout.addStretch() - - info_layout.addWidget(nav_row) - - # 右側控制按鈕區域 - control_widget = QWidget() - control_layout = QVBoxLayout(control_widget) - control_layout.setContentsMargins(0, 0, 0, 0) - control_layout.setSpacing(6) - - # 設置控制區域的固定寬度 - control_widget.setFixedWidth(80) - - mode_btn = QPushButton("切換模式") - mode_btn.setObjectName(f"{drone_id}_mode_btn") - mode_btn.clicked.connect(lambda: self.handle_mode_change(drone_id)) - mode_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 6px; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """) - - arm_btn = QPushButton("解鎖") - arm_btn.setObjectName(f"{drone_id}_arm_btn") - arm_btn.clicked.connect(lambda: self.handle_arm(drone_id)) - arm_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 6px; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """) - - takeoff_btn = QPushButton("起飛") - takeoff_btn.setObjectName(f"{drone_id}_takeoff_btn") - takeoff_btn.clicked.connect(lambda: self.handle_takeoff(drone_id)) - takeoff_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 6px; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """) - - setpoint_btn = QPushButton("Setpoint") - setpoint_btn.setObjectName(f"{drone_id}_setpoint_btn") - setpoint_btn.clicked.connect(lambda: self.handle_single_setpoint(drone_id)) - setpoint_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 6px; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """) - - # 按鈕由上而下排列 - control_layout.addWidget(mode_btn) - control_layout.addWidget(arm_btn) - control_layout.addWidget(takeoff_btn) - control_layout.addWidget(setpoint_btn) - control_layout.addStretch() # 將按鈕推向頂部 - - # 將左側資訊區域和右側控制區域加入主佈局 - main_layout.addWidget(info_widget) - main_layout.addWidget(control_widget) - - return panel - def create_udp_connection_panel(self, conn): - """創建 UDP 連接面板""" - panel = QWidget() - panel.setStyleSheet(""" - QWidget { - background-color: #2A2A2A; - border-radius: 6px; - padding: 8px; - border: 1px solid #444; - } - """) - - layout = QHBoxLayout(panel) - layout.setContentsMargins(8, 8, 8, 8) - - # 連接資訊 - info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") - info_label.setStyleSheet("color: #DDD; font-size: 12px;") - - # 狀態指示器 - status_label = QLabel("●") - if conn.get('enabled', False): - status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") - status_label.setToolTip("運行中") - else: - status_label.setStyleSheet("color: #888; font-size: 16px;") - status_label.setToolTip("已停止") - - # 控制按鈕 - toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") - toggle_btn.setFixedWidth(60) - toggle_btn.clicked.connect(lambda: self.toggle_udp_connection(conn, toggle_btn, status_label)) - toggle_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 4px 8px; - border-radius: 3px; - font-size: 11px; - } - QPushButton:hover { background-color: #555; } - """) - - remove_btn = QPushButton("移除") - remove_btn.setFixedWidth(60) - remove_btn.clicked.connect(lambda: self.remove_udp_connection(conn, panel)) - remove_btn.setStyleSheet(""" - QPushButton { - background-color: #d32f2f; - color: white; - border: none; - padding: 4px 8px; - border-radius: 3px; - font-size: 11px; - } - QPushButton:hover { background-color: #b71c1c; } - """) - - layout.addWidget(status_label) - layout.addWidget(info_label) - layout.addStretch() - layout.addWidget(toggle_btn) - layout.addWidget(remove_btn) - - # 儲存引用 - panel.connection = conn - panel.toggle_btn = toggle_btn - panel.status_label = status_label - - return panel - - def create_ws_connection_panel(self, conn): - """創建 WebSocket 連接面板""" - panel = QWidget() - panel.setStyleSheet(""" - QWidget { - background-color: #2A2A2A; - border-radius: 6px; - padding: 8px; - border: 1px solid #444; - } - """) - - layout = QHBoxLayout(panel) - layout.setContentsMargins(8, 8, 8, 8) - - # 連接資訊 - info_label = QLabel(f"{conn['name']} - {conn['url']}") - info_label.setStyleSheet("color: #DDD; font-size: 12px;") - - # 狀態指示器 - status_label = QLabel("●") - if conn.get('enabled', False): - status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") - status_label.setToolTip("運行中") - else: - status_label.setStyleSheet("color: #888; font-size: 16px;") - status_label.setToolTip("已停止") - - # 控制按鈕 - toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") - toggle_btn.setFixedWidth(60) - toggle_btn.clicked.connect(lambda: self.toggle_ws_connection(conn, toggle_btn, status_label)) - toggle_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 4px 8px; - border-radius: 3px; - font-size: 11px; - } - QPushButton:hover { background-color: #555; } - """) - - remove_btn = QPushButton("移除") - remove_btn.setFixedWidth(60) - remove_btn.clicked.connect(lambda: self.remove_ws_connection(conn, panel)) - remove_btn.setStyleSheet(""" - QPushButton { - background-color: #d32f2f; - color: white; - border: none; - padding: 4px 8px; - border-radius: 3px; - font-size: 11px; - } - QPushButton:hover { background-color: #b71c1c; } - """) - - layout.addWidget(status_label) - layout.addWidget(info_label) - layout.addStretch() - layout.addWidget(toggle_btn) - layout.addWidget(remove_btn) - - # 儲存引用 - panel.connection = conn - panel.toggle_btn = toggle_btn - panel.status_label = status_label - - return panel + def handle_map_click(self, lat, lon): + print(f"地圖點擊位置: {lat:.6f}, {lon:.6f}") - def handle_add_ws_connection(self): - """處理添加 WebSocket 連接""" - input_url = self.ws_url_input.text().strip() - - if not input_url: - self.statusBar().showMessage("請輸入 WebSocket URL", 3000) - return + def handle_udp_connection_added(self, ip, port): + """处理 UDP 连接添加信号""" + # 创建新连接 + new_conn = { + 'name': f'UDP {len(self.udp_connections) + 1}', + 'ip': ip, + 'port': port, + 'enabled': True + } - # 自動添加 ws:// 前綴（如果用戶沒有輸入協議前綴） - if not input_url.startswith('ws://') and not input_url.startswith('wss://'): - url = f'ws://{input_url}' - else: - url = input_url + # 启动接收器 + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + new_conn['receiver'] = receiver - # 基本 URL 格式驗證 - try: - # 檢查是否包含 host:port 格式 - if '://' in url: - parts = url.split('://', 1) - if len(parts) == 2 and ':' not in parts[1]: - self.statusBar().showMessage("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) - return - except: - self.statusBar().showMessage("URL 格式錯誤", 3000) - return + self.udp_connections.append(new_conn) - # 檢查是否已存在相同連接 - for conn in self.ws_connections: - if conn['url'] == url: - self.statusBar().showMessage("連接已存在", 3000) - return + # 添加到 UI + self.comm_panel.add_udp_panel(new_conn) - # 創建新連接 + self.statusBar().showMessage(f"已添加 UDP 连接: {ip}:{port}", 3000) + print(f"UDP MAVLink receiver added and started on {ip}:{port}") + + def handle_ws_connection_added(self, url): + """处理 WebSocket 连接添加信号""" + # 创建新连接 new_conn = { 'name': f'WS {len(self.ws_connections) + 1}', 'url': url, 'enabled': True } - # 啟動接收器 + # 启动接收器 receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) receiver.start() self.monitor.ws_receivers.append(receiver) @@ -1033,14 +315,23 @@ class ControlStationUI(QMainWindow): self.ws_connections.append(new_conn) # 添加到 UI - conn_panel = self.create_ws_connection_panel(new_conn) - self.ws_list_layout.addWidget(conn_panel) - - # 清空輸入框 - self.ws_url_input.clear() + self.comm_panel.add_ws_panel(new_conn) - self.statusBar().showMessage(f"已添加 WebSocket 連接: {url}", 3000) + self.statusBar().showMessage(f"已添加 WebSocket 连接: {url}", 3000) print(f"WebSocket receiver added and started: {url}") + + def create_drone_panel(self, drone_id): + """創建無人機面板""" + panel = DronePanel(drone_id) + + # 連接信號 + panel.mode_change_requested.connect(self.handle_mode_change) + panel.arm_requested.connect(self.handle_arm) + panel.takeoff_requested.connect(self.handle_takeoff) + panel.setpoint_requested.connect(self.handle_single_setpoint) + panel.selection_changed.connect(self.handle_drone_selection) + + return panel def toggle_ws_connection(self, conn, btn, status_label): """切換 WebSocket 連接狀態""" @@ -1066,72 +357,25 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) def remove_ws_connection(self, conn, panel): - """移除 WebSocket 連接""" + """移除 WebSocket 连接""" # 停止接收器 if 'receiver' in conn and conn['receiver']: conn['receiver'].stop() if conn['receiver'] in self.monitor.ws_receivers: self.monitor.ws_receivers.remove(conn['receiver']) - # 從列表移除 + # 从列表移除 if conn in self.ws_connections: self.ws_connections.remove(conn) - # 從 UI 移除 + # 从 comm_panel 列表移除 + self.comm_panel.remove_ws_connection_from_list(conn) + + # 从 UI 移除 panel.setParent(None) panel.deleteLater() - self.statusBar().showMessage(f"已移除 WebSocket 連接: {conn['url']}", 3000) - - def handle_add_udp_connection(self): - """處理添加 UDP 連接""" - ip = self.udp_ip_input.text().strip() - port_text = self.udp_port_input.text().strip() - - if not ip or not port_text: - self.statusBar().showMessage("請輸入 IP 和 Port", 3000) - return - - try: - port = int(port_text) - if port < 1 or port > 65535: - raise ValueError("Port 超出範圍") - except ValueError: - self.statusBar().showMessage("Port 必須是 1-65535 的數字", 3000) - return - - # 檢查是否已存在相同連接 - for conn in self.udp_connections: - if conn['ip'] == ip and conn['port'] == port: - self.statusBar().showMessage("連接已存在", 3000) - return - - # 創建新連接 - new_conn = { - 'name': f'UDP {len(self.udp_connections) + 1}', - 'ip': ip, - 'port': port, - 'enabled': True - } - - # 啟動接收器 - receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) - receiver.start() - self.udp_receivers.append(receiver) - new_conn['receiver'] = receiver - - self.udp_connections.append(new_conn) - - # 添加到 UI - conn_panel = self.create_udp_connection_panel(new_conn) - self.udp_list_layout.addWidget(conn_panel) - - # 清空輸入框 - self.udp_ip_input.clear() - self.udp_port_input.clear() - - self.statusBar().showMessage(f"已添加 UDP 連接: {ip}:{port}", 3000) - print(f"UDP MAVLink receiver added and started on {ip}:{port}") + self.statusBar().showMessage(f"已移除 WebSocket 连接: {conn['url']}", 3000) def toggle_udp_connection(self, conn, btn, status_label): """切換 UDP 連接狀態""" @@ -1157,109 +401,32 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) def remove_udp_connection(self, conn, panel): - """移除 UDP 連接""" + """移除 UDP 连接""" # 停止接收器 if 'receiver' in conn and conn['receiver']: conn['receiver'].stop() if conn['receiver'] in self.udp_receivers: self.udp_receivers.remove(conn['receiver']) - # 從列表移除 + # 从列表移除 if conn in self.udp_connections: self.udp_connections.remove(conn) - # 從 UI 移除 + # 从 comm_panel 列表移除 + self.comm_panel.remove_udp_connection_from_list(conn) + + # 从 UI 移除 panel.setParent(None) panel.deleteLater() - self.statusBar().showMessage(f"已移除 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000) def create_socket_group_panel(self, socket_id): """創建 socket 分組面板""" - group_panel = QWidget() - group_panel.setObjectName(f"socket_group_{socket_id}") - group_panel.setStyleSheet(f""" - QWidget#socket_group_{socket_id} {{ - background-color: #1E1E1E; - border: 2px solid #555; - border-radius: 12px; - margin: 8px; - padding: 12px; - }} - QLabel {{ - color: #DDD; - font-size: 12px; - padding: 2px; - }} - """) - - layout = QVBoxLayout(group_panel) - layout.setContentsMargins(10, 10, 10, 10) - layout.setSpacing(6) - - # Socket 分組標題行 - 包含勾選框 - title_row = QWidget() - title_layout = QHBoxLayout(title_row) - title_layout.setContentsMargins(0, 0, 0, 0) - - # 分組勾選框 - group_checkbox = QCheckBox() - group_checkbox.setObjectName(f"socket_{socket_id}_checkbox") - group_checkbox.setStyleSheet("QCheckBox { color: #DDD; }") - group_checkbox.stateChanged.connect(lambda state: self.handle_group_selection(socket_id, state)) - - # Socket 分組標題 color = self.socket_colors.get(socket_id, self.socket_colors['default']) - title_label = QLabel(f"Socket {socket_id}") - title_label.setStyleSheet(f""" - font-weight: bold; - font-size: 16px; - color: {color}; - margin-bottom: 8px; - padding: 4px 8px; - background-color: #333; - border-radius: 6px; - """) - - title_layout.addWidget(group_checkbox) - title_layout.addWidget(title_label) - title_layout.addStretch() - - layout.addWidget(title_row) - - # 創建子容器用於放置該 socket 下的所有無人機面板 - drones_container = QWidget() - drones_layout = QVBoxLayout(drones_container) - drones_layout.setContentsMargins(0, 0, 0, 0) - drones_layout.setSpacing(4) - - layout.addWidget(drones_container) - - # 儲存容器的引用以便後續添加無人機 - group_panel.drones_container = drones_container - group_panel.drones_layout = drones_layout - - return group_panel - - def create_data_row(self, layout, title, object_name, default): - row = QWidget() - hbox = QHBoxLayout(row) - hbox.setContentsMargins(0, 0, 0, 0) - - # 标题标签 - title_label = QLabel(title) - title_label.setStyleSheet("color: #888; min-width: 80px;") - title_label.setSizePolicy(QSizePolicy.Policy.Fixed, QSizePolicy.Policy.Preferred) - - # 数据标签 - value_label = QLabel(default) - value_label.setObjectName(object_name) - value_label.setWordWrap(True) - value_label.setStyleSheet("margin-left: 10px;") - - hbox.addWidget(title_label) - hbox.addWidget(value_label) - layout.addWidget(row) + panel = SocketGroupPanel(socket_id, color) + panel.group_selection_changed.connect(self.handle_group_selection) + return panel def handle_mode_change(self, drone_id): """處理單個無人機的模式切換""" @@ -1436,11 +603,11 @@ class ControlStationUI(QMainWindow): self.update_overview_table(drone_id, 'pitch', pitch_text) self.update_overview_table(drone_id, 'yaw', yaw_text) - # 新的代碼 - 將地圖更新放入緩存，等待批量處理： + # 更新地圖上的無人機位置 if drone_id in self.drone_positions and drone_id in self.drone_headings: lat, lon = self.drone_positions[drone_id] heading = self.drone_headings[drone_id] - self.pending_map_updates[drone_id] = (lat, lon, heading) + self.drone_map.update_drone_position(drone_id, lat, lon, heading) # 新增處理分組勾選的方法 def handle_group_selection(self, socket_id, state): @@ -1453,7 +620,7 @@ class ControlStationUI(QMainWindow): is_checked = state == Qt.CheckState.Checked.value for drone_id in group_drones: - checkbox = self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox") + checkbox = self.drones[drone_id].get_checkbox() if checkbox: # 暫時斷開信號連接，避免遞迴觸發 checkbox.blockSignals(True) @@ -1489,7 +656,7 @@ class ControlStationUI(QMainWindow): # 檢查該分組內有多少無人機被勾選 checked_count = sum(1 for did in group_drones - if self.drones[did].findChild(QCheckBox, f"{did}_checkbox").isChecked()) + if self.drones[did].is_checked()) # 獲取分組勾選框 if socket_id in self.socket_groups: @@ -1515,7 +682,7 @@ class ControlStationUI(QMainWindow): """處理全選按鈕 - 支援分組結構""" # 檢查是否所有無人機都已被選中 all_selected = all( - self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox").isChecked() + self.drones[drone_id].is_checked() for drone_id in self.drones ) @@ -1524,9 +691,7 @@ class ControlStationUI(QMainWindow): # 更新所有勾選框狀態（無人機和分組） for drone_id in self.drones: - checkbox = self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox") - if checkbox: - checkbox.setChecked(new_state) + self.drones[drone_id].set_checked(new_state) # 更新所有分組勾選框狀態 for socket_id in self.socket_groups: @@ -1557,20 +722,10 @@ class ControlStationUI(QMainWindow): future = self.monitor.set_mode(drone_id, mode) loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) - def handle_single_mode_change(self, drone_id): - mode = self.mode_combo.currentText() - loop = asyncio.get_event_loop() - future = self.monitor.set_mode(drone_id, mode) - loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) - def update_field(self, panel, drone_id, field, text, color=None): - """Update a specific field in the panel - 添加變更檢查""" - if label := panel.findChild(QLabel, f"{drone_id}_{field}"): - # 只有在文字真正改變時才更新 - if label.text() != text: - label.setText(text) - if color: - label.setStyleSheet(f"color: {color};") + """更新面板中的指定欄位""" + if isinstance(panel, DronePanel): + panel.update_field(field, text, color) def update_overview_table(self, drone_id=None, field=None, value=None): # Ensure the widget is available diff --git a/src/unitdev01/unitdev01/map_layout.py b/src/unitdev01/unitdev01/map_layout.py new file mode 100644 index 0000000..11f7171 --- /dev/null +++ b/src/unitdev01/unitdev01/map_layout.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +from PyQt6.QtWebEngineWidgets import QWebEngineView +from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot +from PyQt6.QtWebChannel import QWebChannel + +class DroneMap: + """無人機地圖類別 - 負責管理 Leaflet 地圖顯示""" + + def __init__(self): + """初始化地圖""" + self.map_view = QWebEngineView() + self.map_loaded = False + self.pending_map_updates = {} + + # 創建橋接對象 + self.bridge = MapBridge() + + # 設置 QWebChannel + self.channel = QWebChannel() + self.channel.registerObject('bridge', self.bridge) + self.map_view.page().setWebChannel(self.channel) + + # 設置地圖 HTML + inline_html = ''' + + + + + + + + + + + +
+
+ +
+ + + + + ''' + + self.map_view.setHtml(inline_html) + self.map_view.loadFinished.connect(self._on_map_loaded) + + # 設置地圖更新計時器 + self.map_update_timer = QTimer() + self.map_update_timer.timeout.connect(self.update_map_positions) + self.map_update_timer.start(200) # 每 200ms 更新一次 + + def _on_map_loaded(self, ok: bool): + """地圖加載完成回調""" + if ok: + self.map_loaded = True + else: + print("⚠️ 地圖加載失敗") + + def update_drone_position(self, drone_id, lat, lon, heading): + """更新無人機位置（加入待處理隊列）""" + self.pending_map_updates[drone_id] = (lat, lon, heading) + + def update_map_positions(self): + """批量更新地圖上的無人機位置""" + if not self.map_loaded or not self.pending_map_updates: + return + + # 批量執行所有待更新的位置 + js_commands = [] + for drone_id, (lat, lon, heading) in self.pending_map_updates.items(): + js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});") + + if js_commands: + combined_js = "\n".join(js_commands) + self.map_view.page().runJavaScript(combined_js) + + # 清空待更新緩存 + self.pending_map_updates.clear() + + def clear_trajectories(self): + """清除所有軌跡""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearAllTrajectories();") + + def focus_on_drone(self, drone_id): + """聚焦到指定無人機""" + if self.map_loaded: + self.map_view.page().runJavaScript(f"focusOn('{drone_id}');") + + def get_widget(self): + """獲取地圖 widget""" + return self.map_view + + def get_gps_signal(self): + """獲取 GPS 信號""" + return self.bridge.gps_signal + +class MapBridge(QObject): + """JavaScript 和 Python 之間的橋接類""" + gps_signal = pyqtSignal(float, float) # lat, lon + + def __init__(self): + super().__init__() + + @pyqtSlot(float, float) + def emitGpsSignal(self, lat, lon): + """供 JavaScript 調用的方法""" + self.gps_signal.emit(lat, lon) \ No newline at end of file From e4b658d578fe24f62c0df0c0d98270039d734187 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 8 Jan 2026 17:14:12 +0800 Subject: [PATCH 23/33] Delete 'src/unitdev01/unitdev01/xbee.py' --- src/unitdev01/unitdev01/xbee.py | 619 -------------------------------- 1 file changed, 619 deletions(-) delete mode 100644 src/unitdev01/unitdev01/xbee.py diff --git a/src/unitdev01/unitdev01/xbee.py b/src/unitdev01/unitdev01/xbee.py deleted file mode 100644 index b550e1e..0000000 --- a/src/unitdev01/unitdev01/xbee.py +++ /dev/null @@ -1,619 +0,0 @@ -import sys -import time -import math -import serial -import struct -from PyQt5 import QtWidgets -from PyQt5.QtCore import QThread, pyqtSignal -from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox -from pymavlink import mavutil -from pymavlink.dialects.v20 import ardupilotmega as mavlink2 - -class PacketCapture: - def __init__(self): - self.data = bytearray() - - def write(self, b): - self.data.extend(b) - return len(b) - -class MAVLinkWorker(QThread): - state_signal = pyqtSignal(str, str) - battery_signal = pyqtSignal(str, float) - gps_signal = pyqtSignal(str, float, float) - gps_status_signal = pyqtSignal(str, int, int) - local_position_signal = pyqtSignal(str, float, float, float) - imu_signal = pyqtSignal(str, float) - hdg_signal = pyqtSignal(str, float) - groundspeed_signal = pyqtSignal(str, float) - ping_signal = pyqtSignal(str, float) - loss_signal = pyqtSignal(str, float) - frequency_signal = pyqtSignal(str, float) - param_signal = pyqtSignal(str, int) - kbps_signal = pyqtSignal(str, float) - - def __init__(self): - super().__init__() - self.namespaces = ['UAV1', 'UAV2', 'UAV3'] - self.connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) - self.connection.wait_heartbeat() - for sysid in self.namespaces: - sysid = int(sysid.replace('UAV', '')) - self.set_sr_params(sysid) - self.running = True - - # 用於計算頻率丟包 - self.message_count = {} - self.frequency = {} - self.start_time = {} - self.seq_numbers = {} - self.packet_loss_count = {} - self.total_packet_count = {} - self.loss_percentage = {} - self.total_bytes_received = {} - self.throughput_KBps = {} - - for namespace in self.namespaces: - self.request_param(namespace, "SR1_EXTRA1") - self.connection.mav.timesync_send( - 0, #tc1 - int(time.time() * 1e9) #ts1 - ) - - def run(self): - while self.running: - try: - msg = self.connection.recv_msg() - current_time = time.time() - if not msg: - continue - sysid = msg.get_srcSystem() - if sysid == 0: - continue - namespace = f"UAV{sysid}" - msg_type = msg.get_type() - if msg_type =="BAD_DATA": - continue - - if sysid not in self.total_bytes_received: - self.total_bytes_received[sysid] = 0 - self.throughput_KBps[sysid] = 0 - - # 計算訊息大小 - msg_bytes = msg.get_msgbuf() # 取得封包的 bytes - if msg_bytes: - self.total_bytes_received[sysid] += len(msg_bytes) - - # Packet loss calculation - if sysid not in self.seq_numbers: - self.seq_numbers[sysid] = msg.get_seq() # Initialize sequence number tracking - self.packet_loss_count[sysid] = 0 - self.total_packet_count[sysid] = 1 - else: - current_seq = msg.get_seq() - expected_seq = (self.seq_numbers[sysid] + 1) % 256 - self.total_packet_count[sysid] += 1 - - if current_seq != expected_seq: # Packet(s) lost - lost_packets = (current_seq - expected_seq) % 256 - self.packet_loss_count[sysid] += lost_packets - self.total_packet_count[sysid] += lost_packets - - self.seq_numbers[sysid] = current_seq - self.loss_percentage[sysid] = (self.packet_loss_count[sysid] / self.total_packet_count[sysid]) * 100 - self.loss_signal.emit(namespace, self.loss_percentage[sysid]) - - # Frequency calculation - if sysid not in self.message_count: - self.message_count[sysid] = 0 - self.start_time[sysid] = current_time - - self.message_count[sysid] += 1 - - # 每隔一段時間更新 - elapsed_time = current_time - self.start_time[sysid] - if elapsed_time >= 1: - # 每秒頻率 - self.frequency[sysid] = self.message_count[sysid] / elapsed_time - self.frequency_signal.emit(namespace, self.frequency[sysid]) - - # 發送 timesync request - self.connection.mav.timesync_send( - 0, #tc1 - int(current_time * 1e9) #ts1 - ) - #吞吐量 - self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time - self.kbps_signal.emit(namespace, self.throughput_KBps[sysid]) - - self.start_time[sysid] = current_time - self.message_count[sysid] = 0 - self.total_bytes_received[sysid] = 0 - - if msg_type == "HEARTBEAT": - mode = mavutil.mode_string_v10(msg) - self.state_signal.emit(namespace, mode) - - elif msg_type == "BATTERY_STATUS": - voltage = msg.voltages[0] / 1000 - self.battery_signal.emit(namespace, voltage) - - elif msg_type == "GLOBAL_POSITION_INT": - latitude = msg.lat / 1e7 - longitude = msg.lon / 1e7 - self.gps_signal.emit(namespace, latitude, longitude) - - elif msg_type == "GPS_RAW_INT": - fix_type = msg.fix_type - satellites_visible = msg.satellites_visible - self.gps_status_signal.emit(namespace, fix_type, satellites_visible) - - elif msg_type == "LOCAL_POSITION_NED": - x = msg.y - y = msg.x - z = -msg.z - self.local_position_signal.emit(namespace, x, y, z) - - elif msg_type == "ATTITUDE": - pitch = math.degrees(msg.pitch) - self.imu_signal.emit(namespace, pitch) - - elif msg_type == "VFR_HUD": - groundspeed = msg.groundspeed - heading = msg.heading - self.hdg_signal.emit(namespace, heading) - self.groundspeed_signal.emit(namespace, groundspeed) - - elif msg_type == "TIMESYNC": - round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6 - if(round_trip_time<1e6): - self.ping_signal.emit(namespace, round_trip_time) - - elif msg_type == 'PARAM_VALUE': - param_name = "SR1_EXTRA1" - if msg.param_id == param_name: - self.param_signal.emit(namespace, msg.param_value) - - except Exception as e: - print(f"Error reading message: {e}") - - def stop(self): - self.running = False - self.connection.close() - - def build_api_tx_frame(self, data: bytes, frame_id=0x01): - frame_type = 0x10 - dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # Broadcast - dest_addr16 = b'\xFF\xFE' - broadcast_radius = 0x00 - options = 0x00 - - frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) - frame += dest_addr64 + dest_addr16 - frame += struct.pack(">BB", broadcast_radius, options) + data - checksum = 0xFF - (sum(frame) & 0xFF) - return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum) - - def send_command(self, msg): - # Create the packet and send via serial port - PORT = "/dev/ttyUSB0" - BAUD = 57600 - ser = serial.Serial(PORT, BAUD) - capture = PacketCapture() - mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1) - mav.version = 2 - mav.send(msg) - api_frame = self.build_api_tx_frame(capture.data) - ser.write(api_frame) - print("RAW HEX:", capture.data.hex()) - - def set_mode(self, namespace, mode): - sysid = int(namespace.replace('UAV', '')) - if mode == 'STABILIZE': - mode = 0 - elif mode == 'AUTO': - mode = 3 - elif mode == 'GUIDED': - mode = 4 - elif mode == 'LOITER': - mode = 5 - - msg = self.connection.mav.command_long_encode( - target_system=sysid, - target_component=1, - command=mavutil.mavlink.MAV_CMD_DO_SET_MODE, - confirmation=0, - param1=1, # Custom mode enabled - param2=mode, - param3=0, param4=0, param5=0, param6=0, param7=0 - ) - self.send_command(msg) - - def arm(self, namespace, arm): - sysid = int(namespace.replace('UAV', '')) - msg = self.connection.mav.command_long_encode( - target_system=sysid, - target_component=1, - command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - confirmation=0, - param1=1 if arm else 0, # 1 to arm, 0 to disarm - param2=0, param3=0, param4=0, param5=0, param6=0, param7=0 - ) - self.send_command(msg) - - def takeoff(self, namespace, altitude): - sysid = int(namespace.replace('UAV', '')) - msg = self.connection.mav.command_long_encode( - target_system=sysid, - target_component=1, - command=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - confirmation=0, - param1=0, param2=0, param3=0, param4=0, param5=0, param6=0, - param7=altitude - ) - self.send_command(msg) - - def land(self, namespace): - sysid = int(namespace.replace('UAV', '')) - msg = self.connection.mav.command_long_encode( - target_system=sysid, - target_component=1, - command=mavutil.mavlink.MAV_CMD_NAV_LAND, - confirmation=0, - param1=0, param2=0, param3=0, param4=0, param5=0, param6=0, - param7=0 - ) - self.send_command(msg) - - def set_local_position(self, namespace, x, y, z): - sysid = int(namespace.replace('UAV', '')) - msg = self.connection.mav.set_position_target_local_ned_encode( - 0, sysid, 1, # time_boot_ms, sysid, compid - mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame - 0b110111111000, # Mask - y, x, -z, # Position - 0, 0, 0, # Velocity - 0, 0, 0, # Acceleration - 0, 0 # Yaw, yaw_rate - ) - self.send_command(msg) - - def reboot_drone(self, namespace): - sysid = int(namespace.replace('UAV', '')) - self.connection.mav.command_long_send( - sysid, - 1, # target_component (一般為1) - mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, # Reboot 命令 - 0, # confirmation - 1, # 第一個參數 (1 = reboot，0 = 無操作，2 = 關機) - 0, 0, 0, 0, 0, 0 # 其餘參數保留 - ) - - def set_param(self, namespace, param_name, value): - sysid = int(namespace.replace('UAV', '')) - try: - self.connection.mav.param_set_send( - sysid, - 1, - param_name.encode('utf-8'), - float(value), - mavutil.mavlink.MAV_PARAM_TYPE_REAL32 - ) - except Exception as e: - print(f"Failed to set parameter {param_name}: {e}") - - def request_param(self, namespace, param_name): - sysid = int(namespace.replace('UAV', '')) - try: - self.connection.mav.param_request_read_send( - sysid, # 系統 ID - 1, # 組件 ID - param_name.encode('utf-8'), # 參數名稱 - -1 # 參數索引，-1 表示根據名稱請求 - ) - except Exception as e: - print(f"Failed to set parameter {param_name}: {e}") - - ''' - def set_sr_params(self, sysid): - """ 直接設置 MAVLink 訊息頻率 """ - freqs = [0, 2, 4] - params = { - "SR1_ADSB": freqs[0], - "SR1_EXT_STAT": freqs[1], - "SR1_EXTRA1": freqs[2], - "SR1_EXTRA2": freqs[2], - "SR1_EXTRA3": freqs[1], - "SR1_POSITION": freqs[1], - "SR1_RAW_SENS": freqs[1], - "SR1_RC_CHAN": freqs[1] - } - for param, value in params.items(): - self.set_param(f"UAV{sysid}", param, value) - ''' - def set_sr_params(self, sysid): - """ 直接設置 MAVLink 訊息頻率 """ - freqs = [0, 1, 1] - params = { - "SR1_ADSB": freqs[0], - "SR1_EXT_STAT": freqs[0], - "SR1_EXTRA1": freqs[2], - "SR1_EXTRA2": freqs[2], - "SR1_EXTRA3": freqs[0], - "SR1_POSITION": freqs[1], - "SR1_RAW_SENS": freqs[1], - "SR1_RC_CHAN": freqs[0] - } - for param, value in params.items(): - self.set_param(f"UAV{sysid}", param, value) - -class DroneControlApp(QMainWindow): - def __init__(self): - super().__init__() - self.workers = MAVLinkWorker() - self.namespaces = self.workers.namespaces - self.initUI() - - # Connect signals to update the UI - self.workers.state_signal.connect(self.update_state) - self.workers.battery_signal.connect(self.update_battery) - self.workers.gps_signal.connect(self.update_gps) - self.workers.gps_status_signal.connect(self.update_gps_status) - self.workers.local_position_signal.connect(self.update_local_position) - self.workers.imu_signal.connect(self.update_imu) - self.workers.hdg_signal.connect(self.update_hdg) - self.workers.groundspeed_signal.connect(self.update_groundspeed) - self.workers.ping_signal.connect(self.update_ping) - self.workers.loss_signal.connect(self.update_loss) - self.workers.frequency_signal.connect(self.update_frequency) - self.workers.param_signal.connect(self.update_param) - self.workers.kbps_signal.connect(self.update_kbps) - self.workers.start() - - def initUI(self): - self.setWindowTitle("多無人機控制介面") - self.setGeometry(100, 100, 800, 600) - - # 主佈局 - main_layout = QVBoxLayout() - - # 無人機選擇區域 - uav_layout = QHBoxLayout() - self.uav_checkboxes = {} - for namespace in self.namespaces: - checkbox = QCheckBox(namespace) - checkbox.setChecked(True) # 預設勾選 - self.uav_checkboxes[namespace] = checkbox - uav_layout.addWidget(checkbox) - main_layout.addLayout(uav_layout) - - # 顯示所有無人機資訊，從左到右顯示 - uav_layout = QHBoxLayout() - - # 逐個顯示 UAV 的資訊 - self.uav_labels = {} - for namespace in self.namespaces: - uav_info_layout = QVBoxLayout() # 每台 UAV 的資訊垂直排列 - - # 狀態顯示 - self.uav_labels[namespace] = { - "status": QLabel("狀態：等待數據..."), - "battery": QLabel("電壓：等待數據..."), - "local_position": QLabel("Local Position：等待數據..."), - "gps": QLabel("GPS位置：等待數據...\n\n"), - "fix": QLabel("Fix Type：等待數據..."), - "satellites": QLabel("衛星數量：等待數據..."), - "groundspeed": QLabel("地面速度：等待數據..."), - "pitch": QLabel("Pitch：等待數據..."), - "heading": QLabel("Heading：等待數據..."), - "ping": QLabel("Ping：等待數據..."), - "loss": QLabel("丟包：等待數據..."), - "frequency": QLabel("頻率：等待數據..."), - "kbps": QLabel("吞吐量：等待數據..."), - "param": QLabel("SR1_EXTRA1：等待數據...") - } - - # 把每個資訊添加到該 UAV 的垂直佈局中 - for label in self.uav_labels[namespace].values(): - uav_info_layout.addWidget(label) - - # 將該 UAV 的資訊佈局加到主佈局中（從左到右排列） - uav_layout.addLayout(uav_info_layout) - - main_layout.addLayout(uav_layout) - - # SR1_EXTRA1參數設置 - param_layout = QHBoxLayout() - self.paramInput = QLineEdit() - self.paramInput.setPlaceholderText("輸入SR1_EXTRA1的新值") - self.setParamButton = QPushButton("設置SR1_EXTRA1") - self.setParamButton.clicked.connect(self.set_SR1_EXTRA1) - param_layout.addWidget(self.paramInput) - param_layout.addWidget(self.setParamButton) - main_layout.addLayout(param_layout) - - # 模式切換區域 - mode_layout = QHBoxLayout() - self.modeComboBox = QComboBox() - self.modeComboBox.addItems(["GUIDED", "STABILIZE", "AUTO", "LOITER"]) - self.modeButton = QPushButton("切換模式") - self.modeButton.clicked.connect(self.change_mode) - mode_layout.addWidget(QLabel("選擇模式：")) - mode_layout.addWidget(self.modeComboBox) - mode_layout.addWidget(self.modeButton) - main_layout.addLayout(mode_layout) - - # 解鎖按鈕 - fly_layout = QHBoxLayout() - self.armButton = QPushButton("解鎖") - self.armButton.clicked.connect(self.arm_drone) - - # 起飛按鈕 - self.takeoffButton = QPushButton("起飛") - self.takeoffButton.clicked.connect(self.takeoff_drone) - - # 降落按鈕 - self.landButton = QPushButton("降落") - self.landButton.clicked.connect(self.land_drone) - fly_layout.addWidget(self.armButton) - fly_layout.addWidget(self.takeoffButton) - fly_layout.addWidget(self.landButton) - main_layout.addLayout(fly_layout) - - # XYZ 設置欄位 - xyz_layout = QHBoxLayout() - self.positionX = QLineEdit() - self.positionX.setPlaceholderText("X") - self.positionY = QLineEdit() - self.positionY.setPlaceholderText("Y") - self.positionZ = QLineEdit() - self.positionZ.setPlaceholderText("Z") - self.setPositionButton = QPushButton("設置位置") - self.setPositionButton.clicked.connect(self.set_local_position) - xyz_layout.addWidget(QLabel("輸入位置：")) - xyz_layout.addWidget(self.positionX) - xyz_layout.addWidget(self.positionY) - xyz_layout.addWidget(self.positionZ) - xyz_layout.addWidget(self.setPositionButton) - main_layout.addLayout(xyz_layout) - - # 設置 XYZ 的按鈕 - - #設置重啟按鈕 - self.rebootButton = QPushButton("重啟") - self.rebootButton.clicked.connect(self.reboot_drone) - main_layout.addWidget(self.rebootButton) - - # 設置主佈局 - central_widget = QWidget(self) - central_widget.setLayout(main_layout) - self.setCentralWidget(central_widget) - self.show() - - def closeEvent(self, event): - try: - self.workers.stop() - finally: - event.accept() - - def update_state(self, namespace, mode): - self.uav_labels[namespace]["status"].setText(f"狀態：{mode}") - - def update_battery(self, namespace, voltage): - self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V") - - def update_gps(self, namespace, latitude, longitude): - self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°") - - def update_gps_status(self, namespace, fix_type, satellites_visible): - fix_type_str = { - 0: "No Fix", - 1: "No Fix", - 2: "2D Fix", - 3: "3D Fix", - 4: "RTK Float", - 5: "RTK Fix" - }.get(fix_type, "Unknown") - self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}") - self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}") - - def update_local_position(self, namespace, x, y, z): - self.uav_labels[namespace]["local_position"].setText(f"Local：({x:.2f}, {y:.2f}, {z:.2f})") - - def update_imu(self, namespace, pitch): - self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°") - - def update_hdg(self, namespace, heading): - self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°") - - def update_groundspeed(self, namespace, groundspeed): - self.uav_labels[namespace]["groundspeed"].setText(f"地面速度：{groundspeed:.2f} m/s") - - def update_ping(self, namespace, ping): - self.uav_labels[namespace]["ping"].setText(f"Ping：{ping:.2f} ms") - - def update_loss(self, namespace, loss): - self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%") - - def update_frequency(self, namespace, frequency): - self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz") - - def update_kbps(self, namespace, kbps): - self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s") - - def update_param(self, namespace, value): - self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}") - - def change_mode(self): - try: - selected_mode = self.modeComboBox.currentText() - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.set_mode(namespace, selected_mode) - except Exception as e: - QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}") - - def arm_drone(self): - try: - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.arm(namespace, True) - except Exception as e: - QtWidgets.QMessageBox.warning(self, "錯誤", f"解鎖失敗：{e}") - - def takeoff_drone(self): - try: - z_text = self.positionZ.text().strip() - z = float(z_text) if z_text else 5.0 - h = 2 - for i, namespace in enumerate(self.namespaces): - if self.uav_checkboxes[namespace].isChecked(): - self.workers.takeoff(namespace, z + h * i) - - except ValueError: - QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") - - def land_drone(self): - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.land(namespace) - - def set_local_position(self): - try: - x = float(self.positionX.text().strip()) - y = float(self.positionY.text().strip()) - z = float(self.positionZ.text().strip()) - h = 2 - for i, namespace in enumerate(self.namespaces): - if self.uav_checkboxes[namespace].isChecked(): - self.workers.set_local_position(namespace, x, y, z + h * i) - - except ValueError: - QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") - - def reboot_drone(self): - try: - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.reboot_drone(namespace) - except Exception as e: - QtWidgets.QMessageBox.warning(self, "錯誤", f"重啟失敗：{e}") - - def set_SR1_EXTRA1(self): - param_value = self.paramInput.text().strip() - try: - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.set_param(namespace, "SR1_EXTRA1", param_value) - self.workers.request_param(namespace, "SR1_EXTRA1") - except ValueError: - QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") - -def main(): - app = QtWidgets.QApplication(sys.argv) - window = DroneControlApp() - window.show() - sys.exit(app.exec_()) - -if __name__ == '__main__': - main() \ No newline at end of file From 9f1235197a6da568dd19797cf69040484024c45c Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 2 Feb 2026 01:04:37 +0800 Subject: [PATCH 24/33] Add/Update GUI module from wenchun --- src/GUI/comm_panel.py | 398 ++++++++++++++ src/GUI/communication.py | 662 +++++++++++++++++++++++ src/GUI/drone_panel.py | 378 ++++++++++++++ src/GUI/gui.py | 1009 ++++++++++++++++++++++++++++++++++++ src/GUI/map_layout.py | 436 ++++++++++++++++ src/GUI/mission_planner.py | 323 ++++++++++++ src/GUI/overview_table.py | 89 ++++ 7 files changed, 3295 insertions(+) create mode 100644 src/GUI/comm_panel.py create mode 100644 src/GUI/communication.py create mode 100644 src/GUI/drone_panel.py create mode 100644 src/GUI/gui.py create mode 100644 src/GUI/map_layout.py create mode 100644 src/GUI/mission_planner.py create mode 100644 src/GUI/overview_table.py diff --git a/src/GUI/comm_panel.py b/src/GUI/comm_panel.py new file mode 100644 index 0000000..82b539e --- /dev/null +++ b/src/GUI/comm_panel.py @@ -0,0 +1,398 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QLineEdit) +from PyQt6.QtCore import pyqtSignal + +class CommPanel(QWidget): + """通讯设置面板类""" + + # 定义信号 + udp_connection_added = pyqtSignal(str, int) # ip, port + udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + ws_connection_added = pyqtSignal(str) # url + ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + status_message = pyqtSignal(str, int) # message, timeout + + def __init__(self, parent=None): + super().__init__(parent) + self.udp_connections = [] + self.ws_connections = [] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(10) + + # ========== UDP MAVLink 區域 ========== + udp_title = QLabel("UDP") + udp_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(udp_title) + + # UDP 連接列表容器 + self.udp_list_widget = QWidget() + self.udp_list_layout = QVBoxLayout(self.udp_list_widget) + self.udp_list_layout.setContentsMargins(0, 0, 0, 0) + self.udp_list_layout.setSpacing(5) + layout.addWidget(self.udp_list_widget) + + # UDP 添加新連接區域 + add_udp_widget = QWidget() + add_udp_layout = QHBoxLayout(add_udp_widget) + add_udp_layout.setContentsMargins(0, 0, 0, 0) + + self.udp_ip_input = QLineEdit() + self.udp_ip_input.setText("127.0.0.1") + self.udp_ip_input.setPlaceholderText("IP") + self.udp_ip_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + self.udp_port_input = QLineEdit() + self.udp_port_input.setText("14550") + self.udp_port_input.setPlaceholderText("Port") + self.udp_port_input.setFixedWidth(80) + self.udp_port_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_udp_btn = QPushButton("添加") + add_udp_btn.clicked.connect(self._handle_add_udp) + add_udp_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_ip_input) + add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_port_input) + add_udp_layout.addWidget(add_udp_btn) + + layout.addWidget(add_udp_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + layout.addWidget(separator) + + # ========== WebSocket 區域 ========== + ws_title = QLabel("WebSocket") + ws_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(ws_title) + + # WebSocket 連接列表容器 + self.ws_list_widget = QWidget() + self.ws_list_layout = QVBoxLayout(self.ws_list_widget) + self.ws_list_layout.setContentsMargins(0, 0, 0, 0) + self.ws_list_layout.setSpacing(5) + layout.addWidget(self.ws_list_widget) + + # WebSocket 添加新連接區域 + add_ws_widget = QWidget() + add_ws_layout = QHBoxLayout(add_ws_widget) + add_ws_layout.setContentsMargins(0, 0, 0, 0) + + self.ws_url_input = QLineEdit() + self.ws_url_input.setPlaceholderText("host") + self.ws_url_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_ws_btn = QPushButton("添加") + add_ws_btn.clicked.connect(self._handle_add_ws) + add_ws_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) + add_ws_layout.addWidget(self.ws_url_input) + add_ws_layout.addWidget(add_ws_btn) + + layout.addWidget(add_ws_widget) + layout.addStretch() + + def _handle_add_udp(self): + """處理添加 UDP 連接""" + ip = self.udp_ip_input.text().strip() + port_text = self.udp_port_input.text().strip() + + if not ip or not port_text: + self.status_message.emit("請輸入 IP 和 Port", 3000) + return + + try: + port = int(port_text) + if port < 1 or port > 65535: + raise ValueError("Port 超出範圍") + except ValueError: + self.status_message.emit("Port 必須是 1-65535 的數字", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.udp_connections: + if conn['ip'] == ip and conn['port'] == port: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.udp_connection_added.emit(ip, port) + + # 清空輸入框 + self.udp_ip_input.clear() + self.udp_port_input.clear() + + def _handle_add_ws(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.status_message.emit("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴 + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.status_message.emit("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.ws_connection_added.emit(url) + + # 清空輸入框 + self.ws_url_input.clear() + + def create_udp_connection_panel(self, conn): + """創建 UDP 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def create_ws_connection_panel(self, conn): + """創建 WebSocket 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['url']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def add_udp_panel(self, conn): + """添加 UDP 連接面板到列表""" + panel = self.create_udp_connection_panel(conn) + self.udp_list_layout.addWidget(panel) + self.udp_connections.append(conn) + return panel + + def add_ws_panel(self, conn): + """添加 WebSocket 連接面板到列表""" + panel = self.create_ws_connection_panel(conn) + self.ws_list_layout.addWidget(panel) + self.ws_connections.append(conn) + return panel + + def remove_udp_connection_from_list(self, conn): + """從列表中移除 UDP 連接""" + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + def remove_ws_connection_from_list(self, conn): + """從列表中移除 WebSocket 連接""" + if conn in self.ws_connections: + self.ws_connections.remove(conn) \ No newline at end of file diff --git a/src/GUI/communication.py b/src/GUI/communication.py new file mode 100644 index 0000000..45c960c --- /dev/null +++ b/src/GUI/communication.py @@ -0,0 +1,662 @@ +from rclpy.node import Node +from PyQt6.QtCore import QObject, pyqtSignal +import math +import re +import threading +from threading import Lock +import asyncio +import websockets +import json +import socket +from pymavlink import mavutil +from geometry_msgs.msg import Point, Vector3 +from sensor_msgs.msg import BatteryState, NavSatFix, Imu +from std_msgs.msg import Float64 +from mavros_msgs.msg import State, VfrHud +from mavros_msgs.srv import CommandBool, CommandTOL + +class DroneSignals(QObject): + update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) + +class UDPMavlinkReceiver(threading.Thread): + """UDP MAVLink 接收器""" + def __init__(self, ip, port, signals, connection_name): + super().__init__(daemon=True) + self.ip = ip + self.port = port + self.signals = signals + self.connection_name = connection_name + self.running = False + self.sock = None + + def run(self): + """執行 UDP 接收循環""" + self.running = True + try: + print(f"UDP MAVLink receiver started on {self.ip}:{self.port}") + + # 創建 MAVLink 連接 + mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}') + while self.running: + try: + msg = mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except socket.timeout: + continue + except Exception as e: + print(f"Error receiving MAVLink message: {e}") + + except Exception as e: + print(f"UDP receiver error: {e}") + finally: + if self.sock: + self.sock.close() + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class SerialMavlinkReceiver(threading.Thread): + """串口 MAVLink 接收器""" + def __init__(self, port, baudrate, signals, connection_name): + super().__init__(daemon=True) + self.port = port + self.baudrate = baudrate + self.signals = signals + self.connection_name = connection_name + self.running = False + self.mav = None + + def run(self): + """執行串口接收循環""" + self.running = True + try: + print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud") + + # 創建 MAVLink 串口連接 + self.mav = mavutil.mavlink_connection( + self.port, + baud=self.baudrate, + source_system=255 + ) + + print(f"Waiting for heartbeat from {self.port}...") + self.mav.wait_heartbeat() + print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}") + + while self.running: + try: + msg = self.mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except Exception as e: + if self.running: + print(f"Error receiving MAVLink message from serial: {e}") + + except Exception as e: + print(f"Serial receiver error: {e}") + finally: + if self.mav: + try: + self.mav.close() + except: + pass + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message from serial: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class WebSocketMavlinkReceiver(threading.Thread): + """WebSocket MAVLink 接收器""" + def __init__(self, url, signals, connection_name): + super().__init__(daemon=True) + self.url = url + self.signals = signals + self.connection_name = connection_name + self.running = False + self.max_retries = 5 + self.base_delay = 1.0 + + def run(self): + """執行 WebSocket 接收循環""" + self.running = True + asyncio.set_event_loop(asyncio.new_event_loop()) + asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + + async def ws_client_loop(self): + """WebSocket 連接的主循環""" + retry_count = 0 + + print(f"Starting WebSocket client for {self.connection_name} at {self.url}") + + while self.running and retry_count < self.max_retries: + try: + async with websockets.connect(self.url) as websocket: + print(f"WebSocket {self.connection_name} connected to {self.url}") + retry_count = 0 # 重置重試計數 + + async for message in websocket: + if not self.running: + break + + try: + data = json.loads(message) + data['_connection_source'] = self.connection_name + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket {self.connection_name} JSON decode error: {e}") + except Exception as e: + print(f"WebSocket {self.connection_name} message processing error: {e}") + + except websockets.exceptions.ConnectionClosedError: + print(f"WebSocket {self.connection_name} connection closed") + if self.running: + retry_count += 1 + if retry_count < self.max_retries: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"Reconnecting in {delay}s...") + await asyncio.sleep(delay) + else: + break + except Exception as e: + retry_count += 1 + if retry_count < self.max_retries and self.running: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})") + await asyncio.sleep(delay) + else: + break + + print(f"WebSocket client {self.connection_name} stopped") + + def process_websocket_message(self, data): + """處理 WebSocket 訊息""" + try: + drone_id = data.get('system_id') + drone_id = f"s9_{drone_id}" if drone_id else None + if not drone_id: + return + + # 模式 + if 'mode' in data: + self.signals.update_signal.emit('state', drone_id, { + 'mode': data['mode'], + }) + + # 電池 + if 'battery' in data: + self.signals.update_signal.emit('battery', drone_id, { + 'percentage': data['battery'] + }) + + # 位置 + if 'position' in data: + pos = data['position'] + self.signals.update_signal.emit('gps', drone_id, { + 'lat': pos.get('lat', 0), + 'lon': pos.get('lon', 0) + }) + + # Local position - 設定 x, y 為 0.0 + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': 0.0, + 'y': 0.0, + 'z': 0.0 + }) + + # Altitude - 設定為 0.0 + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': 0.0 + }) + + # 航向 + if 'heading' in data: + self.signals.update_signal.emit('hud', drone_id, { + 'heading': data['heading'], + 'groundspeed': 0.0 + }) + + except Exception as e: + print(f"WebSocket message processing error: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class DroneMonitor(Node): + def __init__(self): + super().__init__('drone_monitor') + self.signals = DroneSignals() + self.drone_topics = {} + self.lock = Lock() + + self.arm_clients = {} + self.takeoff_clients = {} + self.setpoint_pubs = {} + self.selected_drones = set() + self.latest_data = {} + + # 定義需要過濾的模式 + self.filtered_modes = ['Mode(0x000000c0)'] + + # WebSocket 接收器列表 + self.ws_receivers = [] + + # 串口接收器列表 + + # ================================================================================ + # 【新增】儲存 GPS 資料的字典 + # ================================================================================ + self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}} + # ================================================================================ + self.serial_receivers = [] + + # 主题检测定时器 + self.create_timer(1.0, self.scan_topics) + + def scan_topics(self): + topics = self.get_topic_names_and_types() + drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') + + found_drones = set() + for topic_name, _ in topics: + if match := drone_pattern.match(topic_name): + drone_id, topic_type = match.groups() + found_drones.add(drone_id) + with self.lock: + self.drone_topics.setdefault(drone_id, set()).add(topic_type) + + for drone_id in found_drones: + if not hasattr(self, f'drone_{drone_id}_subs'): + self.setup_drone(drone_id) + + def setup_drone(self, drone_id): + base_topic = f'/MavLinkBus/{drone_id}' + + # Add service clients + self.arm_clients[drone_id] = self.create_client( + CommandBool, + f'{base_topic}/cmd/arming' + ) + self.takeoff_clients[drone_id] = self.create_client( + CommandTOL, + f'{base_topic}/cmd/takeoff' + ) + + # Add setpoint publisher + self.setpoint_pubs[drone_id] = self.create_publisher( + Point, + f'{base_topic}/setpoint_position/local', + 10 + ) + + subs = { + 'attitude': self.create_subscription( + Imu, + f'{base_topic}/attitude', + lambda msg, did=drone_id: self.attitude_callback(did, msg), + 10 + ), + 'battery': self.create_subscription( + BatteryState, + f'{base_topic}/battery', + lambda msg, did=drone_id: self.battery_callback(did, msg), + 10 + ), + 'global': self.create_subscription( + NavSatFix, + f'{base_topic}/global_position/global', + lambda msg, did=drone_id: self.gps_callback(did, msg), + 10 + ), + 'rel_alt': self.create_subscription( + Float64, + f'{base_topic}/global_position/rel_alt', + lambda msg, did=drone_id: self.altitude_callback(did, msg), + 10 + ), + 'local_pose': self.create_subscription( + Point, + f'{base_topic}/local_position/pose', + lambda msg, did=drone_id: self.local_pose_callback(did, msg), + 10 + ), + 'local_vel': self.create_subscription( + Vector3, + f'{base_topic}/local_position/velocity', + lambda msg, did=drone_id: self.local_vel_callback(did, msg), + 10 + ), + 'loss_rate': self.create_subscription( + Float64, + f'{base_topic}/packet_loss_rate', + lambda msg, did=drone_id: self.loss_rate_callback(did, msg), + 10 + ), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), + 10 + ), + 'ping': self.create_subscription( + Float64, + f'{base_topic}/ping', + lambda msg, did=drone_id: self.ping_callback(did, msg), + 10 + ), + 'vfr_hud': self.create_subscription( + VfrHud, + f'{base_topic}/vfr_hud', + lambda msg, did=drone_id: self.hud_callback(did, msg), + 10 + ) + } + + setattr(self, f'drone_{drone_id}_subs', subs) + + async def arm_drone(self, drone_id, arm): + if drone_id not in self.arm_clients: + return False + + client = self.arm_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandBool.Request() + request.value = arm + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Arm service call failed: {e}') + return False + + async def takeoff_drone(self, drone_id, altitude=10.0): + if drone_id not in self.takeoff_clients: + return False + + client = self.takeoff_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandTOL.Request() + request.altitude = altitude + request.min_pitch = 0.0 + request.yaw = 0.0 + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Takeoff service call failed: {e}') + return False + + def send_setpoint(self, drone_id, x, y, z): + """Send setpoint position command""" + if drone_id not in self.setpoint_pubs: + return False + + msg = Point() + msg.x = float(x) + msg.y = float(y) + msg.z = float(z) + + self.setpoint_pubs[drone_id].publish(msg) + return True + + def quaternion_to_euler(self, q): + sinr_cosp = 2 * (q.w * q.x + q.y * q.z) + cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) + roll = math.atan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (q.w * q.y - q.z * q.x) + pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp) + + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y**2 + q.z**2) + yaw = math.atan2(siny_cosp, cosy_cosp) + + return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) + + # callbacks + def attitude_callback(self, drone_id, msg): + if hasattr(msg, 'orientation'): + roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) + self.latest_data[(drone_id, 'attitude')] = { + 'roll': roll, + 'pitch': pitch, + 'yaw': yaw, + 'rates': (msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z) + } + + def battery_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'battery')] = { + 'voltage': msg.voltage + } + + def state_callback(self, drone_id, msg): + mode = msg.mode + if mode in self.filtered_modes: + return + self.latest_data[(drone_id, 'state')] = { + 'mode': msg.mode, + 'armed': msg.armed + } + + def gps_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'gps')] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + + # ================================================================================ + # 【新增】儲存 GPS 資料到 drone_gps 字典 + # ================================================================================ + self.drone_gps[drone_id] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + # ================================================================================ + + def local_vel_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'velocity')] = { + 'vx': msg.x, + 'vy': msg.y, + 'vz': msg.z + } + + def altitude_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'altitude')] = { + 'altitude': msg.data + } + + def local_pose_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'local_pose')] = { + 'x': msg.x, + 'y': msg.y, + 'z': msg.z + } + + def hud_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'hud')] = { + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.altitude, + 'climb': msg.climb + } + + def loss_rate_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'loss_rate')] = { + 'loss_rate': msg.data + } + + def ping_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'ping')] = { + 'ping': msg.data + } + + def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): + """啟動串口 MAVLink 連接""" + connection_name = f"Serial_{port.replace('/', '_')}" + receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name) + receiver.start() + self.serial_receivers.append(receiver) + print(f"Started serial connection on {port} at {baudrate} baud") + return receiver \ No newline at end of file diff --git a/src/GUI/drone_panel.py b/src/GUI/drone_panel.py new file mode 100644 index 0000000..a5a7bf5 --- /dev/null +++ b/src/GUI/drone_panel.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QSizePolicy, QCheckBox) +from PyQt6.QtCore import pyqtSignal + +class DronePanel(QWidget): + """單個無人機面板類別""" + + # 定義信號 + mode_change_requested = pyqtSignal(str) # drone_id + arm_requested = pyqtSignal(str) # drone_id + takeoff_requested = pyqtSignal(str) # drone_id + setpoint_requested = pyqtSignal(str) # drone_id + selection_changed = pyqtSignal(str, int) # drone_id, state + + def __init__(self, drone_id, parent=None): + super().__init__(parent) + self.drone_id = drone_id + self.display_id = 's' + drone_id.split('_')[1] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"panel_{self.drone_id}") + self.setFixedHeight(140) + self.setStyleSheet(""" + background-color: #2A2A2A; + border-radius: 8px; + """) + + # 主佈局 + main_layout = QHBoxLayout(self) + main_layout.setContentsMargins(8, 8, 8, 8) + main_layout.setSpacing(0) + + # 創建內容容器（包含 info 和 control） + content_widget = QWidget() + content_widget.setStyleSheet("background-color: #333; border-radius: 6px;") + content_layout = QHBoxLayout(content_widget) + content_layout.setContentsMargins(8, 8, 8, 8) + content_layout.setSpacing(8) + + # 左側資訊區域 + info_widget = self._create_info_section() + + # 右側控制按鈕區域 + control_widget = self._create_control_section() + + # 將 info 和 control 加入內容容器 + content_layout.addWidget(info_widget) + content_layout.addWidget(control_widget) + + # 將內容容器加入主佈局 + main_layout.addWidget(content_widget) + + def _create_info_section(self): + """創建資訊顯示區域""" + info_widget = QWidget() + info_layout = QVBoxLayout(info_widget) + info_layout.setContentsMargins(0, 0, 0, 0) + info_layout.setSpacing(4) + + # 頂部標題欄 + header = QWidget() + header_layout = QHBoxLayout(header) + header_layout.setContentsMargins(0, 0, 0, 0) + + # 勾選框 + self.checkbox = QCheckBox() + self.checkbox.setObjectName(f"{self.drone_id}_checkbox") + self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }") + self.checkbox.stateChanged.connect( + lambda state: self.selection_changed.emit(self.drone_id, state) + ) + + # ID 顯示 + id_label = QLabel(self.display_id) + id_label.setStyleSheet(""" + font-weight: bold; + font-size: 14px; + color: #7FFFD4; + min-width: 80px; + """) + + header_layout.addWidget(self.checkbox) + header_layout.addWidget(id_label) + header_layout.addStretch() + + info_layout.addWidget(header) + + # 第一行：狀態 (模式 + ARM狀態) + status_row = self._create_status_row() + info_layout.addWidget(status_row) + + # 第二行：電池 + battery_row = self._create_battery_row() + info_layout.addWidget(battery_row) + + # 第三行：位置 + 高度 + position_row = self._create_position_row() + info_layout.addWidget(position_row) + + # 第四行：航向 + 速度 + nav_row = self._create_nav_row() + info_layout.addWidget(nav_row) + + return info_widget + + def _create_status_row(self): + """創建狀態行""" + status_row = QWidget() + status_layout = QHBoxLayout(status_row) + status_layout.setContentsMargins(0, 0, 0, 0) + + status_title = QLabel("狀態：") + status_title.setStyleSheet("color: #888; min-width: 50px;") + + self.mode_label = QLabel("--") + self.mode_label.setObjectName(f"{self.drone_id}_mode") + self.mode_label.setStyleSheet("color: #DDD;") + + self.armed_label = QLabel("--") + self.armed_label.setObjectName(f"{self.drone_id}_armed") + self.armed_label.setStyleSheet("color: #DDD;") + + status_layout.addWidget(status_title) + status_layout.addWidget(self.mode_label) + status_layout.addWidget(self.armed_label) + status_layout.addStretch() + + return status_row + + def _create_battery_row(self): + """創建電池行""" + battery_row = QWidget() + battery_layout = QHBoxLayout(battery_row) + battery_layout.setContentsMargins(0, 0, 0, 0) + + battery_title = QLabel("電池：") + battery_title.setStyleSheet("color: #888; min-width: 50px;") + + self.battery_label = QLabel("--") + self.battery_label.setObjectName(f"{self.drone_id}_battery") + self.battery_label.setStyleSheet("color: #DDD;") + + battery_layout.addWidget(battery_title) + battery_layout.addWidget(self.battery_label) + battery_layout.addStretch() + + return battery_row + + def _create_position_row(self): + """創建位置行""" + position_row = QWidget() + position_layout = QHBoxLayout(position_row) + position_layout.setContentsMargins(0, 0, 0, 0) + + position_title = QLabel("位置：") + position_title.setStyleSheet("color: #888; min-width: 50px;") + + self.local_label = QLabel("--") + self.local_label.setObjectName(f"{self.drone_id}_local") + self.local_label.setStyleSheet("color: #DDD;") + + altitude_title = QLabel("高度：") + altitude_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.altitude_label = QLabel("--") + self.altitude_label.setObjectName(f"{self.drone_id}_altitude") + self.altitude_label.setStyleSheet("color: #DDD;") + + position_layout.addWidget(position_title) + position_layout.addWidget(self.local_label) + position_layout.addWidget(altitude_title) + position_layout.addWidget(self.altitude_label) + position_layout.addStretch() + + return position_row + + def _create_nav_row(self): + """創建導航行""" + nav_row = QWidget() + nav_layout = QHBoxLayout(nav_row) + nav_layout.setContentsMargins(0, 0, 0, 0) + + heading_title = QLabel("航向：") + heading_title.setStyleSheet("color: #888; min-width: 50px;") + + self.heading_label = QLabel("--") + self.heading_label.setObjectName(f"{self.drone_id}_heading") + self.heading_label.setStyleSheet("color: #DDD;") + + speed_title = QLabel("速度：") + speed_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.groundspeed_label = QLabel("--") + self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed") + self.groundspeed_label.setStyleSheet("color: #DDD;") + + nav_layout.addWidget(heading_title) + nav_layout.addWidget(self.heading_label) + nav_layout.addWidget(speed_title) + nav_layout.addWidget(self.groundspeed_label) + nav_layout.addStretch() + + return nav_row + + def _create_control_section(self): + """創建控制按鈕區域""" + control_widget = QWidget() + control_layout = QVBoxLayout(control_widget) + control_layout.setContentsMargins(0, 0, 0, 0) + control_layout.setSpacing(6) + + control_widget.setFixedWidth(80) + + btn_style = """ + QPushButton { + background-color: #444; + color: #DDD; + border: none; + border-radius: 4px; + font-size: 11px; + } + QPushButton:hover { + background-color: #555; + } + """ + # 模式切換按鈕 + mode_btn = QPushButton("切換模式") + mode_btn.setStyleSheet(btn_style) + mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id)) + + # 解鎖按鈕 + arm_btn = QPushButton("解鎖") + arm_btn.setStyleSheet(btn_style) + arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id)) + + # 起飛按鈕 + takeoff_btn = QPushButton("起飛") + takeoff_btn.setStyleSheet(btn_style) + takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id)) + + # Setpoint 按鈕 + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.setStyleSheet(btn_style) + setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id)) + + control_layout.addWidget(mode_btn) + control_layout.addWidget(arm_btn) + control_layout.addWidget(takeoff_btn) + control_layout.addWidget(setpoint_btn) + + return control_widget + + def update_field(self, field, text, color=None): + """更新指定欄位的值""" + label = self.findChild(QLabel, f"{self.drone_id}_{field}") + if label and label.text() != text: + label.setText(text) + if color: + label.setStyleSheet(f"color: {color};") + + def get_checkbox(self): + """獲取勾選框""" + return self.checkbox + + def set_checked(self, checked): + """設置勾選狀態""" + self.checkbox.setChecked(checked) + + def is_checked(self): + """獲取勾選狀態""" + return self.checkbox.isChecked() + +class SocketGroupPanel(QWidget): + # 定義信號 + group_selection_changed = pyqtSignal(str, int) # socket_id, state + + def __init__(self, socket_id, color='#AAAAAA', parent=None): + super().__init__(parent) + self.socket_id = socket_id + self.color = color + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"socket_group_{self.socket_id}") + self.setStyleSheet(""" + background-color: #1E1E1E; + border-radius: 12px; + """) + + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(6) + + # Socket 分組標題行 - 包含勾選框 + title_row = QWidget() + title_layout = QHBoxLayout(title_row) + title_layout.setContentsMargins(0, 0, 0, 0) + + # 分組勾選框 + self.group_checkbox = QCheckBox() + self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox") + self.group_checkbox.setStyleSheet(f""" + QCheckBox {{ color: #DDD; }} + QCheckBox::indicator {{ + width: 14px; + height: 14px; + border: 2px solid #888888; + border-radius: 3px; + background: transparent; + }} + QCheckBox::indicator:checked {{ + background-color: {self.color}; + border: 2px solid #888888; + }} + QCheckBox::indicator:indeterminate {{ + background-color: #666; + border: 2px solid #888888; + }} + """) + self.group_checkbox.stateChanged.connect( + lambda state: self.group_selection_changed.emit(self.socket_id, state) + ) + + # Socket 分組標題 + title_label = QLabel(f"Socket {self.socket_id}") + title_label.setStyleSheet(f""" + font-weight: bold; + font-size: 16px; + color: {self.color}; + margin-bottom: 8px; + padding: 4px 8px; + border-radius: 6px; + """) + + title_layout.addWidget(self.group_checkbox) + title_layout.addWidget(title_label) + title_layout.addStretch() + + layout.addWidget(title_row) + + # 創建子容器用於放置該 socket 下的所有無人機面板 + self.drones_container = QWidget() + self.drones_layout = QVBoxLayout(self.drones_container) + self.drones_layout.setContentsMargins(0, 0, 0, 0) + self.drones_layout.setSpacing(4) + + layout.addWidget(self.drones_container) + + def add_drone_panel(self, panel): + """添加無人機面板到分組""" + self.drones_layout.addWidget(panel) + + def clear_drones(self): + """清空所有無人機面板""" + while self.drones_layout.count(): + item = self.drones_layout.takeAt(0) + if item.widget(): + item.widget().setParent(None) + + def get_checkbox(self): + """獲取分組勾選框""" + return self.group_checkbox + + def set_checked(self, checked): + """設置分組勾選狀態""" + self.group_checkbox.setChecked(checked) + + def set_check_state(self, state): + """設置分組勾選狀態（支持半選）""" + self.group_checkbox.setCheckState(state) \ No newline at end of file diff --git a/src/GUI/gui.py b/src/GUI/gui.py new file mode 100644 index 0000000..cefc9c9 --- /dev/null +++ b/src/GUI/gui.py @@ -0,0 +1,1009 @@ +#!/usr/bin/env python3 +import rclpy +from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, + QWidget, QLabel, QSplitter, QScrollArea, + QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, + QHeaderView, QPushButton, QCheckBox, QLineEdit) +from PyQt6.QtCore import Qt, QTimer +import sys +import asyncio + +# 導入分離的類別 +from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver +from map_layout import DroneMap +from drone_panel import DronePanel, SocketGroupPanel +from comm_panel import CommPanel +from overview_table import OverviewTable + +# ================================================================================ +# 【新增】導入任務規劃器 +# ================================================================================ +from mission_planner import FormationPlanner +# ================================================================================ + +class ControlStationUI(QMainWindow): + def __init__(self): + super().__init__() + self.setWindowTitle('GCS') + self.resize(1400, 900) + + # 初始化ROS2 + rclpy.init() + self.monitor = DroneMonitor() + self.monitor.signals.update_signal.connect(self.update_ui) + + # ROS执行器配置 + self.executor = rclpy.executors.SingleThreadedExecutor() + self.executor.add_node(self.monitor) + + # 定时处理ROS事件 + self.timer = QTimer() + self.timer.timeout.connect(self.spin_ros) + self.timer.start(10) + + # 初始化UI + self.drones = {} + self.socket_groups = {} + self.info_types = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "Local", "Velocity", "地速", "航向", + "Roll", "Pitch", "Yaw", "丟包", "延遲"] + self.info_type_map = { + "mode": 0, + "armed": 1, + "battery": 2, + "longitude": 3, + "latitude": 4, + "altitude": 5, + "local": 6, + "velocity": 7, + "groundspeed": 8, + "heading": 9, + "roll": 10, + "pitch": 11, + "yaw": 12, + "loss_rate": 13, + "ping": 14 + } + + self.socket_colors = { + '0': '#00BFFF', # 天藍色 (DeepSkyBlue) + '1': '#FFD700', # 金色 (Gold) + '2': '#FF6969', # 淺紅色 (Light Red) + '3': '#FF69B4', # 熱粉紅 (HotPink) + '4': '#00FA9A', # 中春綠 (MediumSpringGreen) + '5': '#9370DB', # 中紫色 (MediumPurple) - 串口 + '6': '#FFA500', # 橙色 (Orange) + '7': '#20B2AA', # 淺海綠 (LightSeaGreen) + '8': '#7CFC00', # 草綠色 (LawnGreen) + '9': '#FF8C00', # 深橙色 (DarkOrange) + 'default': '#AAAAAA' # 灰色 + } + + self.drone_positions = {} + self.drone_headings = {} + # 初始化地圖 + self.drone_map = DroneMap() + # 初始化連接列表 + self.udp_receivers = [] + self.udp_connections = [] + self.ws_connections = [] + self.monitor.start_serial_connection('/dev/ttyUSB0', 57600) + + # ================================================================================ + # 【新增】初始化任務規劃器 + # ================================================================================ + self.mission_planner = FormationPlanner( + spacing=5.0, # 5 公尺間距 + base_altitude=10.0, # 基準高度 10 公尺 + altitude_diff=2.0 # 高低差 2 公尺 + ) + self.planned_waypoints = None # 儲存規劃結果 + # ================================================================================ + + self.init_ui() + + def init_ui(self): + # 只呼叫一次 + main_splitter = QSplitter(Qt.Orientation.Horizontal) + + # 左側 TabWidget + self.left_tab = QTabWidget() + + # — 分頁 1：Drone Panel + self.drone_panel_container = QWidget() + self.drone_panel_layout = QVBoxLayout(self.drone_panel_container) + self.drone_panel_layout.setAlignment(Qt.AlignmentFlag.AlignTop) + self.drone_panel_layout.setSpacing(0) + self.drone_panel_layout.setContentsMargins(10, 10, 10, 10) + + # Wrap drone panel in scroll area + scroll = QScrollArea() + scroll.setWidget(self.drone_panel_container) + scroll.setWidgetResizable(True) + self.left_tab.addTab(scroll, "無人載具") + + # — 分頁 2：Overview Table + self.overview_table = OverviewTable(self.info_types, self.info_type_map) + self.left_tab.addTab(self.overview_table, "總覽") + + # — 分頁 3：通訊設定 + self.comm_panel = CommPanel() + self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) + self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) + self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection) + self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection) + self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection) + self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection) + self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout)) + + self.left_tab.addTab(self.comm_panel, "通訊") + + # 右侧容器 + right_container = QWidget() + right_layout = QVBoxLayout(right_container) + right_layout.setContentsMargins(10, 10, 10, 10) + right_layout.setSpacing(10) + + # ========== 批次控制區域（直接使用 layout） ========== + batch_control_layout = QHBoxLayout() + + # 添加批次操作標題 + batch_title = QLabel("批次操作") + batch_title.setStyleSheet(""" + color: #DDD; + font-size: 16px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + batch_control_layout.addWidget(batch_title) + + # 上方按鈕區域 + upper_buttons = QHBoxLayout() + select_all_btn = QPushButton("全選") + select_all_btn.clicked.connect(self.handle_select_all) + arm_all_btn = QPushButton("解鎖") + arm_all_btn.clicked.connect(self.handle_arm_selected) + takeoff_all_btn = QPushButton("起飛") + takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) + for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]: + btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + upper_buttons.addWidget(btn) + upper_buttons.addStretch() + + # 模式切換區域 + mode_layout = QHBoxLayout() + mode_label = QLabel("模式：") + mode_label.setStyleSheet("color: #DDD; min-width: 40px;") + + from PyQt6.QtWidgets import QComboBox + self.mode_combo = QComboBox() + self.mode_combo.addItems(["AUTO", "GUIDED", "LOITER", "LAND"]) + self.mode_combo.setCurrentIndex(1) + self.mode_combo.setStyleSheet(""" + QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} + """) + + batch_mode_btn = QPushButton("切換") + batch_mode_btn.clicked.connect(self.handle_batch_mode_change) + batch_mode_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + mode_layout.addWidget(mode_label) + mode_layout.addWidget(self.mode_combo) + mode_layout.addWidget(batch_mode_btn) + mode_layout.addStretch() + + # 座標輸入區域 + coord_widget = QWidget() + coord_layout = QHBoxLayout(coord_widget) + + self.x_input = QLineEdit() + self.y_input = QLineEdit() + self.z_input = QLineEdit() + + for input_field in [self.x_input, self.y_input, self.z_input]: + input_field.setFixedWidth(60) + input_field.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 3px; + } + """) + + coord_layout.addWidget(QLabel("X:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.x_input) + coord_layout.addWidget(QLabel("Y:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.y_input) + coord_layout.addWidget(QLabel("Z:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.z_input) + + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.clicked.connect(self.handle_setpoint_selected) + setpoint_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + + lower_control = QHBoxLayout() + lower_control.addWidget(coord_widget) + lower_control.addWidget(setpoint_btn) + lower_control.addStretch() + + # 組裝批次控制 layout + batch_control_layout.addLayout(upper_buttons) + batch_control_layout.addLayout(mode_layout) + batch_control_layout.addLayout(lower_control) + + # 將批次控制 layout 添加到右側容器 + right_layout.addLayout(batch_control_layout) + + # 添加地圖 + right_layout.addWidget(self.drone_map.get_widget()) + self.drone_map.get_gps_signal().connect(self.handle_map_click) + + + # Add widgets to splitter + main_splitter.addWidget(self.left_tab) + main_splitter.addWidget(right_container) + main_splitter.setSizes([400, 1000]) + + self.setCentralWidget(main_splitter) + + + def handle_udp_connection_added(self, ip, port): + """处理 UDP 连接添加信号""" + # 创建新连接 + new_conn = { + 'name': f'UDP {len(self.udp_connections) + 1}', + 'ip': ip, + 'port': port, + 'enabled': True + } + + # 启动接收器 + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.udp_connections.append(new_conn) + + # 添加到 UI + self.comm_panel.add_udp_panel(new_conn) + + self.statusBar().showMessage(f"已添加 UDP 连接: {ip}:{port}", 3000) + print(f"UDP MAVLink receiver added and started on {ip}:{port}") + + def handle_ws_connection_added(self, url): + """处理 WebSocket 连接添加信号""" + # 创建新连接 + new_conn = { + 'name': f'WS {len(self.ws_connections) + 1}', + 'url': url, + 'enabled': True + } + + # 启动接收器 + receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.ws_connections.append(new_conn) + + # 添加到 UI + self.comm_panel.add_ws_panel(new_conn) + + self.statusBar().showMessage(f"已添加 WebSocket 连接: {url}", 3000) + print(f"WebSocket receiver added and started: {url}") + + def create_drone_panel(self, drone_id): + """創建無人機面板""" + panel = DronePanel(drone_id) + + # 連接信號 + panel.mode_change_requested.connect(self.handle_mode_change) + panel.arm_requested.connect(self.handle_arm) + panel.takeoff_requested.connect(self.handle_takeoff) + panel.setpoint_requested.connect(self.handle_single_setpoint) + panel.selection_changed.connect(self.handle_drone_selection) + + return panel + + def toggle_ws_connection(self, conn, btn, status_label): + """切換 WebSocket 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000) + else: + # 啟動連接 + receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) + + def remove_ws_connection(self, conn, panel): + """移除 WebSocket 连接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.monitor.ws_receivers: + self.monitor.ws_receivers.remove(conn['receiver']) + + # 从列表移除 + if conn in self.ws_connections: + self.ws_connections.remove(conn) + + # 从 comm_panel 列表移除 + self.comm_panel.remove_ws_connection_from_list(conn) + + # 从 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 WebSocket 连接: {conn['url']}", 3000) + + def toggle_udp_connection(self, conn, btn, status_label): + """切換 UDP 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + else: + # 啟動連接 + receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + + def remove_udp_connection(self, conn, panel): + """移除 UDP 连接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.udp_receivers: + self.udp_receivers.remove(conn['receiver']) + + # 从列表移除 + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + # 从 comm_panel 列表移除 + self.comm_panel.remove_udp_connection_from_list(conn) + + # 从 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000) + + def create_socket_group_panel(self, socket_id): + """創建 socket 分組面板""" + color = self.socket_colors.get(socket_id, self.socket_colors['default']) + panel = SocketGroupPanel(socket_id, color) + panel.group_selection_changed.connect(self.handle_group_selection) + return panel + + def handle_mode_change(self, drone_id): + """處理單個無人機的模式切換""" + mode = self.mode_combo.currentText() + loop = asyncio.get_event_loop() + future = self.monitor.set_mode(drone_id, mode) + loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}")) + + def handle_arm(self, drone_id): + loop = asyncio.get_event_loop() + arm_state = not self.monitor.get_arm_state(drone_id) # Toggle arm state + future = self.monitor.arm_drone(drone_id, arm_state) + loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}")) + + def handle_takeoff(self, drone_id): + loop = asyncio.get_event_loop() + future = self.monitor.takeoff_drone(drone_id, 10.0) # 10 meters default + loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}")) + + def handle_setpoint_selected(self): + """處理發送 setpoint 命令""" + try: + x = float(self.x_input.text() or '0') + y = float(self.y_input.text() or '0') + z = float(self.z_input.text() or '0') + + for drone_id in self.monitor.selected_drones: + if self.monitor.send_setpoint(drone_id, x, y, z): + self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000) + else: + self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000) + except ValueError: + self.statusBar().showMessage("座標格式錯誤", 3000) + + def handle_single_setpoint(self, drone_id): + """處理單個無人機的 setpoint 命令""" + try: + x = float(self.x_input.text() or '0') + y = float(self.y_input.text() or '0') + z = float(self.z_input.text() or '0') + + if self.monitor.send_setpoint(drone_id, x, y, z): + self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000) + else: + self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000) + except ValueError: + self.statusBar().showMessage("座標格式錯誤", 3000) + + async def handle_service_response(self, future, action): + try: + result = await future + if result: + self.statusBar().showMessage(f"{action} 成功", 3000) + else: + self.statusBar().showMessage(f"{action} 失敗", 3000) + except Exception as e: + self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) + + def update_ui(self, msg_type, drone_id, data): + # 檢查是否為新無人機，若是則加入無人機面板 + if drone_id not in self.drones: + self.add_drone(drone_id) + return + + # 確認無人機面板存在 + if not (panel := self.drones.get(drone_id)): + return + + # 更新特定欄位並在總覽頁面更新 + if msg_type == 'state': + mode = data.get('mode', 'UNKNOWN') + armed = data.get('armed', None) + mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' + if armed is True: + arm_text = "ARMED" + arm_color = '#55FF55' + elif armed is False: + arm_text = "DISARMED" + arm_color = '#FF5555' + else: + arm_text = "--" + arm_color = '#AAAAAA' # 未知狀態 + + # 更新無人機面板欄位 + self.update_field(panel, drone_id, 'mode', mode, mode_color) + self.update_field(panel, drone_id, 'armed', arm_text, arm_color) + + # 更新總覽頁面欄位 + self.update_overview_table(drone_id, 'mode', mode) + self.update_overview_table(drone_id, 'armed', arm_text) + + elif msg_type == 'battery': + voltage = data.get('voltage', 16) + + # 判斷電池節數 + cells = round(voltage / 3.95) + + # 計算電量百分比 + percentage = (voltage / cells - 3.7) / 0.5 * 100 + + # 根據百分比設置顏色 + if percentage < 20: + voltage_color = '#FF6464' # 紅色 (低電量) + elif percentage < 50: + voltage_color = '#FFA500' # 橘色 (中低電量) + else: + voltage_color = '#FFFFFF' # 白色 (正常電量) + + percentage = data.get('percentage', percentage) + + # 顯示總電壓、電池節數、單節電壓和百分比 + text = f"{percentage:.0f}%" + vol = f"{voltage:.2f}V" + + self.update_field(panel, drone_id, 'battery', text, voltage_color) + self.update_overview_table(drone_id, 'battery', vol) + + elif msg_type == 'gps': + lat, lon = data.get('lat', 0), data.get('lon', 0) + self.drone_positions[drone_id] = (lat, lon) + lon_text = f"{lon:.6f}°" + lat_text = f"{lat:.6f}°" + self.update_field(panel, drone_id, 'longitude', lon_text) + self.update_field(panel, drone_id, 'latitude', lat_text) + self.update_overview_table(drone_id, 'longitude', lon_text) + self.update_overview_table(drone_id, 'latitude', lat_text) + + # ================================================================================ + # 【新增】同時儲存到 monitor.drone_gps（處理 UDP/WebSocket 的 GPS 資料） + # ================================================================================ + alt = data.get('alt', 0) # UDP/WebSocket 可能沒有 alt + if not hasattr(self.monitor, 'drone_gps'): + self.monitor.drone_gps = {} + self.monitor.drone_gps[drone_id] = { + 'lat': lat, + 'lon': lon, + 'alt': alt + } + # ================================================================================ + + # 更新地圖上的無人機位置 + heading = self.drone_headings.get(drone_id, 0) # 如果沒有航向，使用 0 + self.drone_map.update_drone_position(drone_id, lat, lon, heading) + + elif msg_type == 'altitude': + altitude = data.get('altitude', 0) + text = f"{altitude:.1f} m" + self.update_field(panel, drone_id, 'altitude', text) + self.update_overview_table(drone_id, 'altitude', text) + + elif msg_type == 'local_pose': + text = f"{data['x']:.1f}, {data['y']:.1f}" + self.update_field(panel, drone_id, 'local', text) + self.update_overview_table(drone_id, 'local', text) + + elif msg_type == 'hud': + heading = data.get('heading') + self.drone_headings[drone_id] = heading + groundspeed = data.get('groundspeed') + heading_text = f"{heading:.1f}°" + if isinstance(groundspeed, (int, float)): + groundspeed_text = f"{groundspeed:.1f} m/s" + else: + groundspeed_text = "--" + self.update_field(panel, drone_id, 'heading', heading_text) + self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) + self.update_overview_table(drone_id, 'heading', heading_text) + self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) + + # 如果有位置資訊，也更新地圖上的航向 + if drone_id in self.drone_positions: + lat, lon = self.drone_positions[drone_id] + self.drone_map.update_drone_position(drone_id, lat, lon, heading) + + elif msg_type == 'loss_rate': + loss_rate = data.get('loss_rate', 0) + text = f"{loss_rate:.1f}%" + self.update_field(panel, drone_id, 'loss_rate', text) + self.update_overview_table(drone_id, 'loss_rate', text) + + elif msg_type == 'ping': + ping = data.get('ping', 0) + text = f"{ping:.1f} ms" + self.update_field(panel, drone_id, 'ping', text) + self.update_overview_table(drone_id, 'ping', text) + + elif msg_type == 'velocity': + text = f"{data['vx']:.1f}, {data['vy']:.1f}" + self.update_overview_table(drone_id, 'velocity', text) + + elif msg_type == 'attitude': + roll = data.get('roll', 0) + pitch = data.get('pitch', 0) + yaw = data.get('yaw', 0) + roll_text = f"{roll:.1f}°" + pitch_text = f"{pitch:.1f}°" + yaw_text = f"{yaw:.1f}°" + self.update_overview_table(drone_id, 'roll', roll_text) + self.update_overview_table(drone_id, 'pitch', pitch_text) + self.update_overview_table(drone_id, 'yaw', yaw_text) + + # 新增處理分組勾選的方法 + def handle_group_selection(self, socket_id, state): + """處理 socket 分組勾選狀態變化""" + # 獲取該分組下的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + # 根據分組勾選狀態更新所有該分組的無人機勾選狀態 + is_checked = state == Qt.CheckState.Checked.value + + for drone_id in group_drones: + checkbox = self.drones[drone_id].get_checkbox() + if checkbox: + # 暫時斷開信號連接，避免遞迴觸發 + checkbox.blockSignals(True) + checkbox.setChecked(is_checked) + checkbox.blockSignals(False) + + # 手動更新選中集合 + if is_checked: + self.monitor.selected_drones.add(drone_id) + else: + self.monitor.selected_drones.discard(drone_id) + + def handle_drone_selection(self, drone_id, state): + """處理個別無人機勾選狀態變化""" + if state == Qt.CheckState.Checked.value: + self.monitor.selected_drones.add(drone_id) + else: + self.monitor.selected_drones.discard(drone_id) + + # 更新對應 socket 分組的勾選狀態 + socket_id = self.get_socket_id(drone_id) + self.update_group_checkbox_state(socket_id) + + # 新增更新分組勾選框狀態的方法 + def update_group_checkbox_state(self, socket_id): + """更新指定 socket 分組的勾選框狀態""" + # 獲取該分組下的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + if not group_drones: + return + + # 檢查該分組內有多少無人機被勾選 + checked_count = sum(1 for did in group_drones + if self.drones[did].is_checked()) + + # 獲取分組勾選框 + if socket_id in self.socket_groups: + group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox") + if group_checkbox: + # 暫時斷開信號連接 + group_checkbox.blockSignals(True) + + if checked_count == 0: + # 沒有無人機被勾選 + group_checkbox.setCheckState(Qt.CheckState.Unchecked) + elif checked_count == len(group_drones): + # 所有無人機都被勾選 + group_checkbox.setCheckState(Qt.CheckState.Checked) + else: + # 部分無人機被勾選，顯示半勾選狀態 + group_checkbox.setCheckState(Qt.CheckState.PartiallyChecked) + + # 重新連接信號 + group_checkbox.blockSignals(False) + + def handle_select_all(self): + """處理全選按鈕 - 支援分組結構""" + # 檢查是否所有無人機都已被選中 + all_selected = all( + self.drones[drone_id].is_checked() + for drone_id in self.drones + ) + + # 如果全部已選中，則取消全選；否則全選 + new_state = not all_selected + + # 更新所有勾選框狀態（無人機和分組） + for drone_id in self.drones: + self.drones[drone_id].set_checked(new_state) + + # 更新所有分組勾選框狀態 + for socket_id in self.socket_groups: + group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox") + if group_checkbox: + group_checkbox.blockSignals(True) + group_checkbox.setChecked(new_state) + group_checkbox.blockSignals(False) + + def handle_arm_selected(self): + """處理批次解鎖""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.arm_drone(drone_id, True) + loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}")) + + def handle_takeoff_selected(self): + """處理批次起飛""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.takeoff_drone(drone_id, 10.0) + loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) + + def handle_batch_mode_change(self): + mode = self.mode_combo.currentText() + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.set_mode(drone_id, mode) + loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) + + def update_field(self, panel, drone_id, field, text, color=None): + """更新面板中的指定欄位""" + if isinstance(panel, DronePanel): + panel.update_field(field, text, color) + + def update_overview_table(self, drone_id=None, field=None, value=None): + """更新總覽表格""" + if not hasattr(self, 'overview_table') or self.overview_table is None: + return + + # 更新無人機引用 + self.overview_table.set_drones(self.drones) + # 委託給 OverviewTable 處理 + self.overview_table.update_table(drone_id, field, value) + + def get_socket_id(self, drone_id): + """從 drone_id 提取 socket_id (例如 's9_1' -> '9')""" + import re + match = re.match(r's(\d+)_\d+', drone_id) + return match.group(1) if match else 'unknown' + + def add_drone(self, drone_id): + if drone_id in self.drones: + return + + # 獲取 socket_id + socket_id = self.get_socket_id(drone_id) + + # 創建無人機面板 + panel = self.create_drone_panel(drone_id) + self.drones[drone_id] = panel + + # 如果該 socket 分組不存在，創建它 + if socket_id not in self.socket_groups: + group_panel = self.create_socket_group_panel(socket_id) + self.socket_groups[socket_id] = group_panel + + # 將無人機面板添加到對應的 socket 分組中 + group_panel = self.socket_groups[socket_id] + group_panel.drones_layout.addWidget(panel) + + # 重新排序並顯示所有 socket 分組 + self.reorganize_socket_groups() + + # 更新分組勾選框狀態 + self.update_group_checkbox_state(socket_id) + + # 更新總覽表 + self.update_overview_table() + + def reorganize_socket_groups(self): + """重新組織和排序 socket 分組""" + # 先清空主佈局 + while self.drone_panel_layout.count(): + w = self.drone_panel_layout.takeAt(0).widget() + if w: + w.setParent(None) + + # 對每個 socket 分組內的無人機進行排序 + for socket_id, group_panel in self.socket_groups.items(): + # 獲取該分組內的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + # 清空分組佈局 + while group_panel.drones_layout.count(): + w = group_panel.drones_layout.takeAt(0).widget() + if w: + w.setParent(None) + + # 對該分組內的無人機按數字排序 + def sort_key(x): + parts = x[1:].split('_') + return (int(parts[0]), int(parts[1])) + + # 重新添加排序後的無人機面板 + for did in sorted(group_drones, key=sort_key): + group_panel.drones_layout.addWidget(self.drones[did]) + + # 按 socket_id 數字順序添加分組到主佈局 + for socket_id in sorted(self.socket_groups.keys(), key=lambda x: int(x)): + self.drone_panel_layout.addWidget(self.socket_groups[socket_id]) + + def spin_ros(self): + try: + self.executor.spin_once(timeout_sec=0.01) + for (drone_id, msg_type), data in self.monitor.latest_data.items(): + self.monitor.signals.update_signal.emit(msg_type, drone_id, data) + self.monitor.latest_data.clear() + except Exception as e: + print(f"ROS spin error: {e}") + + def closeEvent(self, event): + # 停止 UDP 接收器 + for receiver in self.udp_receivers: + receiver.stop() + + # 停止 WebSocket 接收器 + for receiver in self.monitor.ws_receivers: + receiver.stop() + + self.monitor.destroy_node() + self.executor.shutdown() + rclpy.shutdown() + event.accept() + + # ================================================================================ + # 【新增】以下方法是任務規劃功能 + # ================================================================================ + + def handle_map_click(self, lat, lon): + """ + 處理地圖點擊事件，自動規劃隊形任務 + + Args: + lat: 緯度 + lon: 經度 + """ + print(f"地圖點擊位置: {lat:.6f}, {lon:.6f}") + + selected_drones = self.get_selected_drones() + + if len(selected_drones) == 0: + self.statusBar().showMessage("⚠ 請先選擇無人機（勾選 checkbox）", 3000) + return + + # 設定目標點 + base_alt = 10.0 # 基準高度 + target_gps = (lat, lon, base_alt) + + self.statusBar().showMessage(f"⏳ 正在規劃 {len(selected_drones)} 台無人機的任務...", 2000) + + try: + # 取得當前無人機位置（GPS 座標） + drone_gps_positions = [] + + for drone_id in selected_drones: + # 檢查是否有 GPS 資料 + if hasattr(self.monitor, 'drone_gps') and drone_id in self.monitor.drone_gps: + # 使用實際的 GPS 資料 + gps_data = self.monitor.drone_gps[drone_id] + lat_drone = gps_data['lat'] + lon_drone = gps_data['lon'] + alt_drone = gps_data.get('alt', base_alt) + drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) + print(f"使用實際 GPS: {drone_id} -> ({lat_drone:.6f}, {lon_drone:.6f}, {alt_drone:.1f})") + + elif drone_id in self.drone_positions: + # 備用方案：從 Local 座標轉換 + pos = self.drone_positions[drone_id] + # 使用簡化轉換 + lat_drone = 24.0 + pos[1] / 111000 + lon_drone = 120.0 + pos[0] / (111000 * 0.9) + alt_drone = pos[2] if len(pos) > 2 else base_alt + drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) + print(f"使用 Local 轉換: {drone_id} -> ({lat_drone:.6f}, {lon_drone:.6f}, {alt_drone:.1f})") + + else: + self.statusBar().showMessage(f"⚠ 找不到 {drone_id} 的位置資料", 3000) + return + + # 規劃任務 + # ================================================================================ + # 【修改】接收中心點座標 + # ================================================================================ + stage1_gps, stage2_gps, center_origin = self.mission_planner.plan_formation_mission( + drone_gps_positions, + target_gps + ) + # ================================================================================ + + # 儲存規劃結果 + self.planned_waypoints = { + 'stage1': stage1_gps, + 'stage2': stage2_gps, + 'drone_ids': selected_drones + } + + # 顯示規劃結果 + self.show_planned_waypoints() + + # ================================================================================ + # 【新增】在地圖上顯示任務規劃（中心點 + 虛線） + # ================================================================================ + center_lat, center_lon, center_alt = center_origin + self.drone_map.draw_mission_plan( + center_lat, center_lon, # 中心點座標 + lat, lon # 目標點座標 + ) + # ================================================================================ + + self.statusBar().showMessage( + f"✓ 任務規劃完成！{len(selected_drones)} 台無人機，2 階段飛行", 5000 + ) + + except Exception as e: + self.statusBar().showMessage(f"❌ 規劃失敗: {str(e)}", 5000) + print(f"Mission planning error: {e}") + import traceback + traceback.print_exc() + + def show_planned_waypoints(self): + """ + 顯示規劃的航點（在終端輸出） + """ + if not self.planned_waypoints: + return + + print("\n" + "=" * 60) + print("任務規劃結果") + print("=" * 60) + + drone_ids = self.planned_waypoints['drone_ids'] + stage1 = self.planned_waypoints['stage1'] + stage2 = self.planned_waypoints['stage2'] + + print(f"\n共 {len(drone_ids)} 台無人機") + print(f"參數：間距={self.mission_planner.spacing}m, " + f"基準高度={self.mission_planner.base_altitude}m, " + f"高低差={self.mission_planner.altitude_diff}m") + + for i, drone_id in enumerate(drone_ids): + print(f"\n【{drone_id}】") + lat1, lon1, alt1 = stage1[i] + lat2, lon2, alt2 = stage2[i] + + print(f" 階段 1（集合點）：") + print(f" 緯度: {lat1:.6f}°") + print(f" 經度: {lon1:.6f}°") + print(f" 高度: {alt1:.1f} m") + + print(f" 階段 2（目標點）：") + print(f" 緯度: {lat2:.6f}°") + print(f" 經度: {lon2:.6f}°") + print(f" 高度: {alt2:.1f} m") + + print("\n" + "=" * 60) + + def get_selected_drones(self): + """ + 取得被選中的無人機 ID 列表 + + Returns: + list: 被選中的無人機 ID 列表 + """ + selected = [] + for drone_id, panel in self.drones.items(): + if hasattr(panel, 'checkbox') and panel.checkbox.isChecked(): + selected.append(drone_id) + return selected + + # ================================================================================ + # 【新增結束】 + # ================================================================================ + +if __name__ == '__main__': + app = QApplication(sys.argv) + station = ControlStationUI() + station.show() + app.exec() \ No newline at end of file diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py new file mode 100644 index 0000000..24bd2d2 --- /dev/null +++ b/src/GUI/map_layout.py @@ -0,0 +1,436 @@ +#!/usr/bin/env python3 +from PyQt6.QtWebEngineWidgets import QWebEngineView +from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot +from PyQt6.QtWebChannel import QWebChannel + +class DroneMap: + """無人機地圖類別 - 負責管理 Leaflet 地圖顯示""" + + def __init__(self): + """初始化地圖""" + self.map_view = QWebEngineView() + self.map_loaded = False + self.pending_map_updates = {} + + # 創建橋接對象 + self.bridge = MapBridge() + + # 設置 QWebChannel + self.channel = QWebChannel() + self.channel.registerObject('bridge', self.bridge) + self.map_view.page().setWebChannel(self.channel) + + # 設置地圖 HTML + inline_html = ''' + + + + + + + + + + + +
+
+ +
+ + + + + ''' + + self.map_view.setHtml(inline_html) + self.map_view.loadFinished.connect(self._on_map_loaded) + + # 設置地圖更新計時器 + self.map_update_timer = QTimer() + self.map_update_timer.timeout.connect(self.update_map_positions) + self.map_update_timer.start(200) # 每 200ms 更新一次 + + def _on_map_loaded(self, ok: bool): + """地圖加載完成回調""" + if ok: + self.map_loaded = True + else: + print("⚠️ 地圖加載失敗") + + def update_drone_position(self, drone_id, lat, lon, heading): + """更新無人機位置（加入待處理隊列）""" + self.pending_map_updates[drone_id] = (lat, lon, heading) + + def update_map_positions(self): + """批量更新地圖上的無人機位置""" + if not self.map_loaded or not self.pending_map_updates: + return + + # 批量執行所有待更新的位置 + js_commands = [] + for drone_id, (lat, lon, heading) in self.pending_map_updates.items(): + js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});") + + if js_commands: + combined_js = "\n".join(js_commands) + self.map_view.page().runJavaScript(combined_js) + + # 清空待更新緩存 + self.pending_map_updates.clear() + + def clear_trajectories(self): + """清除所有軌跡""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearAllTrajectories();") + + def focus_on_drone(self, drone_id): + """聚焦到指定無人機""" + if self.map_loaded: + self.map_view.page().runJavaScript(f"focusOn('{drone_id}');") + + # ================================================================================ + # 【新增】任務規劃視覺化方法 + # ================================================================================ + def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon): + """ + 在地圖上繪製任務規劃 + + Args: + center_lat: 中心點緯度 + center_lon: 中心點經度 + target_lat: 目標點緯度 + target_lon: 目標點經度 + """ + if self.map_loaded: + js_code = f"drawMissionPlan({center_lat:.6f}, {center_lon:.6f}, {target_lat:.6f}, {target_lon:.6f});" + self.map_view.page().runJavaScript(js_code) + print(f"📍 地圖已繪製任務規劃: C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})") + + def clear_mission_plan(self): + """清除地圖上的任務規劃標記""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearMissionPlan();") + print("🗑️ 地圖已清除任務規劃") + # ================================================================================ + + def get_widget(self): + """獲取地圖 widget""" + return self.map_view + + def get_gps_signal(self): + """獲取 GPS 信號""" + return self.bridge.gps_signal + +class MapBridge(QObject): + """JavaScript 和 Python 之間的橋接類""" + gps_signal = pyqtSignal(float, float) # lat, lon + + def __init__(self): + super().__init__() + + @pyqtSlot(float, float) + def emitGpsSignal(self, lat, lon): + """供 JavaScript 調用的方法""" + self.gps_signal.emit(lat, lon) \ No newline at end of file diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py new file mode 100644 index 0000000..138795d --- /dev/null +++ b/src/GUI/mission_planner.py @@ -0,0 +1,323 @@ +#!/usr/bin/env python3 +""" +飛行任務規劃模組 +負責將 GPS 點擊座標轉換為隊形飛行任務 +""" +import math +from typing import List, Tuple, Optional + + +class CoordinateConverter: + """GPS 座標與 Local 座標的轉換器""" + + def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0): + """ + 初始化座標轉換器 + + Args: + origin_lat: 參考原點緯度 + origin_lon: 參考原點經度 + origin_alt: 參考原點高度（公尺，相對於海平面） + """ + self.origin_lat = origin_lat + self.origin_lon = origin_lon + self.origin_alt = origin_alt + self.R = 6371000.0 # 地球半徑（公尺） + + def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]: + """ + GPS 座標轉換為 Local 座標（ENU 系統：East-North-Up） + + Args: + lat: 緯度 + lon: 經度 + alt: 高度（公尺，相對於海平面） + + Returns: + (x, y, z): Local 座標（公尺） + x: East（東方） + y: North（北方） + z: Up（向上） + """ + lat_rad = math.radians(lat) + lon_rad = math.radians(lon) + origin_lat_rad = math.radians(self.origin_lat) + origin_lon_rad = math.radians(self.origin_lon) + + dlat = lat_rad - origin_lat_rad + dlon = lon_rad - origin_lon_rad + + # 使用 Equirectangular projection（適用於小範圍 < 10 km） + x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2) # East + y = self.R * dlat # North + z = alt - self.origin_alt # Up + + return x, y, z + + def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]: + """ + Local 座標轉換為 GPS 座標 + + Args: + x: East（東方，公尺） + y: North（北方，公尺） + z: Up（向上，公尺） + + Returns: + (lat, lon, alt): GPS 座標 + """ + origin_lat_rad = math.radians(self.origin_lat) + origin_lon_rad = math.radians(self.origin_lon) + + lat_rad = origin_lat_rad + (y / self.R) + lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2))) + + lat = math.degrees(lat_rad) + lon = math.degrees(lon_rad) + alt = self.origin_alt + z + + return lat, lon, alt + + +class FormationPlanner: + """隊形規劃器""" + + def __init__(self, spacing: float = 5.0, + base_altitude: float = 10.0, + altitude_diff: float = 2.0): + """ + 初始化隊形規劃器 + + Args: + spacing: 無人機間距（公尺） + base_altitude: 基準飛行高度（公尺，相對於參考原點） + altitude_diff: M 字形的高低差（公尺） + """ + self.spacing = spacing + self.base_altitude = base_altitude + self.altitude_diff = altitude_diff + self.current_origin = None + self.converter = None + + def plan_formation_mission(self, + drone_gps_positions: List[Tuple[float, float, float]], + target_gps: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], + List[Tuple[float, float, float]]]: + """ + 規劃兩階段隊形任務 + + Args: + drone_gps_positions: 當前無人機 GPS 位置列表 [(lat, lon, alt), ...] + target_gps: 目標點 GPS 座標 (lat, lon, alt) + + Returns: + stage1_gps: 階段 1（集合點）的 GPS 航點列表 + stage2_gps: 階段 2（目標點）的 GPS 航點列表 + origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt) + origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt) + """ + if len(drone_gps_positions) == 0: + raise ValueError("無人機位置列表不能為空") + + # ===== 步驟 1: 計算中央點並設為參考原點 ===== + center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions) + center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions) + center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions) + + self.current_origin = (center_lat, center_lon, center_alt) + self.converter = CoordinateConverter(center_lat, center_lon, center_alt) + + print(f"📍 參考原點: ({center_lat:.6f}, {center_lon:.6f}, {center_alt:.1f}m)") + + # ===== 步驟 2: 轉換到 Local 座標系 ===== + drone_local_positions = [] + for lat, lon, alt in drone_gps_positions: + x, y, z = self.converter.gps_to_local(lat, lon, alt) + drone_local_positions.append((x, y, z)) + + target_local = self.converter.gps_to_local( + target_gps[0], target_gps[1], target_gps[2] + ) + + # ===== 步驟 3: 在 Local 座標系中計算隊形 ===== + stage1_local, stage2_local = self._calculate_formation_local( + drone_local_positions, + target_local + ) + + # ===== 步驟 4: 轉回 GPS 座標 ===== + stage1_gps = [] + for x, y, z in stage1_local: + lat, lon, alt = self.converter.local_to_gps(x, y, z) + stage1_gps.append((lat, lon, alt)) + + stage2_gps = [] + for x, y, z in stage2_local: + lat, lon, alt = self.converter.local_to_gps(x, y, z) + stage2_gps.append((lat, lon, alt)) + + print(f"✅ 任務規劃完成: {len(stage1_gps)} 台無人機，2 階段飛行") + + # ================================================================================ + # 【修改】回傳中心點座標供地圖視覺化使用 + # ================================================================================ + return stage1_gps, stage2_gps, self.current_origin + # ================================================================================ + + def _calculate_formation_local(self, + drone_positions: List[Tuple[float, float, float]], + target_point: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], + List[Tuple[float, float, float]]]: + """ + 在 Local 座標系中計算隊形 + + Args: + drone_positions: Local 座標的無人機位置 [(x, y, z), ...] + target_point: Local 座標的目標點 (x, y, z) + + Returns: + stage1_positions: 階段 1（中央點分佈）的 Local 座標 + stage2_positions: 階段 2（目標點分佈）的 Local 座標 + """ + N = len(drone_positions) + + # ===== 步驟 1: 計算中央點（在 Local 座標系中應該接近原點）===== + C_x = sum(pos[0] for pos in drone_positions) / N + C_y = sum(pos[1] for pos in drone_positions) / N + C_z = sum(pos[2] for pos in drone_positions) / N + + print(f" 中央點 Local: ({C_x:.2f}, {C_y:.2f}, {C_z:.2f})") + + # ===== 步驟 2: 計算方向向量（中央點 → 目標點）===== + T_x, T_y, T_z = target_point + V_x = T_x - C_x + V_y = T_y - C_y + + print(f" 方向向量: ({V_x:.2f}, {V_y:.2f})") + + # ===== 步驟 3: 計算垂直向量（逆時針旋轉 90°）===== + P_x = -V_y + P_y = V_x + + # 單位化垂直向量 + length = math.sqrt(P_x**2 + P_y**2) + if length < 0.01: # 避免除以零 + # 如果目標點與中央點重合，使用默認方向（向東） + P_x_unit, P_y_unit = 1.0, 0.0 + else: + P_x_unit = P_x / length + P_y_unit = P_y / length + + print(f" 垂直單位向量: ({P_x_unit:.3f}, {P_y_unit:.3f})") + + # ===== 步驟 4: 計算無人機在垂直方向的投影並排序 ===== + projections = [] + for i, pos in enumerate(drone_positions): + relative_x = pos[0] - C_x + relative_y = pos[1] - C_y + projection = relative_x * P_x_unit + relative_y * P_y_unit + projections.append((projection, i)) + + # 按投影值排序（保持左右順序） + projections.sort() + sorted_indices = [idx for _, idx in projections] + + print(f" 排序後的無人機索引: {sorted_indices}") + + # ===== 步驟 5: 分佈無人機位置 ===== + stage1_positions = [None] * N # 預分配列表 + stage2_positions = [None] * N + + for rank, original_idx in enumerate(sorted_indices): + # 計算相對中心的水平偏移 + offset = (rank - (N - 1) / 2) * self.spacing + + # 計算 M 字形高度（交替高低） + if rank % 2 == 0: + altitude = self.base_altitude + self.altitude_diff # 高 + else: + altitude = self.base_altitude - self.altitude_diff # 低 + + # 階段 1：在中央點附近分佈 + stage1_x = C_x + P_x_unit * offset + stage1_y = C_y + P_y_unit * offset + stage1_z = altitude + + # 階段 2：在目標點附近分佈（保持相同的相對位置） + stage2_x = T_x + P_x_unit * offset + stage2_y = T_y + P_y_unit * offset + stage2_z = altitude + + # 按照原始索引存儲（保持無人機 ID 順序） + stage1_positions[original_idx] = (stage1_x, stage1_y, stage1_z) + stage2_positions[original_idx] = (stage2_x, stage2_y, stage2_z) + + return stage1_positions, stage2_positions + + def update_parameters(self, spacing: Optional[float] = None, + base_altitude: Optional[float] = None, + altitude_diff: Optional[float] = None): + """ + 更新隊形參數 + + Args: + spacing: 無人機間距（公尺） + base_altitude: 基準飛行高度（公尺） + altitude_diff: M 字形的高低差（公尺） + """ + if spacing is not None: + self.spacing = spacing + print(f" 間距更新為: {spacing} m") + + if base_altitude is not None: + self.base_altitude = base_altitude + print(f" 基準高度更新為: {base_altitude} m") + + if altitude_diff is not None: + self.altitude_diff = altitude_diff + print(f" 高低差更新為: {altitude_diff} m") + + +# ===== 測試程式碼 ===== +if __name__ == "__main__": + print("=" * 60) + print("隊形任務規劃器測試") + print("=" * 60) + + # 模擬 5 台無人機的 GPS 位置 + drone_gps = [ + (24.123450, 120.567800, 100.0), # 無人機 0 + (24.123470, 120.567820, 102.0), # 無人機 1 + (24.123440, 120.567810, 98.0), # 無人機 2 + (24.123460, 120.567830, 101.0), # 無人機 3 + (24.123445, 120.567795, 99.0), # 無人機 4 + ] + + # 目標點 + target_gps = (24.123600, 120.568000, 105.0) + + # 建立規劃器 + planner = FormationPlanner( + spacing=5.0, # 5 公尺間距 + base_altitude=10.0, # 基準高度 10 公尺 + altitude_diff=2.0 # 高低差 2 公尺 + ) + + # 規劃任務 + print("\n開始規劃任務...") + stage1, stage2 = planner.plan_formation_mission(drone_gps, target_gps) + + # 顯示結果 + print("\n" + "=" * 60) + print("階段 1：集合點位置（GPS）") + print("=" * 60) + for i, (lat, lon, alt) in enumerate(stage1): + print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)") + + print("\n" + "=" * 60) + print("階段 2：目標點位置（GPS）") + print("=" * 60) + for i, (lat, lon, alt) in enumerate(stage2): + print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)") + + print("\n✅ 測試完成！") \ No newline at end of file diff --git a/src/GUI/overview_table.py b/src/GUI/overview_table.py new file mode 100644 index 0000000..11953f0 --- /dev/null +++ b/src/GUI/overview_table.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import QTableWidget, QTableWidgetItem, QHeaderView, QLabel +from PyQt6.QtCore import Qt + +class OverviewTable(QTableWidget): + """總覽表格，顯示所有無人機的狀態資訊""" + + def __init__(self, info_types, info_type_map, parent=None): + super().__init__(parent) + + self.info_types = info_types + self.info_type_map = info_type_map + self.drones = {} # 存儲無人機面板的引用 + + # 初始化表格 + self.setColumnCount(1) + self.setRowCount(len(self.info_types)) + self.setHorizontalHeaderLabels(["資訊"]) + header = self.horizontalHeader() + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.verticalHeader().setVisible(False) + + # 設置第一列的資訊類型 + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.setItem(i, 0, item) + + def set_drones(self, drones): + """設置無人機面板字典的引用""" + self.drones = drones + + def update_table(self, drone_id=None, field=None, value=None): + """更新總覽表格 + + Args: + drone_id: 無人機 ID + field: 欄位名稱 (如 'mode', 'altitude' 等) + value: 要更新的值 + """ + # 更新特定儲存格 + if drone_id and field and value: + if drone_id not in self.drones: + return + + col = 1 + list(self.drones.keys()).index(drone_id) + row = self.info_type_map.get(field, -1) + + if row == -1: + return # 無效的欄位 + + item = self.item(row, col) + if not item: + item = QTableWidgetItem() + self.setItem(row, col, item) + + item.setText(value) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + + # 如果沒有指定更新，刷新整個表格 + if drone_id is None: + self.refresh_all() + + def refresh_all(self): + """刷新整個表格""" + cols = 1 + len(self.drones) + self.setColumnCount(cols) + headers = ["資訊"] + list(self.drones.keys()) + self.setHorizontalHeaderLabels(headers) + + for col, did in enumerate(self.drones, start=1): + panel = self.drones[did] + for field, row in self.info_type_map.items(): + lbl = panel.findChild(QLabel, f"{did}_{field}") + val = lbl.text() if lbl else "--" + val_item = QTableWidgetItem(val) + val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.setItem(row, col, val_item) + + def add_drone_column(self, drone_id): + """當新增無人機時，添加一列""" + if drone_id in self.drones: + self.refresh_all() + + def remove_drone_column(self, drone_id): + """當移除無人機時，刷新表格""" + if drone_id in self.drones: + self.refresh_all() From e54e42aad27cc755b0200f6bd93bfe332711b509 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 2 Feb 2026 01:13:59 +0800 Subject: [PATCH 25/33] Bring fc_network_adapter from master --- .../fc_network_adapter/devRun.py | 508 ------ .../fc_network_adapter/fc_network_adapter.md | 190 ++ .../fc_network_adapter/mainOrchestrator.py | 1597 +++++++++++++++++ .../fc_network_adapter/mavlinkObject.py | 1004 +++++++---- .../fc_network_adapter/mavlinkPublish.py | 129 +- .../fc_network_adapter/mavlinkROS2Nodes.py | 902 ++++++++++ .../fc_network_adapter/mavlinkVehicleView.py | 453 +++++ .../fc_network_adapter/serialManager.py | 611 +++++++ .../fc_network_adapter/utils/__init__.py | 7 + .../fc_network_adapter/utils/acquirePort.py | 129 ++ .../fc_network_adapter/utils/acquireSerial.py | 111 ++ .../fc_network_adapter/utils/ringBuffer.py | 231 +++ .../fc_network_adapter/utils/theLogger.py | 43 + src/fc_network_adapter/setup.py | 1 + src/fc_network_adapter/tests/__init__.py | 0 .../tests/demo_integration.py | 277 +++ .../tests/demo_mavlinkVehicleView.py | 331 ++++ .../tests/demo_ringBuffer.py | 152 ++ .../tests/test_mavlinkObject.py | 468 +++++ .../tests/test_ringBuffer.py | 296 +++ .../tests/test_vehicleStatusPublisher.py | 507 ++++++ 21 files changed, 7070 insertions(+), 877 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/devRun.py create mode 100644 src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md create mode 100644 src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py create mode 100644 src/fc_network_adapter/fc_network_adapter/serialManager.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/__init__.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/theLogger.py create mode 100644 src/fc_network_adapter/tests/__init__.py create mode 100644 src/fc_network_adapter/tests/demo_integration.py create mode 100644 src/fc_network_adapter/tests/demo_mavlinkVehicleView.py create mode 100644 src/fc_network_adapter/tests/demo_ringBuffer.py create mode 100644 src/fc_network_adapter/tests/test_mavlinkObject.py create mode 100644 src/fc_network_adapter/tests/test_ringBuffer.py create mode 100644 src/fc_network_adapter/tests/test_vehicleStatusPublisher.py diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py deleted file mode 100644 index 78abb8f..0000000 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ /dev/null @@ -1,508 +0,0 @@ -# 基礎功能的 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 =10000 -print('test_item : ', test_item) - -if test_item == 51: - # 晉凱的測試項目 - 修改為支援多UDP連接 - 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) - - # === 修改：支援多個UDP連接 === - ports = ["udp:127.0.0.1:14550", - "udp:127.0.0.1:14570", - "/dev/ttyUSB0", - "/dev/ttyUSB1"] # 可以根據需要調整端口 - mavlink_sockets = [] - mavlink_objects = [] - - # 循環創建多個連接 - for i, port in enumerate(ports): - try: - print(f"連接到 UDP:{port}") - mavlink_socket = mavutil.mavlink_connection(port) - mavlink_object = mo.mavlink_object(mavlink_socket) - - # 設定通道流動（所有連接使用相同參數） - mavlink_object.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147] - mavlink_object.multiplexingToReturn = [] - - # 啟動通道 - mavlink_object.run() - - # 保存連接引用 - mavlink_sockets.append(mavlink_socket) - mavlink_objects.append(mavlink_object) - - print(f"UDP:{port} 連接成功，socket_id: {mavlink_object.socket_id}") - - except Exception as e: - print(f"連接 UDP:{port} 失敗: {e}") - continue - - print(f"成功建立 {len(mavlink_sockets)} 個 UDP 連接") - - print('waiting for mavlink data ...') - time.sleep(3) # 等待足夠時間讓所有device object收到MAVLink訊息 - - # 顯示檢測到的系統 - print('目前所有的系統：') - for sysid in analyzer.mavlink_systems: - system = analyzer.mavlink_systems[sysid] - socket_id = getattr(system, 'socket_id', 'Unknown') - print(f" 系統ID: {sysid}, socket_id: {socket_id}") - - # === 為所有檢測到的系統創建topics === - topic_creation_start = time.time() - topics_created = 0 - - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - try: - print(f"為系統 {sysid}, 組件 {compid} 創建topics...") - - analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) - - topics_created += 1 - print(f"系統 {sysid}_{compid} topics創建完成") - - except Exception as e: - print(f"為系統 {sysid}_{compid} 創建topics失敗: {e}") - continue - - topic_creation_end = time.time() - print(f"總共為 {topics_created} 個系統創建topics，耗時: {topic_creation_end - topic_creation_start:.2f} 秒") - - print("start emit info") - - # === 主循環：支援多連接 === - start_time = time.time() - loop_count = 0 - - # while time.time() - start_time < running_time: - while True: - try: - # ROS2 發布 - analyzer.emit_info() # 這邊是測試 node 的運行 - - # 每10次循環顯示一次詳細狀態 - loop_count += 1 - if loop_count % 10 == 0: # 每5秒顯示一次（0.5s * 10） - print(f"\n=== 狀態更新 (第 {loop_count} 次循環) ===") - #print(f"連接的端口: {udp_ports[:len(mavlink_sockets)]}") - print(f"檢測到的系統數: {len(analyzer.mavlink_systems)}") - - for sysid in analyzer.mavlink_systems: - system = analyzer.mavlink_systems[sysid] - socket_id = getattr(system, 'socket_id', 'Unknown') - print(f" 系統 {sysid}: socket_id={socket_id}") - - for compid in system.components: - component = system.components[compid] - - # 安全地獲取msg_count - msg_count = getattr(component, 'msg_count', 0) - if isinstance(msg_count, dict): - total_count = sum(msg_count.values()) if msg_count else 0 - else: - total_count = msg_count if msg_count else 0 - - print(f" 組件 {compid}: 收到 {total_count} 條消息") - - # 顯示emitParams狀態 - if hasattr(component, 'emitParams') and component.emitParams: - param_count = len(component.emitParams) - print(f" emitParams: {param_count} 項數據") - else: - print(f" emitParams: 無數據") - - # 重置消息計數 - try: - system.resetComponentPacketCount(compid) - except Exception as e: - print(f" 重置計數失敗: {e}") - - print("===============================") - - time.sleep(0.5) - - except KeyboardInterrupt: - print("\n使用者中斷程序...") - break - except Exception as e: - print(f"主循環錯誤: {e}") - break - - # === 清理資源 === - print("開始清理資源...") - - # 清理ROS2 - try: - analyzer.destroy_node() - print("ROS2 node 已清理") - except Exception as e: - print(f"清理ROS2 node失敗: {e}") - - try: - rclpy.shutdown() - print("ROS2 已關閉") - except Exception as e: - print(f"關閉ROS2失敗: {e}") - - # 清理所有mavlink objects - for i, mavlink_obj in enumerate(mavlink_objects): - try: - print(f"停止 mavlink_object {i+1} (UDP:{ports[i]}, socket_id: {mavlink_obj.socket_id})") - mavlink_obj.stop() - mavlink_obj.thread.join() - print(f"mavlink_object {i+1} 已停止") - except Exception as e: - print(f"停止 mavlink_object {i+1} 失敗: {e}") - - # 關閉所有mavlink sockets - for i, mavlink_sock in enumerate(mavlink_sockets): - try: - print(f"關閉 UDP 連接 {ports[i]}") - mavlink_sock.close() - except Exception as e: - print(f"關閉 UDP 連接 {ports[i]} 失敗: {e}") - - # 清理analyzer - try: - print("停止 analyzer") - analyzer.stop() - analyzer.thread.join() - print("analyzer 已停止") - except Exception as e: - print(f"停止 analyzer 失敗: {e}") - - print(f"清理完成，共處理了 {len(mavlink_sockets)} 個 UDP 連接") - print('<=== End of Program') - -elif test_item == 54: - # 文鈞的測試項目 - 5輸入2輸出版本 + 結合test51的ROS2功能 - # 加入詳細調試信息 - print('===> Start of Program .Test ', test_item) - - # === ROS2 初始化 (來自test51新版本) === - rclpy.init() - print("ROS2 初始化完成") - - # 1) 啟動 bridge（它已自動建立所有 publisher） - bridge = mo.mavlink_bridge() - - try: - bridge._init_node() - - # 添加Node初始化檢查和修復 - if not hasattr(bridge, '_default_callback_group'): - print("警告：Node 初始化不完整，嘗試修復...") - from rclpy.node import Node - # 強制重新初始化為正確的 Node - Node.__init__(bridge, 'mavlink_bridge_fixed') - print("Node 重新初始化完成") - else: - print("Node 初始化成功") - - except Exception as e: - print(f"Node初始化失敗: {e}") - print("嘗試備用初始化方法...") - - # 備用方法：創建一個新的Node實例 - from rclpy.node import Node - - class BackupNode(Node): - def __init__(self): - super().__init__('mavlink_bridge_backup') - - backup_node = BackupNode() - - # 將備用node的方法附加到bridge對象 - bridge.create_publisher = backup_node.create_publisher - bridge.destroy_node = backup_node.destroy_node - bridge._backup_node = backup_node - - print("備用Node創建完成") - - print("ROS2 bridge 初始化完成") - - # 雙輸出連接設定 (連接到兩個不同的GCS) - gcs_outputs = [ - "udpout:127.0.0.1:14500", # GCS 1 - "udpout:127.0.0.1:14600" # GCS 2 - ] - - # 建立輸出連接物件 - mavlink_objects_out = [] - mavlink_sockets_out = [] - - # 設定5個輸入連接（修改為實際測試可用的端口） - device_inputs = [ - "udp:127.0.0.1:14550", # 無人機1 (UDP) - "udp:127.0.0.1:14570", # 無人機2 (UDP) - "/dev/ttyUSB0", # 無人機3 (UDP) - "/dev/ttyUSB1", # 無人機4 (UDP) - "/dev/ttyUSB2", # 無人機5 (UDP) - ] - - # 建立輸入連接 - mavlink_objects_in = [] - mavlink_sockets_in = [] - - for i, output_conn in enumerate(gcs_outputs): - print(f"建立 GCS {i+1} 輸出連接: {output_conn}") - mavlink_out = mavutil.mavlink_connection(output_conn) - obj_out = mo.mavlink_object(mavlink_out) - obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_out.append(obj_out) - mavlink_sockets_out.append(mavlink_out) - - # 設定GCS到所有設備的轉發關係 - for i, obj_out in enumerate(mavlink_objects_out): - for j, obj_in in enumerate(mavlink_objects_in): - obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有設備 - print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 設備 {j+1} (socket_id: {obj_in.socket_id}) 轉發") - - for i, input_conn in enumerate(device_inputs): - print(f"連接設備 {i+1} 輸入: {input_conn}") - try: - # UDP連接 - mavlink_in = mavutil.mavlink_connection(input_conn) - print(f" UDP連接 {input_conn}") - - obj_in = mo.mavlink_object(mavlink_in) - - # === 設置消息分析類型 (來自test51新版本) === - obj_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 111, 147] # HEARTBEAT + 常用訊息 + TIMESYNC - print(f" 設備 {i+1} 設置分析消息類型: {obj_in.multiplexingToAnalysis}") - - mavlink_objects_in.append(obj_in) - mavlink_sockets_in.append(mavlink_in) - - # 設定設備到所有GCS的轉發關係 - for j, obj_out in enumerate(mavlink_objects_out): - obj_in.multiplexingToSwap[obj_out.socket_id] = [-1, ] # 設備→所有GCS - print(f"設定設備 {i+1} (socket_id: {obj_in.socket_id}) → GCS {j+1} (socket_id: {obj_out.socket_id}) 轉發") - - except Exception as e: - print(f"警告：無法建立連接 {input_conn}，錯誤：{e}") - print(f"跳過設備 {i+1}") - continue - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - print(f"\n成功建立 {len(mavlink_objects_in)} 個輸入連接") - print(f"成功建立 {len(mavlink_objects_out)} 個輸出連接") - - # 啟動所有輸入通道 - for i, obj in enumerate(mavlink_objects_in): - obj.run() - print(f"啟動輸入通道 {i+1}") - - # 啟動所有輸出通道 - for i, obj in enumerate(mavlink_objects_out): - obj.run() - print(f"啟動輸出通道 {i+1}") - - # === 等待MAVLink數據 (來自test51新版本) === - print("waiting for mavlink data...") - time.sleep(3) # 增加等待時間 - print("=== connection established! ===") - - # 顯示目前偵測到的 sysid 清單 - print("Current sysid list:", list(bridge.mavlink_systems.keys())) - for sysid in bridge.mavlink_systems: - print(bridge.mavlink_systems[sysid]) - - # 顯示轉發設定摘要 - print("\n=== 系統配置摘要 ===") - print(f"輸入設備數量: {len(mavlink_objects_in)}") - print("輸入設備類型:") - for i, input_conn in enumerate(device_inputs[:len(mavlink_objects_in)]): - device_type = "串口" if input_conn.startswith("/dev/tty") else "UDP" - print(f" 設備 {i+1}: {input_conn} ({device_type})") - print(f"GCS數量: {len(mavlink_objects_out)}") - print("輸出GCS:") - for i, output_conn in enumerate(gcs_outputs): - print(f" GCS {i+1}: {output_conn}") - print("轉發規則:") - print(" - 每個設備的所有訊息 → 所有GCS") - print(" - 每個GCS的所有訊息 → 所有設備") - print("MAVLink分析:") - print(" - 消息類型: [0, 30, 32, 33, 74, 111, 147] (HEARTBEAT, ATTITUDE, LOCAL_POSITION_NED, GLOBAL_POSITION_INT, VFR_HUD, TIMESYNC, BATTERY_STATUS)") - print("ROS2 Topics: 已自動建立所有publisher") - print("===================\n") - - # === 主運行循環 (來自test51新版本) + 調試信息 === - print("開始ROS2 topics發布...") - - try: - last_timesync = time.time() - show_time = time.time() - message_count = 0 - ros2_publish_count = 0 - - # 調試用計數器 - debug_counters = { - 'swap_messages': 0, - 'analysis_messages': 0, - 'return_messages': 0, - 'ros2_publishes': 0 - } - - while rclpy.ok() and time.time() - last_timesync < running_time: - now = time.time() - - # === 調試：檢查各種queue的消息 === - # 檢查 swap_queues - try: - test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) - print('none object 收到訊息: ', test) - debug_counters['swap_messages'] += 1 - except queue.Empty: - pass - - # 檢查 fixed_stream_bridge_queue - try: - while not mo.fixed_stream_bridge_queue.empty(): - msg = mo.fixed_stream_bridge_queue.get(block=False) - debug_counters['analysis_messages'] += 1 - message_count += 1 - except queue.Empty: - pass - - # 檢查 return_packet_processor_queue - try: - while not mo.return_packet_processor_queue.empty(): - msg = mo.return_packet_processor_queue.get(block=False) - debug_counters['return_messages'] += 1 - message_count += 1 - except queue.Empty: - pass - - # 每秒發送 TIMESYNC (來自test51新版本) - if now - last_timesync >= 1.0: - timesync_sent = 0 - # 對每個輸入設備發送 TIMESYNC request - for i, mavlink_socket in enumerate(mavlink_sockets_in): - try: - mavlink_socket.mav.timesync_send(0, int(now * 1e9)) - timesync_sent += 1 - except Exception as e: - print(f"發送 TIMESYNC 到設備 {i+1} 失敗: {e}") - - last_timesync = now - - # ROS2 發布 (來自test51新版本) - try: - bridge.emit_info() # 將所有 emitParams 發布到 ROS topic - debug_counters['ros2_publishes'] += 1 - ros2_publish_count += 1 - except Exception as e: - print(f"[ERROR] ROS2 發布失敗: {e}") - - # 狀態報告 (更頻繁的調試輸出) - if (time.time() - show_time) >= 2: # 每2秒顯示一次狀態 - show_time = time.time() - print(f"\n=== 調試狀態報告===") - - # 顯示消息統計 - print(f"總消息數: {message_count}, ROS2發布次數: {ros2_publish_count}") - - # 重置調試計數器 - debug_counters = {k: 0 for k in debug_counters} - - if len(bridge.mavlink_systems) > 0: - for sysid in bridge.mavlink_systems: - system = bridge.mavlink_systems[sysid] - print(f"系統 {sysid}: socket_id={getattr(system, 'socket_id', 'N/A')}") - - for compid in system.components: - component = system.components[compid] - msg_count = component.msg_count - - system.resetComponentPacketCount(compid) - else: - print("目前沒有檢測到任何MAVLink系統") - - # 顯示各通道的狀態 - print(f"輸入通道數量: {len(mavlink_objects_in)} (運行中)") - print(f"輸出通道數量: {len(mavlink_objects_out)} (運行中)") - print("ROS2發布狀態: 運行中") - - # 顯示queue狀態 - print(f"Queue 狀態:") - print(f" fixed_stream_bridge_queue: {mo.fixed_stream_bridge_queue.qsize()}") - print(f" return_packet_processor_queue: {mo.return_packet_processor_queue.qsize()}") - for i, q in enumerate(mo.swap_queues): - print(f" swap_queue[{i}]: {q.qsize()}") - - print("===================\n") - - time.sleep(0.1) # 更快的循環，更及時的調試信息 - - except KeyboardInterrupt: - print("\n使用者中斷程序...") - pass - - # === 程序結束清理 (來自test51新版本) === - print("正在關閉所有連接...") - - # -------- 清理 ROS2 -------- - try: - bridge.destroy_node() - except Exception as e: - print(f"清理主Node失敗: {e}") - - # 清理備用node（如果存在） - if hasattr(bridge, '_backup_node'): - try: - bridge._backup_node.destroy_node() - print("備用Node已清理") - except Exception as e: - print(f"清理備用Node失敗: {e}") - - rclpy.shutdown() - - # 關閉輸入通道 - for i, obj in enumerate(mavlink_objects_in): - device_type = "UDP" - print(f"關閉設備 {i+1} ({device_type}) 輸入通道") - obj.stop() - obj.thread.join() - mavlink_sockets_in[i].close() - - # 關閉輸出通道 - for i, obj in enumerate(mavlink_objects_out): - print(f"關閉 GCS {i+1} 輸出通道") - obj.stop() - obj.thread.join() - mavlink_sockets_out[i].close() - - # 關閉分析器 - bridge.stop() - bridge.thread.join() - print('<=== End of Program') \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md new file mode 100644 index 0000000..efd48cd --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md @@ -0,0 +1,190 @@ + +這個檔案整理 此專案下 程式代碼的流程與思路 +只會挑出重要的變數與方法描述 +以利後續開發使用 + +# 開發此專案的注意事項 +- 預設 autopilot 的 component id = 1 +- 不允許 system id 重複 +- 增加一個固定數值監控然後要到 ros2 topic + - mavlinkROS2Node.py 檔案內 + - PublishRateController.topic_intervals 建立 + - VehicleStatusPublisher._publish_vehicle_status 登記 + - VehicleStatusPublisher._publish_XXX 實作 + - mavlinkObject.py 檔案內 + - mavlink_bridge.message_handlers 登記 + - mavlink_bridge._handle_XXX 實作 + - mavlink_object.bridge_msg_types 登記 (這個可以用介面調) + - mavlinkVehicleView.py 檔案內 + - 注意對應的資料存放區 + + +--- +# 檔案結構 + +特別注意: +1. 有標註 [async method] 都是不該被直接呼叫的內部方法 + +- *valuable* 這個是變數 **沒有括號** +- *method (parameters...)* 這個是方法 **有括號** + +## mainOrchestrator.py : 程式進入點 + +### **[Class]** Orchestrator + 最上層的發配資源與啟動終端機面板的調配者 +- *self.manager* 存放 async_io_manager 實例 +- *self.bridge* 存放 mavlink_bridge 實例 +- *self.plumber* 存放 serial_manager 實例 +- *self.vehicle_registry* 存放 vehicle_registry 實例 + +- *self.panel_thread* 面板的執行緒 +- *self.panelState* 暫存面板與調配者互動的資料流動區 + - 面板運行狀態 + - 面板操作結果 + - 其他模組的運行狀態 +--- +- *mainLoop()* 核心方法 + - 更新個模組狀態到 *self.panelState* + - 對應面板來的操作指令 +--- + 對於 async_io_manager 控制實現 +- *create_udp_object()* +- *delete_udp_object()* +- *add_target_to_object()* +- *remove_target_from_object()* +--- + 關於載具管理與檢視 +- *_update_vehicles_list()* +- *_prepare_vehicle_info()* +--- + 關於 serial_manager 控制實現 +- *create_serial_port_object()* + + +### **[Class]** ControlPanel + 面板的核心運行物件 + 把自己的變數 獨立出來都放到 PanelState 去 +- *panel_thread()* 核心方法 + - 主選單的引入 + - 主選單下所有的按鍵操作 + - 定義所有人為操作後續面板執行緒行為 +- *menu_tree()* 基礎選單的定義檔 +--- + 關於 udp object 的操作 +- *create_object_list_menu()* object 選單的定義檔 +- *show_object_info()* 顯示 object 資訊 +- *select_target_socket()* object 對於轉拋功能的操作 +--- + 關於 serial 的操作 +- *create_serial_port_menu()* +- *create_linked_serial_menu()* +- *show_linked_serial_info()* +--- + 關於載具檢視與操作 +- *create_vehicles_list_menu()* +- *show_vehicle_info()* + +### **[Class]** PanelState + 作為面板執行緒(ControlPanel)與調配者(Orchestrator)溝通的管道 + 不包含具體實作方法 是 ControlPanel 的延伸 +- *self.panel_info_msg_list* 顯示在面板上的資訊訊息 + +## mavlinkObject.py + +### 全域變數 +- *stream_bridge_ring* +- *return_packet_ring* + +### **[Class]** mavlink_bridge + 唯一實例 + 實際去解析 mavlink 封包的地方 + 接收 stream_bridge_ring 與 return_packet_ring 的資料 + 這邊是比較偏自動化 不會被操作的 +- *self.thread* 自己的執行緒 +--- +- *_run_thread()* 核心方法 +- *_handle_XXXXX()* 每一種單項 mavlink 封包的解析 +- *send_message()* 是 _send_to_socket() 的高階包裝 跟 ros2 介面做互動的方法 +- *_send_to_socket()* 把要傳送的封包 丟給 mavlink 去處理 + +### **[Class]** async_io_manager + 唯一實例 + 異步 event loop + 沒有核心方法 + 這邊主要是管理 mavlink_object 的地方 (但不會對於某個 mavlink_object 內部需求做操作) + +- *self.thread* 自己的執行緒 +- *self.managed_objects* 資料結構 socket_id: mavlink_object +--- +- *add_mavlink_object(mavlink_object)* [call method] 把一個 mavlink_object 物件加入管理 +- *_async_add_mavlink_object(mavlink_object)* [async method] 對應上面的內部方法 不該直接使用 +- *remove_mavlink_object(socket_id)* [call method] 從管理區把指定 mavlink_object 移除 + +### **[Class]** mavlink_object + 儲存 mavlink socket + 處理 mavlink 封包分流的地方 +- *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) } +- *self.mavlink_socket* 從 pymavlink 繼承的socket物件 +- *self.state* 描述這個 socket 物件的狀態 +--- +- *process_data()* [async method] 核心方法 +- *remove_target_socket()* *add_target_socket()* +- *message_put_queue()* 把要傳送的封包放到自己這個物件的暫存區 會由 process_data() 依照異步流程被實際丟出 + +## serialManager.py + 看這個檔案的重點再於要搞清楚 端口物件 還是 傳輸物件 + +### **[Class]** serial_manager + 異步 event loop + 管理 mavlink_object 的地方 +- *self.thread* 自己的執行緒 +- *self.loop* 自己的事件迴圈 +--- +- *create_serial_link()* [call method] 把 serial 端口跟 UDP 端口打通 +- *_async_create_serial_link()* [async method] 把兩種端口接起來的重點程序 +- *remove_serial_link()* [call method] 關閉指定的 serial 端口 +- *_async_remove_serial_link()* [async method] + +### **[Class]** serial_object + 被塞在 serial_manager 裡面 + 只是一個變數物件 + 用來被儲存 serial 的資訊 +- *self.transport* +- *self.protocol* +- *self.udp_handler* UDP 端口物件 +- *self.serial_handler* Serial 端口物件 + +### **[Class]** UDPHandler + 處理 UDP 收發的端口 作為一個端口物件 + 作為 UDP OutBound 使用 所以不會佔用系統監聽資源 +- *self.transport* 自己的傳輸物件 +--- +- *datagram_received()* 先加碼成 Xbee 再呼叫 Serial 端口物件送出 + +### **[Class]** SerialHandler + 處理 Serial 收發的端口 作為一個端口物件 +- *self.transport* 自己的傳輸物件 +--- +- *data_received()* 先組合 Serial 封包 再解碼 再呼叫 UDP 端口物件送出 + +## mavlinkVehicleView.py + 這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用 + +# 開發記錄 + +## 已實現功能 +1. mavlink 分流解析 +2. mavlink socket 建立 +3. mavlink socket 轉拋 proxy +4. 建立 Serial 轉 UDP 連結 並管理 +5. 建立 serial 連線 +6. 各單元模組化 +7. 終端機介面控制 +8. 基礎載具流量觀測 +9. 載具狀態收集與彙整 +10. a. ros2 topic 應用開發介面 + +### 待開發功能 +5-1. 建立 serial 連線 並可以對接收器下達AT指令 +5-2. 模組化 serial 連線機制 以利後期擴容其他模組 +10. a. ros2 應用開發介面 \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py new file mode 100644 index 0000000..ae89d72 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -0,0 +1,1597 @@ + +''' + +主要調配流程的程式 + +這個檔案包含 Terminal Utility Layer (TUL) 作為人機互動介面，並調用 mavlinkDevice 和 mavlinkObject 來處理 MAVLink 通訊和物件管理。 + +''' + +import os +import time +import sys + +import curses +import threading +import queue +import signal + +from pymavlink import mavutil + +# 自定義的 import +from . import mavlinkObject as mo +from . import serialManager as sm +from . import mavlinkVehicleView as mvv +from . import mavlinkROS2Nodes as mros + +from .utils import RingBuffer, setup_logger +from .utils import acquireSerial, acquirePort +from .utils.acquirePort import find_available_port + +logger = setup_logger(os.path.basename(__file__)) +VERSION_NO = "v0.59" + +class PanelState: + def __init__(self): + self.panel_status = "Idle" + termination_start_time = None + self.mavlink_bridge_state = "Stopped" + self.object_manager_state = "Stopped" + self.serial_manager_state = "Stopped" + self.ros2_manager_state = "Stopped" + self.socket_object_list = [] # 已有的 mavlink object + self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string + self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 + + # 關於創建通道時的暫存資訊 + self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊 + self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": False} # 暫存 Serial 設定資訊 + + # 關於顯示通道資訊 + self.socket_info_single = { + "socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "", + "target_sockets": "", "primary_socket_id": "", "socket_connection_string": "", + "InfoReady": False} # 暫存單一 socket 的資訊 + self.serial_info_single = { + "serial_port": "", "baudrate": "", "receiver_type": "", "target_port": "", + "InfoReady": False} # 暫存單一 serial 連結的資訊 + + # 關於顯示載具資訊 + self.connected_vehicles_dict = {} # {(sysid, compid): {...基本資訊...}} + self.vehicle_info_single = { + "sysid": 0, + "compid": 0, + "vehicle_type": "", + "component_type": "", + "mav_autopilot": "", + "socket_id": None, + "connection_type": "", + "packet_stats": {}, + "msg_type_counts": {}, + "prev_stats": {}, # 用於計算變化率 + "InfoReady": False + } + + def intoSTART(self): + self.panel_status = "Running" + + def intoTERMINATION(self): + self.termination_start_time = time.time() + self.panel_status = "Terminating" + + def intoENGINEER(self): + self.panel_status = "Engineer" + + def intoSTOPPED(self): + self.panel_status = "Stopped" + + # def set_user_input(self, text): + # self.user_input = text + +class MenuNode: + def __init__(self, name, desc="", action=None, children=None): + self.name = name + self.desc = desc + self.action = action # 可以是函式或特殊字串 + self.children = children or [] # 子選單列表 + +class ControlPanel: + def __init__(self): + pass + + def input_dialog(stdscr, prompt="請輸入文字: "): + """顯示輸入對話框""" + height, width = stdscr.getmaxyx() + + # 建立輸入視窗 + dialog_height = 5 + dialog_width = min(60, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + # 建立視窗邊框 + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(1, 2, prompt) + dialog_win.addstr(3, 2, "按 Enter 確認, ESC 取消") + dialog_win.refresh() + + # 輸入區域 + input_win = curses.newwin(1, dialog_width - 6, start_y + 2, start_x + 2) + input_win.keypad(True) + + curses.echo() + curses.curs_set(1) + + user_input = "" + + while True: + input_win.clear() + input_win.addstr(0, 0, user_input[-dialog_width+8:]) # 顯示輸入內容（滾動） + input_win.refresh() + + ch = input_win.getch() + + if ch == 27: # ESC + user_input = None + break + elif ch in (curses.KEY_ENTER, 10, 13): # Enter + break + elif ch in (curses.KEY_BACKSPACE, 127, 8): # Backspace + user_input = user_input[:-1] + elif 32 <= ch <= 126: # 可打印字符 + user_input += chr(ch) + + curses.noecho() + curses.curs_set(0) + + # 清理視窗 + del input_win + del dialog_win + stdscr.clear() + stdscr.refresh() + + return user_input + + # ================ 關於 主要選單 的部份 =================== + + def menu_tree(self): + """建立多層選單結構""" + return MenuNode("Main Menu", children=[ + MenuNode("MavLink Object", "UDP MavLink 通道選項", children=[ + MenuNode("New+", children=[ + MenuNode("UDP InBound", children=[ + MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"), + MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"), + MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"), + ]), + MenuNode("UDP OutBound", children=[ + MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"), + MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"), + MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"), + ]), + ]), + MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"), + ]), + MenuNode("Serial Manager", "Serial 連接埠選項", children=[ + MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"), + MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"), + ]), + MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"), + MenuNode("Engineer Mode", "工程模式", children=[ + MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), + MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), + MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), + ]), + MenuNode("Shutdown", "關閉整個系統", children=[ + MenuNode("Return", "繼續運行", "BACK"), + MenuNode("Confirm", "關閉系統", "QUIT"), + ]), + ]) + + def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event): + stdscr = None + + def cleanup(): + """清理 curses 狀態""" + if stdscr: + stdscr.keypad(False) + curses.nocbreak() + curses.echo() + curses.endwin() + + def pre_panel_shutdown(): + # 先關閉所有模組 再關閉面板 + cmd_q.put("SHUTDOWN_BRIDGE") + cmd_q.put("SHUTDOWN_MANAGER") + cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + + def draw_menu(screen): + nonlocal stdscr + stdscr = screen + + curses.curs_set(0) + stdscr.nodelay(False) # 阻塞讀鍵 + stdscr.keypad(True) + + # 選單導航狀態 + menu_stack = [self.menu_tree()] # 選單堆疊 + idx_stack = [0] # 索引堆疊 + + state.intoSTART() # 設定狀態為運行中 + + while not stop_evt.is_set(): + + current_menu = menu_stack[-1] + current_idx = idx_stack[-1] + + # 獲取終端機尺寸 + height, width = stdscr.getmaxyx() + # 簡單暴力的限制視窗的大小 + MIN_HEIGHT = ( + 2 + # 邊界 + 6 + # 狀態列 操作說明列 一個空白 + 11+ # 最大選單 與 空白區 + 5 # 訊息區域 + ) + if height < MIN_HEIGHT or width < 60: + logger.error("Terminal size too small for Control Panel.") + break + + stdscr.clear() + stdscr.border() + + # 更新模組狀態顯示 + stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) + stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") + stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") + stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}") + stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") + stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}") + stdscr.addstr(3, 36, f"ROS2 Manager State : {state.ros2_manager_state}") + + # 顯示當前選單項目 + start_line = 6 + for i, child in enumerate(current_menu.children): + marker = "➤ " if i == current_idx else " " + # 動態顯示已輸入的值 + desc = child.desc + if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]: + desc = f"{child.desc} [{state.udp_info_temp['IP']}]" + elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]: + desc = f"{child.desc} [{state.udp_info_temp['Port']}]" + elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]: + desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]" + elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]: + desc = f"{child.desc} [{state.serial_info_temp['Baud']}]" + elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP": + link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No" + desc = f"{child.desc} [{link_status}]" + + line = f"{marker}{child.name:15s} – {desc}" + attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL + stdscr.addstr(start_line + i, 4, line, attr) + + # 顯示訊息區域 + # info_start_line = start_line + len(current_menu.children) + 1 + info_start_line = height - 8 + max_msg_lines = 5 # 最多顯示 5 行訊息 + current_time = time.time() + + # 清理過時的訊息 + state.panel_info_msg_list = [ + (msg, timestamp) for msg, timestamp in state.panel_info_msg_list + if current_time - timestamp < 2.0 #秒數 + ] + + # 只顯示最新的 max_msg_lines 條訊息 + display_msgs = state.panel_info_msg_list[-max_msg_lines:] + + for i, msg_data in enumerate(display_msgs): + if info_start_line + i >= help_line - 1: # 避免超出邊界 + break + msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data + # 截斷過長的訊息 + max_msg_width = width - 6 + if len(msg) > max_msg_width: + msg = msg[:max_msg_width-3] + "..." + + stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD) + + + + # 操作說明 + # help_line = start_line + len(current_menu.children) + 2 + help_line = height - 2 + stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM) + stdscr.addstr(height-1 , width-12, f" {VERSION_NO} ", curses.A_DIM) + + stdscr.refresh() + + # 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作 + # 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈 + # 超過幾秒沒有反應就強制關閉 + if state.panel_status == "Terminating": + if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點 + logger.warning("Control Panel forced shutdown after timeout.") + state.intoSTOPPED() + # stop_evt.set() + # continue + break + time.sleep(0.1) + if (state.mavlink_bridge_state == "Stopped" and + state.object_manager_state == "Stopped" and + state.serial_manager_state == "Stopped"): + state.intoSTOPPED() + # stop_evt.set() + break + continue + + # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt + stdscr.timeout(100) + ch = stdscr.getch() + + if ch == -1: # 沒有操作 + continue + + # 處理按鍵 + if ch in (curses.KEY_UP, ord('k')): + idx_stack[-1] = (current_idx - 1) % len(current_menu.children) + + elif ch in (curses.KEY_DOWN, ord('j')): + idx_stack[-1] = (current_idx + 1) % len(current_menu.children) + + elif ch == (ord('O')): + # 進入工程模式 + state.intoENGINEER() + + elif ch == (ord('o')): + # 離開工程模式 + state.intoSTART() + + elif ch == curses.KEY_LEFT: + # 返回上層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + + elif ch == curses.KEY_RIGHT: + # 進入下層 (但不執行動作) + selected = current_menu.children[current_idx] + if selected.children: # 有子選單 + menu_stack.append(selected) + idx_stack.append(0) + + elif ch in (ord('q'), 27): + if state.panel_status == "Engineer": + state.intoTERMINATION() + pre_panel_shutdown() + + elif ch in (curses.KEY_ENTER, 10, 13): + selected = current_menu.children[current_idx] + + # 處理不同類型的動作 + if selected.children: # 有子選單 + menu_stack.append(selected) + idx_stack.append(0) + + elif selected.action == "BACK": + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "QUIT": + state.intoTERMINATION() + pre_panel_shutdown() + + elif selected.action == "TEXT_UDP_IP": + result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ") + if result is not None: + state.udp_info_temp["IP"] = result + + elif selected.action == "TEXT_UDP_PORT": + result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ") + if result is not None: + state.udp_info_temp["Port"] = result + + elif selected.action == "CREATE_UDP_INBOUND": + cmd_q.put("CREATE_UDP_INBOUND") + # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # menu_stack.pop() + # idx_stack.pop() + + elif selected.action == "CREATE_UDP_OUTBOUND": + cmd_q.put("CREATE_UDP_OUTBOUND") + # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # menu_stack.pop() + # idx_stack.pop() + + elif selected.action == "TEXT_BAUD_SERIAL": + result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ") + if result is not None: + try: + baud_rate = int(result) + except ValueError: + state.panel_info_msg_list.append(("Invalid Baud Rate input.", time.time())) + state.serial_info_temp["Baud"] = baud_rate + + elif selected.action == "SET_SERIAL_COMM_XBEE": + state.serial_info_temp["CommunicationType"] = "XBee(API-AT)" + menu_stack.pop() + idx_stack.pop() + elif selected.action == "SET_SERIAL_COMM_TELEMETRY": + state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)" + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES": + state.serial_info_temp["Go2Middleware"] = True + menu_stack.pop() + idx_stack.pop() + elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO": + state.serial_info_temp["Go2Middleware"] = False + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "CREATE_SERIAL_PORT": + state.serial_info_temp["Port"] = menu_stack[-1].name # 從選單取得 Port 名稱 + cmd_q.put("CREATE_SERIAL_PORT") + # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "LIST_SERIAL_RES": + created_list_menu = self.create_serial_port_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "LIST_SERIAL_LINKS": + created_list_menu = self.create_linked_serial_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_LINKED_SERIAL": + # 顯示 Serial 連結詳細資訊 + if hasattr(selected, 'serial_id'): + cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id)) + self.show_linked_serial_info(stdscr, selected.serial_id, state) + + elif selected.action == "REMOVE_LINKED_SERIAL": + # 移除 Serial 連結 + if hasattr(selected, 'serial_id'): + cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id)) + # 返回上層（回到列表） + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # 一樣退兩層 + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "LIST_MAV_OBJECT": + # 動態生成 mavlink_object 列表選單 + created_list_menu = self.create_object_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action in ("PREV_PAGE", "NEXT_PAGE"): + if hasattr(selected, 'page'): + current_list_menu = menu_stack[-1] + menu_stack.pop() + idx_stack.pop() + + # 依據選單種類 重新建立分頁 + if current_list_menu.name == "Serial Port List": + created_list_menu = self.create_serial_port_menu(state, page=selected.page) + elif current_list_menu.name == "Object List": + created_list_menu = self.create_object_list_menu(state, page=selected.page) + elif current_list_menu.name == "Linked Serial List": + created_list_menu = self.create_linked_serial_menu(state, page=selected.page) + elif current_list_menu.name == "Connected Vehicles": + created_list_menu = self.create_vehicles_list_menu(state, page=selected.page) + else: + # 不支援的選單類型，回到原本的選單 + menu_stack.append(current_list_menu) + idx_stack.append(0) + continue + + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_MAV_OBJECT": + # 顯示物件詳細資訊 + if hasattr(selected, 'socket_id'): + cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id)) + self.show_object_info(stdscr, selected.socket_id, state) + + elif selected.action == "REMOVE_MAV_OBJECT": + # 移除物件 + if hasattr(selected, 'socket_id'): + cmd_q.put(("REMOVE_OBJECT", selected.socket_id)) + # 返回上層（回到列表） + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好 + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "MAVOBJ_MAKE_LINK": + # 建立轉發連結 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state) + if target_id is not None: + cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結 + + elif selected.action == "MAVOBJ_CANCEL_LINK": + # 取消轉發連結 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True) + if target_id is not None: + cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id)) # 雙向取消連結 + + elif selected.action == "MAVOBJ_ADD_TARGET": + # 添加目標端口 + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state) + if target_id is not None: + cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + + elif selected.action == "STOP_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_MANAGER") + + elif selected.action == "STOP_BRIDGE": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_BRIDGE") + + elif selected.action == "STOP_SERIAL_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + + elif selected.action == "INSPECT_VEHICLES": + # 進入載具檢視選單 + cmd_q.put("UPDATE_VEHICLES_LIST") + created_list_menu = self.create_vehicles_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_VEHICLE": + # 顯示載具詳細資訊 + if hasattr(selected, 'sysid') and hasattr(selected, 'compid'): + cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid)) + self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state) + + elif callable(selected.action): + # 執行函式 + cmd_q.put(selected.action) + + try: + curses.wrapper(draw_menu) + except KeyboardInterrupt: + pass + finally: + cleanup() + + # ================ 關於 mavlink object 的部份 =================== + + def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 mavlink_object 列表選單（支持分頁）""" + children = [] + + if not state.socket_object_list: + children.append(MenuNode("(Empty)", "目前沒有連結口", None)) + else: + total_items = len(state.socket_object_list) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的物件 + for socket_id in state.socket_object_list[start_idx:end_idx]: + # 為每個 socket 創建子選單 + obj_menu = MenuNode(f"Socket #{socket_id}", f"連結口 {socket_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_MAV_OBJECT"), + MenuNode("Make Link", "建立轉發連結", "MAVOBJ_MAKE_LINK"), + MenuNode("Cancel Link", "取消轉發連結", "MAVOBJ_CANCEL_LINK"), + MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"), + MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 socket_id 附加到每個子選單項目上 + for child in obj_menu.children: + child.socket_id = socket_id + children.append(obj_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_object_info(self, stdscr, socket_id, state: PanelState): + """顯示物件詳細資訊的對話框""" + + start = time.time() + while not state.socket_info_single.get('InfoReady', False): + # 太久沒有回應 + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Socket Info NOT Aquire!", time.time())) + return + time.sleep(0.05) # 等待資訊準備好 + + height, width = stdscr.getmaxyx() + dialog_height = 15 + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD) + + # 這裡顯示基本資訊 + dialog_win.addstr(2, 2, f"Socket ID : {socket_id}") + dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}") + # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', ''))) + dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}") + dialog_win.addstr(4, 30, f"{state.socket_info_single.get('socket_connection_string', '')}") + show_str = ",".join(map(str, state.socket_info_single.get('bridge_msg_types', ''))) + dialog_win.addstr(5, 2, f"Bridge Pack : {show_str if show_str else 'N/A'}") + show_str = ",".join(map(str, state.socket_info_single.get('return_msg_types', ''))) + dialog_win.addstr(6, 2, f"Return Pack : {show_str if show_str else 'N/A'}") + dialog_win.addstr(7, 2, f"Primary Socket ID: {state.socket_info_single.get('primary_socket_id', 'It Self')}") + show_str = ",".join(map(str, state.socket_info_single.get('target_sockets', ''))) + dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}") + + state.socket_info_single['InfoReady'] = False # 重置狀態以便下次使用 + + dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") + dialog_win.refresh() + + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + + def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False): + """選擇目標 socket 的對話框""" + height, width = stdscr.getmaxyx() + dialog_height = min(15, len(state.socket_object_list) + 5) + dialog_width = min(50, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.keypad(True) + + title = "選擇要移除的目標" if remove_mode else "選擇轉發目標" + available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id] + + if not available_sockets: + dialog_win.border() + dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) + dialog_win.addstr(2, 2, "沒有可用的目標") + dialog_win.addstr(4, 2, "按任意鍵返回...") + dialog_win.refresh() + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + return None + + selected_idx = 0 + + while True: + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) + + for i, socket_id in enumerate(available_sockets): + marker = "➤" if i == selected_idx else " " + attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL + dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr) + + dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消") + dialog_win.refresh() + + ch = dialog_win.getch() + + if ch in (curses.KEY_UP, ord('k')): + selected_idx = (selected_idx - 1) % len(available_sockets) + elif ch in (curses.KEY_DOWN, ord('j')): + selected_idx = (selected_idx + 1) % len(available_sockets) + elif ch in (curses.KEY_ENTER, 10, 13): + result = available_sockets[selected_idx] + del dialog_win + stdscr.clear() + stdscr.refresh() + return result + elif ch == 27: # ESC + del dialog_win + stdscr.clear() + stdscr.refresh() + return None + + # ================ 關於 serial link 的部份 =================== + + def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 serial port 列表選單（支持分頁）""" + children = [] + + # 獲取可用的 Serial 連接埠列表 + # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓 + serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*']) + + if not serial_ports: + children.append(MenuNode("(Empty)", "目前沒有串口設備", None)) + else: + total_items = len(serial_ports) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的串口 + for port in serial_ports[start_idx:end_idx]: + port_menu = MenuNode(f"{port}", children=[ + MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[ + MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"), + # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), + ]), + MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), + MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[ + MenuNode("Yes", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES"), + MenuNode("No", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO"), + ]), + MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 port 附加到每個子選單項目上 + for child in port_menu.children: + child.port = port + children.append(port_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能""" + children = [] + + if not state.linked_serial_dict: + children.append(MenuNode("(Empty)", "目前沒有連結口", None)) + else: + total_items = len(state.linked_serial_dict) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的物件 + linked_serial_id_list = list(state.linked_serial_dict.keys()) + for serial_id in linked_serial_id_list[start_idx:end_idx]: + # 為每個 socket 創建子選單 + obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"), + MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"), + # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 serial_id 附加到每個子選單項目上 + for child in obj_menu.children: + child.serial_id = serial_id + children.append(obj_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_linked_serial_info(self, stdscr, serial_id, state: PanelState): + """顯示 Serial 連結詳細資訊的對話框""" + + start = time.time() + while not state.serial_info_single.get('InfoReady', False): + # 太久沒有回應 + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Serial Info NOT Aquire!", time.time())) + return + time.sleep(0.05) # 等待資訊準備好 + + height, width = stdscr.getmaxyx() + dialog_height = 15 + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD) + + # 從 linked_serial_dict 獲取資訊 + serial_info = state.linked_serial_dict.get(serial_id, {}) + + if not serial_info: + dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊") + else: + # 顯示基本資訊 + dialog_win.addstr(2, 2, f"Serial ID : {serial_id}") + dialog_win.addstr(3, 2, f"Serial Port : {state.serial_info_single.get('serial_port', 'N/A')}") + dialog_win.addstr(4, 2, f"Baudrate : {state.serial_info_single.get('baudrate', 'N/A')}") + dialog_win.addstr(5, 2, f"Communication : {state.serial_info_single.get('receiver_type', 'N/A')}") + dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}") + dialog_win.addstr(7, 2, f"Status : {state.serial_info_single.get('status', 'Running')}") + + # 如果有額外資訊可以繼續添加 + if 'thread_alive' in serial_info: + thread_status = "Alive" if serial_info['thread_alive'] else "Stopped" + dialog_win.addstr(8, 2, f"Thread Status : {thread_status}") + + state.serial_info_single['InfoReady'] = False # 重置狀態以便下次使用 + + dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") + dialog_win.refresh() + + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + + # ================ 關於載具檢視的部份 =================== + + def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 已連線載具 列表選單（支持分頁） + 每個 vehicle-component 組合都是獨立的選單項目 + """ + children = [] + + if not state.connected_vehicles_dict: + children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None)) + else: + total_items = len(state.connected_vehicles_dict) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # vehicle_id_list 現在是 (sysid, compid) 的元組列表 + vehicle_comp_list = list(state.connected_vehicles_dict.keys()) + + # 顯示當前頁的物件 + for sysid, compid in vehicle_comp_list[start_idx:end_idx]: + # 建立顯示名稱 + display_name = f"Vehicle #{sysid} - Comp #{compid}" + desc = f"載具 {sysid} 組件 {compid}" + + vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE") + # 將 sysid 和 compid 附加到選單項目上 + vehicle_menu.sysid = sysid + vehicle_menu.compid = compid + children.append(vehicle_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState): + """顯示載具組件詳細資訊（動態更新，顯示變化率）""" + + # 等待資訊準備 + start = time.time() + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time())) + return + time.sleep(0.05) + + info = state.vehicle_info_single + height, width = stdscr.getmaxyx() + dialog_height = min(22, height - 4) + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.nodelay(True) # 非阻塞模式，允許動態更新 + dialog_win.keypad(True) + + # MAV_TYPE 名稱對應 + MAV_TYPE_NAMES = { + 0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter", + 4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover", + 12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera" + } + + # 動態更新迴圈 + last_update = time.time() + while True: + current_time = time.time() + + # 每 1 秒重新請求資料 + if current_time - last_update >= 1.0: + # 觸發資料更新（透過 Orchestrator） + cmd_q.put(("INSPECT_VEHICLE", sysid, compid)) + # 等待新資料準備好 + wait_start = time.time() + state.vehicle_info_single['InfoReady'] = False + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - wait_start > 0.5: # 最多等 0.5 秒 + break + time.sleep(0.01) + # 更新 info 參照 + info = state.vehicle_info_single + last_update = current_time + + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD) + + # === 基礎資訊 === + dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE) + dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE) + + # # MAV Type # 這個用不到 + # mav_type = info.get('vehicle_type', 'N/A') + # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type) + # dialog_win.addstr(3, 2, f"MAV Type : {mav_type_str}") + + # Component Type + dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}") + + # Autopilot Type + if info.get('mav_autopilot') is not None: + dialog_win.addstr(4, 2, f"Autopilot : {info.get('mav_autopilot', 'N/A')}") + + # Connection Info + dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}") + dialog_win.addstr(4, 32, f"Socket ID : #{info.get('socket_id', 'N/A')}") + + # === 封包統計 === + stats = info.get('packet_stats', {}) + dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE) + + received = stats.get('received', 0) + lost = stats.get('lost', 0) + loss_rate = stats.get('loss_rate', 0.0) + last_seq = stats.get('last_seq', 'N/A') + + # 計算變化 + received_delta = stats.get('received_delta', 0) + lost_delta = stats.get('lost_delta', 0) + + # 顯示變化率 + recv_str = f"{received:6d}" + if received_delta > 0: + recv_str += f" (+{received_delta}↑)" + + lost_str = f"{lost:4d}" + if lost_delta > 0: + lost_str += f" (+{lost_delta}↑)" + + dialog_win.addstr(8, 2, f"Received : {recv_str}") + dialog_win.addstr(8, 32, f"Lost : {lost_str}") + dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%") + dialog_win.addstr(9, 32, f"Last Seq : {last_seq}") + + # 最後接收時間 + last_msg_time = stats.get('last_msg_time') + if last_msg_time: + time_str = time.strftime("%H:%M:%S", time.localtime(last_msg_time)) + elapsed = current_time - last_msg_time + dialog_win.addstr(10, 2, f"Last Time : {time_str}") + dialog_win.addstr(10, 32, f"Elapsed : {elapsed:.1f}s") + else: + dialog_win.addstr(10, 2, "Last Time : N/A") + + # === 訊息類型分佈 === + dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE) + + msg_counts = info.get('msg_type_counts', {}) + + # MAVLink 訊息名稱對應（縮寫版本） + MSG_NAMES = { + 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU", + 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL", + 74: "VFR_HUD", 147: "BATT_ST" + } + + # 顯示前 12 個最常見的訊息類型（兩列各 6 個） + msg_items = list(msg_counts.items())[:12] + line = 13 + for i, (msg_id, count) in enumerate(msg_items): + msg_name = MSG_NAMES.get(msg_id, "???") + delta = stats.get(f'msg_delta_{msg_id}', 0) + + # 格式化數據 + if delta > 0: + data_str = f"{count}(+{delta}↑)" + else: + data_str = f"{count}" + + # 格式化顯示：[ID]NAME DATA (ID固定3字符寬度，右對齊) + display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}" + + # 左列(偶數索引)或右列(奇數索引) + col = 2 if i % 2 == 0 else 36 + row = line + (i // 2) + + if row >= dialog_height - 3: # 避免超出邊界 + break + + dialog_win.addstr(row, col, display_str) + + # 確保跳過顯示區域 + line = line + 6 + + dialog_win.addstr(dialog_height - 2, 2, "Press R: Reset Stats, C: Unregister, other key to return...") + dialog_win.refresh() + + # 檢查是否有按鍵（非阻塞） + ch = dialog_win.getch() + if ch in (ord('R'), ord('r')): + cmd_q.put(("RESET_VEHICLE_STATISTICS", sysid, compid)) + elif ch in (ord('C'), ord('c')): + cmd_q.put(("UNREGISTER_VEHICLE", sysid)) + break + elif ch != -1: # 有按鍵則退出 + break + + # 短暫延遲 + time.sleep(0.1) + + state.vehicle_info_single['InfoReady'] = False + del dialog_win + stdscr.clear() + stdscr.refresh() + + + +class Orchestrator: + def __init__(self, stop_sig): + self.stop_evt = stop_sig # 外部操作去中斷 "面板" 執行緒的訊號 (內部自己停止的話不需要用這個) + self.occupied_ip_ports = {} # 紀錄已被佔用的 ip:port 組合 "ip str" : [port int, port int, ...] + + self.vehicle_registry = mvv.vehicle_registry + + # === 1) 面板部分的準備 === + self.cmd_q = queue.Queue() + self.panelState = PanelState() # 面板的狀態儲存 + self.cPanel = ControlPanel() # 面板的功能 + self.panel_thread = None + + # === 2) async_io_manager & mavlink_bridge 部分的準備 === + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + self.manager = mo.async_io_manager() + self.bridge = mo.mavlink_bridge() + + # === 3) serial_manager 部分的準備 === + self.plumber = sm.serial_manager() + + # === 4) ros 部分的準備 === + self.fc_ros_manager = mros.ros2_manager + if not self.fc_ros_manager.initialized: + self.fc_ros_manager.initialize() + self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = { + 'position': 1.0, + 'attitude': 0.0, + 'velocity': 0.0, + 'battery': 1.0, + 'vfr_hud': 1.0, + 'mode': 0.0, + 'summary': 1.0, + } + + def engageWholeSystem(self): + """啟動整個系統""" + # === 1) 面板部分的啟動 === + self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt)) + self.panel_thread.start() + + # === 2) async_io_manager & mavlink_bridge 部分的啟動 === + self.manager.start() + self.bridge.start() + + # === 3) serial_manager 部分的啟動 === + self.plumber.start() + + # === 4) ros 部分的啟動 === + self.fc_ros_manager.start() + + def mainLoop(self): + logger.info("Main orchestrator started <-") + try: + # while not self.stop_evt.is_set(): + while self.panel_thread.is_alive(): + + # A. 更新模組狀態 + if self.manager.running: + self.panelState.object_manager_state = 'Running' + else: + self.panelState.object_manager_state = 'Stopped' + + socketid_list = self.manager.get_managed_objects() + self.panelState.socket_object_list = socketid_list + + if self.bridge.running: + self.panelState.mavlink_bridge_state = 'Running' + else: + self.panelState.mavlink_bridge_state = 'Stopped' + + if self.plumber.running: + self.panelState.serial_manager_state = 'Running' + else: + self.panelState.serial_manager_state = 'Stopped' + + linked_serial_dict = self.plumber.get_serial_link() + self.panelState.linked_serial_dict = linked_serial_dict + + if self.fc_ros_manager.running: + self.panelState.ros2_manager_state = 'Running' + else: + self.panelState.ros2_manager_state = 'Stopped' + + # B. 更新載具列表（從 vehicle_registry 獲取） + self._update_vehicles_list() + + # 取出面板丟過來的「動作」 + try: + cmd = self.cmd_q.get_nowait() + if callable(cmd): + cmd() # 執行對應動作 + elif isinstance(cmd, tuple): + # 處理多參數命令 + action = cmd[0] + if action == "REMOVE_OBJECT": + socket_id = cmd[1] + # 先移除所有跟他關聯的 target sockets + for s_id in mo.mavlink_object.mavlinkObjects: + s_obj = mo.mavlink_object.mavlinkObjects[s_id] + if socket_id in s_obj.target_sockets: + self.remove_target_from_object(s_id, socket_id) + # 再移除該物件 + self.delete_udp_object(socket_id) + elif action == "MAVOBJ_ADD_TARGET": + source_id, target_id = cmd[1], cmd[2] + self.add_target_to_object(source_id, target_id) + elif action == "MAVOBJ_REMOVE_TARGET": + source_id, target_id = cmd[1], cmd[2] + self.remove_target_from_object(source_id, target_id) + elif action == "INSPECT_MAV_OBJECT": + socket_id = cmd[1] + mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id, None) + if mav_obj: + self.panelState.socket_info_single["socket_type"] = mav_obj.socket_type + self.panelState.socket_info_single["socket_state"] = mav_obj.state.name + self.panelState.socket_info_single["bridge_msg_types"] = mav_obj.bridge_msg_types + self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types + self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id + self.panelState.socket_info_single["target_sockets"] = mav_obj.target_sockets + self.panelState.socket_info_single["socket_connection_string"] = mav_obj.mavlink_socket.address + self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好 + elif action == "INSPECT_LINKED_SERIAL": + serial_id = cmd[1] + serial_obj = self.plumber.serial_objects.get(serial_id, None) + if serial_obj: + self.panelState.serial_info_single["serial_port"] = serial_obj.serial_port + self.panelState.serial_info_single["baudrate"] = serial_obj.baudrate + self.panelState.serial_info_single["receiver_type"] = serial_obj.receiver_type.name + self.panelState.serial_info_single["target_port"] = serial_obj.target_port + self.panelState.serial_info_single["InfoReady"] = True # 標記資訊已準備好 + elif action == "REMOVE_LINKED_SERIAL": + serial_id = cmd[1] + self.plumber.remove_serial_link(serial_id) + + elif action == "INSPECT_VEHICLE": + sysid, compid = cmd[1], cmd[2] + self._prepare_vehicle_info(sysid, compid) + # elif action == "UPDATE_VEHICLES_LIST": # TODO 這個擺這邊 不知道為何可以有作用 先不動 也許後面有bug? + # logger.debug("Orchestrator: Updating vehicles list upon request") + # self._update_vehicles_list() + elif action == "RESET_VEHICLE_STATISTICS": + sysid, compid = cmd[1], cmd[2] + vehicle_sys = self.vehicle_registry.get(sysid) + vehicle_sys.reset_component_stats(compid) + elif action == "UNREGISTER_VEHICLE": + sysid = cmd[1] + self.vehicle_registry.unregister(sysid) + + elif cmd == "CREATE_UDP_INBOUND": + self.panelState.udp_info_temp["direction"] = "inbound" + self.create_udp_object() + elif cmd == "CREATE_UDP_OUTBOUND": + self.panelState.udp_info_temp["direction"] = "outbound" + self.create_udp_object() + elif cmd == "CREATE_SERIAL_PORT": + self.create_serial_port_object() + elif cmd == "SHUTDOWN_BRIDGE": + self.bridge.stop() + elif cmd == "SHUTDOWN_MANAGER": + self.manager.shutdown() + elif cmd == "SHUTDOWN_SERIAL_MANAGER": + self.plumber.shutdown() + + except queue.Empty: + pass + except Exception as e: + logger.error(f"Error processing command: {e}") + + time.sleep(0.1) + + except KeyboardInterrupt: + pass + except Exception as e: + logger.error(f"Unexpected error in main loop: {e}") + finally: + + # 驗證並確保所有模組都被下達關閉訊號 + # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發 + if self.bridge.thread.is_alive(): + if self.bridge.running: + self.bridge.stop() + self.bridge.thread.join(timeout=2) + + if self.manager.thread.is_alive(): + if self.manager.running: + self.manager.shutdown() + self.manager.thread.join(timeout=2) + + if self.plumber.thread.is_alive(): + if self.plumber.running: + self.plumber.shutdown() + self.plumber.thread.join(timeout=2) + + if self.fc_ros_manager.spin_thread.is_alive(): + if self.fc_ros_manager.running: + self.fc_ros_manager.stop() + self.fc_ros_manager.spin_thread.join(timeout=2) + + # 關閉面板執行緒 + if self.panel_thread.is_alive(): + self.panel_thread.join(timeout=2) + + time.sleep(0.5) # 等待各模組穩定關閉 + + logger.info("Main orchestrator END!") + + # =============== 面板動作 - Mavlink Object =============== + + def create_udp_object(self, socket_type:str = ""): + # 監聽部分 + if self.panelState.udp_info_temp["direction"] == "inbound": + connection_string = f"udp:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" + # 監聽的 port 要先檢查是否被佔用 + ip = self.panelState.udp_info_temp['IP'] + port = int(self.panelState.udp_info_temp['Port']) + port_check_list = self.occupied_ip_ports.get(ip, []) + self.occupied_ip_ports.get("0.0.0.0", []) + if port in port_check_list: + self.panelState.panel_info_msg_list.append((f"Failed! Port {port} on IP {ip} occupied.", time.time()-1)) + return + # 再記錄被佔用的 port + if ip in self.occupied_ip_ports: + self.occupied_ip_ports[ip].append(port) + else: + self.occupied_ip_ports[ip] = [port] + # 外放資訊部分 + elif self.panelState.udp_info_temp["direction"] == "outbound": + connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" + + try: + # 檢測這個 connection_string 是否能成功建立 mavlink 連結 + mavlink_socket = mavutil.mavlink_connection(connection_string) + except Exception as e: + self.panelState.panel_info_msg_list.append((f"Failed to create UDP {self.panelState.udp_info_temp['direction']} object: {e}", time.time()-1)) + return + + # mavlink 連結建立成功 把他丟到 mavlink_object # 重點句 + mavlink_object = mo.mavlink_object(mavlink_socket) + # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 # 重點句 + self.manager.add_mavlink_object(mavlink_object) + + # 設定一下 mavlink_object 的類型描述 + if socket_type == "": + mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize() + else: + mavlink_object.socket_type = socket_type + + self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time())) + + def delete_udp_object(self, socket_id): + """移除指定的 mavlink_object""" + + mavlink_obj = mo.mavlink_object.mavlinkObjects[socket_id] + connection_string = mavlink_obj.mavlink_socket.address + strings = connection_string.split(':') + ip = strings[0] + port = int(strings[1]) + self.occupied_ip_ports[ip].remove(port) + + self.manager.remove_mavlink_object(socket_id) + + def add_target_to_object(self, source_id, target_id): + """為 mavlink_object 添加轉發目標""" + if source_id in mo.mavlink_object.mavlinkObjects: + source_obj = mo.mavlink_object.mavlinkObjects[source_id] + else: + self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time())) + return + + if source_obj.add_target_socket(target_id): + self.panelState.panel_info_msg_list.append((f"Added target {target_id} to socket {source_id}", time.time())) + else: + self.panelState.panel_info_msg_list.append((f"Fail Adding target {target_id} to socket {source_id}", time.time())) + + def remove_target_from_object(self, source_id, target_id): + """從 mavlink_object 移除轉發目標""" + if source_id in mo.mavlink_object.mavlinkObjects: + source_obj = mo.mavlink_object.mavlinkObjects[source_id] + else: + self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time())) + return + + if source_obj.remove_target_socket(target_id): + self.panelState.panel_info_msg_list.append((f"Removed target {target_id} from socket {source_id}", time.time())) + else: + self.panelState.panel_info_msg_list.append((f"Fail Removing target {target_id} from socket {source_id}", time.time())) + + # =============== 面板動作 - Vehicle Inspector =============== + + def _update_vehicles_list(self): + """更新已連線載具列表（從 vehicle_registry 獲取）""" + vehicles_dict = {} + + # 從 vehicle_registry 獲取所有載具 + all_vehicles = self.vehicle_registry.get_all() + + for sysid, vehicle in all_vehicles.items(): + # 遍歷每個載具的所有組件 + for compid, component in vehicle.components.items(): + # 使用 (sysid, compid) 作為 key + vehicles_dict[(sysid, compid)] = { + 'sysid': sysid, + 'compid': compid, + 'vehicle_type': vehicle.vehicle_type, + 'component_type': component.type.value, + 'connection_via': vehicle.connected_via.value, + 'socket_id': vehicle.custom_meta.get('socket_id', 'N/A') + } + + self.panelState.connected_vehicles_dict = vehicles_dict + + def _prepare_vehicle_info(self, sysid, compid): + """準備載具組件的詳細資訊（包含變化率計算）""" + vehicle = self.vehicle_registry.get(sysid) + if not vehicle: + logger.warning(f"Vehicle {sysid} not found") + return + + socket_id = vehicle.custom_meta.get('socket_id', 'N/A') + + component = vehicle.get_component(compid) + if not component: + logger.warning(f"Component {compid} not found in vehicle {sysid}") + return + + stats = component.packet_stats + + # 取得之前的統計資料（用於計算變化） + prev_stats = self.panelState.vehicle_info_single.get('prev_stats', {}) + prev_received = prev_stats.get('received', 0) + prev_lost = prev_stats.get('lost', 0) + prev_msg_counts = prev_stats.get('msg_counts', {}) + + # 計算變化率 + received_delta = stats.received_count - prev_received + lost_delta = stats.lost_count - prev_lost + + # 準備訊息類型計數（排序後取前幾個） + sorted_msg_counts = dict(sorted( + stats.msg_type_count.items(), + key=lambda x: x[1], + reverse=True + )[:12]) # 取前 12 個最常見的 + + # 計算每種訊息類型的變化 + msg_deltas = {} + for msg_id, count in sorted_msg_counts.items(): + prev_count = prev_msg_counts.get(msg_id, 0) + msg_deltas[f'msg_delta_{msg_id}'] = count - prev_count + + # 更新 vehicle_info_single + socket_type = "N/A" + socket_obj =mo.mavlink_object.mavlinkObjects.get(socket_id, None) + if socket_obj: + socket_type = socket_obj.socket_type + + self.panelState.vehicle_info_single = { + "sysid": sysid, + "compid": compid, + # "vehicle_type": vehicle.vehicle_type, # 這個用不到 + "component_type": component.type.value, + "mav_autopilot": component.mav_autopilot, + "socket_id": socket_id, + "connection_type": socket_type, + "packet_stats": { + "received": stats.received_count, + "lost": stats.lost_count, + "loss_rate": (stats.lost_count / stats.received_count * 100 + if stats.received_count > 0 else 0), + "last_seq": stats.last_seq, + "last_msg_time": stats.last_msg_time, + "received_delta": received_delta, + "lost_delta": lost_delta, + **msg_deltas # 展開訊息類型的變化 + }, + "msg_type_counts": sorted_msg_counts, + "prev_stats": { # 保存當前數據用於下次計算變化 + "received": stats.received_count, + "lost": stats.lost_count, + "msg_counts": dict(stats.msg_type_count) + }, + "InfoReady": True + } + + # =============== 面板動作 - Serial Manager =============== + + def create_serial_port_object(self): + # 先檢查是否已有相同的 Serial Port 被建立 + serial_port_strs = self.panelState.linked_serial_dict.values() # linked_serial_dict 會在上面的 mainLoop 被不斷更新 + if self.panelState.serial_info_temp['Port'] in serial_port_strs: + self.panelState.panel_info_msg_list.append( + (f"Fail! Serial Port {self.panelState.serial_info_temp['Port']} already linked.", time.time()) + ) + return + + # 獲取可用的 udp port + udp_port_tmp = find_available_port(19000, 20000) + + # 定義通訊類型映射表 + COMM_TYPE_MAP = { + "XBee(API-AT)": sm.SerialReceiverType.XBEEAPI2AT, + # "XBee(AT-AT)": sm.SerialReceiverType.TELEMETRY, # TODO: 之後再弄 + # 新增區 + } + + # 驗證輸入 + comm_type = self.panelState.serial_info_temp['CommunicationType'] + if not comm_type: + self.panelState.panel_info_msg_list.append( + ("Please select Communication Type first.", time.time()) + ) + return + + # 查找對應的通訊類型 + comm_type_tmp = COMM_TYPE_MAP.get(comm_type) + if comm_type_tmp is None: + self.panelState.panel_info_msg_list.append( + (f"Communication type '{comm_type}' not supported yet.", time.time()) + ) + return + + ret = self.plumber.create_serial_link( + serial_port=self.panelState.serial_info_temp['Port'], + baudrate=self.panelState.serial_info_temp['Baud'], + target_port=udp_port_tmp, + receiver_type=comm_type_tmp, + ) + + if not ret: + self.panelState.panel_info_msg_list.append((f"Failed to create Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time())) + return + + self.panelState.panel_info_msg_list.append((f"Created Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time())) + + # 自動建立對應的 UDP 監聽端口 + if self.panelState.serial_info_temp['Go2Middleware']: + self.panelState.udp_info_temp['IP'] = "127.0.0.1" + self.panelState.udp_info_temp['Port'] = str(udp_port_tmp) + self.panelState.udp_info_temp['direction'] = "inbound" + self.create_udp_object("SERIAL_XBEE") + + +def main(): + + stop_evt = threading.Event() + + def signal_handler(signum, frame): + """處理 Ctrl+C 信號 藉此訊號 會結束下面的 while 迴圈 並逐步關閉各模組""" + stop_evt.set() + + # 註冊信號處理器 + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + orchestrator = Orchestrator(stop_evt) + orchestrator.engageWholeSystem() + orchestrator.mainLoop() # 程式會 block 在這邊 直到收到中斷信號 + +if __name__ == "__main__": + main() + + +''' +================= 改版記錄 ============================ + +2025.12.23 +1. 新增 serial 通道的資訊顯示完整化 +2. 新增 serial 通道刪除功能 +3. 新增 serial 直接順便開 ip object +4. 修改 避免 serial 與 ip port 重複建立相同的通道 +5. 修改 show_object_info 與 show_linked_serial_info 改變檢核 Ready 方式 避免卡死 + +2025.01.16 +1. 新增 "移除載具" 與 "重置載具統計" 功能 +2. 修正 udp port 在移除後仍被記錄為佔用的問題 +3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式 +4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊 +5. 系統納入 mavlink ROS2 Manager 模組 + +''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 88046b8..c8cfaed 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,60 +1,88 @@ ''' -# 不同的匯流排只有單一種通訊協定 -# 匯流排接到訊息後透過 queue stack 傳送到橋接器 -# 會有三種 Queue 分別作為 1. 固定串流橋接器 2. 回傳封包處理器 3. 轉拋串流 -# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue -# 橋接器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 +這個檔案是對 udp 進來的 mavlink 訊息做 "分流" "轉拋" 的地方 (並不會做 "分析") +概念上 把每個 udp 接口 視為一個獨立的 mavlink bus 針對 bus 來統籌管理 + +主要的重點部分: +1. stream_bridge_ring & return_packet_ring +2. mavlink_object & async_io_manager +3. mavlink_bridge + + +stream_bridge_ring & return_packet_ring: + 這兩個 ring buffer 是用來做 mavlink 訊息的分流 + stream_bridge_ring 這邊的資訊比較是給會固定更新的資訊 (例如 HEARTBEAT, ATTITUDE 之類的) + return_packet_ring 比較是處理指令後的回應封包 (例如 PARAM_VALUE, MISSION_ITEM 等等) + +mavlink_object: + 每個 mavlink bus 都會有一個 mavlink_object + 使用 asyncio 處理資料流 用 RingBuffer 來分配訊息 + 內容中沒有獨立的執行緒 只有一個個 asyncio function 會被放到 async_io_manager 裡面執行 + + 關於分流與轉拋的具體實現是在 process_data 這個 asyncio function 裡面 + +async_io_manager: + 首先它紀律並管理所有 mavlink_object 實例 + 有自己一個獨立的執行緒 執行 asyncio loop (mavlink_object 裡面的 asyncio function 都會被放到這個 loop 裡面執行) + +mavlink_bridge: + 專門處理 stream_bridge_ring 裡面的訊息流 + 會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖 + + -# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 -# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlink_object 裡面 -# 這邊的 main 會是用來初始化 mavlink_object 並啟動他的 run function ''' + # 基礎功能的 import -import threading -import queue +import os +import signal import time +import threading +import asyncio +from enum import Enum, auto +from collections import deque +# from typing import Dict, List, Optional, Set, Any, Tuple # mavlink 的 import from pymavlink import mavutil -# ROS2 的 import -from rclpy.node import Node -import std_msgs.msg - # 自定義的 import -from mavlinkDevice import mavlink_device, mavlink_systems -from mavlinkPublish import mavlink_publisher -from theLogger import setup_logger +from .mavlinkVehicleView import ( + vehicle_registry, + VehicleView, + VehicleComponent, + ComponentType, + ConnectionType +) +from .utils import RingBuffer, setup_logger # ====================== 分割線 ===================== -logger = setup_logger("mavlinkObject.py") +logger = setup_logger(os.path.basename(__file__)) -fixed_stream_bridge_queue = queue.Queue() -return_packet_processor_queue = queue.Queue() -swap_queues = [] # 這個 list 用來存放轉拋串流的 queue # 這些 queue 同時也是各自 mavlink_object 的 output buffer +stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255) +return_packet_ring = RingBuffer(capacity=256, buffer_id=254) # ====================== 分割線 ===================== -class mavlink_bridge(Node, mavlink_publisher): +# 使用 vehicle_registry 來管理所有的載具視圖 +# vehicle_registry 是從 mavlinkVehicleView 導入的全域實例 + +class mavlink_bridge: ''' 這個 class 就是 固定串流橋接器 - 是用來接收 mavlink 訊息 並進行橋接 - 這個地方是針對 fixed_stream_bridge_queue 的資料做處理的 + 是用來接收 mavlink 訊息 並進行橋接處理 + 這個地方是針對 stream_bridge_ring 的資料做處理的 記錄有 mavlink bus 上有那些 system id 和 component id 為了每個 system id 都有一個對應的 device object - 並且看是否有重複 system id - - 整段代碼包含兩大區塊 thread 和 node - - thread 區塊內會對 fixed_stream_bridge_queue 進行監聽 並且將收到的訊息進行處理 - 其中 HEARTBEAT 是一個特殊類別 用來初始化整個 device object - - node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關 - 藉由控制 ros2 的機制再把 device object 的資訊發送出去 - - ps. 我限制了這個 class 只能有一個 instance + + 此類別負責： + 1. 從 stream_bridge_ring 接收訊息 + 2. 管理 mavlink_systems（device 和 component objects） + 3. 處理接收到的訊息並更新對應的 component 狀態 + 4. 提供發送訊息的介面，將訊息路由到正確的 mavlink_object + + ps. 此 class 為 singleton，只能有一個 instance ''' _instance = None _lock = threading.Lock() # 確保多線程安全 @@ -68,139 +96,257 @@ class mavlink_bridge(Node, mavlink_publisher): def __init__(self): if not hasattr(self, "initialized"): # 防止重複初始化 self.initialized = True + self.thread = None + self.running = False - # 關聯到全域變數 - global mavlink_systems - self.mavlink_systems = mavlink_systems + # 初始化訊息處理器字典 (msg_id -> handler_function) + self._init_message_handlers() + else: + logger.error('mavlink_bridge instance already exists. Do not create another one.') - # 當 object 建立時會直接運行 thread 直到消滅 + def _init_message_handlers(self): + """初始化訊息處理器映射表，提高處理效率""" + self.message_handlers = { + 0: self._handle_heartbeat, # HEARTBEAT + 30: self._handle_attitude, # ATTITUDE + 32: self._handle_local_position, # LOCAL_POSITION_NED + 33: self._handle_global_position, # GLOBAL_POSITION_INT + 74: self._handle_vfr_hud, # VFR_HUD + 147: self._handle_battery_status, # BATTERY_STATUS + } + + def start(self): + """啟動 mavlink_bridge 的運作""" + if not self.running: self.running = True - self.thread = threading.Thread(target=self._run_thread) + self.thread = threading.Thread(target=self._run_thread, name="MavlinkBridge") self.thread.start() else: - logger.error('mavlink_bridge instance already exists. Do not create another one.') + logger.warning("mavlink_bridge is already running") def stop(self): + """停止 mavlink_bridge 的運作""" self.running = False + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=5.0) # === Thread 區塊 === def _run_thread(self): - # start_time = time.time() # debug - logger.info('Start of mavlink_bridge._run_thread') - # 從 Queue fixed_stream_bridge_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理 + """主執行緒：從 stream_bridge_ring 接收並處理 mavlink 訊息""" + logger.info('mavlink_bridge started <-') + while self.running: - if fixed_stream_bridge_queue.empty(): + # 檢查是否有訊息 + if stream_bridge_ring.is_empty(): + time.sleep(0.001) # 避免忙碌等待 continue - msg_pack = fixed_stream_bridge_queue.get() - - msg = msg_pack[2] + + # 取出訊息包：(socket_id, timestamp, mavlink_msg) + msg_pack = stream_bridge_ring.get() + socket_id, timestamp, msg = msg_pack[0], msg_pack[1], msg_pack[2] + + # 解析訊息基本資訊 sysid = msg.get_srcSystem() compid = msg.get_srcComponent() msg_id = msg.get_msgId() - - # 若這個 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_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 - this_component.sysid = sysid # ★ 新增 - this_component.compid = compid + + # 確保 VehicleView 存在 + vehicle = vehicle_registry.get(sysid) + if vehicle is None: + vehicle = vehicle_registry.register(sysid) + # 存儲 socket_id 到自定義 meta 中 + vehicle.custom_meta['socket_id'] = socket_id + + # 確保 VehicleComponent 存在 + component = vehicle.get_component(compid) + if component is None: + if msg_id == 0: # 只有透過 HEARTBEAT 才能創建新的 component + # 根據 mav_type 判斷 component 類型 + comp_type = self._determine_component_type(msg.type) + component = vehicle.add_component(compid, comp_type) + component.mav_type = msg.type + component.mav_autopilot = msg.autopilot + else: + # component 不存在且非 HEARTBEAT，忽略此訊息 + continue + + # 使用處理器字典分發訊息處理 + if msg_id in self.message_handlers: + try: + self.message_handlers[msg_id](vehicle, component, msg, timestamp) + except Exception as e: + logger.error(f'Error handling message type {msg_id}: {e}') else: - continue - - # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓ - - if msg_id == 0: # HEARTBEAT 處理 - this_component.emitParams['heartbeat'] = msg - self.ensure_all_publishers(sysid, this_component) - - elif msg_id == 30: # ATTITUDE 處理 - this_component.emitParams['attitude'] = msg - - elif msg_id == 32: # LOCAL_POSITION_NED 處理 - this_component.emitParams['local_position'] = msg - - elif msg_id == 33: # GLOBAL_POSITION_INT 處理 - this_component.emitParams['global_position'] = msg - - elif msg_id == 74: # VFR_HUD 處理 - this_component.emitParams['vfr_hud'] = msg - - elif msg_id == 111: #TIME_SYNC - round_trip_time = std_msgs.msg.Float64() - round_trip_time.data = (int(time.time() * 1e9) - msg.ts1) / 1e6 - if(round_trip_time.data < 1e6): - this_component.emitParams['ping'] = round_trip_time - - elif msg_id == 147: # BATTERY_STATUS 處理 - this_component.emitParams['battery'] = msg - - # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↑↑↑↑↑↑↑↑↑↑↑↑ + logger.debug(f'Unhandled message type: {msg_id} / {msg.get_type()}') + + logger.info('mavlink_bridge END!') + + def _determine_component_type(self, mav_type: int) -> ComponentType: + """根據 MAV_TYPE 判斷組件類型""" + # MAV_TYPE 定義： + # 0=通用, 1=固定翼, 2=四旋翼, 3=直升機, 4=天線追蹤器, 5=GCS, 6=飛船, + # 26=雲台, 27=ADSB, 28=降落傘等 + if mav_type == 6: # MAV_TYPE_GCS + return ComponentType.GCS + elif mav_type == 26: # MAV_TYPE_GIMBAL + return ComponentType.GIMBAL + elif mav_type == 30: # MAV_TYPE_CAMERA + return ComponentType.CAMERA + elif mav_type in [1, 2, 3, 4, 13, 14, 15, 19, 20, 21, 22]: # 各種飛行器類型 + return ComponentType.AUTOPILOT + else: + return ComponentType.OTHER + + # === 訊息處理器 === + def _handle_heartbeat(self, vehicle, component, msg, timestamp): + """處理 HEARTBEAT 訊息 (msg_id: 0)""" + component.status.mode.base_mode = msg.base_mode + component.status.mode.custom_mode = msg.custom_mode + component.status.mode.mode_name = mavutil.mode_string_v10(msg) + component.status.mode.timestamp = timestamp + component.status.system_status = msg.system_status + component.status.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 + # print("get mode:", mavutil.mode_string_v10(msg)) # debug + + def _handle_attitude(self, vehicle, component, msg, timestamp): + """處理 ATTITUDE 訊息 (msg_id: 30)""" + component.status.attitude.roll = msg.roll + component.status.attitude.pitch = msg.pitch + component.status.attitude.yaw = msg.yaw + component.status.attitude.rollspeed = msg.rollspeed + component.status.attitude.pitchspeed = msg.pitchspeed + component.status.attitude.yawspeed = msg.yawspeed + component.status.attitude.timestamp = timestamp + + def _handle_local_position(self, vehicle, component, msg, timestamp): + """處理 LOCAL_POSITION_NED 訊息 (msg_id: 32)""" + # LOCAL_POSITION_NED 提供相對位置資訊 + component.status.position.relative_altitude = -msg.z # NED 座標系，z 為負表示高度 + component.status.position.timestamp = timestamp + # 也可以存儲到 custom_status 中保留原始資料 + component.status.custom_status['local_position'] = { + 'x': msg.x, 'y': msg.y, 'z': msg.z, + 'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz + } + + def _handle_global_position(self, vehicle, component, msg, timestamp): + """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)""" + component.status.position.latitude = msg.lat / 1e7 # 轉換為度 + component.status.position.longitude = msg.lon / 1e7 # 轉換為度 + component.status.position.altitude = msg.alt / 1000.0 # 轉換為公尺 + component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺 + component.status.position.timestamp = timestamp + + def _handle_vfr_hud(self, vehicle, component, msg, timestamp): + """處理 VFR_HUD 訊息 (msg_id: 74)""" + component.status.vfr.airspeed = msg.airspeed + component.status.vfr.groundspeed = msg.groundspeed + component.status.vfr.heading = msg.heading + component.status.vfr.throttle = msg.throttle + component.status.vfr.climb = msg.climb + component.status.vfr.timestamp = timestamp + + def _handle_battery_status(self, vehicle, component, msg, timestamp): + """處理 BATTERY_STATUS 訊息 (msg_id: 147)""" + # 計算電池總電壓（mV 轉 V） + if hasattr(msg, 'voltages') and msg.voltages[0] != 65535: + # voltages 是一個陣列，包含各個電池單元的電壓 + total_voltage = sum(v for v in msg.voltages if v != 65535) / 1000.0 + component.status.battery.voltage = total_voltage + + component.status.battery.current = msg.current_battery / 100.0 if msg.current_battery != -1 else None # cA 轉 A + component.status.battery.remaining = msg.battery_remaining if msg.battery_remaining != -1 else None # 百分比 + component.status.battery.temperature = msg.temperature / 100.0 if hasattr(msg, 'temperature') and msg.temperature != 32767 else None # 轉換為攝氏 + component.status.battery.timestamp = timestamp + + # === 訊息發送功能 === + def send_message(self, message_bytes, target_sysid=None, target_socket_id=None, broadcast=False): + """ + 發送訊息到指定的 mavlink_object + + Args: + message_bytes: 準備好的 mavlink 封包 (bytes) + target_sysid: 目標系統 ID (可選，用於自動查找對應的 socket) + target_socket_id: 目標 socket ID (可選，直接指定) + + Returns: + bool: 是否成功發送 + + 使用方式： + 1. broadcast: 廣播到所有活動的 mavlink_object + 2. 指定 target_socket_id：直接發送到該 socket + 3. 指定 target_sysid：自動查找該系統對應的 socket 並發送 + """ + if not isinstance(message_bytes, (bytes, bytearray)): + logger.error(f"Invalid message type: {type(message_bytes)}") + return False - # 若未定義的訊息類型則不處理 並跳出訊息 + # 情況 1: 廣播到所有活動的 mavlink_object + if broadcast: + success_count = 0 + for socket_id, mav_obj in mavlink_object.mavlinkObjects.items(): + if self._send_to_socket(message_bytes, socket_id): + success_count += 1 + + return success_count > 0 + + # 情況 2: 直接指定 socket_id + if target_socket_id is not None: + return self._send_to_socket(message_bytes, target_socket_id) + + # 情況 3: 透過 sysid 查找對應的 socket + if target_sysid is not None: + vehicle = vehicle_registry.get(target_sysid) + if vehicle and 'socket_id' in vehicle.custom_meta: + socket_id = vehicle.custom_meta['socket_id'] + return self._send_to_socket(message_bytes, socket_id) else: - logger.warning('This Message Type Did not define process method : {} / {}'.format(msg.get_msgId(), msg.get_type())) - continue - - logger.info('End of mavlink_bridge._run_thread') - - # === Node 區塊 === - def _init_node(self): - logger.info('Start of mavlink_bridge._init_node') - super().__init__('mavlink_bridge') # 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(): + logger.warning(f"System ID {target_sysid} not found or no socket_id") + return False + + logger.warning("No target specified for sending message. WTF ARE YOU DOING?") + return False # 若無指定任何目標，則返回失敗 + + def _send_to_socket(self, message_bytes, socket_id): + """ + 將訊息發送到指定的 mavlink_object + + Args: + message_bytes: mavlink 封包 + socket_id: 目標 socket ID + + Returns: + bool: 是否成功 + """ + if socket_id not in mavlink_object.mavlinkObjects: + logger.warning(f"mavlink_object {socket_id} not found") + return False + + mav_obj = mavlink_object.mavlinkObjects[socket_id] + return mav_obj.message_put_queue(message_bytes) + +# 定義 mavlink_object 的狀態 +class MavlinkObjectState(Enum): + INIT = auto() # 初始化狀態 + RUNNING = auto() # 運行中狀態 + SHUTTINGDOWN = auto() # 關閉中狀態 + ERROR = auto() # 錯誤狀態 + CLOSED = auto() # 已關閉狀態 + +class mavlink_object: ''' 每個 mavlink bus 都會有一個 mavlink_object - 其中主要是 thread 做統計封包與分流 - 分流表的控制在三個 list 分別為 - multiplexingToAnalysis : 這個 list 是用來分流到固定串流橋接器的 - multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的 - multiplexingToSwap : 這個 list 是用來分流到轉拋串流的 - 透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為 + 使用 asyncio 處理資料流 用 RingBuffer 來分配訊息 + 直接透過 socket 寫出 ''' - mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } - socket_num = 0 # 用來記錄目前的 socket 數量 + 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(): @@ -214,193 +360,449 @@ class mavlink_object(): return instance def __init__(self, socket): + # 登入所需的 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 - self.mavlink_systems = mavlink_systems - - # 這三個 list 用來分配不同的訊息到不同的 queue - self.multiplexingToAnalysis = [ - 0, # HEARTBEAT # 挺必要的項目 - # 24, # GPS_RAW_INT - # 30, # ATTITUDE - # 33, # GLOBAL_POSITION_INT - # 74, # VFR_HUD - # 147, # BATTERY_STATUS - ] - 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): - # 停下自己的 thread - if self.mavlink_socket != None: - self.mavlink_socket.close() - self.stop() + # 用於主線程發送消息的緩衝區 + self.outgoing_msgs = deque() + + # 記錄訊息過濾類型 (可選) + self.bridge_msg_types = set([0, 30, 33, 74, 147]) # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS + self.return_msg_types = set() + + # 轉發到別的 mavlink object 作為目標端口 的列表 + self.target_sockets = set() + + # 物件變數 + self.task = None # Task reference + self.dirtyDataCount = 0 # 髒資料計數器 + self.dirtyDataMax = 10 # 髒資料容許閾值 + + self.state = MavlinkObjectState.INIT + # logger.info(f'mavlink_object instance {self.socket_id} created') # 先註解掉避免太多 log 但是為了 debug 保留 + + # 附加參數 (並非 mavlink_object 運行本體必要 但是要給上層結構運用的) + # 若這個 socket 是 另一個"主要 socket"的備用連接 則設定為"主要 socket id" + self.primary_socket_id = None # None 表示不是備用連接 + # socket type + self.socket_type = 'UNDEFINED' # 可選 'UDP_INBOUND', 'SERIAL_XBEE'...etc - # 移除其他 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 + def __del__(self): + # # 先移除其他 socket 指向這個 socket 的目標 # 還是不要在這邊做了 畢竟本來就有判斷 object 不活躍就不轉拋 + # for mavlink_obj in self.mavlinkObjects.values(): + # if self.socket_id in mavlink_obj.target_sockets: + # mavlink_obj.remove_target_socket(self.socket_id) + # 關閉 socket + if hasattr(self, 'mavlink_socket') and self.mavlink_socket: + try: + self.mavlink_socket.close() + except: + pass + # 處理 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)) + if hasattr(self.__class__, 'socket_num'): + self.__class__.socket_num -= 1 + + if hasattr(self.__class__, 'mavlinkObjects') and hasattr(self, 'socket_id'): + if self.socket_id in self.__class__.mavlinkObjects: + self.__class__.mavlinkObjects.pop(self.socket_id) + + # 這段不知道怎麼了 反正會一直讓 logger ERROR 我先關掉 # try: - # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + # logger.info(f'mavlink_object instance {self.socket_id} deleted') # 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): - # TODO 檢查 socket 是否有連線 - self.thread = threading.Thread(target=self._run_thread) - self.running = self.updateMultiplexingList() - self.thread.start() - - def stop(self): - 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)) - while self.running: - timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 + async def process_data(self): + """處理 mavlink 數據的主要 asyncio 協程""" + # logger.info(f'Start of mavlink_object id: {self.socket_id}') # 先註解掉避免太多 log 但是為了 debug 保留 + + while self.state == MavlinkObjectState.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 + mavlinkMsg = self.mavlink_socket.recv_msg() - if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega. etc...' + if 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 類型 - device = self.mavlink_systems[sysid] - mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) + + # 更新封包統計資訊 + vehicle = vehicle_registry.get(sysid) + if vehicle: # 只有當這個 system id 已經被註冊過才會記錄統計 + component = vehicle.get_component(compid) + if component: + component.update_packet_stats( + mavlinkMsg.get_seq(), + mavlinkMsg.get_msgId(), + timestamp + ) + + # 分發訊息到 RingBuffer + msg_id = mavlinkMsg.get_msgId() + + if (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types): + stream_bridge_ring.put((self.socket_id, timestamp, mavlinkMsg)) - comp = device.components.get(compid) - if comp and comp.loss_rate_publisher: - if timestamp - comp.last_loss_publish_time >= 1.0: - loss_rate = comp.loss_rate # loss_rate 是在 updateComponentPacketCount 中計算並儲存的欄位 - msg = std_msgs.msg.Float64() - msg.data = float(loss_rate) - comp.loss_rate_publisher.publish(msg) - comp.last_loss_publish_time = timestamp - - # 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 (self._multiplexingList[i] == []): - continue - elif (mavlinkMsg.get_msgId() in self._multiplexingList[i]) or (self._multiplexingList[i][0] == -1): - if i == 0: - fixed_stream_bridge_queue.put((self.socket_id,timestamp,mavlinkMsg)) - elif i == 1: - return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) - else: - _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)) - - def updateMultiplexingList(self): - ''' - 更新 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)) + if (msg_id in self.return_msg_types or -1 in self.return_msg_types): + return_packet_ring.put((self.socket_id, timestamp, mavlinkMsg)) + + # 將全部接收到的訊息轉發給目標列表中的 mavlink_object + for target_socket in self.target_sockets: + if target_socket in self.mavlinkObjects: + target_obj = self.mavlinkObjects[target_socket] + if target_obj.state == MavlinkObjectState.RUNNING: + target_obj.mavlink_socket.write(mavlinkMsg.get_msgbuf()) + + if self.outgoing_msgs: + send_msg = self.outgoing_msgs.popleft() + self.mavlink_socket.write(send_msg) + + + # 這邊的重點不是延遲 而是透過 await 讓出 event loop 的控制權 + await asyncio.sleep(0.001) + + logger.info(f'End of mavlink_object id: {self.socket_id}') + self.state = MavlinkObjectState.CLOSED + + def add_target_socket(self, target_socket_id): + """添加一個目標端口用於轉發""" + if (target_socket_id != self.socket_id) and (target_socket_id != self.primary_socket_id): + if target_socket_id not in self.target_sockets: + self.target_sockets.add(target_socket_id) + logger.info(f"mavlink_object Added target port {target_socket_id} to mavlink_object {self.socket_id}") + return True + return False # 已存在 + return False # 不能添加自己 也不能添加主要 socket + + def remove_target_socket(self, target_socket_id): + """移除目標端口""" + if target_socket_id in self.target_sockets: + self.target_sockets.remove(target_socket_id) + logger.info(f"mavlink_object Removed target port {target_socket_id} from mavlink_object {self.socket_id}") + return True + return False # 不存在 + + def set_bridge_message_types(self, msg_types): + """設置需要分流到橋接器的訊息類型""" + if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types): + self.bridge_msg_types = set(msg_types) + return True + logger.error(f"Invalid bridge message types: {msg_types}") + return False + + def set_return_message_types(self, msg_types): + """設置需要分流到回傳處理器的訊息類型""" + if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types): + self.return_msg_types = set(msg_types) + return True + logger.error(f"Invalid return message types: {msg_types}") + return False + + def message_put_queue(self, message_bytes): + """ + 從主線程向此 mavlink_object 的 socket 發送數據 + 將數據添加到簡單的列表中，由 asyncio 任務處理 + + Args: + message_bytes: 要發送的 mavlink 消息的字節數據 + + Returns: + bool: 是否成功添加消息到列表 + """ + # 狀態檢查 + if self.state != MavlinkObjectState.RUNNING: + logger.warning(f"Cannot send message: mavlink_object {self.socket_id} is not running") + return False + + # 基本數據類型檢查（輕量級） + if not isinstance(message_bytes, (bytes, bytearray)): + logger.error(f"Invalid message type: expected bytes/bytearray, got {type(message_bytes)}") + return False + + # 基本長度檢查（MAVLink v1.0 最小8字節，v2.0最小12字節） + if len(message_bytes) < 8: + logger.error(f"Message too short: {len(message_bytes)} bytes (minimum 8)") + return False + + # MAVLink 起始標記檢查（輕量級） + if len(message_bytes) > 0 and message_bytes[0] not in (0xFE, 0xFD): # v1.0: 0xFE, v2.0: 0xFD + logger.error(f"Invalid MAVLink start marker: 0x{message_bytes[0]:02X}") return False + + # 緩衝區大小保護（防止記憶體耗盡） + if len(self.outgoing_msgs) > 1000: # 可調整的閾值 + logger.warning(f"Outgoing message buffer full for mavlink_object {self.socket_id}") + return False + + self.outgoing_msgs.append(message_bytes) + return True + +class async_io_manager: + """ + 管理所有 mavlink_object 實例的 asyncio 任務 + 提供單一線程來處理所有 mavlink 通道的數據 + + 首先 async_io_manager 是 singleton 的 所以只能有一個實例 + + 這個 async_io_manager 是藉由 start 方法來啟動的 + + start 方法 會先做一個新的執行緒 然後讓新的執行緒 透過 _run_event_loop 方法來建立一個空的事件循環 self.loop + 然後在 _run_event_loop 方法中 會建立一個異步任務 _main_task 來監控和管理所有的 mavlink_object 任務 + """ + + _instance = None + _lock = threading.Lock() + + def __new__(cls, *args, **kwargs): + with cls._lock: + if cls._instance is None: + cls._instance = super(async_io_manager, cls).__new__(cls) + return cls._instance + + def __init__(self): + if not hasattr(self, 'initialized'): + self.initialized = True + self.loop = None + self.running = False + # self.main_task = None + self.thread = None - # 檢查 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)) + def __del__(self): + self.loop = None + self.thread = None + + def start(self): + """ + 啟動 async_io_manager + + """ + if self.running: + logger.warning("async_io_manager already running") + return + + self.running = True + + # 啟動獨立線程 命名為 AsyncIOManager + self.thread = threading.Thread( + target=self._run_event_loop, + name="AsyncIOManager" + ) + self.thread.daemon = False # 不設為 daemon，確保正確關閉 + self.thread.start() + + # 等待 _run_event_loop 建立事件循環的物件 self.loop + start_timeout = 2.0 + start_time = time.time() + while not self.loop and time.time() - start_time < start_timeout: + time.sleep(0.1) + + # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop + if self.loop: + logger.info("async_io_manager thread started <-") + return True + else: + logger.error("async_io_manager failed to start") return False + + def shutdown(self): + """停止 async_io_manager 和其管理的所有 mavlink_object""" + + # 自己在 running 狀態下才執行停止程序 + if not self.running: + return + + # 停止所有被管理的 mavlink_object 所屬的 task + for socket_id in list(mavlink_object.mavlinkObjects.keys()): + self.remove_mavlink_object(socket_id) + + # 停止自己的 task + self.running = False - # 對應到自己的 multiplexingToSwap 必需為空 避免對自己迴圈轉拋 + # 解開事件循環的阻塞 + self.loop.call_soon_threadsafe(self.loop.stop) + + # print("mark A", len(asyncio.all_tasks(self.loop))) # debug + + # 等待線程結束 + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=5.0) + if self.thread.is_alive(): + logger.warning("async_io_manager thread did not stop gracefully") + os.kill(os.getpid(), signal.SIGTERM) # 強制終止程序 + + logger.info("async_io_manager thread END!") + + def _run_event_loop(self): + """在獨立線程中運行事件循環""" 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 + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + + logger.info("async_io_manager event loop started <-") + + # 創建主任務 + # main_task = self.loop.create_task(self._main_task()) + + # 運行事件循環 + self.loop.run_forever() + + except Exception as e: + logger.error(f"Error in async_io_manager event loop: {e}") + finally: + # 清理 + if self.loop: + self.loop.close() + self.loop = None + self.running = False + logger.info("async_io_manager event loop END!") + + def add_mavlink_object(self, mavlink_obj: mavlink_object): + """添加 mavlink_object""" + # 一個防呆 確保有 event loop 與 _main_task 正在運作 + if not self.running or not self.loop: + logger.error("Cannot add mavlink_object: async_io_manager is not running") + return False + + socket_id = mavlink_obj.socket_id + + # 檢查該對象是否已經在運行中 + if socket_id in mavlink_object.mavlinkObjects: + existing_obj = mavlink_object.mavlinkObjects[socket_id] + if existing_obj.state == MavlinkObjectState.RUNNING: + logger.warning(f"mavlink_object {socket_id} already managed") + return False + + # 使用 run_coroutine_threadsafe 執行協程並獲取結果 + future = asyncio.run_coroutine_threadsafe( + self._async_add_mavlink_object(mavlink_obj), + self.loop + ) - # 組合 multiplexing list - multiL_tmp = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToSwap + try: + # 等待結果，設定合理的超時時間 + result = future.result(timeout=3.0) + return result + except asyncio.TimeoutError: + logger.error(f"Timeout adding mavlink_object {socket_id}") + return False + except Exception as e: + logger.error(f"Failed to add mavlink_object {socket_id}: {e}") + return False + + async def _async_add_mavlink_object(self, mavlink_obj): + """在事件循環線程中同步執行""" + socket_id = mavlink_obj.socket_id - # 檢查 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. socket id : {}'.format(self.socket_id)) - return False # 若有錯誤則回傳 False + try: + task = asyncio.create_task(mavlink_obj.process_data()) + mavlink_obj.task = task + mavlink_obj.state = MavlinkObjectState.RUNNING + mavlink_obj.outgoing_msgs.clear() + logger.info(f"Added mavlink_object {socket_id} into manager.") + return True + except Exception as e: + logger.error(f"Failed to create task for mavlink_object {socket_id}: {e}") + return False + + def remove_mavlink_object(self, socket_id: int): + """移除 mavlink_object""" - # 更新 multiplexing list - self._multiplexingList = multiL_tmp + # 一個防呆 確保有 event loop 正在運作 + if not self.loop: + return False + + # 同樣使用 run_coroutine_threadsafe + future = asyncio.run_coroutine_threadsafe( + self._async_remove_mavlink_object(socket_id), + self.loop + ) + + try: + result = future.result(timeout=3.0) + return result + except asyncio.TimeoutError: + logger.error(f"Timeout removing mavlink_object {socket_id}") + return False + except Exception as e: + logger.error(f"Failed to remove mavlink_object {socket_id}: {e}") + return False + + async def _async_remove_mavlink_object(self, socket_id): + """在事件循環線程中同步執行""" + if socket_id not in mavlink_object.mavlinkObjects: + logger.warning(f"mavlink_object {socket_id} not found") + return False + + mavlink_obj = mavlink_object.mavlinkObjects[socket_id] + mavlink_obj.state = MavlinkObjectState.SHUTTINGDOWN + + if not mavlink_obj.task.done(): + mavlink_obj.task.cancel() + + # 等待一秒或者 task完全結束 + timeout = 1.0 + start_time = asyncio.get_event_loop().time() + while not mavlink_obj.task.done(): + if asyncio.get_event_loop().time() - start_time > timeout: + break + await asyncio.sleep(0.1) - return True + # 如果正常結束 則設置為關閉狀態（物件的清理由 __del__ 處理） + if mavlink_obj.task.done(): + mavlink_obj.state = MavlinkObjectState.CLOSED + logger.info(f"Removed mavlink_object {socket_id} from manager.") + return True + else: + mavlink_obj.state = MavlinkObjectState.ERROR + logger.warning(f"mavlink_object {socket_id} task did not terminate in time") + return False + + def get_managed_objects(self): + """獲取所有被管理的對象列表（狀態為 RUNNING 的對象）""" + return [socket_id for socket_id, obj in mavlink_object.mavlinkObjects.items() + if obj.state == MavlinkObjectState.RUNNING] # ====================== 分割線 ===================== -# 整合到 ros2 之後的程式進入點 -def main_node(): +if __name__ == '__main__': pass -if __name__ == '__main__': - pass +''' +================= 改版記錄 ============================ + +2025年 6月 20日 +1. mavlink_object 中 由於 Queue 的效能太差 會完全移除 + 其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現 + 而 multiplexingToSwap 會完全被移除代替方式下一條描述 +2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流 + (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object) + 因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移 + 並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出 +3. mavlink_object 需要加上 state 去管理其狀態 +4. mavlink_object 需要加上 target port 去管理寫出的目標 +5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道 +6. 基於第1,2項 updateMultiplexingList 會被完全移除 +7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object +8. 基於第1項全域變數 fixed_stream_bridge_queue return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除 + + +2025年 11月 15日 +1. mavlink_bridge 類別新增為 singleton 模式 確保全系統只有一個實例在運行 +2. mavlink_bridge 處理封包改為映射表 (需要在 _init_message_handlers 中新增處理器函式) +3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包 +4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚 +5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期 +6. mavlink_object 優化 message_put_queue 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 +7. 移除迴圈內的 try except 堆疊 增加效能 +8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具 + +2026年 01月 15日 +1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects +2. async_io_manager 的 _stop_event 無效變數移除 + +''' + diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index a0ce087..5dc3ca1 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -9,66 +9,48 @@ publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> ''' -from pymavlink import mavutil +import os +# ROS2 的 import import std_msgs.msg import sensor_msgs.msg import geometry_msgs.msg import mavros_msgs.msg -from theLogger import setup_logger import math -logger = setup_logger("mavlinkPublish.py") +# 自定義的 import +from .utils import setup_logger + +logger = setup_logger(os.path.basename(__file__)) class mavlink_publisher(): prefix_path = 'MavLinkBus' - TOPIC_CREATORS = ( - "create_state", - "create_attitude", - "create_local_position_pose", - "create_local_position_velocity", - "create_global_global", - "create_global_rel", - "create_vfr_hud", - "create_battery", - "create_ping" - ) - - def ensure_all_publishers(self, sysid: int, component): - """若還沒替這個 component 建立過 publisher，則一次補齊。""" - if getattr(component, "_pub_ready", False): - return # 已做過就跳過 - for fn in self.TOPIC_CREATORS: - getattr(self, fn)(sysid, component) - # 發布丟包率 - if not hasattr(component, "loss_rate_publisher") or component.loss_rate_publisher is None: - target_topic = 'packet_loss_rate' - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - component.loss_rate_publisher = self.create_publisher(std_msgs.msg.Float64, topic_name, 10) - component.loss_rate_topic_name = topic_name - - component._pub_ready = True # 打個旗標以免重複 - logger.info("✔︎ sysid=%d compid=%d → 所有 topic 已註冊", sysid, component.compid) - - def create_state(self, sysid, component_obj): + def create_flightMode(self, sysid, component_obj): # target topic name # 請跟這個 method 的名稱保持一致 - target_topic = 'state' + target_topic = 'flightMode' + + # 這邊要檢查 flight_mode 是否存在 + try: + _ = component_obj.emitParams['flightMode_mode'] + except KeyError: + # 這個 component id 還不存在 + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + + # 若存在則 建立 publisher object 並回傳 true topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(mavros_msgs.msg.State, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_state] + publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode] return True - def packEmit_state(cls, emitParams, publisher): - if 'heartbeat' not in emitParams: # ← 沒資料就直接返回 - return - mav_msg = emitParams['heartbeat'] - msg = mavros_msgs.msg.State() - msg.mode = mavutil.mode_string_v10(mav_msg) - msg.armed = (mav_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 + def packEmit_flightMode(cls, emitParams, publisher): + msg_str = emitParams['flightMode_mode'] + msg = std_msgs.msg.String() + msg.data = msg_str publisher.publish(msg) pass @@ -82,13 +64,18 @@ class mavlink_publisher(): def create_attitude(self, sysid, component_obj): target_topic = 'attitude' + + try: + _ = component_obj.emitParams['attitude'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] def packEmit_attitude(self, emitParams, publisher): - if 'attitude' not in emitParams: # ← 沒資料就直接返回 - return mav_msg = emitParams['attitude'] msg = sensor_msgs.msg.Imu() x, y, z, w = self.euler_to_quaternion(mav_msg.roll, mav_msg.pitch, mav_msg.yaw) @@ -104,13 +91,16 @@ class mavlink_publisher(): def create_local_position_pose(self, sysid, component_obj): target_topic = 'local_position/pose' + try: + _ = component_obj.emitParams['local_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose] def packEmit_local_pose(cls, emitParams, publisher): - if 'local_position' not in emitParams: - return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Point() msg.x = mav_msg.x @@ -121,13 +111,16 @@ class mavlink_publisher(): def create_local_position_velocity(self, sysid, component_obj): target_topic = 'local_position/velocity' + try: + _ = component_obj.emitParams['local_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel] def packEmit_local_vel(cls, emitParams, publisher): - if 'local_position' not in emitParams: - return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Vector3() msg.x = mav_msg.vx @@ -138,13 +131,16 @@ class mavlink_publisher(): def create_global_global(self, sysid, component_obj): target_topic = 'global_position/global' + try: + _ = component_obj.emitParams['global_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global] def packEmit_global_global(cls, emitParams, publisher): - if 'global_position' not in emitParams: - return mav_msg = emitParams['global_position'] msg = sensor_msgs.msg.NavSatFix() msg.latitude = mav_msg.lat/1e7 @@ -155,13 +151,16 @@ class mavlink_publisher(): def create_global_rel(self, sysid, component_obj): target_topic = 'global_position/rel_alt' + try: + _ = component_obj.emitParams['global_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel] def packEmit_global_rel(cls, emitParams, publisher): - if 'global_position' not in emitParams: - return mav_msg = emitParams['global_position'] msg = std_msgs.msg.Float64() msg.data = float(mav_msg.relative_alt/1e3) @@ -170,13 +169,16 @@ class mavlink_publisher(): def create_vfr_hud(self, sysid, component_obj): target_topic = 'vfr_hud' + try: + _ = component_obj.emitParams['vfr_hud'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud] def packEmit_vfr_hud(cls, emitParams, publisher): - if 'vfr_hud' not in emitParams: - return mav_msg = emitParams['vfr_hud'] msg = mavros_msgs.msg.VfrHud() msg.airspeed = mav_msg.airspeed @@ -190,30 +192,21 @@ class mavlink_publisher(): def create_battery(self, sysid, component_obj): target_topic = 'battery' + try: + _ = component_obj.emitParams['battery'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery] def packEmit_battery(cls, emitParams, publisher): - if 'battery' not in emitParams: - return mav_msg = emitParams['battery'] msg = sensor_msgs.msg.BatteryState() msg.voltage = mav_msg.voltages[0]/1e3 publisher.publish(msg) pass - - def create_ping(self, sysid, component_obj): - target_topic = 'ping' - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_ping] - def packEmit_ping(cls, emitParams, publisher): - if 'ping' not in emitParams: - return - mav_msg = emitParams['ping'] - publisher.publish(mav_msg) - pass # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py new file mode 100644 index 0000000..a95fb26 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -0,0 +1,902 @@ +""" +MAVLink ROS2 Nodes +包含兩個獨立的 ROS2 Node： +1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics +2. MavlinkCommandService - 提供 MAVLink 指令 service 介面 + +從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計 +""" + +import os +import time +import math +import threading +from typing import Dict, Optional + +# ROS2 imports +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor + +# ROS2 Message imports +import std_msgs.msg +import sensor_msgs.msg +import geometry_msgs.msg +import mavros_msgs.msg + +# 自定義 imports +from . import mavlinkVehicleView as mvv +from .utils import setup_logger + +logger = setup_logger(os.path.basename(__file__)) + + +# ============================================================================ +# 頻率控制器 +# ============================================================================ + +class PublishRateController: + """發布頻率控制器 - 按時間間隔控制發布頻率""" + + def __init__(self): + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 1/4】 + # 若要新增 topic 種類，請在此字典中加入新的 topic 名稱和發布間隔 + # 例如：'ekf_status': 1.0, # EKF 狀態 + # ═══════════════════════════════════════════════════════════════ + # 各 topic 的發布間隔（秒） + self.topic_intervals = { + 'position': 0.5, # GPS位置 + 'attitude': 0.5, # 姿態 + 'velocity': 0.5, # 速度 + 'battery': 1.0, # 電池 + 'vfr_hud': 0.5, # VFR HUD + 'mode': 1.0, # 飛行模式 + 'summary': 1.0, # 載具摘要 + # 在這裡新增更多 topics... + } + # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp} + self.last_publish_time: Dict[tuple, float] = {} + + def should_publish(self, sysid: int, topic: str) -> bool: + """ + 檢查是否應該發布此 topic + + Args: + sysid: 系統ID + topic: topic 名稱 + + Returns: + bool: True 表示應該發布 + """ + key = (sysid, topic) + now = time.time() + + # 當間隔設定為0或負數時 關閉該 topic 的發布 + interval = self.topic_intervals.get(topic, 0) + if interval <= 0: + return False + + # 首次發布 + if key not in self.last_publish_time: + self.last_publish_time[key] = now + return True + + # 檢查時間間隔 + if now - self.last_publish_time[key] >= interval: + self.last_publish_time[key] = now + return True + + return False + + def reset(self): + """重置所有計時器""" + self.last_publish_time.clear() + + +# ============================================================================ +# VehicleStatusPublisher Node +# ============================================================================ + +class VehicleStatusPublisher(Node): + """ + 載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics + + 職責: + - 定期從 vehicle_registry 讀取載具狀態 + - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz） + - 發布標準 ROS2 消息類型 + - 檢測訂閱者，按需發布 + """ + topicString_prefix = f'/fc_network/vehicle' + + def __init__(self): + super().__init__('vehicle_status_publisher') + + # 頻率控制器 + self.rate_controller = PublishRateController() + + # fc_publishers 字典 {(sysid, topic_name): publisher} + self.fc_publishers: Dict[tuple, any] = {} + + # 定時器：以較高頻率檢查 vehicle_registry 並發布 + # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率 + self.timer_period = 0.1 # 100ms + self.timer = self.create_timer(self.timer_period, self.timer_callback) + + # 狀態標誌 + self.running = True + + # logger.info("VehicleStatusPublisher initialized") + + def timer_callback(self): + """定時器回調 - 檢查所有載具並發布狀態""" + if not self.running: + return + + # 從 vehicle_registry 獲取所有載具 + all_vehicles = mvv.vehicle_registry.get_all() + + for sysid, vehicle in all_vehicles.items(): + self._publish_vehicle_status(vehicle) + + def _publish_vehicle_status(self, vehicle: mvv.VehicleView): + """ + 發布單個載具的所有狀態 + + Args: + vehicle: VehicleView 實例 + """ + sysid = vehicle.sysid + + # 假設只有一個 autopilot component (component_id=1) + component = vehicle.get_component(1) + if not component: + return + + status = component.status + + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 2/4】 + # 若要新增 topic，請在此處調用對應的發布方法 + # 例如：self._publish_ekf_status(sysid, status) + # ═══════════════════════════════════════════════════════════════ + # 發布各種狀態（通過頻率控制器判斷是否發布） + self._publish_position(sysid, status) + self._publish_attitude(sysid, status) + self._publish_velocity(sysid, status) + self._publish_battery(sysid, status) + self._publish_vfr_hud(sysid, status) + self._publish_mode(sysid, status) + self._publish_summary(vehicle) + # 在這裡新增更多 publish 方法調用... + + def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1): + """ + 獲取或創建 publisher + + Args: + sysid: 系統ID + topic: topic 名稱 + msg_type: ROS2 消息類型 + qos: QoS 設置 + + Returns: + publisher 對象 + """ + key = (sysid, topic) + if key not in self.fc_publishers: + topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}' + publisher = self.create_publisher(msg_type, topic_name, qos) + self.fc_publishers[key] = publisher + logger.info(f"Created publisher: {topic_name}") + return self.fc_publishers[key] + + def _publish_position(self, sysid: int, status: mvv.ComponentStatus): + """發布 GPS 位置""" + if not self.rate_controller.should_publish(sysid, 'position'): + return + + pos = status.position + if pos.latitude is None or pos.longitude is None: + return + + publisher = self._get_or_create_publisher(sysid, 'position', sensor_msgs.msg.NavSatFix) + + # 檢查是否有訂閱者 + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.NavSatFix() + msg.latitude = pos.latitude + msg.longitude = pos.longitude + msg.altitude = pos.altitude if pos.altitude is not None else 0.0 + + # GPS 狀態資訊 + gps = status.gps + if gps.fix_type is not None: + msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus + + publisher.publish(msg) + + def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus): + """發布姿態（IMU）""" + if not self.rate_controller.should_publish(sysid, 'attitude'): + return + + att = status.attitude + if att.roll is None: + return + + publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu) + + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.Imu() + + # 歐拉角轉四元數 + qx, qy, qz, qw = self._euler_to_quaternion( + att.roll, att.pitch, att.yaw + ) + msg.orientation.x = qx + msg.orientation.y = qy + msg.orientation.z = qz + msg.orientation.w = qw + + # 角速度 + if att.rollspeed is not None: + msg.angular_velocity.x = att.rollspeed + msg.angular_velocity.y = att.pitchspeed + msg.angular_velocity.z = att.yawspeed + + publisher.publish(msg) + + def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus): + """發布速度""" + if not self.rate_controller.should_publish(sysid, 'velocity'): + return + + vfr = status.vfr + if vfr.groundspeed is None: + return + + publisher = self._get_or_create_publisher(sysid, 'velocity', geometry_msgs.msg.TwistStamped) + + if publisher.get_subscription_count() == 0: + return + + msg = geometry_msgs.msg.TwistStamped() + msg.header.stamp = self.get_clock().now().to_msg() + + # 使用 VFR HUD 的地速和航向計算速度分量 + if vfr.heading is not None: + heading_rad = math.radians(vfr.heading) + msg.twist.linear.x = vfr.groundspeed * math.cos(heading_rad) + msg.twist.linear.y = vfr.groundspeed * math.sin(heading_rad) + + # 爬升率作為 z 軸速度 + if vfr.climb is not None: + msg.twist.linear.z = vfr.climb + + publisher.publish(msg) + + def _publish_battery(self, sysid: int, status: mvv.ComponentStatus): + """發布電池狀態""" + if not self.rate_controller.should_publish(sysid, 'battery'): + return + + bat = status.battery + if bat.voltage is None: + return + + publisher = self._get_or_create_publisher(sysid, 'battery', sensor_msgs.msg.BatteryState) + + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.BatteryState() + msg.voltage = bat.voltage + + if bat.current is not None: + msg.current = bat.current + + if bat.remaining is not None: + msg.percentage = bat.remaining / 100.0 + + if bat.temperature is not None: + msg.temperature = bat.temperature + + publisher.publish(msg) + + def _publish_vfr_hud(self, sysid: int, status: mvv.ComponentStatus): + """發布 VFR HUD""" + if not self.rate_controller.should_publish(sysid, 'vfr_hud'): + return + + vfr = status.vfr + if vfr.airspeed is None: + return + + publisher = self._get_or_create_publisher(sysid, 'vfr_hud', mavros_msgs.msg.VfrHud) + + if publisher.get_subscription_count() == 0: + return + + msg = mavros_msgs.msg.VfrHud() + msg.airspeed = vfr.airspeed if vfr.airspeed is not None else 0.0 + msg.groundspeed = vfr.groundspeed if vfr.groundspeed is not None else 0.0 + msg.heading = vfr.heading if vfr.heading is not None else 0 + msg.throttle = float(vfr.throttle) if vfr.throttle is not None else 0.0 + msg.climb = vfr.climb if vfr.climb is not None else 0.0 + + # altitude 需要從 position 獲取 + if status.position.altitude is not None: + msg.altitude = status.position.altitude + + publisher.publish(msg) + + def _publish_mode(self, sysid: int, status: mvv.ComponentStatus): + """發布飛行模式""" + if not self.rate_controller.should_publish(sysid, 'mode'): + return + + mode = status.mode + if mode.mode_name is None: + return + + publisher = self._get_or_create_publisher(sysid, 'mode', std_msgs.msg.String) + + if publisher.get_subscription_count() == 0: + return + + msg = std_msgs.msg.String() + msg.data = mode.mode_name + publisher.publish(msg) + + def _publish_summary(self, vehicle: mvv.VehicleView): + """ + 發布載具摘要資訊（自定義格式，使用 String 暫時代替） + TODO: 未來可以定義專門的 VehicleSummary.msg + """ + sysid = vehicle.sysid + + if not self.rate_controller.should_publish(sysid, 'summary'): + return + + publisher = self._get_or_create_publisher(sysid, 'summary', std_msgs.msg.String) + + if publisher.get_subscription_count() == 0: + return + + # 獲取 autopilot component + component = vehicle.get_component(1) + if not component: + return + + status = component.status + + # 構建摘要資訊（JSON 格式字串） + import json + summary = { + 'sysid': sysid, + 'vehicle_type': vehicle.vehicle_type if vehicle.vehicle_type else 0, + 'autopilot': component.mav_autopilot if component.mav_autopilot else 0, + 'socket_id': vehicle.custom_meta.get('socket_id', -1), # 重要！ + 'armed': status.armed if status.armed is not None else False, + # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0, + 'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN", + # 'latitude': status.position.latitude if status.position.latitude else 0.0, + # 'longitude': status.position.longitude if status.position.longitude else 0.0, + # 'altitude': status.position.altitude if status.position.altitude else 0.0, + # 'battery_percent': status.battery.remaining if status.battery.remaining else 0, + 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0, + 'connection_type': vehicle.connected_via.value, + 'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0, + } + + msg = std_msgs.msg.String() + msg.data = json.dumps(summary) + publisher.publish(msg) + + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 3/4】 + # 若要新增 topic，請在此處實作對應的發布方法 + # 方法命名規則：def _publish_(self, sysid: int, status: mvv.ComponentStatus): + # 例如： + # def _publish_ekf_status(self, sysid: int, status: mvv.ComponentStatus): + # """發布 EKF 狀態""" + # if not self.rate_controller.should_publish(sysid, 'ekf_status'): + # return + # + # ekf = status.ekf + # if ekf.flags is None: + # return + # + # publisher = self._get_or_create_publisher(sysid, 'ekf_status', ... + # # ... 實作發布邏輯 + # ═══════════════════════════════════════════════════════════════ + + @staticmethod + def _euler_to_quaternion(roll, pitch, yaw): + """ + 歐拉角轉四元數 + + Args: + roll: 橫滾角 (弧度) + pitch: 俯仰角 (弧度) + yaw: 偏航角 (弧度) + + Returns: + tuple: (qx, qy, qz, qw) + """ + qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \ + math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) + qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \ + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) + qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \ + math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \ + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) + return (qx, qy, qz, qw) + + def stop(self): + """停止發布""" + self.running = False + # logger.info("VehicleStatusPublisher stopped") + + +# ============================================================================ +# MavlinkCommandService Node +# ============================================================================ + +class MavlinkCommandService(Node): + """ + MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令 + + 職責: + - 作為 service server，等待 client 請求 + - 接收請求，組裝 MAVLink 封包 + - 調用 mavlinkObject 發送封包 + - 處理 ACK 等待和超時（未來實現） + + 設計理念：回歸 MAVLink 純粹結構 + - 只負責將 ROS2 請求轉換為 MAVLink 封包 + - 不預設功能（如 ARM/DISARM），保持模組化 + - 高層應用可透過此 service 實現各種功能 + """ + + def __init__(self): + super().__init__('mavlink_command_service') + + # ═══════════════════════════════════════════════════════════════════ + # ROS2 Service 架構說明： + # + # 1. Service 定義：由 .srv 檔案定義（Request + Response） + # - Request: client 發送的請求內容 + # - Response: server 回傳的結果 + # + # 2. Service Server 創建： + # self.create_service(srv_type, service_name, callback_function) + # - srv_type: service 的訊息類型（需要自定義或使用標準） + # - service_name: service 的名稱（client 用此名稱呼叫） + # - callback_function: 處理請求的回調函數 + # + # 3. Callback 函數： + # def callback(self, request, response): + # # request: 包含 client 發送的數據 + # # response: 需要填充並返回給 client + # return response + # + # 4. Service Client 使用方式（在其他程式中）： + # client = node.create_client(srv_type, service_name) + # request = srv_type.Request() + # future = client.call_async(request) # 異步調用 + # # 或 response = client.call(request) # 同步調用 + # ═══════════════════════════════════════════════════════════════════ + + # 由於 ROS2 自定義 service 需要 .srv 檔案編譯 + # 這裡先使用標準 String service 作為簡化實現 + # TODO: 未來可創建專門的 .srv 檔案 + from std_srvs.srv import Trigger + from example_interfaces.srv import SetBool + + # ═══════════════════════════════════════════════════════════════════ + # Service 1: 發送 MAVLink Message（通用介面） + # 使用 Trigger 作為臨時實現，未來應使用自定義 service + # ═══════════════════════════════════════════════════════════════════ + # TODO: 創建 SendMavlinkMessage.srv + # Request: + # uint8 target_sysid + # uint8 target_compid + # uint16 message_id + # string fields_json # JSON 格式的字段數據 + # bool wait_response + # uint16 response_msgid + # float32 timeout + # Response: + # bool success + # string response_json + # string error_message + + # 暫時使用簡化版本（僅示範架構） + self.srv_send_message = self.create_service( + Trigger, + '/mavlink/send_message', + self.handle_send_message + ) + + # ═══════════════════════════════════════════════════════════════════ + # Service 2: 發送 COMMAND_LONG + # ═══════════════════════════════════════════════════════════════════ + self.srv_command_long = self.create_service( + Trigger, + '/mavlink/send_command_long', + self.handle_command_long + ) + + # ═══════════════════════════════════════════════════════════════════ + # Service 3: 參數請求 + # ═══════════════════════════════════════════════════════════════════ + self.srv_param_request = self.create_service( + Trigger, + '/mavlink/param_request', + self.handle_param_request + ) + + # 狀態標記 + self.running = True + + # mavlinkObject 的引用（將由外部設置） + self.mavlink_analyzer = None + + logger.info("MavlinkCommandService initialized") + + def set_mavlink_analyzer(self, mavlink_analyzer): + """ + 設置 mavlink_analyzer 引用 + + Args: + mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例 + """ + self.mavlink_analyzer = mavlink_analyzer + logger.info("MavlinkCommandService: mavlink_analyzer set") + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 發送 MAVLink Message + # ═══════════════════════════════════════════════════════════════════════ + def handle_send_message(self, request, response): + """ + 處理發送 MAVLink 訊息的請求 + + ROS2 Service Callback 說明： + - 此函數會在 client 調用 service 時被執行 + - request: 包含 client 傳入的參數 + - response: 需要填充結果並返回給 client + - 必須 return response + + Args: + request: Trigger.Request (暫時使用，未來改為自定義) + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received send_message request") + + # 檢查 mavlink_analyzer 是否已設置 + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + logger.error(response.message) + return response + + # TODO: 實際實現 + # 1. 從 request 解析參數（target_sysid, message_id, fields 等） + # 2. 使用 pymavlink 組裝 MAVLink 封包 + # 3. 調用 mavlink_analyzer.send_message() 發送 + # 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應 + + # 暫時返回成功（示範用） + response.success = True + response.message = "Message sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 發送 COMMAND_LONG + # ═══════════════════════════════════════════════════════════════════════ + def handle_command_long(self, request, response): + """ + 處理發送 COMMAND_LONG 的請求 + + COMMAND_LONG (MAVLink message ID=76): + - 用於發送簡單命令給載具 + - 常用於 ARM/DISARM, 模式切換, TAKEOFF, LAND 等 + + Args: + request: Trigger.Request + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received command_long request") + + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + return response + + # TODO: 實際實現 + # 1. 從 request 解析 COMMAND_LONG 參數 + # - target_sysid, target_compid + # - command (MAV_CMD_xxx) + # - param1~param7 + # 2. 組裝 COMMAND_LONG 封包 + # 3. 發送並等待 COMMAND_ACK (message ID=77) + # 4. 解析 ACK 結果（ACCEPTED/FAILED 等） + + response.success = True + response.message = "Command sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 參數請求 + # ═══════════════════════════════════════════════════════════════════════ + def handle_param_request(self, request, response): + """ + 處理參數讀取請求 + + MAVLink 參數協議： + - PARAM_REQUEST_READ (ID=20): 請求讀取參數 + - PARAM_VALUE (ID=22): 參數值回應 + - PARAM_SET (ID=23): 設置參數值 + + ═══════════════════════════════════════════════════════════════════ + 【使用 mavlinkObject 回應機制的步驟】 + + 1. 設置回應訊息類型： + self.mavlink_analyzer.set_return_message_types([22]) # PARAM_VALUE + + 2. 發送請求封包： + message_bytes = ... # 組裝 PARAM_REQUEST_READ + self.mavlink_analyzer.send_message( + message_bytes, + target_sysid=1 + ) + + 3. 監聽回應（在獨立線程或定時器中）： + from ..fc_network_adapter import mavlinkObject as mo + + # 等待回應（帶超時） + timeout = 3.0 + start_time = time.time() + while time.time() - start_time < timeout: + items = mo.return_packet_ring.get_all() + for socket_id, timestamp, msg in items: + if msg.get_type() == 'PARAM_VALUE': + # 找到回應！ + param_id = msg.param_id + param_value = msg.param_value + # 處理回應... + return + time.sleep(0.01) # 短暫等待 + + # 超時處理 + + 4. 清理（可選）： + self.mavlink_analyzer.set_return_message_types([]) # 清空 + mo.return_packet_ring.clear() # 清空緩衝區 + + 注意事項： + - return_packet_ring 是全域的，所有 mavlink_object 共用 + - 需要通過 socket_id 或 sysid 來識別回應來源 + - 實際使用時建議實現專門的回應管理器 + ═══════════════════════════════════════════════════════════════════ + + Args: + request: Trigger.Request + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received param_request") + + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + return response + + # TODO: 實際實現 + # 1. 從 request 解析參數名稱或索引 + # 2. 設置 mavlink_analyzer.set_return_message_types([22]) # PARAM_VALUE + # 3. 發送 PARAM_REQUEST_READ + # 4. 監聽 return_packet_ring，等待 PARAM_VALUE + # 5. 解析回應並填充到 response + + response.success = True + response.message = "Param request sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # 【新增 Service 位置 4/4】 + # 若要新增 service，請在此處添加新的 handler 方法 + # 並在 __init__ 中創建對應的 service server + # ═══════════════════════════════════════════════════════════════════════ + + def stop(self): + """停止服務""" + self.running = False + logger.info("MavlinkCommandService stopped") + + +# ============================================================================ +# ROS2 節點管理器 +# ============================================================================ + +class fc_ros_manager: + """ + MAVLink ROS2 節點管理器 + + 管理兩個獨立的 ROS2 Node： + - VehicleStatusPublisher + - MavlinkCommandService + + 提供統一的啟動/停止介面給 mainOrchestrator + """ + + def __init__(self): + self.initialized = False + self.running = False + + # 两个 node 实例 + self.status_publisher: Optional[VehicleStatusPublisher] = None + self.command_service: Optional[MavlinkCommandService] = None + + # Executor & Thread + self.spin_thread: Optional[threading.Thread] = None + self.executor: Optional[MultiThreadedExecutor] = None + + def initialize(self): + """初始化 ROS2 环境和 nodes""" + if self.initialized: + logger.warning("fc_ros_manager already initialized") + return False + + try: + # 初始化 ROS2 + rclpy.init() + + # 創建節點 node + self.status_publisher = VehicleStatusPublisher() + self.command_service = MavlinkCommandService() + + # 創建執行者 MultiThreadedExecutor + self.executor = MultiThreadedExecutor() + self.executor.add_node(self.status_publisher) + self.executor.add_node(self.command_service) + + self.initialized = True + # logger.info("fc_ros_manager initialized") + return True + + except Exception as e: + logger.error(f"Failed to initialize fc_ros_manager: {e}") + return False + + def start(self): + """啟動 ROS2 nodes (在獨立執行緒中運行 executor) """ + if not self.initialized: + logger.error("fc_ros_manager initialize failed or not called") + return False + + if self.running: + logger.warning("fc_ros_manager already running") + return False + + try: + self.running = True + + self.spin_thread = threading.Thread( + target=self._spin_executor, + daemon=True, + name="ROS2ExecutorThread" + ) + self.spin_thread.start() + + logger.info("fc_ros_manager started <-") + return True + + except Exception as e: + logger.error(f"Failed to start fc_ros_manager: {e}") + self.running = False + return False + + def _spin_executor(self): + """在 thread 中運行的 executor""" + try: + # logger.info("ROS2 executor spinning...") + while self.running: + self.executor.spin_once(timeout_sec=0.1) + except Exception as e: + logger.error(f"fc_ros_manager error in spinning executor: {e}") + + def stop(self): + """停止 ROS2 nodes""" + if not self.running: + logger.warning("fc_ros_manager not running") + return False + + try: + # 標記停止 + self.running = False + + # 停止各個 node + if self.status_publisher: + self.status_publisher.stop() + if self.command_service: + self.command_service.stop() + + # 等待 spin 執行緒結束 + if self.spin_thread and self.spin_thread.is_alive(): + self.spin_thread.join(timeout=2.0) + + logger.info("fc_ros_manager thread END!") + return True + + except Exception as e: + logger.error(f"Error stopping fc_ros_manager: {e}") + return False + + def shutdown(self): + """完全關閉並清理資源""" + if self.running: + self.stop() + + if self.initialized: + try: + # 銷毀 nodes + if self.status_publisher: + self.status_publisher.destroy_node() + if self.command_service: + self.command_service.destroy_node() + + # 關閉 ROS2 + if rclpy.ok(): + rclpy.shutdown() + + self.initialized = False + logger.info("fc_ros_manager Node END!") + + except Exception as e: + logger.error(f"Error during shutdown: {e}") + + def get_status(self) -> dict: + return { + 'initialized': self.initialized, + 'running': self.running, + 'status_publisher_active': self.status_publisher is not None and self.status_publisher.running, + 'command_service_active': self.command_service is not None, + } + + +# ============================================================================ +# 全域實例 +# ============================================================================ + +# 全域管理器實例（供 mainOrchestrator 使用） +ros2_manager = fc_ros_manager() + + +''' +================= 改版記錄 ============================ + +2026.01.20 +1. 重構自 mavlinkPublish.py (該檔案將被棄用) +2. 提供 fc_ros_manager 統一管理介面 +3. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態 +4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布 +5. 預留 MavlinkCommandService 結構（稍後實現） + +''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py new file mode 100644 index 0000000..b924c42 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -0,0 +1,453 @@ +""" +VehicleView - Pure State Container +純粹的狀態容器，不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量 +只提供狀態存取介面，由外部手動餵資料（push state） +""" + +import os +from typing import Dict, Optional, Any +from dataclasses import dataclass, field +from enum import Enum + +from .utils import setup_logger + +logger = setup_logger(os.path.basename(__file__)) + + +# ====================== Enums ===================== + +class ConnectionType(Enum): + """連接類型""" + SERIAL = "serial" + UDP = "udp" + TCP = "tcp" + OTHER = "other" + + +class ComponentType(Enum): + """組件類型""" + AUTOPILOT = "autopilot" + GCS = "gcs" + CAMERA = "camera" + GIMBAL = "gimbal" + OTHER = "other" + + +class RFModuleType(Enum): + """RF模組類型""" + XBEE = "xbee" + UDP = "udp" + TCP = "tcp" + OTHER = "other" + + +# ====================== Data Classes ===================== + +@dataclass +class Position: + """位置資訊""" + latitude: Optional[float] = None # 緯度 (度) + longitude: Optional[float] = None # 經度 (度) + altitude: Optional[float] = None # 海拔高度 (公尺) + relative_altitude: Optional[float] = None # 相對高度 (公尺) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class Attitude: + """姿態資訊""" + roll: Optional[float] = None # 橫滾角 (弧度) + pitch: Optional[float] = None # 俯仰角 (弧度) + yaw: Optional[float] = None # 偏航角 (弧度) + rollspeed: Optional[float] = None # 橫滾速度 (弧度/秒) + pitchspeed: Optional[float] = None # 俯仰速度 (弧度/秒) + yawspeed: Optional[float] = None # 偏航速度 (弧度/秒) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class FlightMode: + """飛行模式資訊""" + base_mode: Optional[int] = None # MAVLink base mode + custom_mode: Optional[int] = None # 自定義模式 + mode_name: Optional[str] = None # 模式名稱 (例如: "GUIDED", "AUTO") + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class Battery: + """電池資訊""" + voltage: Optional[float] = None # 電壓 (V) + current: Optional[float] = None # 電流 (A) + remaining: Optional[int] = None # 剩餘電量 (%) + temperature: Optional[float] = None # 溫度 (攝氏) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class EKF: + """EKF狀態資訊""" + flags: Optional[int] = None # EKF 旗標 + velocity_variance: Optional[float] = None # 速度變異 + pos_horiz_variance: Optional[float] = None # 水平位置變異 + pos_vert_variance: Optional[float] = None # 垂直位置變異 + compass_variance: Optional[float] = None # 羅盤變異 + terrain_alt_variance: Optional[float] = None # 地形高度變異 + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class GPS: + """GPS資訊""" + fix_type: Optional[int] = None # GPS fix 類型 (0=無, 1=無fix, 2=2D, 3=3D, 4=DGPS, 5=RTK) + satellites_visible: Optional[int] = None # 可見衛星數 + eph: Optional[int] = None # GPS HDOP 水平精度因子 + epv: Optional[int] = None # GPS VDOP 垂直精度因子 + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class VFR: + """VFR HUD資訊""" + airspeed: Optional[float] = None # 空速 (m/s) + groundspeed: Optional[float] = None # 地速 (m/s) + heading: Optional[int] = None # 航向 (度) + throttle: Optional[int] = None # 油門 (%) + climb: Optional[float] = None # 爬升率 (m/s) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class ComponentStatus: + """組件狀態容器""" + position: Position = field(default_factory=Position) + attitude: Attitude = field(default_factory=Attitude) + mode: FlightMode = field(default_factory=FlightMode) + battery: Battery = field(default_factory=Battery) + ekf: EKF = field(default_factory=EKF) + gps: GPS = field(default_factory=GPS) + vfr: VFR = field(default_factory=VFR) + + # 系統狀態 + system_status: Optional[int] = None # MAV_STATE + armed: Optional[bool] = None # 解鎖狀態 + + # 其他自定義狀態 + custom_status: Dict[str, Any] = field(default_factory=dict) + + +@dataclass +class PacketStats: + """封包統計資訊""" + received_count: int = 0 # 接收封包數 + lost_count: int = 0 # 遺失封包數 + last_seq: Optional[int] = None # 最後序號 + last_msg_time: Optional[float] = None # 最後訊息時間 + msg_type_count: Dict[int, int] = field(default_factory=dict) # 各類訊息計數 {msg_type: count} + + +@dataclass +class RFStatus: + """RF模組狀態""" + rssi: Optional[int] = None # 信號強度 + noise: Optional[int] = None # 噪音水平 + at_response: Optional[str] = None # AT 命令回應 + link_quality: Optional[int] = None # 連接品質 + timestamp: Optional[float] = None # 時間戳記 + custom_status: Dict[str, Any] = field(default_factory=dict) # 其他自定義狀態 + + +@dataclass +class SocketInfo: + """Socket連接資訊""" + ip: Optional[str] = None # IP位址 + port: Optional[int] = None # 埠號 + local_ip: Optional[str] = None # 本地IP + local_port: Optional[int] = None # 本地埠號 + connected: bool = False # 連接狀態 + + +# ====================== Component Class ===================== + +class VehicleComponent: + """載具組件 - 代表一個 MAVLink component""" + + def __init__(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER): + self.component_id = component_id + self.type = comp_type + + # MAVLink 組件資訊 + self.mav_type: Optional[int] = None # MAV_TYPE + self.mav_autopilot: Optional[int] = None # MAV_AUTOPILOT + + # 狀態容器 + self.status = ComponentStatus() + + # 參數容器 (只存放，不主動下載) + self.parameters: Dict[str, Any] = {} + + # 封包統計 + self.packet_stats = PacketStats() + + def update_packet_stats(self, seq: int, msg_type: int, timestamp: float) -> None: + """ + 更新封包統計 + + Args: + seq: 訊息序號 + msg_type: 訊息類型 + timestamp: 時間戳記 + """ + stats = self.packet_stats + + # 檢查遺失封包 + if stats.last_seq is not None: + expected_seq = (stats.last_seq + 1) % 256 + diff = seq - expected_seq + if diff < 0: + diff += 256 + stats.lost_count += diff + + # 更新統計資訊 + stats.received_count += 1 + stats.last_seq = seq + stats.last_msg_time = timestamp + + # 更新訊息類型計數 + if msg_type in stats.msg_type_count: + stats.msg_type_count[msg_type] += 1 + else: + stats.msg_type_count[msg_type] = 1 + + def reset_packet_stats(self) -> None: + """重置封包統計""" + self.packet_stats = PacketStats() + + def set_parameter(self, param_name: str, param_value: Any) -> None: + """設定參數 (手動餵入)""" + self.parameters[param_name] = param_value + + def get_parameter(self, param_name: str, default: Any = None) -> Any: + """取得參數""" + return self.parameters.get(param_name, default) + + def __str__(self) -> str: + return (f"Component(id={self.component_id}, type={self.type.value}, " + f"mav_type={self.mav_type}, received={self.packet_stats.received_count}, " + f"lost={self.packet_stats.lost_count})") + + +# ====================== RF Module Class ===================== + +class RFModule: + """RF模組資訊容器""" + + def __init__(self, rf_type: RFModuleType = RFModuleType.OTHER): + self.type = rf_type + self.status = RFStatus() + self.socket_info = SocketInfo() + + def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None: + """更新RSSI""" + self.status.rssi = rssi + if timestamp: + self.status.timestamp = timestamp + + def update_at_response(self, response: str, timestamp: Optional[float] = None) -> None: + """更新AT命令回應""" + self.status.at_response = response + if timestamp: + self.status.timestamp = timestamp + + def update_socket_info(self, ip: str = None, port: int = None, + local_ip: str = None, local_port: int = None, + connected: bool = None) -> None: + """更新Socket資訊""" + if ip is not None: + self.socket_info.ip = ip + if port is not None: + self.socket_info.port = port + if local_ip is not None: + self.socket_info.local_ip = local_ip + if local_port is not None: + self.socket_info.local_port = local_port + if connected is not None: + self.socket_info.connected = connected + + def __str__(self) -> str: + return (f"RFModule(type={self.type.value}, rssi={self.status.rssi}, " + f"connected={self.socket_info.connected})") + + +# ====================== Main VehicleView Class ===================== + +class VehicleView: + """ + 載具視圖 - 純狀態容器 + + 特點: + 1. 只有狀態容器，沒有任何主動通訊功能 + 2. 不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量 + 3. 可以手動餵資料 (push state) + 4. 可擴充 (支援 RF 模組狀態) + """ + + # TODO: connected_via 這個值可能用不到 之後可能要移除 不要用它再加功能了 + + def __init__(self, sysid: int): + # Meta 資訊 + self.sysid = sysid + self.kind: Optional[str] = None # 載具種類描述 (例如: "Copter", "Plane") + self.vehicle_type: Optional[int] = None # MAV_TYPE + self.connected_via: ConnectionType = ConnectionType.OTHER + + # 組件容器 {component_id: VehicleComponent} + self.components: Dict[int, VehicleComponent] = {} + + # RF模組 + self.rf_module: Optional[RFModule] = None + + # 其他自定義meta資訊 + self.custom_meta: Dict[str, Any] = {} + + def add_component(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER) -> VehicleComponent: + """ + 新增組件 + + Args: + component_id: 組件ID + comp_type: 組件類型 + + Returns: + VehicleComponent: 新增的組件 + """ + if component_id not in self.components: + self.components[component_id] = VehicleComponent(component_id, comp_type) + # logger.debug(f"Added component {component_id} to system {self.sysid}") + return self.components[component_id] + + def get_component(self, component_id: int) -> Optional[VehicleComponent]: + """取得組件""" + return self.components.get(component_id) + + def remove_component(self, component_id: int) -> bool: + """移除組件""" + if component_id in self.components: + del self.components[component_id] + # logger.debug(f"Removed component {component_id} from system {self.sysid}") + return True + return False + + def reset_component_stats(self, component_id: int) -> None: + """重置指定組件的封包統計""" + component = self.get_component(component_id) + if component: + component.reset_packet_stats() + # logger.info(f"Reset packet stats for component {component_id} in system {self.sysid}") + + def set_rf_module(self, rf_type: RFModuleType) -> RFModule: + """設定RF模組""" + self.rf_module = RFModule(rf_type) + return self.rf_module + + def get_rf_module(self) -> Optional[RFModule]: + """取得RF模組""" + return self.rf_module + + def __str__(self) -> str: + comp_list = ", ".join([str(cid) for cid in self.components.keys()]) + return (f"VehicleView(sysid={self.sysid}, kind={self.kind}, " + f"connected_via={self.connected_via.value}, " + f"components=[{comp_list}], " + f"rf_module={self.rf_module is not None})") + + def to_dict(self) -> Dict[str, Any]: + """轉換為字典格式 (方便序列化/除錯)""" + return { + 'meta': { + 'sysid': self.sysid, + 'kind': self.kind, + 'vehicle_type': self.vehicle_type, + 'connected_via': self.connected_via.value, + 'custom_meta': self.custom_meta + }, + 'components': { + cid: { + 'component_id': comp.component_id, + 'type': comp.type.value, + 'mav_type': comp.mav_type, + 'mav_autopilot': comp.mav_autopilot, + 'packet_stats': { + 'received': comp.packet_stats.received_count, + 'lost': comp.packet_stats.lost_count, + 'last_seq': comp.packet_stats.last_seq, + 'last_msg_time': comp.packet_stats.last_msg_time + }, + 'parameters_count': len(comp.parameters) + } + for cid, comp in self.components.items() + }, + 'rf_module': { + 'type': self.rf_module.type.value, + 'rssi': self.rf_module.status.rssi, + 'socket_connected': self.rf_module.socket_info.connected + } if self.rf_module else None + } + + +# ====================== Registry ===================== + +class VehicleViewRegistry: + """載具視圖註冊表 - 管理所有的 VehicleView""" + + def __init__(self): + self._vehicles: Dict[int, VehicleView] = {} + + def register(self, sysid: int) -> VehicleView: + """註冊新的載具視圖""" + if sysid not in self._vehicles: + self._vehicles[sysid] = VehicleView(sysid) + logger.info(f"Registered new VehicleView for system {sysid}") + return self._vehicles[sysid] + + def get(self, sysid: int) -> Optional[VehicleView]: + """取得載具視圖""" + return self._vehicles.get(sysid) + + def unregister(self, sysid: int) -> bool: + """註銷載具視圖""" + if sysid in self._vehicles: + del self._vehicles[sysid] + logger.info(f"Unregistered VehicleView for system {sysid}") + return True + return False + + def get_all(self) -> Dict[int, VehicleView]: + """取得所有載具視圖""" + return self._vehicles.copy() + + def clear(self) -> None: + """清空所有載具視圖""" + self._vehicles.clear() + logger.info("Cleared all VehicleViews") + + def __len__(self) -> int: + return len(self._vehicles) + + def __contains__(self, sysid: int) -> bool: + return sysid in self._vehicles + + +# ====================== Global Instance ===================== + +# 全域註冊表實例 +vehicle_registry = VehicleViewRegistry() + +''' +================= 改版記錄 ============================ + +2026.01.16 +1. 新增 重置指定組件的封包統計 功能 + +''' + diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py new file mode 100644 index 0000000..731a950 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -0,0 +1,611 @@ +''' + + +''' + +# 基礎功能的 import +import asyncio +import serial_asyncio + +import os +import sys +import serial +import signal +import time +import threading +import struct +from enum import Enum, auto + +# # XBee 模組 +# from xbee.frame import APIFrame + +# 自定義的 import +from .utils import setup_logger + +# ====================== 分割線 ===================== + +logger = setup_logger(os.path.basename(__file__)) + +# ====================== 分割線 ===================== +class XBeeFrameHandler: + """XBee API Frame 處理器""" + + @staticmethod + def parse_at_command_response(frame: bytes) -> dict: + """解析 AT Command Response (0x88)""" + if len(frame) < 8: + return None + + frame_type = frame[3] + if frame_type != 0x88: + return None + + frame_id = frame[4] + at_command = frame[5:7] + status = frame[7] + data = frame[8:] if len(frame) > 8 else b'' + + return { + 'frame_id': frame_id, + 'command': at_command, + 'status': status, + 'data': data, + 'is_ok': status == 0x00 + } + + @staticmethod + def parse_receive_packet(frame: bytes) -> dict: + # """解析 RX Packet (0x90) - 未來擴展用""" + # if len(frame) < 15 or frame[3] != 0x90: + # return None + + # return { + # 'source_addr': frame[4:12], + # 'reserved': frame[12:14], + # 'options': frame[14], + # 'data': frame[15:-1] + # } + pass + return None + + @staticmethod + def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes: + """ + 將數據封裝為 XBee API 傳輸幀 + + 使用 XBee API 格式封裝數據: + - 傳輸請求幀 (0x10) + - 使用廣播地址 + - 添加適當的頭部和校驗和 + """ + frame_type = 0x10 + dest_addr16 = b'\xFF\xFE' + broadcast_radius = 0x00 + options = 0x00 + + frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) + frame += dest_addr64 + dest_addr16 + frame += struct.pack(">BB", broadcast_radius, options) + data + checksum = 0xFF - (sum(frame) & 0xFF) + return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum) + + @staticmethod + def decapsulate_data(data: bytes): + # 這裡可以根據需要進行數據解封裝 + + # XBee API 幀格式: + # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節) + + # 檢查幀起始符 (0x7E) + if not data or len(data) < 5 or data[0] != 0x7E: + return data + + # 獲取數據長度 (不包括校驗和) + # length = (data[1] << 8) + data[2] + length = (data[1] << 8) | data[2] + + # 檢查幀完整性 + if len(data) < length + 4: # 起始符 + 長度(2字節) + 數據 + 校驗和 + return data + + # 提取API標識符和數據 + frame_type = data[3] + # frame_data = data[4:4+length-1] # 減1是因為API標識符已經算在長度中 + + # 根據不同的幀類型進行處理 + if frame_type == 0x90: # 例如，這是"接收數據包"類型 + rf_data_start = 3 + 12 + return data[rf_data_start:3 + length] + else: + return None + return data + + +class ATCommandHandler: + """AT 指令回應處理器""" + + def __init__(self, serial_port: str): + self.serial_port = serial_port + self.handlers = { + b'DB': self._handle_rssi, + b'SH': self._handle_serial_high, + b'SL': self._handle_serial_low, + # 可擴展其他 AT 指令 + } + + def handle_response(self, response: dict): + """根據 AT 指令類型分派處理""" + if not response or not response['is_ok']: + if response: + print(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}") + return + + command = response['command'] + handler = self.handlers.get(command) + + if handler: + handler(response['data']) + else: + print(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}") + + def _handle_rssi(self, data: bytes): + """處理 DB (RSSI) 回應""" + if not data: + return + + rssi_value = data[0] + now = time.time() + + # 檢查是否最近有收到 MAVLink + last_mavlink_time = serial_last_mavlink_time.get(self.serial_port, 0) + if now - last_mavlink_time > 0.5: + print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略") + return + + # 取得對應的 sysid + sysid = serial_to_sysid.get(self.serial_port) + if sysid is None: + print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略") + return + + # 記錄 RSSI + rssi_history[sysid].append(-rssi_value) + time_history[sysid].append(now) + # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") + + def _handle_serial_high(self, data: bytes): + # """處理 SH (Serial Number High) - 範例""" + # if len(data) >= 4: + # serial_high = int.from_bytes(data[:4], 'big') + # print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}") + pass + + def _handle_serial_low(self, data: bytes): + # """處理 SL (Serial Number Low) - 範例""" + # if len(data) >= 4: + # serial_low = int.from_bytes(data[:4], 'big') + # print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}") + pass + +# ====================== 分割線 ===================== + +class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發 + def __init__(self, udp_handler, serial_port_str): + self.udp_handler = udp_handler # UDP 的傳輸物件 + self.serial_port_str = serial_port_str + self.at_handler = ATCommandHandler(serial_port_str) + + self.buffer = bytearray() # 用於緩存接收到的資料 + self.transport = None # Serial 自己的傳輸物件 + # self.first_data = True # 標記是否為第一次收到資料 + # self.has_processed = False # 測試模式用 處理數據旗標 # debug + + def connection_made(self, transport): + self.transport = transport + if hasattr(self.udp_handler, 'set_serial_handler'): + self.udp_handler.set_serial_handler(self) + # logger.info(f"Serial port {self.serial_port_str} connected.") # debug + + # Serial 收到資料的處理過程 + def data_received(self, data): + # 1. 把收到的資料加入緩衝區 + self.buffer.extend(data) + + # 2. 需要完整的 header 才能解析 + while len(self.buffer) >= 3: + # 3. 瞄準 XBee API Frame (0x7E 開頭的封包) + if self.buffer[0] != 0x7E: + self.buffer.pop(0) # 如果不是就丟掉 + continue + + # 4. 讀取 payload 長度 + length = (self.buffer[1] << 8) | self.buffer[2] + full_length = 3 + length + 1 + + # 5. 等待完整封包 + if len(self.buffer) < full_length: + break + + # 6. 提取完整 frame 並從緩衝區移除 + an_frame = self.buffer[:full_length] + del self.buffer[:full_length] + + # 7. 判斷 frame 類型 + frame_type = an_frame[3] + + if frame_type == 0x88: + # 處理 AT Command 回應 + # response = XBeeFrameHandler.parse_at_command_response(an_frame) + # self.at_handler.handle_response(response) + pass + + elif frame_type == 0x90: + # Receive Packet (RX) payload 先解碼 + processed_data = XBeeFrameHandler.decapsulate_data(bytes(an_frame)) + # 轉換失敗就捨棄了 + if processed_data is None: + continue + # 再透過 UDP 送出 + self.udp_handler.transport.sendto(processed_data, (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port)) + + elif frame_type == 0x8B: + pass + + else: + # 其他類型的 frame 未來可擴展處理 現在忽略 + logger.warning(f"[{self.serial_port_str}] Undefined frame type: 0x{frame_type:02X}") + + # # RSSI + # if frame[3] == 0x88 and frame[5:7] == b'DB': # frame[3] == 0x88 AT -> API 封包 + # # frame[5:7] == b'DB' -> API 封包的DB參數 + # status = frame[7] # + # if status == 0x00 and len(frame) > 8: # status == 0x00 -> 這個封包是有效封包 + # rssi_value = frame[8] + # now = time.time() + + # # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port + # last_time = serial_last_mavlink_time.get(self.serial_port, 0) + # if now - last_time <= 0.5: + # sysid = serial_to_sysid.get(self.serial_port, None) + # if sysid is not None: + # rssi_history[sysid].append(-rssi_value) + # time_history[sysid].append(now) + # # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") + # else: + # print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略") + # else: + # print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略") + # else: + # print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}") + + +class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發 + + LOCAL_HOST_IP = '127.0.0.1' # 只送給本地端IP + + def __init__(self, target_port): + self.target_port = target_port # 目標 UDP 端口 + + self.serial_handler = None # Serial 的傳輸物件 + self.transport = None # UDP 自己的傳輸物件 + self.remote_addr = None # 儲存動態獲取的遠程地址 # debug + # self.has_processed = False # 測試模式用 處理數據旗標 # debug + + def connection_made(self, transport): + self.transport = transport + # logger.info(f"UDP transport ready. Waiting for serial data before sending.") # debug + + def set_serial_handler(self, serial_handler): + self.serial_handler = serial_handler + + # UDP 收到資料的處理過程 + def datagram_received(self, data, addr): + # 儲存對方的地址（這樣就能向同一個來源回傳數據） + # self.remote_addr = addr # debug + # print(f"Received UDP data from {addr}, setting as remote address") + + processed_data = XBeeFrameHandler.encapsulate_data(data) + + if self.serial_handler: + self.serial_handler.transport.write(processed_data) + +#================================================================== + +class SerialReceiverType(Enum): + """連接類型""" + TELEMETRY = auto() + XBEEAPI2AT = auto() + OTHER = auto() + + +class serial_manager: + + class serial_object: + def __init__(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc + self.baudrate = baudrate + self.receiver_type = receiver_type + self.target_port = target_port # 指向的 UPD 端口 + + self.transport = None # TODO 這個變數可能沒有作用 + self.protocol = None # TODO 這個變數可能沒有作用 + self.udp_handler = None + self.serial_handler = None + + def __init__(self): + self.thread = None + self.loop = None + self.running = False + self.serial_count = 0 + self.serial_objects = {} # serial id num : serial_object + + def __del__(self): + self.loop = None + self.thread = None + + def start(self): + + if self.running: + logger.warning("serial_manager already running") + return + + self.running = True + + # 啟動獨立線程 命名為 SerialManager + self.thread = threading.Thread( + target=self._run_event_loop, + name="SerialManager" + ) + self.thread.daemon = False # 不設為 daemon，確保正確關閉 + self.thread.start() + + # 等待 _run_event_loop 建立事件循環的物件 self.loop + start_timeout = 2.0 + start_time = time.time() + while not self.loop and time.time() - start_time < start_timeout: + time.sleep(0.1) + + # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop + if self.loop: + logger.info("serial_manager thread started <-") + return True + else: + logger.error("serial_manager failed to start") + return False + + def shutdown(self): + """停止 serial_manager 和其管理的所有 serial_object""" + # 自己在 running 狀態下才執行停止程序 + if not self.running: + logger.warning("serial_manager is not running") + return + + # 停止所有被管理的 serial_object + for serial_id in list(self.serial_objects.keys()): + self.remove_serial_link(serial_id) + + # 停止自己 + self.running = False + + # 解開事件循環的阻塞 + if self.loop: + self.loop.call_soon_threadsafe(self.loop.stop) + + # 等待線程結束 + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=5.0) + if self.thread.is_alive(): + logger.warning("serial_manager thread did not stop gracefully") + + logger.info("serial_manager thread END!") + + def _run_event_loop(self): + """在獨立線程中運行 asyncio 事件循環""" + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + + # # 為每個 serial_object 建立連接 + # for serial_obj in self.serial_objects: + # coro = serial_asyncio.create_serial_connection( + # self.loop, + # lambda: SerialProtocol(serial_obj.receiver_type), + # serial_obj.serial_port, + # baudrate=serial_obj.baudrate + # ) + # transport, protocol = self.loop.run_until_complete(coro) + # serial_obj.transport = transport + # serial_obj.protocol = protocol + + try: + self.loop.run_forever() + finally: + self.loop.close() + + def create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + + if not self.running or not self.loop: + logger.error("Event loop not running, cannot create serial link") + return False + + # 檢查 serial port 有效 + if not self.check_serial_port(serial_port, baudrate): + logger.error(f"Serial port {serial_port} validation failed") + return False + + # 使用 run_coroutine_threadsafe 執行協程並獲取結果 + future = asyncio.run_coroutine_threadsafe( + self._async_create_serial_link(serial_port, baudrate, target_port, receiver_type), + self.loop + ) + + try: + # 等待結果，設定合理的超時時間 + result = future.result(timeout=5.0) + if result: + logger.info(f"Create Serial Link: {serial_port} -> UDP {target_port}") + return True + except asyncio.TimeoutError: + logger.error(f"Timeout creating serial link for {serial_port}") + return False + except Exception as e: + logger.error(f"Failed to create serial link for {serial_port}: {e}") + return False + + async def _async_create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + """在事件循環線程中執行實際的連接創建""" + try: + # 創建 serial_object 實例 + serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type) + + # 建立 UDP 處理器並指定目標端口位置 + serial_obj.udp_handler = UDPHandler(target_port) + + # 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配 + udp_transport, udp_protocol = await self.loop.create_datagram_endpoint( + lambda: serial_obj.udp_handler, + local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 + ) + serial_obj.transport = udp_transport + serial_obj.protocol = udp_protocol + + # logger.info(f"UDP endpoint created for {serial_port}") # debug + + # 建立 Serial 處理器，將 UDP 處理器傳給它 + serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port) + + # 建立 Serial 連接 + serial_transport, _ = await serial_asyncio.create_serial_connection( + self.loop, + lambda: serial_obj.serial_handler, + serial_port, + baudrate=baudrate + ) + + # logger.info(f"Serial connection created for {serial_port}") # debug + + # 將 serial_object 加入管理列表 + serial_id = self.serial_count + 1 + self.serial_objects[serial_id] = serial_obj + self.serial_count += 1 + + # logger.info(f"Serial object {serial_id} added to manager") # debug + return True + + except Exception as e: + logger.error(f"Exception in _async_create_serial_link for {serial_port}: {str(e)}") + # 清理已創建的資源 + if 'serial_obj' in locals(): + if hasattr(serial_obj, 'transport') and serial_obj.transport: + serial_obj.transport.close() + return False + + def remove_serial_link(self, serial_id): + """移除串口連接（線程安全方式）""" + # 確保事件循環正在運行 + if not self.loop: + logger.error("Event loop not running") + return False + + # 檢查 serial_id 是否存在 + if serial_id not in self.serial_objects: + logger.warning(f"Serial object {serial_id} not found") + return False + + # 使用 run_coroutine_threadsafe 執行協程 + future = asyncio.run_coroutine_threadsafe( + self._async_remove_serial_link(serial_id), + self.loop + ) + + try: + result = future.result(timeout=3.0) + if result: + logger.info(f"Remove Serial Link {serial_id}") + return result + except asyncio.TimeoutError: + logger.error(f"Timeout removing serial link {serial_id}") + return False + except Exception as e: + logger.error(f"Failed to remove serial link {serial_id}: {e}") + return False + + async def _async_remove_serial_link(self, serial_id): + """在事件循環線程中執行實際的連接移除""" + if serial_id not in self.serial_objects: + logger.warning(f"Serial object {serial_id} not in managed list") + return False + + try: + serial_obj = self.serial_objects[serial_id] + + # 關閉 UDP transport + if hasattr(serial_obj, 'transport') and serial_obj.transport: + serial_obj.transport.close() + + # 關閉 Serial transport + if hasattr(serial_obj, 'serial_handler') and serial_obj.serial_handler: + if hasattr(serial_obj.serial_handler, 'transport') and serial_obj.serial_handler.transport: + serial_obj.serial_handler.transport.close() + + # 從管理列表中移除 + del self.serial_objects[serial_id] + # logger.info(f"Serial object {serial_id} removed from manager") # debug + return True + + except Exception as e: + logger.error(f"Exception in _async_remove_serial_link for {serial_id}: {str(e)}") + return False + + def get_serial_link(self): + ret = {} # serial id num : serial_port string + for key, obj in self.serial_objects.items(): + ret[key] = obj.serial_port + return ret + + @staticmethod + def check_serial_port(serial_port, baudrate): + """檢查串口是否存在與可用""" + # 檢查設備是否存在 + if not os.path.exists(serial_port): + logger.error(f"Serial Device {serial_port} Not Found") + return False + + # 檢查是否有權限訪問設備 + try: + if not os.access(serial_port, os.R_OK | os.W_OK): + logger.error(f"No permission to access {serial_port}") + return False + except Exception as e: + logger.error(f"Cannot Access Serial Device {serial_port}: {str(e)}") + return False + + # 檢查是否被占用 + try: + # 嘗試打開串口 + ser = serial.Serial(serial_port, baudrate) + ser.close() # 打開成功後立即關閉 + return True + except serial.SerialException as e: + logger.error(f"Serial Device {serial_port} is Occupied or Inaccessible: {str(e)}") + return False + except Exception as e: + logger.error(f"Unknown Error: {str(e)}") + return False + + +if __name__ == '__main__': + sm = serial_manager() + sm.start() + + SERIAL_PORT = '/dev/ttyUSB0' # 手動指定 + SERIAL_BAUDRATE = 115200 + UDP_REMOTE_PORT = 14571 + sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT) + + linked_serial = sm.get_serial_link() + print(linked_serial) + time.sleep(10) + + sm.remove_serial_link(1) + time.sleep(3) + sm.shutdown() \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/__init__.py b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py new file mode 100644 index 0000000..921faf4 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py @@ -0,0 +1,7 @@ +""" +共用工具模組 +""" +from .ringBuffer import RingBuffer +from .theLogger import setup_logger + +__all__ = ['RingBuffer', 'setup_logger'] \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py b/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py new file mode 100644 index 0000000..0c6a873 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py @@ -0,0 +1,129 @@ +import socket +import random +import os + + +def get_used_ports(): + """ + 從 /proc/net/tcp 和 /proc/net/udp 讀取系統已占用的 port + 直接讀取 Linux 系統資訊，避免暴力嘗試 + + Returns: + set: 已被占用的 port 號集合 + """ + used_ports = set() + + # 讀取 TCP 占用的 port (包含 IPv4 和 IPv6) + for filepath in ['/proc/net/tcp', '/proc/net/tcp6']: + if os.path.exists(filepath): + try: + with open(filepath, 'r') as f: + lines = f.readlines()[1:] # 跳過標題行 + for line in lines: + parts = line.split() + if len(parts) > 1: + # local_address 格式: "0100007F:1F90" (hex) + local_addr = parts[1] + port_hex = local_addr.split(':')[1] + port = int(port_hex, 16) + used_ports.add(port) + except (IOError, PermissionError): + pass + + # 讀取 UDP 占用的 port (包含 IPv4 和 IPv6) + for filepath in ['/proc/net/udp', '/proc/net/udp6']: + if os.path.exists(filepath): + try: + with open(filepath, 'r') as f: + lines = f.readlines()[1:] # 跳過標題行 + for line in lines: + parts = line.split() + if len(parts) > 1: + local_addr = parts[1] + port_hex = local_addr.split(':')[1] + port = int(port_hex, 16) + used_ports.add(port) + except (IOError, PermissionError): + pass + + return used_ports + + +def is_port_available(port): + """ + 測試指定 port 是否可用 (TCP 和 UDP 都測試) + + Args: + port (int): 要測試的 port 號 + + Returns: + bool: True 表示可用，False 表示被占用 + """ + # 測試 TCP + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind(('', port)) + except OSError: + return False + + # 測試 UDP + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind(('', port)) + except OSError: + return False + + return True + + +def find_available_port(start_port=1024, end_port=65535): + """ + 在指定的 port 區間內隨機找出一個未被占用的 port + 使用 Linux /proc/net 系統資訊來過濾已占用的 port，避免暴力嘗試 + 確保 TCP 和 UDP 都可用 + + Args: + start_port (int): 起始 port 號 (預設 1024) + end_port (int): 結束 port 號 (預設 65535) + + Returns: + int: 可用的 port 號，如果找不到則返回 None + """ + if start_port < 1 or end_port > 65535 or start_port >= end_port: + raise ValueError("Port 範圍必須在 1-65535 之間，且起始 port 必須小於結束 port") + + # 從系統讀取已占用的 port + used_ports = get_used_ports() + + # 計算可用的 port 列表 (排除已占用的) + available_ports = [p for p in range(start_port, end_port + 1) if p not in used_ports] + + if not available_ports: + return None + + # 隨機打亂順序 + random.shuffle(available_ports) + + # 從可用列表中挑選，再用 socket 雙重確認 TCP 和 UDP 都可用 + for port in available_ports: + if is_port_available(port): + return port + + # 如果都不可用 + return None + + +if __name__ == "__main__": + # 使用範例 + port = find_available_port(8000, 9000) + if port: + print(f"找到可用的 port: {port}") + else: + print("找不到可用的 port") + + # 自訂範圍範例 + port = find_available_port(10000, 20000) + if port: + print(f"在 10000-20000 範圍找到可用的 port: {port}") diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py new file mode 100644 index 0000000..1e1a73f --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py @@ -0,0 +1,111 @@ +""" +Serial Port Discovery Utility + +This module provides functions to discover available serial ports on the system. +It uses glob pattern matching to find serial device files in /dev/. +""" + +import glob +from typing import List, Union + + +def get_serial_ports() -> List[str]: + """ + 獲取系統中所有可用的串口設備列表 + + 在 Linux 系統中，會搜尋以下模式的設備： + - /dev/ttyUSB* (USB 串口設備) + - /dev/ttyACM* (USB CDC ACM 設備) + - /dev/ttyS* (標準串口) + + Returns: + List[str]: 包含所有找到的串口設備路徑的列表 + + Example: + >>> ports = get_serial_ports() + >>> print(ports) + ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyS0'] + """ + serial_ports = [] + + # 搜尋不同類型的串口設備 + patterns = [ + '/dev/ttyUSB*', # USB 串口轉換器 + '/dev/ttyACM*', # USB CDC ACM 設備（如 Arduino） + '/dev/ttyS*', # 標準串口 + ] + + for pattern in patterns: + serial_ports.extend(glob.glob(pattern)) + + # 排序以便於閱讀 + serial_ports.sort() + + return serial_ports + + +def get_serial_ports_with_filter(filter_patterns: Union[str, List[str]] = None) -> List[str]: + """ + 獲取串口設備列表，可選擇性地使用自訂篩選模式 + + Args: + filter_patterns (Union[str, List[str]], optional): + 單一或多個 glob 模式 + - 字串: '/dev/ttyUSB*' + - 列表: ['/dev/ttyUSB*', '/dev/ttyACM*'] + 如果為 None，則使用預設模式搜尋所有串口 + + Returns: + List[str]: 符合條件的串口設備路徑列表 + + Example: + >>> # 單一 pattern + >>> ports = get_serial_ports_with_filter('/dev/ttyUSB*') + >>> print(ports) + ['/dev/ttyUSB0', '/dev/ttyUSB1'] + + >>> # 多個 patterns + >>> ports = get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*']) + >>> print(ports) + ['/dev/ttyACM0', '/dev/ttyUSB0', '/dev/ttyUSB1'] + """ + if filter_patterns is None: + return get_serial_ports() + + # 統一轉成 list 處理 + if isinstance(filter_patterns, str): + filter_patterns = [filter_patterns] + + serial_ports = [] + for pattern in filter_patterns: + serial_ports.extend(glob.glob(pattern)) + + serial_ports.sort() + return serial_ports + + +if __name__ == "__main__": + # 使用範例 + print("=== Serial Port Discovery ===\n") + + # 1. 獲取所有串口設備 + all_ports = get_serial_ports() + print(f"找到 {len(all_ports)} 個串口設備:") + for port in all_ports: + print(f" - {port}") + + print("\n" + "="*30 + "\n") + + # 2. 只搜尋 USB 串口 + usb_ports = get_serial_ports_with_filter('/dev/ttyUSB*') + print(f"找到 {len(usb_ports)} 個 USB 串口設備:") + for port in usb_ports: + print(f" - {port}") + + print("\n" + "="*30 + "\n") + + # 3. 只搜尋 ACM 設備 + acm_ports = get_serial_ports_with_filter('/dev/ttyACM*') + print(f"找到 {len(acm_ports)} 個 ACM 設備:") + for port in acm_ports: + print(f" - {port}") diff --git a/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py b/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py new file mode 100644 index 0000000..6728426 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py @@ -0,0 +1,231 @@ +# import array +import threading +import ctypes +from typing import Any, Optional, Tuple + +class RingBuffer: + """ + 高效能無鎖環形緩衝區，使用原子操作避免鎖定 + 用於在不同執行緒間高效傳遞資料 + """ + # 緩衝區計數器，用於自動分配 buffer_id + _buffer_counter = 0 + _counter_lock = threading.Lock() + + def __init__(self, capacity: int = 256, buffer_id: int = None): + """ + 初始化環形緩衝區 + + Args: + capacity: 緩衝區容量 (必須是 2 的次方) + buffer_id: 緩衝區 ID，如果為 None 則自動分配 + """ + # 確保容量是 2 的次方，便於使用位運算取模 + if capacity & (capacity - 1) != 0: + # 找到大於等於 capacity 的最小 2 的次方 + capacity = 1 << (capacity - 1).bit_length() + + # 分配緩衝區 ID + if buffer_id is None: + with RingBuffer._counter_lock: + buffer_id = RingBuffer._buffer_counter + RingBuffer._buffer_counter += 1 + + self.buffer_id = buffer_id + self.capacity = capacity + self.mask = capacity - 1 # 用於快速取模 + self.buffer = [None] * capacity + + # 原子計數器作為讀寫指標 + self.write_index = ctypes.c_uint64(0) + self.read_index = ctypes.c_uint64(0) + + # 用於檢測上次操作的執行緒 ID + self._last_read_thread = None + self._last_write_thread = None + + # 添加同時讀寫統計 + self.concurrent_write_count = ctypes.c_uint64(0) # 同時寫入計數 + self.concurrent_read_count = ctypes.c_uint64(0) # 同時讀取計數 + self.total_write_count = ctypes.c_uint64(0) # 總寫入操作計數 + self.total_read_count = ctypes.c_uint64(0) # 總讀取操作計數 + self.overflow_count = ctypes.c_uint64(0) # 緩衝區溢出次數 + + # 記錄各執行緒的操作次數 + self.thread_write_counts = {} # {thread_id: count} + self.thread_read_counts = {} # {thread_id: count} + + # 用於保護統計數據的鎖（僅用於統計，不影響主要讀寫操作） + self._stats_lock = threading.Lock() + + def put(self, item: Any) -> bool: + """ + 將項目存入緩衝區 + + Args: + item: 要存入的項目 + + Returns: + bool: 成功寫入返回 True，緩衝區已滿返回 False + """ + # 更新寫入統計 + self.total_write_count.value += 1 + + # 檢測上次寫入的執行緒 + current_thread = threading.get_ident() + + # 記錄當前執行緒寫入次數 + with self._stats_lock: + self.thread_write_counts[current_thread] = self.thread_write_counts.get(current_thread, 0) + 1 + + # 檢測是否為不同執行緒同時寫入 + if self._last_write_thread is not None and current_thread != self._last_write_thread: + self.concurrent_write_count.value += 1 + + self._last_write_thread = current_thread + + # 原子獲取當前寫入位置 + current = self.write_index.value + next_pos = (current + 1) & self.mask + + # 檢查緩衝區是否已滿 + if next_pos == self.read_index.value: + self.overflow_count.value += 1 + return False + + # 寫入資料並原子更新寫入位置 + self.buffer[current] = item + self.write_index.value = next_pos + return True + + def get(self) -> Optional[Any]: + """ + 從緩衝區讀取項目 + + Returns: + 項目或 None（如果緩衝區為空） + """ + # 更新讀取統計 + self.total_read_count.value += 1 + + # 檢測上次讀取的執行緒 + current_thread = threading.get_ident() + + # 記錄當前執行緒讀取次數 + with self._stats_lock: + self.thread_read_counts[current_thread] = self.thread_read_counts.get(current_thread, 0) + 1 + + # 檢測是否為不同執行緒同時讀取 + if self._last_read_thread is not None and current_thread != self._last_read_thread: + self.concurrent_read_count.value += 1 + + self._last_read_thread = current_thread + + # 檢查緩衝區是否為空 + if self.read_index.value == self.write_index.value: + return None + + # 原子獲取當前讀取位置並讀取資料 + current = self.read_index.value + item = self.buffer[current] + + # 原子更新讀取位置 + self.read_index.value = (current + 1) & self.mask + return item + + def get_all(self) -> list: + """ + 獲取緩衝區中的所有項目 + + Returns: + list: 所有可用項目的列表 + """ + items = [] + while True: + item = self.get() + if item is None: + break + items.append(item) + return items + + def size(self) -> int: + # 返回目前緩衝區內項目數量 + return (self.write_index.value - self.read_index.value) & self.mask + + def is_empty(self) -> bool: + # 檢查緩衝區是否為空 + return self.read_index.value == self.write_index.value + + def is_full(self) -> bool: + # 檢查緩衝區是否已滿 + return ((self.write_index.value + 1) & self.mask) == self.read_index.value + + def clear(self) -> None: + """清空緩衝區""" + self.read_index.value = self.write_index.value + + def get_stats(self) -> dict: + """ + 獲取緩衝區統計資訊 + + Returns: + dict: 包含各種統計數據的字典 + """ + with self._stats_lock: + stats = { + "buffer_id": self.buffer_id, + "capacity": self.capacity, + "current_size": self.size(), + "is_full": self.is_full(), + "is_empty": self.is_empty(), + "total_writes": self.total_write_count.value, + "total_reads": self.total_read_count.value, + "concurrent_writes": self.concurrent_write_count.value, + "concurrent_reads": self.concurrent_read_count.value, + "overflow_count": self.overflow_count.value, + "write_threads": len(self.thread_write_counts), + "read_threads": len(self.thread_read_counts), + "concurrent_write_ratio": self.concurrent_write_count.value / max(1, self.total_write_count.value), + "concurrent_read_ratio": self.concurrent_read_count.value / max(1, self.total_read_count.value), + "top_writer_threads": sorted(self.thread_write_counts.items(), key=lambda x: x[1], reverse=True)[:3], + "top_reader_threads": sorted(self.thread_read_counts.items(), key=lambda x: x[1], reverse=True)[:3], + } + return stats + + def print_stats(self) -> None: + """輸出當前緩衝區統計資訊""" + stats = self.get_stats() + + print(f"\n=== RingBuffer #{stats['buffer_id']} Statistics ===") + print(f"Capacity: {stats['capacity']}, Current Size: {stats['current_size']}") + print(f"Total Write Operations: {stats['total_writes']}") + print(f"Total Read Operations: {stats['total_reads']}") + print(f"Concurrent Write Events: {stats['concurrent_writes']} ({stats['concurrent_write_ratio']:.2%})") + print(f"Concurrent Read Events: {stats['concurrent_reads']} ({stats['concurrent_read_ratio']:.2%})") + print(f"Buffer Overflow Count: {stats['overflow_count']}") + print(f"Writing Threads: {stats['write_threads']}") + print(f"Reading Threads: {stats['read_threads']}") + + print("Top Writer Threads:") + for thread_id, count in stats['top_writer_threads']: + print(f" Thread {thread_id}: {count} writes") + + print("Top Reader Threads:") + for thread_id, count in stats['top_reader_threads']: + print(f" Thread {thread_id}: {count} reads") + print("=============================\n") + + def reset_stats(self) -> None: + """重置所有統計數據""" + with self._stats_lock: + self.concurrent_write_count.value = 0 + self.concurrent_read_count.value = 0 + self.total_write_count.value = 0 + self.total_read_count.value = 0 + self.overflow_count.value = 0 + self.thread_write_counts.clear() + self.thread_read_counts.clear() + + def __str__(self) -> str: + """返回緩衝區的字符串表示""" + return f"RingBuffer(id={self.buffer_id}, capacity={self.capacity}, size={self.size()})" \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py new file mode 100644 index 0000000..dce2ce7 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py @@ -0,0 +1,43 @@ +import logging +import os +from logging.handlers import TimedRotatingFileHandler + +# 全域 Logger 實例 +_global_logger = None + +def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger: + global _global_logger + + if _global_logger is None: + # 確保 logs 資料夾存在 + os.makedirs(log_dir, exist_ok=True) + + # 建立全域 Logger + _global_logger = logging.getLogger("global_logger") + _global_logger.setLevel(level) + _global_logger.propagate = False # 防止重複輸出 + + formatter = logging.Formatter( + fmt="%(asctime)s | %(levelname)s | %(name)s | %(message)s", + datefmt="%m-%d %H:%M:%S" + ) + + # 檔案輸出（每天輪替） + file_handler = TimedRotatingFileHandler( + filename=os.path.join(log_dir, "application.log"), + when="midnight", # 每天 0 點輪替 + backupCount=7, # 保留 7 天 + encoding="utf-8" + ) + file_handler.setFormatter(formatter) + _global_logger.addHandler(file_handler) + + # 終端機輸出 + console_handler = logging.StreamHandler() + console_handler.setFormatter(formatter) + _global_logger.addHandler(console_handler) + + # 為每個模組建立子 Logger，並設定名稱 + module_logger = _global_logger.getChild(name) + module_logger.name = name # 修改子 Logger 的名稱，僅保留子 Logger 名稱 + return module_logger diff --git a/src/fc_network_adapter/setup.py b/src/fc_network_adapter/setup.py index 33414cb..b28ac96 100644 --- a/src/fc_network_adapter/setup.py +++ b/src/fc_network_adapter/setup.py @@ -20,6 +20,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'mavlink_orchestrator = fc_network_adapter.mainOrchestrator:main', ], }, ) diff --git a/src/fc_network_adapter/tests/__init__.py b/src/fc_network_adapter/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_adapter/tests/demo_integration.py b/src/fc_network_adapter/tests/demo_integration.py new file mode 100644 index 0000000..11ac1d7 --- /dev/null +++ b/src/fc_network_adapter/tests/demo_integration.py @@ -0,0 +1,277 @@ +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + +# 基礎功能的 import +import queue +import time + +# ROS2 的 import +import rclpy + +# mavlink 的 import +from pymavlink import mavutil + +# 自定義的 import +from ..fc_network_adapter import mavlinkObject as mo +from ..fc_network_adapter import mavlinkVehicleView as mvv +# from ..fc_network_adapter import mavlinkDevice as md + +# ====================== 分割線 ===================== + +test_item = 1 +running_time = 3 + + +print('test_item : ', test_item) + +''' +測試項 個位數 表示分離的功能 + +測試項 1X 表示 mavlink_object 的功能 測試連線的能力 +''' + +if test_item == 1: + print('===> Start of Program .Test ', test_item) + + connection_string="udp:127.0.0.1:14591" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + # mavlink_object1 = mo.mavlink_object(mavlink_socket1) + + time.sleep(1) + + print("mark A") + + # print("Socket IP:", mavlink_socket1.target_system) + print("Socket port:", mavlink_socket1.port.getsockname()) + + # print("=== ", dir(mavlink_socket1.port)) + + +elif test_item == 10: + # 需要開啟一個 ardupilot 的模擬器 + # 測試 mavlink_object 放入 ring buffer 的應用 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0..1:14571" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + mavlink_object1 = mo.mavlink_object(mavlink_socket1) + + sock = mavlink_socket1.port + print("Socket port:", sock) + + manager.add_mavlink_object(mavlink_object1) + + start_time = time.time() + while (time.time() - start_time) < running_time: + items_a = mo.stream_bridge_ring.get_all() + items_b = mo.return_packet_ring.get_all() + try: + print(f"data num {len(items_a)} in return_packet_ring, first data : {items_a[0][2]}") + except IndexError: + print("stream_bridge_ring is empty") + + try: + print(f"data num {len(items_b)} in return_packet_ring, first data : {items_b[0][2]}") + except IndexError: + print("return_packet_ring is empty") + time.sleep(1) + + manager.shutdown() + + print('<=== End of Program') + +elif test_item == 11: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0.0.1:14571" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + mavlink_object1 = mo.mavlink_object(mavlink_socket1) + manager.add_mavlink_object(mavlink_object1) + + # 啟動 mavlink_bridge + bridge = mo.mavlink_bridge() + bridge.start() + + time.sleep(3) + + # 印出目前所有 mavlink_systems 的內容 + print('目前所有的系統 : ') + all_vehicles = mvv.vehicle_registry.get_all() + for sysid, vehicle in all_vehicles.items(): + print(f" System {sysid}: {vehicle}") + + start_time = time.time() + show_time = time.time() + while time.time() - start_time < running_time: + if (time.time() - show_time) >= 2: + # print("mark B") + + show_time = time.time() + for sysid, vehicle in all_vehicles.items(): + for compid in vehicle.components: + comp = vehicle.get_component(compid) + print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, comp.packet_stats.received_count)) + comp.reset_packet_stats() + print("===================") + + manager.shutdown() + bridge.stop() + + print('<=== End of Program') + +elif test_item == 12: + # 需要開啟一個 ardupilot 的模擬器 與 GCS + # 這邊是測試 mavlink object 作為交換器功能的代碼 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0.0.1:14571" + mavlink_socket_in1 = mavutil.mavlink_connection(connection_string) + mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1) + + connection_string="udp:127.0.0.1:14571" + mavlink_socket_in2 = mavutil.mavlink_connection(connection_string) + mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2) + + # 初始化輸出通道 + connection_string="udpout:127.0.0.1:14551" + mavlink_socket_out = mavutil.mavlink_connection(connection_string) + mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + + manager.add_mavlink_object(mavlink_object_out) + manager.add_mavlink_object(mavlink_object_in1) + manager.add_mavlink_object(mavlink_object_in2) + + time.sleep(1) # 等待通道啟動 + + mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id) + mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id) + + mavlink_object_in2.add_target_socket(mavlink_object_out.socket_id) + mavlink_object_out.add_target_socket(mavlink_object_in2.socket_id) + + start_time = time.time() + while (time.time() - start_time) < running_time: + + time.sleep(1) + + manager.shutdown() + + print('<=== End of Program') + + + +elif test_item == 21: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 + + print('===> Start of Program .Test ', test_item) + # 初始化 rclpy 才能使用 node + rclpy.init() + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + # 關於 Node 的初始化 + show_time = time.time() + analyzer._init_node() # 初始化 node + print('初始化 node 完成 耗時 : ',time.time() - show_time) + + time.sleep(0.5) # 系統 Setup 完成 + + + # 創建通道 + connection_string="udp:127.0.0.1:14560" + mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_object3 = mo.mavlink_object(mavlink_socket) + manager.add_mavlink_object(mavlink_object3) + + 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: + # print(analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode']) + analyzer.emit_info() # 這邊是測試 node 的運行 + time.sleep(1) + except KeyboardInterrupt: + break + + + # 程式結束 + analyzer.destroy_node() + rclpy.shutdown() + + # 結束程式 退出所有 thread + manager.stop() + analyzer.stop() + analyzer.thread.join() + + mavlink_socket.close() + print('<=== End of Program') + + +elif test_item == 52: + print('===> Start of Program .Test ', test_item) + + manager = mo.async_io_manager() + manager.start() + + # print(manager.thread.is_alive()) + + manager.shutdown() + + time.sleep(1) + + print('manager stopped') + diff --git a/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py b/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py new file mode 100644 index 0000000..d6954d3 --- /dev/null +++ b/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py @@ -0,0 +1,331 @@ +""" +VehicleView 使用範例 +展示如何使用純狀態容器來管理 MAVLink 載具資訊 +""" + +import time +from ..fc_network_adapter.mavlinkVehicleView import ( + VehicleView, + VehicleComponent, + RFModule, + vehicle_registry, + ConnectionType, + ComponentType, + RFModuleType +) + + +def example_basic_usage(): + """基本使用範例""" + print("=== 基本使用範例 ===\n") + + # 1. 建立載具視圖 + vehicle = VehicleView(sysid=1) + vehicle.kind = "Copter" + vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR + vehicle.connected_via = ConnectionType.UDP + + print(f"建立載具: {vehicle}\n") + + # 2. 新增 autopilot 組件 + autopilot = vehicle.add_component( + component_id=1, + comp_type=ComponentType.AUTOPILOT + ) + autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR + autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA + + print(f"新增組件: {autopilot}\n") + + # 3. 手動餵入位置資訊 + autopilot.status.position.latitude = 25.0330 + autopilot.status.position.longitude = 121.5654 + autopilot.status.position.altitude = 100.5 + autopilot.status.position.timestamp = time.time() + + print(f"位置: 緯度={autopilot.status.position.latitude}, " + f"經度={autopilot.status.position.longitude}, " + f"高度={autopilot.status.position.altitude}m\n") + + # 4. 手動餵入姿態資訊 + autopilot.status.attitude.roll = 0.05 # 弧度 + autopilot.status.attitude.pitch = -0.02 + autopilot.status.attitude.yaw = 1.57 + autopilot.status.attitude.timestamp = time.time() + + print(f"姿態: Roll={autopilot.status.attitude.roll:.3f}, " + f"Pitch={autopilot.status.attitude.pitch:.3f}, " + f"Yaw={autopilot.status.attitude.yaw:.3f} rad\n") + + # 5. 手動餵入飛行模式 + autopilot.status.mode.base_mode = 89 + autopilot.status.mode.custom_mode = 4 + autopilot.status.mode.mode_name = "GUIDED" + autopilot.status.mode.timestamp = time.time() + + print(f"飛行模式: {autopilot.status.mode.mode_name}\n") + + # 6. 手動餵入電池資訊 + autopilot.status.battery.voltage = 12.6 + autopilot.status.battery.current = 15.2 + autopilot.status.battery.remaining = 75 + autopilot.status.battery.timestamp = time.time() + + print(f"電池: 電壓={autopilot.status.battery.voltage}V, " + f"電流={autopilot.status.battery.current}A, " + f"剩餘={autopilot.status.battery.remaining}%\n") + + +def example_packet_tracking(): + """封包追蹤範例""" + print("\n=== 封包追蹤範例 ===\n") + + vehicle = VehicleView(sysid=2) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # 模擬接收封包 + timestamp = time.time() + + # 接收 HEARTBEAT (msg_type=0) + autopilot.update_packet_stats(seq=0, msg_type=0, timestamp=timestamp) + + # 接收 ATTITUDE (msg_type=30) + autopilot.update_packet_stats(seq=1, msg_type=30, timestamp=timestamp+0.1) + + # 接收 GLOBAL_POSITION_INT (msg_type=33) + autopilot.update_packet_stats(seq=2, msg_type=33, timestamp=timestamp+0.2) + + # 模擬封包遺失 (seq 跳過 3, 4, 5) + autopilot.update_packet_stats(seq=6, msg_type=0, timestamp=timestamp+0.3) + + stats = autopilot.packet_stats + print(f"封包統計:") + print(f" 接收: {stats.received_count}") + print(f" 遺失: {stats.lost_count}") + print(f" 最後序號: {stats.last_seq}") + print(f" 訊息類型計數: {stats.msg_type_count}\n") + + +def example_parameters(): + """參數管理範例""" + print("\n=== 參數管理範例 ===\n") + + vehicle = VehicleView(sysid=3) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # 手動設定參數 (不會主動下載) + autopilot.set_parameter("ARMING_CHECK", 1) + autopilot.set_parameter("ANGLE_MAX", 4500) + autopilot.set_parameter("WPNAV_SPEED", 500) + + print(f"參數數量: {len(autopilot.parameters)}") + print(f"ARMING_CHECK = {autopilot.get_parameter('ARMING_CHECK')}") + print(f"ANGLE_MAX = {autopilot.get_parameter('ANGLE_MAX')}") + print(f"WPNAV_SPEED = {autopilot.get_parameter('WPNAV_SPEED')}\n") + + +def example_rf_module(): + """RF模組範例""" + print("\n=== RF模組範例 ===\n") + + vehicle = VehicleView(sysid=4) + vehicle.connected_via = ConnectionType.SERIAL + + # 設定 XBee RF 模組 + rf = vehicle.set_rf_module(RFModuleType.XBEE) + + # 更新 Socket 資訊 + rf.update_socket_info( + ip="192.168.1.100", + port=14550, + local_ip="192.168.1.1", + local_port=14551, + connected=True + ) + + # 更新 RSSI + rf.update_rssi(rssi=-65, timestamp=time.time()) + + # 更新 AT 命令回應 + rf.update_at_response("OK", timestamp=time.time()) + + # 自定義狀態 + rf.status.custom_status['signal_quality'] = 'excellent' + rf.status.custom_status['packet_error_rate'] = 0.001 + + print(f"RF模組: {rf}") + print(f"Socket: {rf.socket_info.ip}:{rf.socket_info.port}") + print(f"RSSI: {rf.status.rssi} dBm") + print(f"AT回應: {rf.status.at_response}") + print(f"自定義狀態: {rf.status.custom_status}\n") + + +def example_multiple_components(): + """多組件範例""" + print("\n=== 多組件範例 ===\n") + + vehicle = VehicleView(sysid=5) + vehicle.kind = "Copter with Gimbal" + + # Autopilot 組件 + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 + autopilot.status.mode.mode_name = "AUTO" + + # Gimbal 組件 + gimbal = vehicle.add_component(154, ComponentType.GIMBAL) + gimbal.mav_type = 26 # MAV_TYPE_GIMBAL + gimbal.status.attitude.pitch = -0.785 # 向下45度 + gimbal.status.attitude.yaw = 0.0 + + # Camera 組件 + camera = vehicle.add_component(100, ComponentType.CAMERA) + camera.mav_type = 30 # MAV_TYPE_CAMERA + camera.status.custom_status['recording'] = True + camera.status.custom_status['photo_interval'] = 2.0 + + print(f"載具: {vehicle}") + print(f"組件數量: {len(vehicle.components)}") + for cid, comp in vehicle.components.items(): + print(f" 組件 {cid}: {comp.type.value}, MAV_TYPE={comp.mav_type}") + print() + + +def example_registry(): + """註冊表使用範例""" + print("\n=== 註冊表使用範例 ===\n") + + # 註冊多個載具 + v1 = vehicle_registry.register(sysid=1) + v1.kind = "Copter-1" + v1.add_component(1, ComponentType.AUTOPILOT) + + v2 = vehicle_registry.register(sysid=2) + v2.kind = "Plane-1" + v2.add_component(1, ComponentType.AUTOPILOT) + + v3 = vehicle_registry.register(sysid=3) + v3.kind = "Rover-1" + v3.add_component(1, ComponentType.AUTOPILOT) + + print(f"註冊表中的載具數量: {len(vehicle_registry)}") + + # 取得所有載具 + all_vehicles = vehicle_registry.get_all() + for sysid, vehicle in all_vehicles.items(): + print(f" System {sysid}: {vehicle.kind}") + + # 檢查載具是否存在 + print(f"\nSystem 2 存在? {2 in vehicle_registry}") + print(f"System 99 存在? {99 in vehicle_registry}") + + # 取得特定載具 + vehicle = vehicle_registry.get(2) + if vehicle: + print(f"\n取得載具: {vehicle}") + + # 註銷載具 + vehicle_registry.unregister(3) + print(f"\n註銷 System 3 後，剩餘載具: {len(vehicle_registry)}\n") + + +def example_serialization(): + """序列化範例 (除錯/日誌用)""" + print("\n=== 序列化範例 ===\n") + + vehicle = VehicleView(sysid=10) + vehicle.kind = "Test Copter" + vehicle.connected_via = ConnectionType.UDP + vehicle.custom_meta['firmware'] = 'ArduCopter 4.3.0' + vehicle.custom_meta['frame_type'] = 'X' + + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 + autopilot.status.position.altitude = 50.0 + autopilot.status.battery.voltage = 12.4 + autopilot.update_packet_stats(0, 0, time.time()) + autopilot.update_packet_stats(1, 30, time.time()) + + rf = vehicle.set_rf_module(RFModuleType.UDP) + rf.update_rssi(-70) + rf.update_socket_info(ip="192.168.1.200", port=14550, connected=True) + + # 轉換為字典 + data = vehicle.to_dict() + + print("載具資料 (字典格式):") + import json + print(json.dumps(data, indent=2, ensure_ascii=False)) + + +def example_gps_ekf(): + """GPS 與 EKF 範例""" + print("\n\n=== GPS 與 EKF 範例 ===\n") + + vehicle = VehicleView(sysid=11) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # GPS 資訊 + autopilot.status.gps.fix_type = 3 # 3D Fix + autopilot.status.gps.satellites_visible = 12 + autopilot.status.gps.eph = 120 # HDOP = 1.2 + autopilot.status.gps.epv = 180 # VDOP = 1.8 + autopilot.status.gps.timestamp = time.time() + + print(f"GPS:") + print(f" Fix Type: {autopilot.status.gps.fix_type}") + print(f" 衛星數: {autopilot.status.gps.satellites_visible}") + print(f" HDOP: {autopilot.status.gps.eph/100}") + + # EKF 資訊 + autopilot.status.ekf.flags = 0x1FF # 所有 flags 都 OK + autopilot.status.ekf.velocity_variance = 0.5 + autopilot.status.ekf.pos_horiz_variance = 1.2 + autopilot.status.ekf.pos_vert_variance = 2.0 + autopilot.status.ekf.timestamp = time.time() + + print(f"\nEKF:") + print(f" Flags: 0x{autopilot.status.ekf.flags:X}") + print(f" 速度變異: {autopilot.status.ekf.velocity_variance}") + print(f" 水平位置變異: {autopilot.status.ekf.pos_horiz_variance}") + print(f" 垂直位置變異: {autopilot.status.ekf.pos_vert_variance}\n") + + +def example_vfr_hud(): + """VFR HUD 範例""" + print("\n=== VFR HUD 範例 ===\n") + + vehicle = VehicleView(sysid=12) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # VFR HUD 資訊 + autopilot.status.vfr.airspeed = 15.5 # m/s + autopilot.status.vfr.groundspeed = 14.8 # m/s + autopilot.status.vfr.heading = 90 # 東方 + autopilot.status.vfr.throttle = 65 # % + autopilot.status.vfr.climb = 2.5 # m/s + autopilot.status.vfr.timestamp = time.time() + + print(f"VFR HUD:") + print(f" 空速: {autopilot.status.vfr.airspeed} m/s") + print(f" 地速: {autopilot.status.vfr.groundspeed} m/s") + print(f" 航向: {autopilot.status.vfr.heading}°") + print(f" 油門: {autopilot.status.vfr.throttle}%") + print(f" 爬升率: {autopilot.status.vfr.climb} m/s\n") + + +if __name__ == "__main__": + # 執行所有範例 + # example_basic_usage() + # example_packet_tracking() + # example_parameters() + # example_rf_module() + # example_multiple_components() + # example_registry() + # example_serialization() + # example_gps_ekf() + example_vfr_hud() + + print("\n" + "="*50) + print("所有範例執行完成！") + print("="*50) diff --git a/src/fc_network_adapter/tests/demo_ringBuffer.py b/src/fc_network_adapter/tests/demo_ringBuffer.py new file mode 100644 index 0000000..a01ed73 --- /dev/null +++ b/src/fc_network_adapter/tests/demo_ringBuffer.py @@ -0,0 +1,152 @@ +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + +import time +import threading + +from ..fc_network_adapter.utils import RingBuffer + + +def producer(buffer, count, interval=0.01): + """生產者：向緩衝區添加資料""" + print(f"Producer started (thread {threading.get_ident()})") + for i in range(count): + # 嘗試寫入數據，直到成功 + while not buffer.put(f"Item-{i}"): + print(f"Buffer full, producer waiting... (thread {threading.get_ident()})") + time.sleep(0.1) + + print(f"Produced: Item-{i}, buffer size: {buffer.size()}") + time.sleep(interval) # 模擬生產過程 + + print(f"Producer finished (thread {threading.get_ident()})") + +def consumer(buffer, max_items, interval=0.05): + """消費者：從緩衝區讀取資料""" + print(f"Consumer started (thread {threading.get_ident()})") + items_consumed = 0 + + while items_consumed < max_items: + # 嘗試讀取數據 + item = buffer.get() + if item: + print(f"Consumed: {item}, buffer size: {buffer.size()}") + items_consumed += 1 + else: + print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})") + + time.sleep(interval) # 模擬消費過程 + + print(f"Consumer finished (thread {threading.get_ident()})") + +def batch_consumer(buffer, interval=0.2): + """批量消費者：一次性讀取緩衝區中的所有資料""" + print(f"Batch consumer started (thread {threading.get_ident()})") + + for _ in range(5): # 執行5次批量讀取 + time.sleep(interval) # 等待緩衝區積累數據 + items = buffer.get_all() + if items: + print(f"Batch consumed {len(items)} items: {items}") + else: + print("Buffer empty for batch consumer") + + print(f"Batch consumer finished (thread {threading.get_ident()})") + +def demonstrate_multi_writer(): + """示範多個寫入執行緒同時操作緩衝區""" + print("\n=== Demonstrating Multiple Writers ===") + buffer = RingBuffer(capacity=80) + + # 創建多個生產者執行緒 + threads = [] + for i in range(3): + thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1))) + threads.append(thread) + thread.start() + + # 等待所有執行緒完成 + for thread in threads: + thread.join() + + buffer.print_stats() # 印出統計資訊 + + # 讀出所有剩餘資料 + remaining = buffer.get_all() + print(f"Remaining items in buffer after multiple writers: {remaining}") + +def demonstrate_basic_usage(): + """示範基本使用方式""" + print("\n=== Demonstrating Basic Usage ===") + # 創建緩衝區 + buffer = RingBuffer(capacity=20, buffer_id=7) + + # 檢查初始狀態 + print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 添加幾個項目 + for i in range(5): + buffer.put(f"Test-{i}") + + # 檢查狀態 + print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 讀取一個項目 + item = buffer.get() + print(f"Read item: {item}") + print(f"After reading 1 item - Content Size: {buffer.size()}") + + # 添加更多項目直到滿 + items_added = 0 + while not buffer.is_full(): + buffer.put(f"Fill-{items_added}") + items_added += 1 + + print(f"Added {items_added} more items until full") + print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 嘗試添加到已滿的緩衝區 + result = buffer.put("Overflow") + print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}") + + # 獲取所有項目 + all_items = buffer.get_all() + print(f"All items in buffer: {all_items}") + print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}") + + # 印出統計資訊 + buffer.print_stats() + +def demonstrate_producer_consumer(): + """示範生產者-消費者模式""" + print("\n=== Demonstrating Producer-Consumer Pattern ===") + buffer = RingBuffer(capacity=16) + + # 創建生產者和消費者執行緒 + producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1)) + consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2)) + batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5)) + + # 啟動執行緒 + producer_thread.start() + consumer_thread.start() + batch_thread.start() + + # 等待執行緒完成 + producer_thread.join() + consumer_thread.join() + batch_thread.join() + + # 檢查最終狀態 + print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}") + + buffer.print_stats() + +if __name__ == "__main__": + # 展示各種使用場景 + # demonstrate_basic_usage() + # demonstrate_producer_consumer() + demonstrate_multi_writer() + + print("\nAll demonstrations completed!") \ No newline at end of file diff --git a/src/fc_network_adapter/tests/test_mavlinkObject.py b/src/fc_network_adapter/tests/test_mavlinkObject.py new file mode 100644 index 0000000..9c3ca78 --- /dev/null +++ b/src/fc_network_adapter/tests/test_mavlinkObject.py @@ -0,0 +1,468 @@ +#!/usr/bin/env python +""" +測試腳本，用於測試 mavlinkObject.py 中的 mavlink_object 和 async_io_manager 類別 +""" + +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + + +import unittest +import time +import threading +import socket +import asyncio + +# 導入要測試的模組 +from ..fc_network_adapter.mavlinkObject import ( + mavlink_object, + async_io_manager, + MavlinkObjectState, + stream_bridge_ring, + return_packet_ring +) + +# 預先定義好的真實 MAVLink heartbeat 封包 (MAVLink 1.0 格式) +# Format: STX(0xFE) + LEN + SEQ + SYS + COMP + MSG_ID + PAYLOAD(9 bytes for heartbeat) + CRC(2 bytes) +HEARTBEAT_PACKET_1 = bytes([ + 0xFE, # STX (MAVLink 1.0) + 0x09, # payload length (9 bytes) + 0x00, # sequence + 0x01, # system ID = 1 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + # Payload (9 bytes): custom_mode(4), type(1), autopilot(1), base_mode(1), system_status(1), mavlink_version(1) + 0x00, 0x00, 0x00, 0x00, # custom_mode = 0 + 0x02, # type = MAV_TYPE_QUADROTOR (2) + 0x03, # autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA (3) + 0x40, # base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (64) + 0x03, # system_status = MAV_STATE_STANDBY (3) + 0x03, # mavlink_version = 3 + 0x62, 0x8E # CRC (simplified placeholder) +]) + +HEARTBEAT_PACKET_2 = bytes([ + 0xFE, # STX + 0x09, # payload length + 0x01, # sequence (增加) + 0x01, # system ID = 1 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + 0x00, 0x00, 0x00, 0x00, + 0x02, 0x03, 0x41, 0x03, 0x03, + 0x33, 0xEC +]) + +HEARTBEAT_PACKET_3 = bytes([ + 0xFE, # STX + 0x09, # payload length + 0x02, # sequence + 0x02, # system ID = 2 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + 0x00, 0x00, 0x00, 0x00, + 0x02, 0x03, 0x42, 0x03, 0x03, + 0x37, 0x44 +]) + + +class MockMavlinkSocket: + """模擬 Mavlink Socket 的類別，用於測試 + 使用真實的 MAVLink 封包，而不是模擬的訊息對象 + """ + + def __init__(self, test_packets=None): + """ + Args: + test_packets: list of bytes，每個元素都是完整的 MAVLink 封包 + """ + self.closed = False + self.test_packets = test_packets or [] + self.packet_index = 0 + self.written_data = [] + + # 使用 pymavlink 來解析封包 + from pymavlink import mavutil + self.mav_parser = mavutil.mavlink.MAVLink(self) + + def recv_msg(self): + """返回解析後的 MAVLink 訊息對象""" + if not self.test_packets or self.packet_index >= len(self.test_packets): + return None + + packet = self.test_packets[self.packet_index] + self.packet_index += 1 + + # 使用 pymavlink 解析封包 + try: + for byte in packet: + msg = self.mav_parser.parse_char(bytes([byte])) + if msg: + return msg + except Exception as e: + print(f"Error parsing packet: {e}") + return None + + return None + + def write(self, data): + """寫入數據（用於檢查轉發）""" + self.written_data.append(data) + + def close(self): + """關閉 socket""" + self.closed = True + + +class TestMavlinkObject(unittest.TestCase): + """測試 mavlink_object 類別的獨立功能""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建模擬的 socket，使用真實封包 + self.mock_socket = MockMavlinkSocket([HEARTBEAT_PACKET_1]) + + # 創建測試對象 + self.mavlink_obj = mavlink_object(self.mock_socket) + + def test_initialization(self): + """測試 mavlink_object 初始化是否正確""" + self.assertEqual(self.mavlink_obj.socket_id, 0) + self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT) + self.assertEqual(len(self.mavlink_obj.target_sockets), 0) + self.assertEqual(self.mavlink_obj.bridge_msg_types, [0]) + self.assertEqual(self.mavlink_obj.return_msg_types, []) + + def test_add_remove_target_socket(self): + """測試添加和移除目標端口功能""" + # 添加目標端口 + self.assertTrue(self.mavlink_obj.add_target_socket(1)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 1) + self.assertEqual(self.mavlink_obj.target_sockets[0], 1) + + self.assertTrue(self.mavlink_obj.add_target_socket(2)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + self.assertIn(2, self.mavlink_obj.target_sockets) + + # 嘗試添加已存在的端口 + self.assertFalse(self.mavlink_obj.add_target_socket(1)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + + # 嘗試添加自己的端口 + self.assertFalse(self.mavlink_obj.add_target_socket(0)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + + # 移除端口 + self.assertTrue(self.mavlink_obj.remove_target_socket(2)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 1) + + # 嘗試移除不存在的端口 + self.assertFalse(self.mavlink_obj.remove_target_socket(2)) + + def test_set_message_types(self): + """測試設置訊息類型功能""" + # 設置橋接器訊息類型 + self.assertTrue(self.mavlink_obj.set_bridge_message_types([0, 30])) + self.assertEqual(self.mavlink_obj.bridge_msg_types, [0, 30]) + + # 設置回傳處理器訊息類型 + self.assertTrue(self.mavlink_obj.set_return_message_types([32])) + self.assertEqual(self.mavlink_obj.return_msg_types, [32]) + + # 測試無效的訊息類型 + self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid")) + self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"])) + + def test_send_message_validation(self): + """測試 send_message 的數據驗證功能（不需要啟動 manager）""" + # 測試非運行狀態下發送消息 + self.assertFalse(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + + # 測試無效的數據類型 + self.mavlink_obj.state = MavlinkObjectState.RUNNING # 臨時設置狀態 + self.assertFalse(self.mavlink_obj.send_message("invalid")) + self.assertFalse(self.mavlink_obj.send_message(123)) + + # 測試太短的封包 + self.assertFalse(self.mavlink_obj.send_message(bytes([0xFE, 0x00]))) + + # 測試無效的起始標記 + invalid_packet = bytes([0xFF, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00]) + self.assertFalse(self.mavlink_obj.send_message(invalid_packet)) + + # 測試有效的封包可以加入佇列 + self.assertTrue(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(self.mavlink_obj.outgoing_msgs), 1) + + self.mavlink_obj.state = MavlinkObjectState.INIT # 恢復狀態 + +class TestAsyncIOManager(unittest.TestCase): + """測試 async_io_manager 類別的獨立功能""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建 async_io_manager 實例 + self.manager = async_io_manager() + + # 創建模擬 mavlink 對象，使用真實封包 + self.mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2]) + self.mock_socket2 = MockMavlinkSocket([HEARTBEAT_PACKET_3]) + + self.mavlink_obj1 = mavlink_object(self.mock_socket1) + self.mavlink_obj2 = mavlink_object(self.mock_socket2) + + def tearDown(self): + """在每個測試方法執行後清理環境""" + if self.manager.running: + self.manager.shutdown() + + def test_singleton_pattern(self): + """測試 async_io_manager 的單例模式""" + manager1 = async_io_manager() + manager2 = async_io_manager() + self.assertIs(manager1, manager2) + + def test_start_stop(self): + """測試 async_io_manager 的啟動和停止功能""" + # 啟動管理器 + self.manager.start() + self.assertTrue(self.manager.running) + self.assertIsNotNone(self.manager.thread) + + # 再次啟動應該沒有效果 + old_thread = self.manager.thread + self.manager.start() + self.assertIs(self.manager.thread, old_thread) + + # 停止管理器 + self.manager.shutdown() + self.assertFalse(self.manager.running) + + # 最多等待 5 秒讓線程結束 + start_time = time.time() + while self.manager.thread.is_alive() and time.time() - start_time < 5: + time.sleep(0.1) + self.assertFalse(self.manager.thread.is_alive()) + + def test_add_remove_objects(self): + """測試添加和移除 mavlink_object""" + # 啟動管理器 + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加對象 + self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj1)) + self.assertEqual(len(self.manager.managed_objects), 1) + self.assertEqual(self.mavlink_obj1.state, MavlinkObjectState.RUNNING) + + # 添加另一個對象 + self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj2)) + self.assertEqual(len(self.manager.managed_objects), 2) + + # 檢查受管理對象列表 + managed_objects = self.manager.get_managed_objects() + self.assertEqual(len(managed_objects), 2) + self.assertIn(0, managed_objects) + self.assertIn(1, managed_objects) + + # 移除對象 + self.assertTrue(self.manager.remove_mavlink_object(0)) + self.assertEqual(len(self.manager.managed_objects), 1) + + # 嘗試移除不存在的對象 + self.assertFalse(self.manager.remove_mavlink_object(999)) + + # 停止管理器 + self.manager.shutdown() + +class TestIntegration(unittest.TestCase): + """整合測試，測試多個 mavlink_object 之間的互動與資料流""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建 async_io_manager 實例 + self.manager = async_io_manager() + + def tearDown(self): + """在每個測試方法執行後清理環境""" + if self.manager.running: + self.manager.shutdown() + + def test_send_message_with_manager(self): + """測試透過 async_io_manager 發送訊息的完整流程""" + # 創建一個新的 mavlink_object 實例 + mock_socket = MockMavlinkSocket() + mavlink_obj = mavlink_object(mock_socket) + + # 測試初始狀態 + self.assertEqual(len(mock_socket.written_data), 0) + + # 測試非運行狀態下發送消息 + self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(mock_socket.written_data), 0) + + # 啟動 manager + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加對象到 manager + self.manager.add_mavlink_object(mavlink_obj) + time.sleep(0.1) # 等待對象啟動 + + # 確認對象狀態 + self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING) + + # 測試發送消息 + self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + time.sleep(0.2) # 等待消息處理 + + # 確認消息已發送 + self.assertEqual(len(mock_socket.written_data), 1) + self.assertEqual(mock_socket.written_data[0], HEARTBEAT_PACKET_1) + + # 測試連續發送多條消息 + self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_2)) + time.sleep(0.2) # 等待消息處理 + + # 確認兩條消息都已發送 + self.assertEqual(len(mock_socket.written_data), 2) + self.assertEqual(mock_socket.written_data[1], HEARTBEAT_PACKET_2) + + # 停止 manager + self.manager.shutdown() + time.sleep(0.5) # 等待 manager 停止 + + # 測試對象已關閉後發送消息 + self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加 + + def test_data_processing_and_forwarding(self): + """測試數據處理與轉發流程""" + # 創建用於轉發的 mavlink_objects + mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2,]) + mock_socket3 = MockMavlinkSocket() + + mavlink_obj1 = mavlink_object(mock_socket1) + mavlink_obj3 = mavlink_object(mock_socket3) + + # 設置訊息類型 + mavlink_obj1.set_bridge_message_types([0]) # 只處理 HEARTBEAT + + # 設置轉發: obj1 -> obj3 + mavlink_obj1.add_target_socket(mavlink_obj3.socket_id) # socket1 轉發到 socket3 (socket_id=1) + + # 啟動管理器並添加對象 + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + """ + 這邊出現很奇怪的狀況 應該說 設計時沒有考量 但是實測會發現 + mavlink_obj3 是接收端 必需要被優先加入 manager 才能正確接收來自 mavlink_obj1 的轉發封包 + 若先把 mavlink_ojb1 加入 manger 則可能會導致前面幾個封包丟失 + """ + self.manager.add_mavlink_object(mavlink_obj3) + self.manager.add_mavlink_object(mavlink_obj1) + + # 等待處理完成 + time.sleep(0.5) + + # 檢查 Ring buffer 是否有正確的數據 + self.assertGreaterEqual(stream_bridge_ring.size(), 2) # 至少 2 個 HEARTBEAT + + # 檢查是否正確轉發 + self.assertGreaterEqual(len(mock_socket3.written_data), 2) # 至少 2 個 HEARTBEAT + + # 停止管理器 + self.manager.shutdown() + + def test_bidirectional_forwarding(self): + """測試雙向轉發""" + # 清空全局變數和 ring buffer + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建三個 mavlink 對象，模擬三個通道 + socket1 = MockMavlinkSocket() + socket2 = MockMavlinkSocket() + socket3 = MockMavlinkSocket() + + obj1 = mavlink_object(socket1) + obj2 = mavlink_object(socket2) + obj3 = mavlink_object(socket3) + + # 設置雙向轉發 + # obj1 <-> obj2 <-> obj3 + obj1.add_target_socket(1) # obj1 -> obj2 + obj2.add_target_socket(0) # obj2 -> obj1 + obj2.add_target_socket(2) # obj2 -> obj3 + obj3.add_target_socket(1) # obj3 -> obj2 + + # 啟動 async_io_manager + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加所有 mavlink_object + self.manager.add_mavlink_object(obj1) + self.manager.add_mavlink_object(obj2) + self.manager.add_mavlink_object(obj3) + + # 對三個對象添加數據 + socket1.test_packets.append(HEARTBEAT_PACKET_1) + socket2.test_packets.append(HEARTBEAT_PACKET_2) + socket3.test_packets.append(HEARTBEAT_PACKET_3) + + # 等待處理所有訊息 + time.sleep(1.0) + + # 檢查轉發結果 + # socket1 應該收到 socket2 的訊息 + self.assertGreaterEqual(len(socket1.written_data), 1) + + # socket2 應該收到 socket1 和 socket3 的訊息 + self.assertGreaterEqual(len(socket2.written_data), 2) + + # socket3 應該收到 socket2 的訊息 + self.assertGreaterEqual(len(socket3.written_data), 1) + + # 檢查 ring buffer 的數據 + # 所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0] + self.assertGreaterEqual(stream_bridge_ring.size(), 3) # 至少 3 個 HEARTBEAT + + # 停止管理器 + self.manager.shutdown() + + +if __name__ == "__main__": + # 可以指定要運行的測試 + # unittest.main(defaultTest="TestMavlinkObject.test_send_message_validation") + # unittest.main(defaultTest="TestAsyncIOManager.test_add_remove_objects") + unittest.main(defaultTest="TestIntegration.test_bidirectional_forwarding") + unittest.main() + diff --git a/src/fc_network_adapter/tests/test_ringBuffer.py b/src/fc_network_adapter/tests/test_ringBuffer.py new file mode 100644 index 0000000..287a057 --- /dev/null +++ b/src/fc_network_adapter/tests/test_ringBuffer.py @@ -0,0 +1,296 @@ +#!/usr/bin/env python +""" +測試 RingBuffer 類別的功能 +""" + +import unittest +import threading +import time +from concurrent.futures import ThreadPoolExecutor + +from ..fc_network_adapter.utils import RingBuffer + +class TestRingBuffer(unittest.TestCase): + """測試 RingBuffer 基本功能""" + + def setUp(self): + """每個測試前的準備""" + self.buffer = RingBuffer(capacity=8) + + def test_initialization(self): + """測試初始化""" + self.assertEqual(self.buffer.capacity, 8) + self.assertTrue(self.buffer.is_empty()) + self.assertFalse(self.buffer.is_full()) + self.assertEqual(self.buffer.size(), 0) + + def test_put_get_basic(self): + """測試基本的放入和取出""" + # 測試放入 + self.assertTrue(self.buffer.put("item1")) + self.assertTrue(self.buffer.put("item2")) + self.assertEqual(self.buffer.size(), 2) + self.assertFalse(self.buffer.is_empty()) + + # 測試取出 + item1 = self.buffer.get() + self.assertEqual(item1, "item1") + self.assertEqual(self.buffer.size(), 1) + + item2 = self.buffer.get() + self.assertEqual(item2, "item2") + self.assertTrue(self.buffer.is_empty()) + + # 空緩衝區取出應返回 None + self.assertIsNone(self.buffer.get()) + + def test_capacity_overflow(self): + """測試緩衝區容量限制""" + # 填滿緩衝區 (容量-1，因為需要預留一個位置) + for i in range(7): # 8-1=7 + self.assertTrue(self.buffer.put(f"item{i}")) + + self.assertTrue(self.buffer.is_full()) + + # 嘗試再放入一個應該失敗 + self.assertFalse(self.buffer.put("overflow")) + self.assertEqual(self.buffer.overflow_count.value, 1) + + def test_get_all(self): + """測試取出所有項目""" + items = ["a", "b", "c", "d"] + for item in items: + self.buffer.put(item) + + all_items = self.buffer.get_all() + self.assertEqual(all_items, items) + self.assertTrue(self.buffer.is_empty()) + + def test_clear(self): + """測試清空緩衝區""" + for i in range(5): + self.buffer.put(f"item{i}") + + self.buffer.clear() + self.assertTrue(self.buffer.is_empty()) + self.assertEqual(self.buffer.size(), 0) + + +class TestRingBufferThreadSafety(unittest.TestCase): + """測試 RingBuffer 的線程安全性""" + + def setUp(self): + """每個測試前的準備""" + self.buffer = RingBuffer(capacity=256) + self.results = [] + self.write_count = 1000 + self.read_count = 1000 + + def test_concurrent_producers_consumers(self): + """測試多生產者多消費者場景""" + self.results = [] + stats = self.buffer.get_stats() + self.assertEqual(stats['total_writes'], 0) + + def producer(producer_id, count): + """生產者函數""" + for i in range(count): + item = f"producer_{producer_id}_item_{i}" + while not self.buffer.put(item): + time.sleep(0.001) # 緩衝區滿時稍微等待 + + def consumer(consumer_id, count): + """消費者函數""" + items = [] + for _ in range(count): + item = None + while item is None: + item = self.buffer.get() + if item is None: + time.sleep(0.001) # 緩衝區空時稍微等待 + items.append(item) + self.results.extend(items) + + # 創建多個生產者和消費者 + with ThreadPoolExecutor(max_workers=8) as executor: + # 2 個生產者，每個寫入 500 個項目 + producer_futures = [ + executor.submit(producer, 0, 500), + executor.submit(producer, 1, 500) + ] + + # 2 個消費者，每個讀取 500 個項目 + consumer_futures = [ + executor.submit(consumer, 0, 500), + executor.submit(consumer, 1, 500) + ] + + # 等待所有任務完成 + for future in producer_futures + consumer_futures: + future.result() + + # 驗證結果 + self.assertEqual(len(self.results), 1000) + self.assertTrue(self.buffer.is_empty()) + + # 檢查統計數據 + stats = self.buffer.get_stats() + self.assertEqual(stats['total_writes'], 1000) + self.assertGreater(stats['total_reads'], 1000) # 包含失敗的讀取嘗試 + self.assertGreater(stats['write_threads'], 1) + self.assertGreater(stats['read_threads'], 1) + + def test_high_throughput(self): + """測試高吞吐量場景""" + items_per_thread = 10000 + num_threads = 4 + + def writer(): + for i in range(items_per_thread): + while not self.buffer.put(i): + pass # 忙等待 + + def reader(): + items = [] + for _ in range(items_per_thread): + item = None + while item is None: + item = self.buffer.get() + items.append(item) + self.results.extend(items) + + start_time = time.time() + + with ThreadPoolExecutor(max_workers=num_threads * 2) as executor: + # 啟動寫入線程 + writer_futures = [executor.submit(writer) for _ in range(num_threads)] + + # 啟動讀取線程 + reader_futures = [executor.submit(reader) for _ in range(num_threads)] + + # 等待完成 + for future in writer_futures + reader_futures: + future.result() + + end_time = time.time() + + # 驗證結果 + total_items = items_per_thread * num_threads + self.assertEqual(len(self.results), total_items) + + # 性能統計 + duration = end_time - start_time + throughput = total_items / duration + + print(f"\nHigh Throughput Test Results:") + print(f"Total items: {total_items}") + print(f"Duration: {duration:.2f}s") + print(f"Throughput: {throughput:.0f} items/sec") + + # 顯示詳細統計 + self.buffer.print_stats() + + +class TestRingBufferStatistics(unittest.TestCase): + """測試 RingBuffer 的統計功能""" + + def test_statistics_tracking(self): + """測試統計數據追蹤""" + buffer = RingBuffer(capacity=16) + + # 寫入一些數據 + for i in range(10): + buffer.put(f"item{i}") + + # 讀取一些數據 + for _ in range(5): + buffer.get() + + stats = buffer.get_stats() + + # 驗證基本統計 + self.assertEqual(stats['total_writes'], 10) + self.assertEqual(stats['total_reads'], 5) + self.assertEqual(stats['current_size'], 5) + self.assertEqual(stats['write_threads'], 1) + self.assertEqual(stats['read_threads'], 1) + + def test_reset_statistics(self): + """測試重置統計數據""" + buffer = RingBuffer(capacity=16) + + # 產生一些活動 + for i in range(5): + buffer.put(f"item{i}") + for _ in range(3): + buffer.get() + + # 重置統計 + buffer.reset_stats() + + stats = buffer.get_stats() + self.assertEqual(stats['total_writes'], 0) + self.assertEqual(stats['total_reads'], 0) + self.assertEqual(stats['concurrent_writes'], 0) + self.assertEqual(stats['concurrent_reads'], 0) + self.assertEqual(stats['overflow_count'], 0) + + +def benchmark_ringbuffer(): + """RingBuffer 性能基準測試""" + print("\n=== RingBuffer Performance Benchmark ===") + + buffer = RingBuffer(capacity=1024) + num_operations = 100000 + + # 單線程性能測試 + start_time = time.time() + for i in range(num_operations): + buffer.put(i) + for _ in range(num_operations): + buffer.get() + end_time = time.time() + + single_thread_time = end_time - start_time + throughput = (num_operations * 2) / single_thread_time + + print(f"Single Thread: {throughput:.0f} ops/sec") + + # 多線程性能測試 + buffer = RingBuffer(capacity=1024) + + def producer(): + for i in range(num_operations // 2): + while not buffer.put(i): + pass + + def consumer(): + for _ in range(num_operations // 2): + while buffer.get() is None: + pass + + start_time = time.time() + + with ThreadPoolExecutor(max_workers=2) as executor: + future1 = executor.submit(producer) + future2 = executor.submit(consumer) + future1.result() + future2.result() + + end_time = time.time() + + multi_thread_time = end_time - start_time + throughput = num_operations / multi_thread_time + + print(f"Multi Thread: {throughput:.0f} ops/sec") + print(f"Speedup: {single_thread_time/multi_thread_time:.2f}x") + + buffer.print_stats() + + +if __name__ == "__main__": + # 運行單元測試 + unittest.main(argv=[''], exit=False, verbosity=2) + + # 運行性能基準測試 + benchmark_ringbuffer() \ No newline at end of file diff --git a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py new file mode 100644 index 0000000..6fd1914 --- /dev/null +++ b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py @@ -0,0 +1,507 @@ +""" +VehicleStatusPublisher 測試程式 + +測試從 vehicle_registry 讀取資料並發布到 ROS2 topics +""" + +import time +import json +import threading + +# ROS2 imports +import rclpy +from rclpy.node import Node + +# 標準 ROS2 消息類型 +import std_msgs.msg +import sensor_msgs.msg +import geometry_msgs.msg +import mavros_msgs.msg + +# 專案 imports +from ..fc_network_adapter.mavlinkROS2Nodes import ( + VehicleStatusPublisher, + fc_ros_manager, + ros2_manager +) +from ..fc_network_adapter.mavlinkVehicleView import ( + vehicle_registry, + ConnectionType, + ComponentType, +) + + +class TestSubscriber(Node): + """測試用的訂閱者節點 - 接收並記錄收到的消息""" + + def __init__(self, sysid: int = 1): + super().__init__(f'test_subscriber_sys{sysid}') + + self.sysid = sysid + self.received_messages = { + 'position': [], + 'attitude': [], + 'velocity': [], + 'battery': [], + 'vfr_hud': [], + 'mode': [], + 'summary': [], + } + + # 建立所有訂閱者 + self._create_subscriptions() + + print(f"[TestSubscriber] 已建立，監聽 sys{sysid} 的所有 topics") + + def _create_subscriptions(self): + """建立所有 topic 的訂閱者""" + + base_topic = f'{VehicleStatusPublisher.topicString_prefix}/sys{self.sysid}' + + # Position + self.create_subscription( + sensor_msgs.msg.NavSatFix, + f'{base_topic}/position', + lambda msg: self._on_message('position', msg), + 10 + ) + + # Attitude + self.create_subscription( + sensor_msgs.msg.Imu, + f'{base_topic}/attitude', + lambda msg: self._on_message('attitude', msg), + 10 + ) + + # Velocity + self.create_subscription( + geometry_msgs.msg.TwistStamped, + f'{base_topic}/velocity', + lambda msg: self._on_message('velocity', msg), + 10 + ) + + # Battery + self.create_subscription( + sensor_msgs.msg.BatteryState, + f'{base_topic}/battery', + lambda msg: self._on_message('battery', msg), + 10 + ) + + # VFR HUD + self.create_subscription( + mavros_msgs.msg.VfrHud, + f'{base_topic}/vfr_hud', + lambda msg: self._on_message('vfr_hud', msg), + 10 + ) + + # Mode + self.create_subscription( + std_msgs.msg.String, + f'{base_topic}/mode', + lambda msg: self._on_message('mode', msg), + 10 + ) + + # Summary + self.create_subscription( + std_msgs.msg.String, + f'{base_topic}/summary', + lambda msg: self._on_message('summary', msg), + 10 + ) + + def _on_message(self, topic_name: str, msg): + """通用消息接收回調""" + self.received_messages[topic_name].append(msg) + print(f"[TestSubscriber] 收到 {topic_name}: {self._format_msg(topic_name, msg)}") + + def _format_msg(self, topic_name: str, msg) -> str: + """格式化消息以便顯示""" + if topic_name == 'position': + return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m" + elif topic_name == 'attitude': + return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})" + elif topic_name == 'velocity': + return f"linear=({msg.twist.linear.x:.2f}, {msg.twist.linear.y:.2f}, {msg.twist.linear.z:.2f})" + elif topic_name == 'battery': + return f"voltage={msg.voltage:.2f}V, percent={msg.percentage*100:.1f}%" + elif topic_name == 'vfr_hud': + return f"airspeed={msg.airspeed:.2f}, groundspeed={msg.groundspeed:.2f}, heading={msg.heading}" + elif topic_name == 'mode': + return f"mode={msg.data}" + elif topic_name == 'summary': + try: + data = json.loads(msg.data) + return f"sysid={data['sysid']}, socket_id={data['socket_id']}, mode={data['mode_name']}" + except: + return msg.data + return str(msg) + + def get_message_count(self, topic_name: str) -> int: + """獲取收到的消息數量""" + return len(self.received_messages[topic_name]) + + def clear_messages(self): + """清空已收到的消息""" + for key in self.received_messages: + self.received_messages[key].clear() + + +def setup_test_vehicle(sysid: int = 1, socket_id: int = 10): + """ + 建立測試用的載具數據 + + Args: + sysid: 系統 ID + socket_id: Socket ID + """ + print(f"\n=== 建立測試載具 (sysid={sysid}, socket_id={socket_id}) ===") + + # 註冊載具 + vehicle = vehicle_registry.register(sysid) + vehicle.kind = "Copter" + vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR + vehicle.connected_via = ConnectionType.UDP + vehicle.custom_meta['socket_id'] = socket_id + + # 新增 autopilot 組件 (component_id=1) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR + autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA + + # 填充狀態資料 + status = autopilot.status + + # 位置 + status.position.latitude = 25.0330 + status.position.longitude = 121.5654 + status.position.altitude = 100.5 + status.position.relative_altitude = 50.0 + status.position.timestamp = time.time() + + # 姿態 + status.attitude.roll = 0.1 + status.attitude.pitch = -0.05 + status.attitude.yaw = 1.57 + status.attitude.rollspeed = 0.01 + status.attitude.pitchspeed = 0.02 + status.attitude.yawspeed = 0.03 + status.attitude.timestamp = time.time() + + # 飛行模式 + status.mode.base_mode = 89 + status.mode.custom_mode = 4 + status.mode.mode_name = "GUIDED" + status.mode.timestamp = time.time() + + # 電池 + status.battery.voltage = 12.6 + status.battery.current = 15.3 + status.battery.remaining = 75 + status.battery.temperature = 35.2 + status.battery.timestamp = time.time() + + # GPS + status.gps.fix_type = 3 # 3D fix + status.gps.satellites_visible = 12 + status.gps.eph = 100 + status.gps.epv = 150 + status.gps.timestamp = time.time() + + # VFR + status.vfr.airspeed = 5.5 + status.vfr.groundspeed = 6.0 + status.vfr.heading = 90 + status.vfr.throttle = 65 + status.vfr.climb = 1.2 + status.vfr.timestamp = time.time() + + # 系統狀態 + status.armed = True + status.system_status = 4 # MAV_STATE_ACTIVE + + # 更新封包統計 + autopilot.update_packet_stats(seq=10, msg_type=33, timestamp=time.time()) + + print(f"✓ 載具 {sysid} 已建立並填充測試數據") + return vehicle + + +def test_basic_publishing(): + """測試基本的發布功能""" + print("\n" + "="*60) + print("測試 1: 基本發布功能") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立測試載具 + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + # 初始化 ROS2 管理器 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 開始發布，等待 5 秒 ---") + + # 運行 5 秒，持續 spin + start_time = time.time() + while time.time() - start_time < 5.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + # 檢查收到的消息 + print("\n--- 消息統計 ---") + total_messages = 0 + for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']: + count = test_sub.get_message_count(topic) + total_messages += count + print(f" {topic:15s}: {count:3d} 條消息") + + print(f"\n總計收到: {total_messages} 條消息") + + # 驗證 + if total_messages > 0: + print("\n✓ 測試通過：成功接收到消息") + else: + print("\n✗ 測試失敗：沒有接收到任何消息") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def test_frequency_control(): + """測試頻率控制功能""" + print("\n" + "="*60) + print("測試 2: 頻率控制") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立測試載具 + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + # 初始化（如果還沒初始化） + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 修改頻率設定 + publisher_node = ros2_manager.status_publisher + publisher_node.rate_controller.topic_intervals = { + 'position': 1.5, + 'attitude': 1.0, + 'velocity': 1.0, + 'battery': 1.0, + 'vfr_hud': 0.5, + 'mode': 0.0, + 'summary': 0.0, + } + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 測試頻率控制，運行 3 秒 ---") + + # 運行 3 秒 + start_time = time.time() + while time.time() - start_time < 3.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + # 檢查頻率 + print("\n--- 頻率分析 ---") + print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布") + + print("2Hz Topics (預期 ~6 條):") + for topic in ['position', 'attitude', 'velocity', 'vfr_hud']: + count = test_sub.get_message_count(topic) + print(f" {topic:15s}: {count:3d} 條") + for topic in ['battery', 'mode', 'summary']: + count = test_sub.get_message_count(topic) + print(f" {topic:15s}: {count:3d} 條") + + print("\n✓ 頻率控制測試完成") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def test_multi_vehicle(): + """測試多載具發布""" + print("\n" + "="*60) + print("測試 3: 多載具發布") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立 3 個測試載具 + v1 = setup_test_vehicle(sysid=1, socket_id=10) + v2 = setup_test_vehicle(sysid=2, socket_id=11) + v3 = setup_test_vehicle(sysid=3, socket_id=12) + + # 修改各載具的位置以便區分 + v2.components[1].status.position.latitude = 26.0 + v3.components[1].status.position.latitude = 27.0 + + # 初始化 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立 3 個測試訂閱者 + test_sub1 = TestSubscriber(sysid=1) + test_sub2 = TestSubscriber(sysid=2) + test_sub3 = TestSubscriber(sysid=3) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 測試多載具，運行 3 秒 ---") + + # 運行 3 秒 + start_time = time.time() + while time.time() - start_time < 3.0: + rclpy.spin_once(test_sub1, timeout_sec=0.05) + rclpy.spin_once(test_sub2, timeout_sec=0.05) + rclpy.spin_once(test_sub3, timeout_sec=0.05) + time.sleep(0.1) + + # 檢查每個載具的消息 + print("\n--- 各載具消息統計 ---") + for sysid, test_sub in [(1, test_sub1), (2, test_sub2), (3, test_sub3)]: + total = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"載具 {sysid}: {total:3d} 條消息") + + # 檢查 summary 中的 socket_id + if test_sub.get_message_count('summary') > 0: + last_summary = test_sub.received_messages['summary'][-1] + data = json.loads(last_summary.data) + print(f" └─ socket_id={data['socket_id']}, lat={data['latitude']:.1f}") + + print("\n✓ 多載具測試完成") + + # 停止 + ros2_manager.stop() + test_sub1.destroy_node() + test_sub2.destroy_node() + test_sub3.destroy_node() + + +def test_dynamic_vehicle(): + """測試動態新增/移除載具""" + print("\n" + "="*60) + print("測試 4: 動態載具管理") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 初始化 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 階段 1: 無載具，運行 1 秒 ---") + start_time = time.time() + while time.time() - start_time < 1.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_before = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_before} 條") + + # 新增載具 + print("\n--- 階段 2: 新增載具，運行 2 秒 ---") + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + start_time = time.time() + while time.time() - start_time < 2.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_after = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_after - count_before} 條") + + # 移除載具 + print("\n--- 階段 3: 移除載具，運行 1 秒 ---") + vehicle_registry.unregister(1) + + start_time = time.time() + while time.time() - start_time < 1.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_final = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_final - count_after} 條（應該為 0）") + + if count_final - count_after == 0: + print("\n✓ 動態載具管理測試通過") + else: + print("\n✗ 移除載具後仍收到消息") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def main(): + """主測試函數""" + print("\n" + "="*60) + print("VehicleStatusPublisher 測試程式") + print("="*60) + + try: + # 執行各項測試 + test_basic_publishing() + # time.sleep(1) + + # test_frequency_control() + # time.sleep(1) + + # test_multi_vehicle() + # time.sleep(1) + + # test_dynamic_vehicle() + + print("\n" + "="*60) + print("所有測試完成！") + print("="*60) + + except KeyboardInterrupt: + print("\n\n測試被中斷") + except Exception as e: + print(f"\n\n測試出錯: {e}") + import traceback + traceback.print_exc() + finally: + # 清理 + if ros2_manager.initialized: + ros2_manager.shutdown() + vehicle_registry.clear() + print("\n清理完成") + + +if __name__ == '__main__': + main() From 44d63979b52e401485c5334b57e07d917efb0231 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 2 Feb 2026 03:00:49 +0800 Subject: [PATCH 26/33] ROS2 sub --- src/GUI/communication.py | 97 +++++++++++++++++++++------------------- 1 file changed, 50 insertions(+), 47 deletions(-) diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 45c960c..74add17 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -11,7 +11,7 @@ import socket from pymavlink import mavutil from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import BatteryState, NavSatFix, Imu -from std_msgs.msg import Float64 +from std_msgs.msg import Float64, String from mavros_msgs.msg import State, VfrHud from mavros_msgs.srv import CommandBool, CommandTOL @@ -371,6 +371,7 @@ class WebSocketMavlinkReceiver(threading.Thread): self.running = False class DroneMonitor(Node): + # Subscribe to drone ROS2 topics def __init__(self): super().__init__('drone_monitor') self.signals = DroneSignals() @@ -403,12 +404,15 @@ class DroneMonitor(Node): def scan_topics(self): topics = self.get_topic_names_and_types() - drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') + drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)') found_drones = set() for topic_name, _ in topics: if match := drone_pattern.match(topic_name): - drone_id, topic_type = match.groups() + sys_id, topic_type = match.groups() + # 將 sys11 轉換為 s0_11 格式以保持兼容性 + drone_num = sys_id.replace('sys', '') + drone_id = f's0_{drone_num}' found_drones.add(drone_id) with self.lock: self.drone_topics.setdefault(drone_id, set()).add(topic_type) @@ -418,9 +422,11 @@ class DroneMonitor(Node): self.setup_drone(drone_id) def setup_drone(self, drone_id): - base_topic = f'/MavLinkBus/{drone_id}' + # 從 s0_11 轉換回 sys11 格式 + sys_id = drone_id.replace('s0_', 'sys') + base_topic = f'/fc_network/vehicle/{sys_id}' - # Add service clients + # Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務) self.arm_clients[drone_id] = self.create_client( CommandBool, f'{base_topic}/cmd/arming' @@ -438,58 +444,22 @@ class DroneMonitor(Node): ) subs = { - 'attitude': self.create_subscription( - Imu, - f'{base_topic}/attitude', - lambda msg, did=drone_id: self.attitude_callback(did, msg), - 10 - ), 'battery': self.create_subscription( BatteryState, f'{base_topic}/battery', lambda msg, did=drone_id: self.battery_callback(did, msg), 10 ), - 'global': self.create_subscription( + 'position': self.create_subscription( NavSatFix, - f'{base_topic}/global_position/global', + f'{base_topic}/position', lambda msg, did=drone_id: self.gps_callback(did, msg), 10 ), - 'rel_alt': self.create_subscription( - Float64, - f'{base_topic}/global_position/rel_alt', - lambda msg, did=drone_id: self.altitude_callback(did, msg), - 10 - ), - 'local_pose': self.create_subscription( - Point, - f'{base_topic}/local_position/pose', - lambda msg, did=drone_id: self.local_pose_callback(did, msg), - 10 - ), - 'local_vel': self.create_subscription( - Vector3, - f'{base_topic}/local_position/velocity', - lambda msg, did=drone_id: self.local_vel_callback(did, msg), - 10 - ), - 'loss_rate': self.create_subscription( - Float64, - f'{base_topic}/packet_loss_rate', - lambda msg, did=drone_id: self.loss_rate_callback(did, msg), - 10 - ), - 'state': self.create_subscription( - State, - f'{base_topic}/state', - lambda msg, did=drone_id: self.state_callback(did, msg), - 10 - ), - 'ping': self.create_subscription( - Float64, - f'{base_topic}/ping', - lambda msg, did=drone_id: self.ping_callback(did, msg), + 'summary': self.create_subscription( + String, + f'{base_topic}/summary', + lambda msg, did=drone_id: self.summary_callback(did, msg), 10 ), 'vfr_hud': self.create_subscription( @@ -596,6 +566,39 @@ class DroneMonitor(Node): 'armed': msg.armed } + def summary_callback(self, drone_id, msg): + """處理 summary topic (JSON 格式)""" + try: + data = json.loads(msg.data) + mode = data.get('mode_name', 'UNKNOWN') + if mode in self.filtered_modes: + return + + # 根據 socket_id 更新 drone_id + socket_id = data.get('socket_id') + sysid = data.get('sysid') + if socket_id is not None and sysid is not None: + # 使用 socket_id 作為前綴 + actual_drone_id = f's{socket_id}_{sysid}' + else: + actual_drone_id = drone_id + + self.latest_data[(actual_drone_id, 'state')] = { + 'mode': mode, + 'armed': data.get('armed', False), + 'socket_id': socket_id, + 'sysid': sysid, + 'vehicle_type': data.get('vehicle_type'), + 'autopilot': data.get('autopilot'), + 'gps_fix': data.get('gps_fix'), + 'gps_fix_type': data.get('gps_fix'), + 'connected': data.get('connected') + } + except json.JSONDecodeError as e: + print(f"Error parsing summary JSON for {drone_id}: {e}") + except Exception as e: + print(f"Error in summary_callback for {drone_id}: {e}") + def gps_callback(self, drone_id, msg): self.latest_data[(drone_id, 'gps')] = { 'lat': msg.latitude, From 8fdbbbc5dcf4778330916f400bf65a9e27c2a731 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 4 Feb 2026 03:46:31 +0800 Subject: [PATCH 27/33] update gui --- src/GUI/comm_panel.py | 294 +++++++++++++++++++++++++++++++++++++- src/GUI/communication.py | 41 +++++- src/GUI/gui.py | 136 +++++++++++++++--- src/GUI/map_layout.py | 18 ++- src/GUI/overview_table.py | 33 ++++- 5 files changed, 491 insertions(+), 31 deletions(-) diff --git a/src/GUI/comm_panel.py b/src/GUI/comm_panel.py index 82b539e..bd12794 100644 --- a/src/GUI/comm_panel.py +++ b/src/GUI/comm_panel.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, - QPushButton, QLineEdit) + QPushButton, QLineEdit, QComboBox) from PyQt6.QtCore import pyqtSignal +import glob +import os class CommPanel(QWidget): """通讯设置面板类""" @@ -13,12 +15,16 @@ class CommPanel(QWidget): ws_connection_added = pyqtSignal(str) # url ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + serial_connection_added = pyqtSignal(str, int) # port, baudrate + serial_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + serial_connection_removed = pyqtSignal(dict, QWidget) # conn, panel status_message = pyqtSignal(str, int) # message, timeout def __init__(self, parent=None): super().__init__(parent) self.udp_connections = [] self.ws_connections = [] + self.serial_connections = [] self._init_ui() def _init_ui(self): @@ -105,6 +111,118 @@ class CommPanel(QWidget): separator.setFixedHeight(20) layout.addWidget(separator) + # ========== Serial 區域 ========== + serial_title = QLabel("Serial") + serial_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(serial_title) + + # Serial 連接列表容器 + self.serial_list_widget = QWidget() + self.serial_list_layout = QVBoxLayout(self.serial_list_widget) + self.serial_list_layout.setContentsMargins(0, 0, 0, 0) + self.serial_list_layout.setSpacing(5) + layout.addWidget(self.serial_list_widget) + + # Serial 添加新連接區域 + add_serial_widget = QWidget() + add_serial_layout = QHBoxLayout(add_serial_widget) + add_serial_layout.setContentsMargins(0, 0, 0, 0) + + self.serial_port_combo = QComboBox() + self.serial_port_combo.setStyleSheet(""" + QComboBox { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + QComboBox::drop-down { + border: none; + } + QComboBox::down-arrow { + image: none; + border-left: 5px solid transparent; + border-right: 5px solid transparent; + border-top: 5px solid #DDD; + } + """) + self._refresh_serial_ports() + + refresh_ports_btn = QPushButton("↻") + refresh_ports_btn.setFixedWidth(35) + refresh_ports_btn.clicked.connect(self._refresh_serial_ports) + refresh_ports_btn.setToolTip("重新掃描串口") + refresh_ports_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 6px; + border-radius: 4px; + font-size: 16px; + } + QPushButton:hover { background-color: #555; } + """) + + self.serial_baudrate_combo = QComboBox() + self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200']) + self.serial_baudrate_combo.setCurrentText('57600') + self.serial_baudrate_combo.setFixedWidth(100) + self.serial_baudrate_combo.setStyleSheet(""" + QComboBox { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + QComboBox::drop-down { + border: none; + } + QComboBox::down-arrow { + image: none; + border-left: 5px solid transparent; + border-right: 5px solid transparent; + border-top: 5px solid #DDD; + } + """) + + add_serial_btn = QPushButton("添加") + add_serial_btn.clicked.connect(self._handle_add_serial) + add_serial_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_serial_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_serial_layout.addWidget(self.serial_port_combo) + add_serial_layout.addWidget(refresh_ports_btn) + add_serial_layout.addWidget(QLabel("Baud:", styleSheet="color: #DDD;")) + add_serial_layout.addWidget(self.serial_baudrate_combo) + add_serial_layout.addWidget(add_serial_btn) + + layout.addWidget(add_serial_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + layout.addWidget(separator) + # ========== WebSocket 區域 ========== ws_title = QLabel("WebSocket") ws_title.setStyleSheet(""" @@ -229,6 +347,94 @@ class CommPanel(QWidget): # 清空輸入框 self.ws_url_input.clear() + def _refresh_serial_ports(self): + """重新掃描可用的串口""" + self.serial_port_combo.clear() + + # 掃描 Linux 下的串口設備 + ports = [] + + # 掃描 USB 串口 + usb_ports = glob.glob('/dev/ttyUSB*') + ports.extend(usb_ports) + + # 掃描 ACM 串口 + acm_ports = glob.glob('/dev/ttyACM*') + ports.extend(acm_ports) + + # 排序 + ports.sort() + + if ports: + self.serial_port_combo.addItems(ports) + else: + self.serial_port_combo.addItem("沒有找到串口") + self.serial_port_combo.setEnabled(False) + return + + self.serial_port_combo.setEnabled(True) + + def _handle_add_serial(self): + """處理添加 Serial 連接""" + port = self.serial_port_combo.currentText() + baudrate_text = self.serial_baudrate_combo.currentText() + + if not port or port == "沒有找到串口": + self.status_message.emit("請選擇有效的串口", 3000) + return + + try: + baudrate = int(baudrate_text) + except ValueError: + self.status_message.emit("波特率格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.serial_connections: + if conn['port'] == port: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.serial_connection_added.emit(port, baudrate) + + def _handle_add_ws(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.status_message.emit("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴 + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.status_message.emit("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.ws_connection_added.emit(url) + + # 清空輸入框 + self.ws_url_input.clear() + def create_udp_connection_panel(self, conn): """創建 UDP 連接面板""" panel = QWidget() @@ -395,4 +601,88 @@ class CommPanel(QWidget): def remove_ws_connection_from_list(self, conn): """從列表中移除 WebSocket 連接""" if conn in self.ws_connections: - self.ws_connections.remove(conn) \ No newline at end of file + self.ws_connections.remove(conn) + + def create_serial_connection_panel(self, conn): + """創建 Serial 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.serial_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def add_serial_panel(self, conn): + """添加 Serial 連接面板到列表""" + panel = self.create_serial_connection_panel(conn) + self.serial_list_layout.addWidget(panel) + self.serial_connections.append(conn) + return panel + + def remove_serial_connection_from_list(self, conn): + """從列表中移除 Serial 連接""" + if conn in self.serial_connections: + self.serial_connections.remove(conn) \ No newline at end of file diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 74add17..c95e74e 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -397,6 +397,12 @@ class DroneMonitor(Node): # ================================================================================ self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}} # ================================================================================ + + # ================================================================================ + # 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取) + # ================================================================================ + self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'} + # ================================================================================ self.serial_receivers = [] # 主题检测定时器 @@ -553,7 +559,13 @@ class DroneMonitor(Node): } def battery_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'battery')] = { + # 使用映射獲取實際的 drone_id + actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + # 如果還沒有從 summary 獲取到映射，則不處理 + if actual_drone_id is None: + return + + self.latest_data[(actual_drone_id, 'battery')] = { 'voltage': msg.voltage } @@ -580,6 +592,15 @@ class DroneMonitor(Node): if socket_id is not None and sysid is not None: # 使用 socket_id 作為前綴 actual_drone_id = f's{socket_id}_{sysid}' + + # ================================================================================ + # 【關鍵】保存 sys_id 到 actual_drone_id 的映射 + # ================================================================================ + sys_key = f'sys{sysid}' + self.sys_to_actual_id[sys_key] = actual_drone_id + # 也保存原始的 s0_ 格式到實際 ID 的映射 + self.sys_to_actual_id[drone_id] = actual_drone_id + # ================================================================================ else: actual_drone_id = drone_id @@ -600,7 +621,13 @@ class DroneMonitor(Node): print(f"Error in summary_callback for {drone_id}: {e}") def gps_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'gps')] = { + # 使用映射獲取實際的 drone_id + actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + # 如果還沒有從 summary 獲取到映射，則不處理 + if actual_drone_id is None: + return + + self.latest_data[(actual_drone_id, 'gps')] = { 'lat': msg.latitude, 'lon': msg.longitude, 'alt': msg.altitude @@ -609,7 +636,7 @@ class DroneMonitor(Node): # ================================================================================ # 【新增】儲存 GPS 資料到 drone_gps 字典 # ================================================================================ - self.drone_gps[drone_id] = { + self.drone_gps[actual_drone_id] = { 'lat': msg.latitude, 'lon': msg.longitude, 'alt': msg.altitude @@ -636,7 +663,13 @@ class DroneMonitor(Node): } def hud_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'hud')] = { + # 使用映射獲取實際的 drone_id + actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + # 如果還沒有從 summary 獲取到映射，則不處理 + if actual_drone_id is None: + return + + self.latest_data[(actual_drone_id, 'hud')] = { 'airspeed': msg.airspeed, 'groundspeed': msg.groundspeed, 'heading': msg.heading, diff --git a/src/GUI/gui.py b/src/GUI/gui.py index cefc9c9..1e0c5ab 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -44,25 +44,6 @@ class ControlStationUI(QMainWindow): # 初始化UI self.drones = {} self.socket_groups = {} - self.info_types = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "Local", "Velocity", "地速", "航向", - "Roll", "Pitch", "Yaw", "丟包", "延遲"] - self.info_type_map = { - "mode": 0, - "armed": 1, - "battery": 2, - "longitude": 3, - "latitude": 4, - "altitude": 5, - "local": 6, - "velocity": 7, - "groundspeed": 8, - "heading": 9, - "roll": 10, - "pitch": 11, - "yaw": 12, - "loss_rate": 13, - "ping": 14 - } self.socket_colors = { '0': '#00BFFF', # 天藍色 (DeepSkyBlue) @@ -86,7 +67,8 @@ class ControlStationUI(QMainWindow): self.udp_receivers = [] self.udp_connections = [] self.ws_connections = [] - self.monitor.start_serial_connection('/dev/ttyUSB0', 57600) + self.serial_receivers = [] + self.serial_connections = [] # ================================================================================ # 【新增】初始化任務規劃器 @@ -122,17 +104,20 @@ class ControlStationUI(QMainWindow): self.left_tab.addTab(scroll, "無人載具") # — 分頁 2：Overview Table - self.overview_table = OverviewTable(self.info_types, self.info_type_map) + self.overview_table = OverviewTable() self.left_tab.addTab(self.overview_table, "總覽") # — 分頁 3：通訊設定 self.comm_panel = CommPanel() self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) + self.comm_panel.serial_connection_added.connect(self.handle_serial_connection_added) self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection) self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection) + self.comm_panel.serial_connection_toggled.connect(self.toggle_serial_connection) self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection) self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection) + self.comm_panel.serial_connection_removed.connect(self.remove_serial_connection) self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout)) self.left_tab.addTab(self.comm_panel, "通訊") @@ -269,6 +254,7 @@ class ControlStationUI(QMainWindow): # 添加地圖 right_layout.addWidget(self.drone_map.get_widget()) self.drone_map.get_gps_signal().connect(self.handle_map_click) + self.drone_map.get_drone_clicked_signal().connect(self.handle_drone_clicked) # Add widgets to splitter @@ -427,6 +413,69 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000) + def handle_serial_connection_added(self, port, baudrate): + """處理添加 Serial 連接""" + conn = { + 'name': f'Serial', + 'port': port, + 'baudrate': baudrate, + 'enabled': False, + 'receiver': None + } + + # 添加到列表 + self.serial_connections.append(conn) + + # 在 UI 中顯示 + panel = self.comm_panel.add_serial_panel(conn) + + self.statusBar().showMessage(f"已添加 Serial 连接: {port} @ {baudrate}", 3000) + + def toggle_serial_connection(self, conn, btn, status_label): + """切換 Serial 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if conn.get('receiver'): + conn['receiver'].stop() + conn['receiver'] = None + + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 Serial 連接: {conn['port']}", 3000) + else: + # 啟動連接 + try: + receiver = self.monitor.start_serial_connection(conn['port'], conn['baudrate']) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000) + except Exception as e: + self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000) + + def remove_serial_connection(self, conn, panel): + """移除 Serial 連接""" + # 停止連接 + if conn.get('enabled', False) and conn.get('receiver'): + conn['receiver'].stop() + + # 从列表移除 + if conn in self.serial_connections: + self.serial_connections.remove(conn) + + # 从 comm_panel 列表移除 + self.comm_panel.remove_serial_connection_from_list(conn) + + # 从 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 Serial 连接: {conn['port']}", 3000) + def create_socket_group_panel(self, socket_id): """創建 socket 分組面板""" color = self.socket_colors.get(socket_id, self.socket_colors['default']) @@ -592,15 +641,45 @@ class ControlStationUI(QMainWindow): heading = data.get('heading') self.drone_headings[drone_id] = heading groundspeed = data.get('groundspeed') + airspeed = data.get('airspeed') + throttle = data.get('throttle') + hud_alt = data.get('alt') + climb = data.get('climb') + heading_text = f"{heading:.1f}°" if isinstance(groundspeed, (int, float)): groundspeed_text = f"{groundspeed:.1f} m/s" else: groundspeed_text = "--" + + if isinstance(airspeed, (int, float)): + airspeed_text = f"{airspeed:.1f} m/s" + else: + airspeed_text = "--" + + if isinstance(throttle, (int, float)): + throttle_text = f"{throttle:.0f}%" + else: + throttle_text = "--" + + if isinstance(hud_alt, (int, float)): + hud_alt_text = f"{hud_alt:.1f} m" + else: + hud_alt_text = "--" + + if isinstance(climb, (int, float)): + climb_text = f"{climb:.1f} m/s" + else: + climb_text = "--" + self.update_field(panel, drone_id, 'heading', heading_text) self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) self.update_overview_table(drone_id, 'heading', heading_text) self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) + self.update_overview_table(drone_id, 'airspeed', airspeed_text) + self.update_overview_table(drone_id, 'throttle', throttle_text) + self.update_overview_table(drone_id, 'hud_alt', hud_alt_text) + self.update_overview_table(drone_id, 'climb', climb_text) # 如果有位置資訊，也更新地圖上的航向 if drone_id in self.drone_positions: @@ -948,6 +1027,21 @@ class ControlStationUI(QMainWindow): import traceback traceback.print_exc() + def handle_drone_clicked(self, drone_id): + """ + 處理地圖上無人機被點擊事件，切換該無人機的選取狀態 + + Args: + drone_id: 無人機 ID + """ + print(f"地圖上點擊無人機: {drone_id}") + + if drone_id in self.drones: + panel = self.drones[drone_id] + checkbox = panel.get_checkbox() + # 切換勾選狀態 + checkbox.setChecked(not checkbox.isChecked()) + def show_planned_waypoints(self): """ 顯示規劃的航點（在終端輸出） diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py index 24bd2d2..5b1f6ef 100644 --- a/src/GUI/map_layout.py +++ b/src/GUI/map_layout.py @@ -214,6 +214,9 @@ class DroneMap: rotationOrigin: 'center' }) .on('click', function () { + if (bridge) { + bridge.emitDroneClicked(id); + } focusOn(id); }) .addTo(map); @@ -223,6 +226,9 @@ class DroneMap: zIndexOffset: 1000 }) .on('click', function() { + if (bridge) { + bridge.emitDroneClicked(id); + } focusOn(id); }) .addTo(map); @@ -423,9 +429,14 @@ class DroneMap: """獲取 GPS 信號""" return self.bridge.gps_signal + def get_drone_clicked_signal(self): + """獲取無人機點擊信號""" + return self.bridge.drone_clicked + class MapBridge(QObject): """JavaScript 和 Python 之間的橋接類""" gps_signal = pyqtSignal(float, float) # lat, lon + drone_clicked = pyqtSignal(str) # drone_id def __init__(self): super().__init__() @@ -433,4 +444,9 @@ class MapBridge(QObject): @pyqtSlot(float, float) def emitGpsSignal(self, lat, lon): """供 JavaScript 調用的方法""" - self.gps_signal.emit(lat, lon) \ No newline at end of file + self.gps_signal.emit(lat, lon) + + @pyqtSlot(str) + def emitDroneClicked(self, drone_id): + """供 JavaScript 調用的方法 - 當無人機被點擊時""" + self.drone_clicked.emit(drone_id) \ No newline at end of file diff --git a/src/GUI/overview_table.py b/src/GUI/overview_table.py index 11953f0..452eb06 100644 --- a/src/GUI/overview_table.py +++ b/src/GUI/overview_table.py @@ -5,11 +5,38 @@ from PyQt6.QtCore import Qt class OverviewTable(QTableWidget): """總覽表格，顯示所有無人機的狀態資訊""" - def __init__(self, info_types, info_type_map, parent=None): + # 默認的資訊類型和映射 + DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "Local", "Velocity", "地速", "航向", + "空速", "油門", "HUD高", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"] + + DEFAULT_INFO_TYPE_MAP = { + "mode": 0, + "armed": 1, + "battery": 2, + "longitude": 3, + "latitude": 4, + "altitude": 5, + "local": 6, + "velocity": 7, + "groundspeed": 8, + "heading": 9, + "airspeed": 10, + "throttle": 11, + "hud_alt": 12, + "climb": 13, + "roll": 14, + "pitch": 15, + "yaw": 16, + "loss_rate": 17, + "ping": 18 + } + + def __init__(self, info_types=None, info_type_map=None, parent=None): super().__init__(parent) - self.info_types = info_types - self.info_type_map = info_type_map + # 使用提供的或默認的資訊類型 + self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES + self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP self.drones = {} # 存儲無人機面板的引用 # 初始化表格 From 36c881b22940533b2b76c7eb042ee6821b7e23e4 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 4 Feb 2026 12:43:57 +0800 Subject: [PATCH 28/33] Upload files to 'src/GUI' --- src/GUI/gui.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 1e0c5ab..f110256 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -580,7 +580,10 @@ class ControlStationUI(QMainWindow): cells = round(voltage / 3.95) # 計算電量百分比 - percentage = (voltage / cells - 3.7) / 0.5 * 100 + if cells == 0: + percentage = 0 + else: + percentage = (voltage / cells - 3.7) / 0.5 * 100 # 根據百分比設置顏色 if percentage < 20: From f7f90c31685684db9530280b90516318bfb130c1 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 7 Feb 2026 18:31:22 +0800 Subject: [PATCH 30/33] Upload files to 'src/GUI' --- src/GUI/communication.py | 156 ++++++++----- src/GUI/gui.py | 13 +- src/GUI/map_layout.py | 483 ++++++++++++++++++++++++++++++++++++++- 3 files changed, 582 insertions(+), 70 deletions(-) diff --git a/src/GUI/communication.py b/src/GUI/communication.py index c95e74e..b97828b 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -20,12 +20,15 @@ class DroneSignals(QObject): class UDPMavlinkReceiver(threading.Thread): """UDP MAVLink 接收器""" - def __init__(self, ip, port, signals, connection_name): + def __init__(self, ip, port, signals, connection_name, monitor=None): super().__init__(daemon=True) self.ip = ip self.port = port self.signals = signals self.connection_name = connection_name + self.monitor = monitor # 保存 monitor 引用 + # UDP 使用原始 socket_id = 8 + self.socket_id = monitor.get_or_assign_socket_id('8') if monitor else 0 self.running = False self.sock = None @@ -61,7 +64,7 @@ class UDPMavlinkReceiver(threading.Thread): try: msg_type = msg.get_type() system_id = msg.get_srcSystem() - drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源 + drone_id = f"s{self.socket_id}_{system_id}" if msg_type == "HEARTBEAT": mode = mavutil.mode_string_v10(msg) @@ -116,11 +119,13 @@ class UDPMavlinkReceiver(threading.Thread): }) elif msg_type == "VFR_HUD": - groundspeed = msg.groundspeed - heading = msg.heading self.signals.update_signal.emit('hud', drone_id, { - 'heading': heading, - 'groundspeed': groundspeed + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.alt, + 'climb': msg.climb }) except Exception as e: @@ -132,12 +137,15 @@ class UDPMavlinkReceiver(threading.Thread): class SerialMavlinkReceiver(threading.Thread): """串口 MAVLink 接收器""" - def __init__(self, port, baudrate, signals, connection_name): + def __init__(self, port, baudrate, signals, connection_name, monitor=None): super().__init__(daemon=True) self.port = port self.baudrate = baudrate self.signals = signals self.connection_name = connection_name + self.monitor = monitor # 保存 monitor 引用 + # Serial 使用原始 socket_id = 5 + self.socket_id = monitor.get_or_assign_socket_id('5') if monitor else 0 self.running = False self.mav = None @@ -184,7 +192,7 @@ class SerialMavlinkReceiver(threading.Thread): try: msg_type = msg.get_type() system_id = msg.get_srcSystem() - drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源 + drone_id = f"s{self.socket_id}_{system_id}" if msg_type == "HEARTBEAT": mode = mavutil.mode_string_v10(msg) @@ -239,11 +247,13 @@ class SerialMavlinkReceiver(threading.Thread): }) elif msg_type == "VFR_HUD": - groundspeed = msg.groundspeed - heading = msg.heading self.signals.update_signal.emit('hud', drone_id, { - 'heading': heading, - 'groundspeed': groundspeed + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.alt, + 'climb': msg.climb }) except Exception as e: @@ -255,12 +265,12 @@ class SerialMavlinkReceiver(threading.Thread): class WebSocketMavlinkReceiver(threading.Thread): """WebSocket MAVLink 接收器""" - def __init__(self, url, signals, connection_name): + def __init__(self, url, signals, connection_name, monitor=None): super().__init__(daemon=True) self.url = url self.signals = signals self.connection_name = connection_name - self.running = False + self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False self.max_retries = 5 self.base_delay = 1.0 @@ -319,10 +329,10 @@ class WebSocketMavlinkReceiver(threading.Thread): def process_websocket_message(self, data): """處理 WebSocket 訊息""" try: - drone_id = data.get('system_id') - drone_id = f"s9_{drone_id}" if drone_id else None - if not drone_id: + system_id = data.get('system_id') + if not system_id: return + drone_id = f"s{self.socket_id}_{system_id}" # 模式 if 'mode' in data: @@ -398,15 +408,39 @@ class DroneMonitor(Node): self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}} # ================================================================================ + # ================================================================================ + # 【新增】Socket ID 重新分配機制 (從 0 開始) + # ================================================================================ + self.socket_id_mapping = {} # {原始socket_id: 重新分配的socket_id} + self.socket_id_counter = 0 # 當前分配到的最大socket_id + self.socket_id_lock = Lock() # 線程安全鎖 + # ================================================================================ + # ================================================================================ # 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取) # ================================================================================ self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'} + self.sys_to_socket_id = {} # {sys_id: assigned_socket_id} e.g. {'sys11': 0} # ================================================================================ self.serial_receivers = [] # 主题检测定时器 self.create_timer(1.0, self.scan_topics) + + def get_or_assign_socket_id(self, original_socket_id): + """根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配） + 同一個原始 socket_id 會得到同一個分配的 ID + """ + original_socket_id = str(original_socket_id) + + with self.socket_id_lock: + if original_socket_id not in self.socket_id_mapping: + # 分配新的 socket_id + self.socket_id_mapping[original_socket_id] = self.socket_id_counter + print(f"🆕 Socket ID 映射: 原始 {original_socket_id} -> 分配 {self.socket_id_counter}") + self.socket_id_counter += 1 + + return self.socket_id_mapping[original_socket_id] def scan_topics(self): topics = self.get_topic_names_and_types() @@ -416,34 +450,36 @@ class DroneMonitor(Node): for topic_name, _ in topics: if match := drone_pattern.match(topic_name): sys_id, topic_type = match.groups() - # 將 sys11 轉換為 s0_11 格式以保持兼容性 - drone_num = sys_id.replace('sys', '') - drone_id = f's0_{drone_num}' - found_drones.add(drone_id) + found_drones.add(sys_id) with self.lock: - self.drone_topics.setdefault(drone_id, set()).add(topic_type) + self.drone_topics.setdefault(sys_id, set()).add(topic_type) - for drone_id in found_drones: - if not hasattr(self, f'drone_{drone_id}_subs'): - self.setup_drone(drone_id) + for sys_id in found_drones: + # 為每個 sys_id 分配 socket_id（如果還沒有分配） + # 注意：如果後續 summary 提供了 socket_id，會使用 summary 的映射覆蓋 + if sys_id not in self.sys_to_socket_id: + # 暫時所有 ROS2 topic 共享同一個原始 socket_id，等 summary 提供實際的 socket_id + self.sys_to_socket_id[sys_id] = self.get_or_assign_socket_id('0') + + if not hasattr(self, f'drone_{sys_id}_subs'): + self.setup_drone(sys_id) - def setup_drone(self, drone_id): - # 從 s0_11 轉換回 sys11 格式 - sys_id = drone_id.replace('s0_', 'sys') + def setup_drone(self, sys_id): + # sys_id 格式： sys11, sys12, ... base_topic = f'/fc_network/vehicle/{sys_id}' # Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務) - self.arm_clients[drone_id] = self.create_client( + self.arm_clients[sys_id] = self.create_client( CommandBool, f'{base_topic}/cmd/arming' ) - self.takeoff_clients[drone_id] = self.create_client( + self.takeoff_clients[sys_id] = self.create_client( CommandTOL, f'{base_topic}/cmd/takeoff' ) # Add setpoint publisher - self.setpoint_pubs[drone_id] = self.create_publisher( + self.setpoint_pubs[sys_id] = self.create_publisher( Point, f'{base_topic}/setpoint_position/local', 10 @@ -453,30 +489,30 @@ class DroneMonitor(Node): 'battery': self.create_subscription( BatteryState, f'{base_topic}/battery', - lambda msg, did=drone_id: self.battery_callback(did, msg), + lambda msg, sid=sys_id: self.battery_callback(sid, msg), 10 ), 'position': self.create_subscription( NavSatFix, f'{base_topic}/position', - lambda msg, did=drone_id: self.gps_callback(did, msg), + lambda msg, sid=sys_id: self.gps_callback(sid, msg), 10 ), 'summary': self.create_subscription( String, f'{base_topic}/summary', - lambda msg, did=drone_id: self.summary_callback(did, msg), + lambda msg, sid=sys_id: self.summary_callback(sid, msg), 10 ), 'vfr_hud': self.create_subscription( VfrHud, f'{base_topic}/vfr_hud', - lambda msg, did=drone_id: self.hud_callback(did, msg), + lambda msg, sid=sys_id: self.hud_callback(sid, msg), 10 ) } - setattr(self, f'drone_{drone_id}_subs', subs) + setattr(self, f'drone_{sys_id}_subs', subs) async def arm_drone(self, drone_id, arm): if drone_id not in self.arm_clients: @@ -558,9 +594,9 @@ class DroneMonitor(Node): msg.angular_velocity.z) } - def battery_callback(self, drone_id, msg): + def battery_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id - actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return @@ -578,7 +614,7 @@ class DroneMonitor(Node): 'armed': msg.armed } - def summary_callback(self, drone_id, msg): + def summary_callback(self, sys_id, msg): """處理 summary topic (JSON 格式)""" try: data = json.loads(msg.data) @@ -586,28 +622,34 @@ class DroneMonitor(Node): if mode in self.filtered_modes: return - # 根據 socket_id 更新 drone_id - socket_id = data.get('socket_id') + # 從 summary 獲取原始 socket_id，並映射到分配的 socket_id + original_socket_id = data.get('socket_id') + if original_socket_id is not None: + # 使用原始 socket_id 獲取或分配統一的 socket_id + assigned_socket_id = self.get_or_assign_socket_id(original_socket_id) + else: + # 如果沒有 socket_id，使用 sys_to_socket_id 映射 + assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0) + sysid = data.get('sysid') - if socket_id is not None and sysid is not None: - # 使用 socket_id 作為前綴 - actual_drone_id = f's{socket_id}_{sysid}' + if sysid is not None: + actual_drone_id = f's{assigned_socket_id}_{sysid}' # ================================================================================ # 【關鍵】保存 sys_id 到 actual_drone_id 的映射 # ================================================================================ - sys_key = f'sys{sysid}' - self.sys_to_actual_id[sys_key] = actual_drone_id - # 也保存原始的 s0_ 格式到實際 ID 的映射 - self.sys_to_actual_id[drone_id] = actual_drone_id + self.sys_to_actual_id[sys_id] = actual_drone_id # ================================================================================ else: - actual_drone_id = drone_id + # 如果沒有 sysid，使用 sys_id 中的數字 + sys_num = sys_id.replace('sys', '') + actual_drone_id = f's{assigned_socket_id}_{sys_num}' + self.sys_to_actual_id[sys_id] = actual_drone_id self.latest_data[(actual_drone_id, 'state')] = { 'mode': mode, 'armed': data.get('armed', False), - 'socket_id': socket_id, + 'socket_id': original_socket_id, 'sysid': sysid, 'vehicle_type': data.get('vehicle_type'), 'autopilot': data.get('autopilot'), @@ -616,13 +658,13 @@ class DroneMonitor(Node): 'connected': data.get('connected') } except json.JSONDecodeError as e: - print(f"Error parsing summary JSON for {drone_id}: {e}") + print(f"Error parsing summary JSON for {sys_id}: {e}") except Exception as e: - print(f"Error in summary_callback for {drone_id}: {e}") + print(f"Error in summary_callback for {sys_id}: {e}") - def gps_callback(self, drone_id, msg): + def gps_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id - actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return @@ -662,9 +704,9 @@ class DroneMonitor(Node): 'z': msg.z } - def hud_callback(self, drone_id, msg): + def hud_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id - actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return @@ -691,7 +733,7 @@ class DroneMonitor(Node): def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): """啟動串口 MAVLink 連接""" connection_name = f"Serial_{port.replace('/', '_')}" - receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name) + receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self) receiver.start() self.serial_receivers.append(receiver) print(f"Started serial connection on {port} at {baudrate} baud") diff --git a/src/GUI/gui.py b/src/GUI/gui.py index f110256..d73b08d 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -276,7 +276,7 @@ class ControlStationUI(QMainWindow): } # 启动接收器 - receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'], self.monitor) receiver.start() self.udp_receivers.append(receiver) new_conn['receiver'] = receiver @@ -299,7 +299,7 @@ class ControlStationUI(QMainWindow): } # 启动接收器 - receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) + receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'], self.monitor) receiver.start() self.monitor.ws_receivers.append(receiver) new_conn['receiver'] = receiver @@ -845,10 +845,13 @@ class ControlStationUI(QMainWindow): self.overview_table.update_table(drone_id, field, value) def get_socket_id(self, drone_id): - """從 drone_id 提取 socket_id (例如 's9_1' -> '9')""" + """從 drone_id 提取 socket_id (例如 's0_1' -> '0')""" import re - match = re.match(r's(\d+)_\d+', drone_id) - return match.group(1) if match else 'unknown' + match = re.match(r's(\d+)_(\d+)', drone_id) + if not match: + return 'unknown' + + return match.group(1) def add_drone(self, drone_id): if drone_id in self.drones: diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py index 5b1f6ef..e0a8951 100644 --- a/src/GUI/map_layout.py +++ b/src/GUI/map_layout.py @@ -32,15 +32,27 @@ class DroneMap: @@ -58,6 +151,25 @@ class DroneMap:
+
+ +
+
+
+ + + +
+
+ 中心點: + 未設定 +
+
+ 目標點: + 未設定 +
+ +