From e54e42aad27cc755b0200f6bd93bfe332711b509 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 2 Feb 2026 01:13:59 +0800 Subject: [PATCH] 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()