chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Bring fc_network_adapter from master

Browse Source
This commit is contained in:
ken910606
2026-02-02 01:13:59 +08:00
parent 9f1235197a
commit e54e42aad2
21 changed files with 7064 additions and 871 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_adapter/fc_network_adapter/devRun.py
-508
View File
@@ -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')
src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
+190
View File
@@ -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 應用開發介面
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+1597
View File
File diff suppressed because it is too large Load Diff
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
+684 -282
View File
File diff suppressed because it is too large Load Diff
src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py
+61 -68
View File
@@ -9,66 +9,48 @@
publisher topic name 命名規則為 <前綴詞>/s<sysid>/<具體 topic> publisher topic name 命名規則為 <前綴詞>/s<sysid>/<具體 topic>
''' '''
from pymavlink import mavutil import os
# ROS2 的 import
import std_msgs.msg import std_msgs.msg
import sensor_msgs.msg import sensor_msgs.msg
import geometry_msgs.msg import geometry_msgs.msg
import mavros_msgs.msg import mavros_msgs.msg
from theLogger import setup_logger
import math import math
logger = setup_logger("mavlinkPublish.py") # 自定義的 import
from .utils import setup_logger
logger = setup_logger(os.path.basename(__file__))
class mavlink_publisher(): class mavlink_publisher():
prefix_path = 'MavLinkBus' prefix_path = 'MavLinkBus'
TOPIC_CREATORS = ( def create_flightMode(self, sysid, component_obj):
"create_state",
"create_attitude",
"create_local_position_pose",
"create_local_position_velocity",
"create_global_global",
"create_global_rel",
"create_vfr_hud",
"create_battery",
"create_ping"
)
def ensure_all_publishers(self, sysid: int, component):
"""若還沒替這個 component 建立過 publisher，則一次補齊。"""
if getattr(component, "_pub_ready", False):
return # 已做過就跳過
for fn in self.TOPIC_CREATORS:
getattr(self, fn)(sysid, component)
# 發布丟包率
if not hasattr(component, "loss_rate_publisher") or component.loss_rate_publisher is None:
target_topic = 'packet_loss_rate'
topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
component.loss_rate_publisher = self.create_publisher(std_msgs.msg.Float64, topic_name, 10)
component.loss_rate_topic_name = topic_name
component._pub_ready = True # 打個旗標以免重複
logger.info("✔︎ sysid=%d compid=%d → 所有 topic 已註冊", sysid, component.compid)
def create_state(self, sysid, component_obj):
# target topic name # 請跟這個 method 的名稱保持一致 # target topic 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) topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
publisher_ = self.create_publisher(mavros_msgs.msg.State, topic_name, 1) publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1)
component_obj.publishers[target_topic] = [publisher_, self.packEmit_state] component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode]
return True return True
def packEmit_state(cls, emitParams, publisher): def packEmit_flightMode(cls, emitParams, publisher):
if 'heartbeat' not in emitParams: # ← 沒資料就直接返回 msg_str = emitParams['flightMode_mode']
return msg = std_msgs.msg.String()
mav_msg = emitParams['heartbeat'] msg.data = msg_str
msg = mavros_msgs.msg.State()
msg.mode = mavutil.mode_string_v10(mav_msg)
msg.armed = (mav_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
publisher.publish(msg) publisher.publish(msg)
pass pass
@@ -82,13 +64,18 @@ class mavlink_publisher():
def create_attitude(self, sysid, component_obj): def create_attitude(self, sysid, component_obj):
target_topic = 'attitude' 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) topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1) publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1)
component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude]
def packEmit_attitude(self, emitParams, publisher): def packEmit_attitude(self, emitParams, publisher):
if 'attitude' not in emitParams: # ← 沒資料就直接返回
return
mav_msg = emitParams['attitude'] mav_msg = emitParams['attitude']
msg = sensor_msgs.msg.Imu() msg = sensor_msgs.msg.Imu()
x, y, z, w = self.euler_to_quaternion(mav_msg.roll, mav_msg.pitch, mav_msg.yaw) 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): def create_local_position_pose(self, sysid, component_obj):
target_topic = 'local_position/pose' 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) topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1) publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1)
component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose] component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose]
def packEmit_local_pose(cls, emitParams, publisher): def packEmit_local_pose(cls, emitParams, publisher):
if 'local_position' not in emitParams:
return
mav_msg = emitParams['local_position'] mav_msg = emitParams['local_position']
msg = geometry_msgs.msg.Point() msg = geometry_msgs.msg.Point()
msg.x = mav_msg.x msg.x = mav_msg.x
@@ -121,13 +111,16 @@ class mavlink_publisher():
def create_local_position_velocity(self, sysid, component_obj): def create_local_position_velocity(self, sysid, component_obj):
target_topic = 'local_position/velocity' 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) topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1) publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1)
component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel] component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel]
def packEmit_local_vel(cls, emitParams, publisher): def packEmit_local_vel(cls, emitParams, publisher):
if 'local_position' not in emitParams:
return
mav_msg = emitParams['local_position'] mav_msg = emitParams['local_position']
msg = geometry_msgs.msg.Vector3() msg = geometry_msgs.msg.Vector3()
msg.x = mav_msg.vx msg.x = mav_msg.vx
@@ -138,13 +131,16 @@ class mavlink_publisher():
def create_global_global(self, sysid, component_obj): def create_global_global(self, sysid, component_obj):
target_topic = 'global_position/global' 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) topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1) publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1)
component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global] component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global]
def packEmit_global_global(cls, emitParams, publisher): def packEmit_global_global(cls, emitParams, publisher):
if 'global_position' not in emitParams:
return
mav_msg = emitParams['global_position'] mav_msg = emitParams['global_position']
msg = sensor_msgs.msg.NavSatFix() msg = sensor_msgs.msg.NavSatFix()
msg.latitude = mav_msg.lat/1e7 msg.latitude = mav_msg.lat/1e7
@@ -155,13 +151,16 @@ class mavlink_publisher():
def create_global_rel(self, sysid, component_obj): def create_global_rel(self, sysid, component_obj):
target_topic = 'global_position/rel_alt' 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) topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1)
component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel] component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel]
def packEmit_global_rel(cls, emitParams, publisher): def packEmit_global_rel(cls, emitParams, publisher):
if 'global_position' not in emitParams:
return
mav_msg = emitParams['global_position'] mav_msg = emitParams['global_position']
msg = std_msgs.msg.Float64() msg = std_msgs.msg.Float64()
msg.data = float(mav_msg.relative_alt/1e3) msg.data = float(mav_msg.relative_alt/1e3)
@@ -170,13 +169,16 @@ class mavlink_publisher():
def create_vfr_hud(self, sysid, component_obj): def create_vfr_hud(self, sysid, component_obj):
target_topic = 'vfr_hud' 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) topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1) publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1)
component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud] component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud]
def packEmit_vfr_hud(cls, emitParams, publisher): def packEmit_vfr_hud(cls, emitParams, publisher):
if 'vfr_hud' not in emitParams:
return
mav_msg = emitParams['vfr_hud'] mav_msg = emitParams['vfr_hud']
msg = mavros_msgs.msg.VfrHud() msg = mavros_msgs.msg.VfrHud()
msg.airspeed = mav_msg.airspeed msg.airspeed = mav_msg.airspeed
@@ -190,30 +192,21 @@ class mavlink_publisher():
def create_battery(self, sysid, component_obj): def create_battery(self, sysid, component_obj):
target_topic = 'battery' 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) topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1) publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1)
component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery] component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery]
def packEmit_battery(cls, emitParams, publisher): def packEmit_battery(cls, emitParams, publisher):
if 'battery' not in emitParams:
return
mav_msg = emitParams['battery'] mav_msg = emitParams['battery']
msg = sensor_msgs.msg.BatteryState() msg = sensor_msgs.msg.BatteryState()
msg.voltage = mav_msg.voltages[0]/1e3 msg.voltage = mav_msg.voltages[0]/1e3
publisher.publish(msg) publisher.publish(msg)
pass 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 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
+902
View File
@@ -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_<topic_name>(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 結構（稍後實現）
'''
src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py
+453
View File
@@ -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. 新增 重置指定組件的封包統計 功能
'''
src/fc_network_adapter/fc_network_adapter/serialManager.py
+611
View File
@@ -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()
src/fc_network_adapter/fc_network_adapter/utils/__init__.py
+7
View File
@@ -0,0 +1,7 @@
"""
共用工具模組
"""
from .ringBuffer import RingBuffer
from .theLogger import setup_logger
__all__ = ['RingBuffer', 'setup_logger']
src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py
+129
View File
@@ -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}")
src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py
+111
View File
@@ -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}")
src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py
+231
View File
@@ -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()})"
src/fc_network_adapter/fc_network_adapter/utils/theLogger.py
+43
View File
@@ -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
src/fc_network_adapter/setup.py
+1
View File
@@ -20,6 +20,7 @@ setup(
tests_require=['pytest'], tests_require=['pytest'],
entry_points={ entry_points={
'console_scripts': [ 'console_scripts': [
'mavlink_orchestrator = fc_network_adapter.mainOrchestrator:main',
], ],
}, },
) )
src/fc_network_adapter/tests/__init__.py
View File
src/fc_network_adapter/tests/demo_integration.py
+277
View File
@@ -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')
src/fc_network_adapter/tests/demo_mavlinkVehicleView.py
+331
View File
@@ -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)
src/fc_network_adapter/tests/demo_ringBuffer.py
+152
View File
@@ -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!")
src/fc_network_adapter/tests/test_mavlinkObject.py
+468
View File
@@ -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()
src/fc_network_adapter/tests/test_ringBuffer.py
+296
View File
@@ -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()
src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
+507
View File
@@ -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()