wenchun/AirTrapMine
1
0
Fork 0
forked from chiyu1468/AirTrapMine
Code Pull Requests Activity

(Sort Out) 1. 新增 fc_network_adapter.md

Browse Source 
整理並記錄專案結構
程式功能無改動 修正排版與命名
This commit is contained in:
chiyu1468
2026-01-08 13:00:00 +08:00
parent f8f5ff5a15
commit c12959d964
4 changed files with 626 additions and 463 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
+166
View File
@@ -0,0 +1,166 @@
這個檔案整理 此專案下 程式代碼的流程與思路
只會挑出重要的變數與方法描述
以利後續開發使用
# 檔案結構
特別注意:
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 封包的解析
### **[Class]** async_io_manager
唯一實例
異步 event loop
管理 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()*
- *send_message()*
## serialManager.py
看這個檔案的重點再於要搞清楚 端口物件 還是 傳輸物件
### **[Class]** serial_manager
異步 event loop
管理 mavlink_object 的地方
- *self.thread* 自己的執行緒
- *self.loop* 自己的事件迴圈
- *self.serial_objects* 存放管理的物件 serial id num : serial_object
---
- *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 轉拋
4. 連結 Serial 轉 UDP
5. 各單元模組化
6. 終端機介面控制
7. 基礎載具流量觀測
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+448 -451
View File
@@ -27,9 +27,8 @@ from .utils import RingBuffer, setup_logger
from .utils import acquireSerial, acquirePort
from .utils.acquirePort import find_available_port
logger = setup_logger(os.path.basename(__file__))
VERSION_NO = "v0.56"
class PanelState:
def __init__(self):
@@ -84,8 +83,8 @@ class PanelState:
def intoSTOPPED(self):
self.panel_status = "Stopped"
def set_user_input(self, text):
self.user_input = text
# def set_user_input(self, text):
# self.user_input = text
class MenuNode:
def __init__(self, name, desc="", action=None, children=None):
@@ -152,7 +151,446 @@ class ControlPanel:
return user_input
# ================ 關於 mavlink object 的部份 ===================
# ================ 關於 主要選單 的部份 ===================
def menu_tree(self):
"""建立多層選單結構"""
return MenuNode("Main Menu", children=[
MenuNode("MavLink Object", "UDP MavLink 通道選項", children=[
MenuNode("New+", children=[
MenuNode("UDP InBound", children=[
MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"),
MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"),
MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"),
]),
MenuNode("UDP OutBound", children=[
MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"),
MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"),
MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"),
]),
]),
MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"),
]),
MenuNode("Serial Manager", "Serial 連接埠選項", children=[
MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"),
MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"),
]),
MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
MenuNode("Engineer Mode", "工程模式", children=[
MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"),
MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"),
MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"),
]),
MenuNode("Shutdown", "關閉整個系統", children=[
MenuNode("Return", "繼續運行", "BACK"),
MenuNode("Confirm", "關閉系統", "QUIT"),
]),
])
def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event):
stdscr = None
def cleanup():
"""清理 curses 狀態"""
if stdscr:
stdscr.keypad(False)
curses.nocbreak()
curses.echo()
curses.endwin()
def pre_panel_shutdown():
# 先關閉所有模組 再關閉面板
cmd_q.put("SHUTDOWN_BRIDGE")
cmd_q.put("SHUTDOWN_MANAGER")
cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
def draw_menu(screen):
nonlocal stdscr
stdscr = screen
curses.curs_set(0)
stdscr.nodelay(False) # 阻塞讀鍵
stdscr.keypad(True)
# 選單導航狀態
menu_stack = [self.menu_tree()] # 選單堆疊
idx_stack = [0] # 索引堆疊
state.intoSTART() # 設定狀態為運行中
while not stop_evt.is_set():
current_menu = menu_stack[-1]
current_idx = idx_stack[-1]
# 獲取終端機尺寸
height, width = stdscr.getmaxyx()
# 簡單暴力的限制視窗的大小
MIN_HEIGHT = (
2 + # 邊界
6 + # 狀態列 操作說明列 一個空白
11+ # 最大選單 與 空白區
5 # 訊息區域
)
if height < MIN_HEIGHT or width < 60:
logger.error("Terminal size too small for Control Panel.")
break
stdscr.clear()
stdscr.border()
# 更新模組狀態顯示
stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD)
stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}")
stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}")
stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}")
stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}")
stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}")
# 顯示當前選單項目
start_line = 6
for i, child in enumerate(current_menu.children):
marker = "➤ " if i == current_idx else " "
# 動態顯示已輸入的值
desc = child.desc
if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]:
desc = f"{child.desc} [{state.udp_info_temp['IP']}]"
elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]:
desc = f"{child.desc} [{state.udp_info_temp['Port']}]"
elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]:
desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]"
elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]:
desc = f"{child.desc} [{state.serial_info_temp['Baud']}]"
elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP":
link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No"
desc = f"{child.desc} [{link_status}]"
line = f"{marker}{child.name:15s} – {desc}"
attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL
stdscr.addstr(start_line + i, 4, line, attr)
# 顯示訊息區域
# info_start_line = start_line + len(current_menu.children) + 1
info_start_line = height - 8
max_msg_lines = 5 # 最多顯示 5 行訊息
current_time = time.time()
# 清理過時的訊息
state.panel_info_msg_list = [
(msg, timestamp) for msg, timestamp in state.panel_info_msg_list
if current_time - timestamp < 2.0 #秒數
]
# 只顯示最新的 max_msg_lines 條訊息
display_msgs = state.panel_info_msg_list[-max_msg_lines:]
for i, msg_data in enumerate(display_msgs):
if info_start_line + i >= help_line - 1: # 避免超出邊界
break
msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data
# 截斷過長的訊息
max_msg_width = width - 6
if len(msg) > max_msg_width:
msg = msg[:max_msg_width-3] + "..."
stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD)
# 操作說明
# help_line = start_line + len(current_menu.children) + 2
help_line = height - 2
stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM)
stdscr.addstr(height-1 , width-12, f" {VERSION_NO} ", curses.A_DIM)
stdscr.refresh()
# 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作
# 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈
# 超過幾秒沒有反應就強制關閉
if state.panel_status == "Terminating":
if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點
logger.warning("Control Panel forced shutdown after timeout.")
state.intoSTOPPED()
# stop_evt.set()
# continue
break
time.sleep(0.1)
if (state.mavlink_bridge_state == "Stopped" and
state.object_manager_state == "Stopped" and
state.serial_manager_state == "Stopped"):
state.intoSTOPPED()
# stop_evt.set()
break
continue
# 設定短暫的 timeout，讓執行緒能夠響應 stop_evt
stdscr.timeout(100)
ch = stdscr.getch()
if ch == -1: # 沒有操作
continue
# 處理按鍵
if ch in (curses.KEY_UP, ord('k')):
idx_stack[-1] = (current_idx - 1) % len(current_menu.children)
elif ch in (curses.KEY_DOWN, ord('j')):
idx_stack[-1] = (current_idx + 1) % len(current_menu.children)
elif ch == (ord('O')):
# 進入工程模式
state.intoENGINEER()
elif ch == (ord('o')):
# 離開工程模式
state.intoSTART()
elif ch == curses.KEY_LEFT:
# 返回上層
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
elif ch == curses.KEY_RIGHT:
# 進入下層 (但不執行動作)
selected = current_menu.children[current_idx]
if selected.children: # 有子選單
menu_stack.append(selected)
idx_stack.append(0)
elif ch in (ord('q'), 27):
if state.panel_status == "Engineer":
state.intoTERMINATION()
pre_panel_shutdown()
elif ch in (curses.KEY_ENTER, 10, 13):
selected = current_menu.children[current_idx]
# 處理不同類型的動作
if selected.children: # 有子選單
menu_stack.append(selected)
idx_stack.append(0)
elif selected.action == "BACK":
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
elif selected.action == "QUIT":
state.intoTERMINATION()
pre_panel_shutdown()
elif selected.action == "TEXT_UDP_IP":
result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
if result is not None:
state.udp_info_temp["IP"] = result
elif selected.action == "TEXT_UDP_PORT":
result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ")
if result is not None:
state.udp_info_temp["Port"] = result
elif selected.action == "CREATE_UDP_INBOUND":
cmd_q.put("CREATE_UDP_INBOUND")
# 確認後回到上兩層
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
# menu_stack.pop()
# idx_stack.pop()
elif selected.action == "CREATE_UDP_OUTBOUND":
cmd_q.put("CREATE_UDP_OUTBOUND")
# 確認後回到上兩層
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
# menu_stack.pop()
# idx_stack.pop()
elif selected.action == "TEXT_BAUD_SERIAL":
result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ")
if result is not None:
try:
baud_rate = int(result)
except ValueError:
state.panel_info_msg_list.append(("Invalid Baud Rate input.", time.time()))
state.serial_info_temp["Baud"] = baud_rate
elif selected.action == "SET_SERIAL_COMM_XBEE":
state.serial_info_temp["CommunicationType"] = "XBee(API-AT)"
menu_stack.pop()
idx_stack.pop()
elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)"
menu_stack.pop()
idx_stack.pop()
elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES":
state.serial_info_temp["Go2Middleware"] = True
menu_stack.pop()
idx_stack.pop()
elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO":
state.serial_info_temp["Go2Middleware"] = False
menu_stack.pop()
idx_stack.pop()
elif selected.action == "CREATE_SERIAL_PORT":
state.serial_info_temp["Port"] = menu_stack[-1].name # 從選單取得 Port 名稱
cmd_q.put("CREATE_SERIAL_PORT")
# 確認後回到上兩層
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
menu_stack.pop()
idx_stack.pop()
elif selected.action == "LIST_SERIAL_RES":
created_list_menu = self.create_serial_port_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "LIST_SERIAL_LINKS":
created_list_menu = self.create_linked_serial_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "INSPECT_LINKED_SERIAL":
# 顯示 Serial 連結詳細資訊
if hasattr(selected, 'serial_id'):
cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id))
self.show_linked_serial_info(stdscr, selected.serial_id, state)
elif selected.action == "REMOVE_LINKED_SERIAL":
# 移除 Serial 連結
if hasattr(selected, 'serial_id'):
cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id))
# 返回上層（回到列表）
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
# 一樣退兩層
menu_stack.pop()
idx_stack.pop()
elif selected.action == "LIST_MAV_OBJECT":
# 動態生成 mavlink_object 列表選單
created_list_menu = self.create_object_list_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
if hasattr(selected, 'page'):
current_list_menu = menu_stack[-1]
menu_stack.pop()
idx_stack.pop()
# 依據選單種類 重新建立分頁
if current_list_menu.name == "Serial Port List":
created_list_menu = self.create_serial_port_menu(state, page=selected.page)
elif current_list_menu.name == "Object List":
created_list_menu = self.create_object_list_menu(state, page=selected.page)
elif current_list_menu.name == "Linked Serial List":
created_list_menu = self.create_linked_serial_menu(state, page=selected.page)
elif current_list_menu.name == "Connected Vehicles":
created_list_menu = self.create_vehicles_list_menu(state, page=selected.page)
else:
# 不支援的選單類型，回到原本的選單
menu_stack.append(current_list_menu)
idx_stack.append(0)
continue
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "INSPECT_MAV_OBJECT":
# 顯示物件詳細資訊
if hasattr(selected, 'socket_id'):
cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id))
self.show_object_info(stdscr, selected.socket_id, state)
elif selected.action == "REMOVE_MAV_OBJECT":
# 移除物件
if hasattr(selected, 'socket_id'):
cmd_q.put(("REMOVE_OBJECT", selected.socket_id))
# 返回上層（回到列表）
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
# 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好
menu_stack.pop()
idx_stack.pop()
elif selected.action == "MAVOBJ_MAKE_LINK":
# 建立轉發連結
if hasattr(selected, 'socket_id'):
target_id = self.select_target_socket(stdscr, selected.socket_id, state)
if target_id is not None:
cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結
elif selected.action == "MAVOBJ_CANCEL_LINK":
# 取消轉發連結
if hasattr(selected, 'socket_id'):
target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True)
if target_id is not None:
cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id))
cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id)) # 雙向取消連結
elif selected.action == "MAVOBJ_ADD_TARGET":
# 添加目標端口
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
if hasattr(selected, 'socket_id'):
target_id = self.select_target_socket(stdscr, selected.socket_id, state)
if target_id is not None:
cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
elif selected.action == "STOP_MANAGER":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("SHUTDOWN_MANAGER")
elif selected.action == "STOP_BRIDGE":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("SHUTDOWN_BRIDGE")
elif selected.action == "STOP_SERIAL_MANAGER":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
elif selected.action == "INSPECT_VEHICLES":
# 進入載具檢視選單
cmd_q.put("UPDATE_VEHICLES_LIST")
created_list_menu = self.create_vehicles_list_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "INSPECT_VEHICLE":
# 顯示載具詳細資訊
if hasattr(selected, 'sysid') and hasattr(selected, 'compid'):
cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid))
self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state)
elif callable(selected.action):
# 執行函式
cmd_q.put(selected.action)
try:
curses.wrapper(draw_menu)
except KeyboardInterrupt:
pass
finally:
cleanup()
# ================ 關於 mavlink object 的部份 ===================
def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8):
"""動態創建 mavlink_object 列表選單（支持分頁）"""
@@ -303,7 +741,7 @@ class ControlPanel:
stdscr.refresh()
return None
# ================ 關於 serial link 的部份 ===================
# ================ 關於 serial link 的部份 ===================
def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8):
"""動態創建 serial port 列表選單（支持分頁）"""
@@ -452,7 +890,7 @@ class ControlPanel:
stdscr.clear()
stdscr.refresh()
# ================ 關於載具檢視的部份 ===================
# ================ 關於載具檢視的部份 ===================
def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8):
"""動態創建 已連線載具 列表選單（支持分頁）
@@ -666,445 +1104,6 @@ class ControlPanel:
stdscr.clear()
stdscr.refresh()
# ================ 關於 主要選單 的部份 ===================
def menu_tree(self):
"""建立多層選單結構"""
return MenuNode("Main Menu", children=[
MenuNode("MavLink Object", "MavLink 通道選項", children=[
MenuNode("New+", children=[
MenuNode("UDP InBound", children=[
MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"),
MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"),
MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"),
]),
MenuNode("UDP OutBound", children=[
MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"),
MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"),
MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"),
]),
]),
MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"),
]),
MenuNode("Serial Manager", "Serial 連接埠選項", children=[
MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"),
MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"),
]),
MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
MenuNode("Engineer Mode", "工程模式", children=[
MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"),
MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"),
MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"),
]),
MenuNode("Shutdown", "關閉整個系統", children=[
MenuNode("Return", "繼續運行", "BACK"),
MenuNode("Confirm", "關閉系統", "QUIT"),
]),
])
def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event):
stdscr = None
def cleanup():
"""清理 curses 狀態"""
if stdscr:
stdscr.keypad(False)
curses.nocbreak()
curses.echo()
curses.endwin()
def pre_panel_shutdown():
# 先關閉所有模組 再關閉面板
cmd_q.put("SHUTDOWN_BRIDGE")
cmd_q.put("SHUTDOWN_MANAGER")
cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
def draw_menu(screen):
nonlocal stdscr
stdscr = screen
curses.curs_set(0)
stdscr.nodelay(False) # 阻塞讀鍵
stdscr.keypad(True)
# 選單導航狀態
menu_stack = [self.menu_tree()] # 選單堆疊
idx_stack = [0] # 索引堆疊
state.intoSTART() # 設定狀態為運行中
while not stop_evt.is_set():
current_menu = menu_stack[-1]
current_idx = idx_stack[-1]
# 獲取終端機尺寸
height, width = stdscr.getmaxyx()
# 簡單暴力的限制視窗的大小
MIN_HEIGHT = (
2 + # 邊界
6 + # 狀態列 操作說明列 一個空白
11+ # 最大選單 與 空白區
5 # 訊息區域
)
if height < MIN_HEIGHT or width < 60:
logger.error("Terminal size too small for Control Panel.")
break
stdscr.clear()
stdscr.border()
# 更新模組狀態顯示
stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD)
stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}")
stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}")
stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}")
stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}")
stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}")
# 顯示當前選單項目
start_line = 6
for i, child in enumerate(current_menu.children):
marker = "➤ " if i == current_idx else " "
# 動態顯示已輸入的值
desc = child.desc
if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]:
desc = f"{child.desc} [{state.udp_info_temp['IP']}]"
elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]:
desc = f"{child.desc} [{state.udp_info_temp['Port']}]"
elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]:
desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]"
elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]:
desc = f"{child.desc} [{state.serial_info_temp['Baud']}]"
elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP":
link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No"
desc = f"{child.desc} [{link_status}]"
line = f"{marker}{child.name:15s} – {desc}"
attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL
stdscr.addstr(start_line + i, 4, line, attr)
# 顯示訊息區域
# info_start_line = start_line + len(current_menu.children) + 1
info_start_line = height - 8
max_msg_lines = 5 # 最多顯示 5 行訊息
current_time = time.time()
# 清理過時的訊息
state.panel_info_msg_list = [
(msg, timestamp) for msg, timestamp in state.panel_info_msg_list
if current_time - timestamp < 2.0 #秒數
]
# 只顯示最新的 max_msg_lines 條訊息
display_msgs = state.panel_info_msg_list[-max_msg_lines:]
for i, msg_data in enumerate(display_msgs):
if info_start_line + i >= help_line - 1: # 避免超出邊界
break
msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data
# 截斷過長的訊息
max_msg_width = width - 6
if len(msg) > max_msg_width:
msg = msg[:max_msg_width-3] + "..."
stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD)
# 操作說明
# help_line = start_line + len(current_menu.children) + 2
help_line = height - 2
stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM)
stdscr.refresh()
# 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作
# 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈
# 超過幾秒沒有反應就強制關閉
if state.panel_status == "Terminating":
if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點
logger.warning("Control Panel forced shutdown after timeout.")
state.intoSTOPPED()
# stop_evt.set()
# continue
break
time.sleep(0.1)
if (state.mavlink_bridge_state == "Stopped" and
state.object_manager_state == "Stopped" and
state.serial_manager_state == "Stopped"):
state.intoSTOPPED()
# stop_evt.set()
break
continue
# 設定短暫的 timeout，讓執行緒能夠響應 stop_evt
stdscr.timeout(100)
ch = stdscr.getch()
if ch == -1: # 沒有操作
continue
# 處理按鍵
if ch in (curses.KEY_UP, ord('k')):
idx_stack[-1] = (current_idx - 1) % len(current_menu.children)
elif ch in (curses.KEY_DOWN, ord('j')):
idx_stack[-1] = (current_idx + 1) % len(current_menu.children)
elif ch == (ord('O')):
# 進入工程模式
state.intoENGINEER()
elif ch == (ord('o')):
# 離開工程模式
state.intoSTART()
elif ch == curses.KEY_LEFT:
# 返回上層
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
elif ch == curses.KEY_RIGHT:
# 進入下層 (但不執行動作)
selected = current_menu.children[current_idx]
if selected.children: # 有子選單
menu_stack.append(selected)
idx_stack.append(0)
elif ch in (ord('q'), 27):
if state.panel_status == "Engineer":
state.intoTERMINATION()
pre_panel_shutdown()
elif ch in (curses.KEY_ENTER, 10, 13):
selected = current_menu.children[current_idx]
# 處理不同類型的動作
if selected.children: # 有子選單
menu_stack.append(selected)
idx_stack.append(0)
elif selected.action == "BACK":
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
elif selected.action == "QUIT":
state.intoTERMINATION()
pre_panel_shutdown()
elif selected.action == "TEXT_UDP_IP":
result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
if result is not None:
state.udp_info_temp["IP"] = result
elif selected.action == "TEXT_UDP_PORT":
result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ")
if result is not None:
state.udp_info_temp["Port"] = result
elif selected.action == "CREATE_UDP_INBOUND":
cmd_q.put("CREATE_UDP_INBOUND")
# 確認後回到上兩層
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
# menu_stack.pop()
# idx_stack.pop()
elif selected.action == "CREATE_UDP_OUTBOUND":
cmd_q.put("CREATE_UDP_OUTBOUND")
# 確認後回到上兩層
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
# menu_stack.pop()
# idx_stack.pop()
elif selected.action == "TEXT_BAUD_SERIAL":
result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ")
if result is not None:
try:
baud_rate = int(result)
except ValueError:
state.panel_info_msg_list.append(("Invalid Baud Rate input.", time.time()))
state.serial_info_temp["Baud"] = baud_rate
elif selected.action == "SET_SERIAL_COMM_XBEE":
state.serial_info_temp["CommunicationType"] = "XBee(API-AT)"
menu_stack.pop()
idx_stack.pop()
elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)"
menu_stack.pop()
idx_stack.pop()
elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES":
logger.info("mark A")
state.serial_info_temp["Go2Middleware"] = True
menu_stack.pop()
idx_stack.pop()
elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO":
state.serial_info_temp["Go2Middleware"] = False
menu_stack.pop()
idx_stack.pop()
elif selected.action == "CREATE_SERIAL_PORT":
state.serial_info_temp["Port"] = menu_stack[-1].name # 從選單取得 Port 名稱
cmd_q.put("CREATE_SERIAL_PORT")
# 確認後回到上兩層
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
menu_stack.pop()
idx_stack.pop()
elif selected.action == "LIST_SERIAL_RES":
created_list_menu = self.create_serial_port_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "LIST_SERIAL_LINKS":
created_list_menu = self.create_linked_serial_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "INSPECT_LINKED_SERIAL":
# 顯示 Serial 連結詳細資訊
if hasattr(selected, 'serial_id'):
cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id))
self.show_linked_serial_info(stdscr, selected.serial_id, state)
elif selected.action == "REMOVE_LINKED_SERIAL":
# 移除 Serial 連結
if hasattr(selected, 'serial_id'):
cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id))
# 返回上層（回到列表）
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
# 一樣退兩層
menu_stack.pop()
idx_stack.pop()
elif selected.action == "LIST_MAV_OBJECT":
# 動態生成 mavlink_object 列表選單
created_list_menu = self.create_object_list_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
if hasattr(selected, 'page'):
current_list_menu = menu_stack[-1]
menu_stack.pop()
idx_stack.pop()
# 依據選單種類 重新建立分頁
if current_list_menu.name == "Serial Port List":
created_list_menu = self.create_serial_port_menu(state, page=selected.page)
elif current_list_menu.name == "Object List":
created_list_menu = self.create_object_list_menu(state, page=selected.page)
elif current_list_menu.name == "Linked Serial List":
created_list_menu = self.create_linked_serial_menu(state, page=selected.page)
elif current_list_menu.name == "Connected Vehicles":
created_list_menu = self.create_vehicles_list_menu(state, page=selected.page)
else:
# 不支援的選單類型，回到原本的選單
menu_stack.append(current_list_menu)
idx_stack.append(0)
continue
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "INSPECT_MAV_OBJECT":
# 顯示物件詳細資訊
if hasattr(selected, 'socket_id'):
cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id))
self.show_object_info(stdscr, selected.socket_id, state)
elif selected.action == "REMOVE_MAV_OBJECT":
# 移除物件
if hasattr(selected, 'socket_id'):
cmd_q.put(("REMOVE_OBJECT", selected.socket_id))
# 返回上層（回到列表）
if len(menu_stack) > 1:
menu_stack.pop()
idx_stack.pop()
# 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好
menu_stack.pop()
idx_stack.pop()
elif selected.action == "MAVOBJ_MAKE_LINK":
# 建立轉發連結
if hasattr(selected, 'socket_id'):
target_id = self.select_target_socket(stdscr, selected.socket_id, state)
if target_id is not None:
cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結
elif selected.action == "MAVOBJ_CANCEL_LINK":
# 取消轉發連結
if hasattr(selected, 'socket_id'):
target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True)
if target_id is not None:
cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id))
cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id)) # 雙向取消連結
elif selected.action == "MAVOBJ_ADD_TARGET":
# 添加目標端口
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
if hasattr(selected, 'socket_id'):
target_id = self.select_target_socket(stdscr, selected.socket_id, state)
if target_id is not None:
cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
elif selected.action == "STOP_MANAGER":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("SHUTDOWN_MANAGER")
elif selected.action == "STOP_BRIDGE":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("SHUTDOWN_BRIDGE")
elif selected.action == "STOP_SERIAL_MANAGER":
if state.panel_status != "Engineer":
state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
continue # 只有在工程模式下才能操作
cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
elif selected.action == "INSPECT_VEHICLES":
# 進入載具檢視選單
cmd_q.put("UPDATE_VEHICLES_LIST")
created_list_menu = self.create_vehicles_list_menu(state, page=0)
menu_stack.append(created_list_menu)
idx_stack.append(0)
elif selected.action == "INSPECT_VEHICLE":
# 顯示載具詳細資訊
if hasattr(selected, 'sysid') and hasattr(selected, 'compid'):
cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid))
self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state)
elif callable(selected.action):
# 執行函式
cmd_q.put(selected.action)
try:
curses.wrapper(draw_menu)
except KeyboardInterrupt:
pass
finally:
cleanup()
class Orchestrator:
@@ -1189,7 +1188,7 @@ class Orchestrator:
if socket_id in s_obj.target_sockets:
self.remove_target_from_object(s_id, socket_id)
# 再移除該物件
self.delete_mavlink_object(socket_id)
self.delete_udp_object(socket_id)
elif action == "MAVOBJ_ADD_TARGET":
source_id, target_id = cmd[1], cmd[2]
self.add_target_to_object(source_id, target_id)
@@ -1206,9 +1205,7 @@ class Orchestrator:
self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types
self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id
self.panelState.socket_info_single["target_sockets"] = mav_obj.target_sockets
ip_info = mav_obj.mavlink_socket.port.getsockname()
self.panelState.socket_info_single["socket_connection_string"] = f"{ip_info[0]}:{ip_info[1]}"
# getattr(mav_obj.mavlink_socket, "connection_string", "")
self.panelState.socket_info_single["socket_connection_string"] = mav_obj.mavlink_socket.address
self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好
elif action == "INSPECT_LINKED_SERIAL":
serial_id = cmd[1]
@@ -1322,7 +1319,7 @@ class Orchestrator:
self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time()))
def delete_mavlink_object(self, socket_id):
def delete_udp_object(self, socket_id):
"""移除指定的 mavlink_object"""
self.manager.remove_mavlink_object(socket_id)
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
+8 -8
View File
@@ -663,16 +663,16 @@ class async_io_manager:
self.running = False
logger.info("async_io_manager event loop END!")
async def _main_task(self): # 當初想說可能要一個額外的 task 來管理 但是目前好像用不掉 先放著不管
"""主任務協程 讓 async_io_manager 在執行緒中持續運作"""
logger.info("async_io_manager main task started")
# async def _main_task(self): # 當初想說可能要一個額外的 task 來管理 但是目前好像用不掉 先放著不管
# """主任務協程 讓 async_io_manager 在執行緒中持續運作"""
# logger.info("async_io_manager main task started")
while self.running and not self._stop_event.is_set():
await asyncio.sleep(0.1)
# while self.running and not self._stop_event.is_set():
# await asyncio.sleep(0.1)
logger.info("async_io_manager main task ended")
# logger.info("async_io_manager main task ended")
def add_mavlink_object(self, mavlink_obj):
def add_mavlink_object(self, mavlink_obj: mavlink_object):
"""添加 mavlink_object"""
# 一個防呆 確保有 event loop 與 _main_task 正在運作
if not self.running or not self.loop:
@@ -718,7 +718,7 @@ class async_io_manager:
logger.error(f"Failed to create task for mavlink_object {socket_id}: {e}")
return False
def remove_mavlink_object(self, socket_id):
def remove_mavlink_object(self, socket_id: int):
"""移除 mavlink_object"""
# 一個防呆 確保有 event loop 正在運作
src/fc_network_adapter/fc_network_adapter/serialManager.py
+4 -4
View File
@@ -187,11 +187,11 @@ class ATCommandHandler:
# 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.udp_handler = udp_handler # UDP 的傳輸物件
self.serial_port_str = serial_port_str
self.at_handler = ATCommandHandler(serial_port_str)
@@ -327,8 +327,8 @@ class serial_manager:
self.receiver_type = receiver_type
self.target_port = target_port # 指向的 UPD 端口
self.transport = None
self.protocol = None
self.transport = None # TODO 這個變數可能沒有作用
self.protocol = None # TODO 這個變數可能沒有作用
self.udp_handler = None
self.serial_handler = None