diff --git a/.gitignore b/.gitignore
index 34df110..ac2d19f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -8,7 +8,8 @@
# 編譯的資料夾
**/build/
**/install/
-log/
+**/log/
+**/logs/
# CMAKE 的衍生檔
CMakeCache.txt
@@ -24,3 +25,10 @@ Makefile
**/*.class
**/*.pyc
**/*.pyo
+**/.cursor/
+CLAUDE.md
+
+# 本地筆記與硬體 dump
+my_env_notes.md
+notes/
+**/eeprom.bin
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..1742bc8
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,7 @@
+[submodule "src/external/angles"]
+ path = src/external/angles
+ url = https://github.com/ros/angles.git
+[submodule "src/external/geographic_info"]
+ path = src/external/geographic_info
+ url = https://github.com/ros-geographic-info/geographic_info.git
+ branch = ros2
diff --git a/README.md b/README.md
index c93bd79..ec9f627 100644
--- a/README.md
+++ b/README.md
@@ -1,20 +1,99 @@
這是天雷系統的專案
+
+
===
-專案核心環境
-1. ROS2 Humble
-2. Python
-3. Ardupilot
+## 功能簡介
+1. mavlink 多對多支援平台
+2. 不允許進到 ros 系統有相同 sysid
+3. 假設一台載具上所有 component 共用同一 socket
===
-必要相依套件
+## 運行環境
+### 專案核心框架
+1. ROS2 Humble
+2. Python 3.8.10
-===
-建議的開發測試
+### 必要相依套件及版本
+- Python
+[Package] fc_network_adapter
+ 1. pymavlink -> Version: 2.4.42
+ 2. conda-forge 中的 pyserial-asyncio
+ 3. importlib_metadata -> Version: 8.5.0
+ 4. setuptools -> Version: 58.2.0 (版本太新不行)
+ 5. pyserial-asyncio
+[Package] GUI
+ 1. testresources
+ 2. websockets
+ 3. PyQt6
+ 4. PyQt6-WebEngine
+
+- ROS2
+1. source ~/ros2_humble/install/setup.bash
+2. geographic_info (https://github.com/ros-geographic-info/geographic_info.git) 已經作為 submodule 放在 external
+3. angles (https://github.com/ros/angles.git) 已經作為 submodule 放在 external
+4. mavros_msgs (https://github.com/mavlink/mavros) 這個專案中的一個資料夾 這邊手動複製的
+
+### 開發用輔助專案
1. Gazebo Garden
+2. Ardupilot
+
+===
+## 使用說明
+Clone 專案後 請先執行這些指令
+```bash
+# 1.同步 submodule
+cd ~/AirTrapMine
+git submodule init
+git submodule update
+# 2. build 需要的 package
+colcon build --packages-select angles geographic_msgs
+colcon build --packages-select mavros_msgs # 這個依賴前面的
+colcon build --packages-select fc_interfaces # 自己定義的
-===
-Package 簡述
+```
+1.
+主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 路徑下 以模組化啟動程式
+```bash
+# 記得先開啟 依賴 Package 到 overlay
+. ./install/local_setup.bash
+# 範例
+cd ~/AirTrapMine/src/ # 這是範例!!!
+python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+python -m fc_network_adapter.tests.demo_integration
+python -m someotherpkg.src.example_wholeMoving
+```
+2.
+GUI 介面
+在 ~/AirTrapMine/src/GUI 路徑下 直接啟動
+```bash
+cd ~/AirTrapMine/src/GUI
+python gui.py
+```
+===
+## 資料夾說明
+1. unitdev 為各自協作者做開發時的測試區
+ 01 -> 晉凱(ken)
+ 02 -> 其宇(chiyu)
+ 03 -> 文鈞
+ 04 -> 倫渝
+2. fc_network_adapter (核心)
+ 建立、維護與飛控韌體的連接
+ 構築 mavlink 封包
+ 處理無線模組的通訊格式 (XBee)
+ --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
+3. fc_interfaces (重要)
+ 自定義的 ROS2 介面檔
+4. fc_network_apps
+ 與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
+ 使用者的外層包裝
+5. someotherpkg
+ 如何使用 fc_network_apps 的範例檔案
+6. GUI
+ 由 PyQt6 開發的互動式介面
+N. logs 是執行時期的記錄檔
+
+===
diff --git a/src/GUI/comm_panel.py b/src/GUI/comm_panel.py
new file mode 100644
index 0000000..3a41226
--- /dev/null
+++ b/src/GUI/comm_panel.py
@@ -0,0 +1,687 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
+ QPushButton, QLineEdit, QComboBox)
+from PyQt6.QtCore import pyqtSignal
+import glob
+import os
+
+class CommPanel(QWidget):
+ """通讯设置面板类"""
+
+ # 定义信号
+ udp_connection_added = pyqtSignal(str, int) # ip, port
+ udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
+ udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
+ ws_connection_added = pyqtSignal(str) # url
+ ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
+ ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
+ serial_connection_added = pyqtSignal(str, int) # port, baudrate
+ serial_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
+ serial_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
+ status_message = pyqtSignal(str, int) # message, timeout
+
+ def __init__(self, parent=None):
+ super().__init__(parent)
+ self.udp_connections = []
+ self.ws_connections = []
+ self.serial_connections = []
+ self._init_ui()
+
+ def _init_ui(self):
+ """初始化UI"""
+ layout = QVBoxLayout(self)
+ layout.setContentsMargins(10, 10, 10, 10)
+ layout.setSpacing(10)
+
+ # ========== UDP MAVLink 區域 ==========
+ udp_title = QLabel("UDP")
+ udp_title.setStyleSheet("""
+ color: #DDD;
+ font-size: 14px;
+ font-weight: bold;
+ padding: 5px;
+ background-color: #333;
+ border-radius: 4px;
+ """)
+ layout.addWidget(udp_title)
+
+ # UDP 連接列表容器
+ self.udp_list_widget = QWidget()
+ self.udp_list_layout = QVBoxLayout(self.udp_list_widget)
+ self.udp_list_layout.setContentsMargins(0, 0, 0, 0)
+ self.udp_list_layout.setSpacing(5)
+ layout.addWidget(self.udp_list_widget)
+
+ # UDP 添加新連接區域
+ add_udp_widget = QWidget()
+ add_udp_layout = QHBoxLayout(add_udp_widget)
+ add_udp_layout.setContentsMargins(0, 0, 0, 0)
+
+ self.udp_ip_input = QLineEdit()
+ self.udp_ip_input.setText("127.0.0.1")
+ self.udp_ip_input.setPlaceholderText("IP")
+ self.udp_ip_input.setStyleSheet("""
+ QLineEdit {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 5px;
+ }
+ """)
+
+ self.udp_port_input = QLineEdit()
+ self.udp_port_input.setText("14550")
+ self.udp_port_input.setPlaceholderText("Port")
+ self.udp_port_input.setFixedWidth(80)
+ self.udp_port_input.setStyleSheet("""
+ QLineEdit {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 5px;
+ }
+ """)
+
+ add_udp_btn = QPushButton("添加")
+ add_udp_btn.clicked.connect(self._handle_add_udp)
+ add_udp_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #4CAF50;
+ color: white;
+ border: none;
+ padding: 6px 12px;
+ border-radius: 4px;
+ min-width: 30px;
+ }
+ QPushButton:hover { background-color: #45a049; }
+ """)
+
+ add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;"))
+ add_udp_layout.addWidget(self.udp_ip_input)
+ add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
+ add_udp_layout.addWidget(self.udp_port_input)
+ add_udp_layout.addWidget(add_udp_btn)
+
+ layout.addWidget(add_udp_widget)
+
+ # 分隔線
+ separator = QWidget()
+ separator.setFixedHeight(20)
+ layout.addWidget(separator)
+
+ # ========== Serial 區域 ==========
+ serial_title = QLabel("Serial")
+ serial_title.setStyleSheet("""
+ color: #DDD;
+ font-size: 14px;
+ font-weight: bold;
+ padding: 5px;
+ background-color: #333;
+ border-radius: 4px;
+ """)
+ layout.addWidget(serial_title)
+
+ # Serial 連接列表容器
+ self.serial_list_widget = QWidget()
+ self.serial_list_layout = QVBoxLayout(self.serial_list_widget)
+ self.serial_list_layout.setContentsMargins(0, 0, 0, 0)
+ self.serial_list_layout.setSpacing(5)
+ layout.addWidget(self.serial_list_widget)
+
+ # Serial 添加新連接區域
+ add_serial_widget = QWidget()
+ add_serial_layout = QHBoxLayout(add_serial_widget)
+ add_serial_layout.setContentsMargins(0, 0, 0, 0)
+
+ self.serial_port_combo = QComboBox()
+ self.serial_port_combo.setStyleSheet("""
+ QComboBox {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 5px;
+ }
+ QComboBox::drop-down {
+ border: none;
+ }
+ QComboBox::down-arrow {
+ image: none;
+ border-left: 5px solid transparent;
+ border-right: 5px solid transparent;
+ border-top: 5px solid #DDD;
+ }
+ """)
+ self._refresh_serial_ports()
+
+ refresh_ports_btn = QPushButton("↻")
+ refresh_ports_btn.setFixedWidth(35)
+ refresh_ports_btn.clicked.connect(self._refresh_serial_ports)
+ refresh_ports_btn.setToolTip("重新掃描串口")
+ refresh_ports_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 6px;
+ border-radius: 4px;
+ font-size: 16px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+
+ self.serial_baudrate_combo = QComboBox()
+ self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200'])
+ self.serial_baudrate_combo.setCurrentText('57600')
+ self.serial_baudrate_combo.setFixedWidth(100)
+ self.serial_baudrate_combo.setStyleSheet("""
+ QComboBox {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 5px;
+ }
+ QComboBox::drop-down {
+ border: none;
+ }
+ QComboBox::down-arrow {
+ image: none;
+ border-left: 5px solid transparent;
+ border-right: 5px solid transparent;
+ border-top: 5px solid #DDD;
+ }
+ """)
+
+ add_serial_btn = QPushButton("添加")
+ add_serial_btn.clicked.connect(self._handle_add_serial)
+ add_serial_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #4CAF50;
+ color: white;
+ border: none;
+ padding: 6px 12px;
+ border-radius: 4px;
+ min-width: 30px;
+ }
+ QPushButton:hover { background-color: #45a049; }
+ """)
+
+ add_serial_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
+ add_serial_layout.addWidget(self.serial_port_combo)
+ add_serial_layout.addWidget(refresh_ports_btn)
+ add_serial_layout.addWidget(QLabel("Baud:", styleSheet="color: #DDD;"))
+ add_serial_layout.addWidget(self.serial_baudrate_combo)
+ add_serial_layout.addWidget(add_serial_btn)
+
+ layout.addWidget(add_serial_widget)
+
+ # 分隔線
+ separator = QWidget()
+ separator.setFixedHeight(20)
+ layout.addWidget(separator)
+
+ # ========== WebSocket 區域 ==========
+ ws_title = QLabel("WebSocket")
+ ws_title.setStyleSheet("""
+ color: #DDD;
+ font-size: 14px;
+ font-weight: bold;
+ padding: 5px;
+ background-color: #333;
+ border-radius: 4px;
+ """)
+ layout.addWidget(ws_title)
+
+ # WebSocket 連接列表容器
+ self.ws_list_widget = QWidget()
+ self.ws_list_layout = QVBoxLayout(self.ws_list_widget)
+ self.ws_list_layout.setContentsMargins(0, 0, 0, 0)
+ self.ws_list_layout.setSpacing(5)
+ layout.addWidget(self.ws_list_widget)
+
+ # WebSocket 添加新連接區域
+ add_ws_widget = QWidget()
+ add_ws_layout = QHBoxLayout(add_ws_widget)
+ add_ws_layout.setContentsMargins(0, 0, 0, 0)
+
+ self.ws_url_input = QLineEdit()
+ self.ws_url_input.setPlaceholderText("host")
+ self.ws_url_input.setStyleSheet("""
+ QLineEdit {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 5px;
+ }
+ """)
+
+ add_ws_btn = QPushButton("添加")
+ add_ws_btn.clicked.connect(self._handle_add_ws)
+ add_ws_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #4CAF50;
+ color: white;
+ border: none;
+ padding: 6px 12px;
+ border-radius: 4px;
+ min-width: 30px;
+ }
+ QPushButton:hover { background-color: #45a049; }
+ """)
+
+ add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;"))
+ add_ws_layout.addWidget(self.ws_url_input)
+ add_ws_layout.addWidget(add_ws_btn)
+
+ layout.addWidget(add_ws_widget)
+ layout.addStretch()
+
+ def _handle_add_udp(self):
+ """處理添加 UDP 連接"""
+ ip = self.udp_ip_input.text().strip()
+ port_text = self.udp_port_input.text().strip()
+
+ if not ip or not port_text:
+ self.status_message.emit("請輸入 IP 和 Port", 3000)
+ return
+
+ try:
+ port = int(port_text)
+ if port < 1 or port > 65535:
+ raise ValueError("Port 超出範圍")
+ except ValueError:
+ self.status_message.emit("Port 必須是 1-65535 的數字", 3000)
+ return
+
+ # 檢查是否已存在相同連接
+ for conn in self.udp_connections:
+ if conn['ip'] == ip and conn['port'] == port:
+ self.status_message.emit("連接已存在", 3000)
+ return
+
+ # 發送信號通知主窗口
+ self.udp_connection_added.emit(ip, port)
+
+ # 只清空 port 輸入框，保留 IP
+ self.udp_port_input.clear()
+
+ def _handle_add_ws(self):
+ """處理添加 WebSocket 連接"""
+ input_url = self.ws_url_input.text().strip()
+
+ if not input_url:
+ self.status_message.emit("請輸入 WebSocket URL", 3000)
+ return
+
+ # 自動添加 ws:// 前綴
+ if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
+ url = f'ws://{input_url}'
+ else:
+ url = input_url
+
+ # 基本 URL 格式驗證
+ try:
+ if '://' in url:
+ parts = url.split('://', 1)
+ if len(parts) == 2 and ':' not in parts[1]:
+ self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
+ return
+ except:
+ self.status_message.emit("URL 格式錯誤", 3000)
+ return
+
+ # 檢查是否已存在相同連接
+ for conn in self.ws_connections:
+ if conn['url'] == url:
+ self.status_message.emit("連接已存在", 3000)
+ return
+
+ # 發送信號通知主窗口
+ self.ws_connection_added.emit(url)
+
+ # 清空輸入框
+ self.ws_url_input.clear()
+
+ def _refresh_serial_ports(self):
+ """重新掃描可用的串口"""
+ self.serial_port_combo.clear()
+
+ # 掃描 Linux 下的串口設備
+ ports = []
+
+ # 掃描 USB 串口
+ usb_ports = glob.glob('/dev/ttyUSB*')
+ ports.extend(usb_ports)
+
+ # 掃描 ACM 串口
+ acm_ports = glob.glob('/dev/ttyACM*')
+ ports.extend(acm_ports)
+
+ # 排序
+ ports.sort()
+
+ if ports:
+ self.serial_port_combo.addItems(ports)
+ else:
+ self.serial_port_combo.addItem("沒有找到串口")
+ self.serial_port_combo.setEnabled(False)
+ return
+
+ self.serial_port_combo.setEnabled(True)
+
+ def _handle_add_serial(self):
+ """處理添加 Serial 連接"""
+ port = self.serial_port_combo.currentText()
+ baudrate_text = self.serial_baudrate_combo.currentText()
+
+ if not port or port == "沒有找到串口":
+ self.status_message.emit("請選擇有效的串口", 3000)
+ return
+
+ try:
+ baudrate = int(baudrate_text)
+ except ValueError:
+ self.status_message.emit("波特率格式錯誤", 3000)
+ return
+
+ # 檢查是否已存在相同連接
+ for conn in self.serial_connections:
+ if conn['port'] == port:
+ self.status_message.emit("連接已存在", 3000)
+ return
+
+ # 發送信號通知主窗口
+ self.serial_connection_added.emit(port, baudrate)
+
+ def _handle_add_ws(self):
+ """處理添加 WebSocket 連接"""
+ input_url = self.ws_url_input.text().strip()
+
+ if not input_url:
+ self.status_message.emit("請輸入 WebSocket URL", 3000)
+ return
+
+ # 自動添加 ws:// 前綴
+ if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
+ url = f'ws://{input_url}'
+ else:
+ url = input_url
+
+ # 基本 URL 格式驗證
+ try:
+ if '://' in url:
+ parts = url.split('://', 1)
+ if len(parts) == 2 and ':' not in parts[1]:
+ self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
+ return
+ except:
+ self.status_message.emit("URL 格式錯誤", 3000)
+ return
+
+ # 檢查是否已存在相同連接
+ for conn in self.ws_connections:
+ if conn['url'] == url:
+ self.status_message.emit("連接已存在", 3000)
+ return
+
+ # 發送信號通知主窗口
+ self.ws_connection_added.emit(url)
+
+ # 清空輸入框
+ self.ws_url_input.clear()
+
+ def create_udp_connection_panel(self, conn):
+ """創建 UDP 連接面板"""
+ panel = QWidget()
+ panel.setStyleSheet("""
+ QWidget {
+ background-color: #2A2A2A;
+ border-radius: 6px;
+ padding: 8px;
+ border: 1px solid #444;
+ }
+ """)
+
+ layout = QHBoxLayout(panel)
+ layout.setContentsMargins(8, 8, 8, 8)
+
+ # 連接資訊
+ info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
+ info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+
+ # 狀態指示器
+ status_label = QLabel("●")
+ if conn.get('enabled', False):
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ else:
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+
+ # 控制按鈕
+ toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
+ toggle_btn.setFixedWidth(60)
+ toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label))
+ toggle_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+
+ remove_btn = QPushButton("移除")
+ remove_btn.setFixedWidth(60)
+ remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
+ remove_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #d32f2f;
+ color: white;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #b71c1c; }
+ """)
+
+ layout.addWidget(status_label)
+ layout.addWidget(info_label)
+ layout.addStretch()
+ layout.addWidget(toggle_btn)
+ layout.addWidget(remove_btn)
+
+ # 儲存引用
+ panel.connection = conn
+ panel.toggle_btn = toggle_btn
+ panel.status_label = status_label
+
+ return panel
+
+ def create_ws_connection_panel(self, conn):
+ """創建 WebSocket 連接面板"""
+ panel = QWidget()
+ panel.setStyleSheet("""
+ QWidget {
+ background-color: #2A2A2A;
+ border-radius: 6px;
+ padding: 8px;
+ border: 1px solid #444;
+ }
+ """)
+
+ layout = QHBoxLayout(panel)
+ layout.setContentsMargins(8, 8, 8, 8)
+
+ # 連接資訊
+ info_label = QLabel(f"{conn['name']} - {conn['url']}")
+ info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+
+ # 狀態指示器
+ status_label = QLabel("●")
+ if conn.get('enabled', False):
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ else:
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+
+ # 控制按鈕
+ toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
+ toggle_btn.setFixedWidth(60)
+ toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label))
+ toggle_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+
+ remove_btn = QPushButton("移除")
+ remove_btn.setFixedWidth(60)
+ remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
+ remove_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #d32f2f;
+ color: white;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #b71c1c; }
+ """)
+
+ layout.addWidget(status_label)
+ layout.addWidget(info_label)
+ layout.addStretch()
+ layout.addWidget(toggle_btn)
+ layout.addWidget(remove_btn)
+
+ # 儲存引用
+ panel.connection = conn
+ panel.toggle_btn = toggle_btn
+ panel.status_label = status_label
+
+ return panel
+
+ def add_udp_panel(self, conn):
+ """添加 UDP 連接面板到列表"""
+ panel = self.create_udp_connection_panel(conn)
+ self.udp_list_layout.addWidget(panel)
+ self.udp_connections.append(conn)
+ return panel
+
+ def add_ws_panel(self, conn):
+ """添加 WebSocket 連接面板到列表"""
+ panel = self.create_ws_connection_panel(conn)
+ self.ws_list_layout.addWidget(panel)
+ self.ws_connections.append(conn)
+ return panel
+
+ def remove_udp_connection_from_list(self, conn):
+ """從列表中移除 UDP 連接"""
+ if conn in self.udp_connections:
+ self.udp_connections.remove(conn)
+
+ def remove_ws_connection_from_list(self, conn):
+ """從列表中移除 WebSocket 連接"""
+ if conn in self.ws_connections:
+ self.ws_connections.remove(conn)
+
+ def create_serial_connection_panel(self, conn):
+ """創建 Serial 連接面板"""
+ panel = QWidget()
+ panel.setStyleSheet("""
+ QWidget {
+ background-color: #2A2A2A;
+ border-radius: 6px;
+ padding: 8px;
+ border: 1px solid #444;
+ }
+ """)
+
+ layout = QHBoxLayout(panel)
+ layout.setContentsMargins(8, 8, 8, 8)
+
+ # 連接資訊
+ info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}")
+ info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+
+ # 狀態指示器
+ status_label = QLabel("●")
+ if conn.get('enabled', False):
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ else:
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+
+ # 控制按鈕
+ toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
+ toggle_btn.setFixedWidth(60)
+ toggle_btn.clicked.connect(lambda: self.serial_connection_toggled.emit(conn, toggle_btn, status_label))
+ toggle_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+
+ remove_btn = QPushButton("移除")
+ remove_btn.setFixedWidth(60)
+ remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel))
+ remove_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #d32f2f;
+ color: white;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #b71c1c; }
+ """)
+
+ layout.addWidget(status_label)
+ layout.addWidget(info_label)
+ layout.addStretch()
+ layout.addWidget(toggle_btn)
+ layout.addWidget(remove_btn)
+
+ # 儲存引用
+ panel.connection = conn
+ panel.toggle_btn = toggle_btn
+ panel.status_label = status_label
+
+ return panel
+
+ def add_serial_panel(self, conn):
+ """添加 Serial 連接面板到列表"""
+ panel = self.create_serial_connection_panel(conn)
+ self.serial_list_layout.addWidget(panel)
+ self.serial_connections.append(conn)
+ return panel
+
+ def remove_serial_connection_from_list(self, conn):
+ """從列表中移除 Serial 連接"""
+ if conn in self.serial_connections:
+ self.serial_connections.remove(conn)
\ No newline at end of file
diff --git a/src/GUI/command_sender.py b/src/GUI/command_sender.py
new file mode 100644
index 0000000..a0ddd12
--- /dev/null
+++ b/src/GUI/command_sender.py
@@ -0,0 +1,165 @@
+#!/usr/bin/env python3
+"""
+指令發送模組
+負責將目標位置轉成具體的通訊指令發送到飛控
+
+目前使用 Ros2CommandSender，經 fc_network_adapter 的
+/fc_network/vehicle/pos_global_int service 發送。
+
+MavlinkSender 保留作為直連 SITL 的 fallback 工具，但已不是預設路徑。
+"""
+import asyncio
+import traceback
+from abc import ABC, abstractmethod
+
+from PyQt6.QtCore import QObject, pyqtSignal
+from pymavlink import mavutil
+
+
+class CommandSender(ABC):
+ """指令發送抽象介面"""
+
+ @abstractmethod
+ def send_position_global(self, drone_id, sysid, lat, lon, alt):
+ """
+ 發送全球座標位置指令
+
+ Args:
+ drone_id: GUI 用的 drone 識別字串 (如 's0_11')
+ sysid: 目標無人機的 MAVLink system ID
+ lat: 緯度 (度)
+ lon: 經度 (度)
+ alt: 高度 (公尺)
+ """
+ pass
+
+ @abstractmethod
+ def close(self):
+ """關閉連線"""
+ pass
+
+
+class MavlinkSender(CommandSender):
+ """
+ MAVLink 直接發送 (驗證階段用)
+ 透過 SET_POSITION_TARGET_GLOBAL_INT 發送目標位置
+
+ 使用方式:
+ sender = MavlinkSender("udpout:127.0.0.1:14550")
+ sender.send_position_global(sysid=1, lat=24.123, lon=120.567, alt=10.0)
+ """
+
+ # type_mask: 只使用位置 (lat, lon, alt)
+ # 忽略速度 (bit 3,4,5)、加速度 (bit 6,7,8)、yaw (bit 10)、yaw_rate (bit 11)
+ TYPE_MASK_POSITION_ONLY = (
+ 0b0000_1101_1111_1000 # = 0x0DF8
+ )
+
+ def __init__(self, connection_string="udpout:127.0.0.1:14550"):
+ """
+ Args:
+ connection_string: MAVLink 連線字串
+ SITL 範例: "udpout:127.0.0.1:14550"
+ """
+ self.connection_string = connection_string
+ self.mav = mavutil.mavlink_connection(connection_string)
+ print(f"MavlinkSender 已建立連線: {connection_string}")
+
+ def send_position_global(self, drone_id, sysid, lat, lon, alt):
+ """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
+ self.mav.mav.set_position_target_global_int_send(
+ 0, # time_boot_ms (不使用)
+ sysid, # target_system
+ 1, # target_component (autopilot)
+ mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+ self.TYPE_MASK_POSITION_ONLY,
+ int(lat * 1e7), # lat_int
+ int(lon * 1e7), # lon_int
+ float(alt), # alt
+ 0, 0, 0, # vx, vy, vz (忽略)
+ 0, 0, 0, # afx, afy, afz (忽略)
+ 0, 0 # yaw, yaw_rate (忽略)
+ )
+
+ def close(self):
+ """關閉 MAVLink 連線"""
+ if self.mav:
+ self.mav.close()
+ self.mav = None
+ print("MavlinkSender 已關閉")
+
+
+class Ros2CommandSender(QObject):
+ """
+ 透過 fc_network_apps.PositionTargetGlobalIntClient 送 goto 指令。
+
+ 設計重點:
+ - send_position_global 是同步介面，立刻回傳。結果透過 send_result signal 回報
+ - 每台 drone 維護自己的 asyncio.Task pipeline，新的目標會 cancel 掉舊的 in-flight
+ - client 由 DroneMonitor.get_or_create_position_client 管理，per-drone 獨立節點
+ """
+
+ # (drone_id, sysid, success, message)
+ send_result = pyqtSignal(str, int, bool, str)
+
+ DEFAULT_TIMEOUT_SEC = 2.0
+
+ def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
+ super().__init__()
+ self._monitor = monitor
+ self._timeout_sec = timeout_sec
+ self._pending = {} # drone_id -> asyncio.Task
+
+ def send_position_global(self, drone_id, sysid, lat, lon, alt):
+ """同步介面。立刻回，結果透過 send_result signal 回報。"""
+ # 取消該機上一個未完成的 task，避免舊指令覆蓋新指令
+ old = self._pending.get(drone_id)
+ if old is not None and not old.done():
+ old.cancel()
+
+ try:
+ loop = asyncio.get_event_loop()
+ except RuntimeError:
+ loop = asyncio.new_event_loop()
+ asyncio.set_event_loop(loop)
+
+ task = loop.create_task(
+ self._do_send(drone_id, int(sysid), float(lat), float(lon), float(alt))
+ )
+ self._pending[drone_id] = task
+
+ async def _do_send(self, drone_id, sysid, lat, lon, alt):
+ try:
+ client = self._monitor.get_or_create_position_client(drone_id)
+ if client is None:
+ self.send_result.emit(
+ drone_id, sysid, False,
+ "PositionTargetGlobalIntClient 無法建立"
+ )
+ return
+
+ result = await client.goto_position_only_async(
+ target_sysid=sysid,
+ target_compid=0,
+ latitude_deg=lat,
+ longitude_deg=lon,
+ alt=alt,
+ timeout_sec=self._timeout_sec,
+ )
+ self.send_result.emit(
+ drone_id, sysid, bool(result.success), str(result.message or "")
+ )
+ except asyncio.CancelledError:
+ # 被新的目標取代，正常狀況，不用回報
+ pass
+ except Exception as e:
+ traceback.print_exc()
+ self.send_result.emit(drone_id, sysid, False, f"exception: {e}")
+
+ def close(self):
+ """取消所有未完成的 task。PositionTargetGlobalIntClient 由 DroneMonitor 負責 destroy。"""
+ for task in self._pending.values():
+ if not task.done():
+ task.cancel()
+ self._pending.clear()
+ print("Ros2CommandSender 已關閉")
\ No newline at end of file
diff --git a/src/GUI/communication.py b/src/GUI/communication.py
new file mode 100644
index 0000000..3e83df5
--- /dev/null
+++ b/src/GUI/communication.py
@@ -0,0 +1,1043 @@
+from rclpy.node import Node
+from PyQt6.QtCore import QObject, pyqtSignal
+import math
+import re
+import threading
+from threading import Lock
+from concurrent.futures import ThreadPoolExecutor
+import asyncio
+import websockets
+import json
+import socket
+import sys
+import os
+import traceback
+from pymavlink import mavutil
+from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
+from sensor_msgs.msg import BatteryState, NavSatFix, Imu
+from std_msgs.msg import Float64, String
+from mavros_msgs.msg import State, VfrHud
+from nav_msgs.msg import Odometry
+from mavros_msgs.srv import CommandBool, CommandTOL
+
+# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
+_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+if _src_path not in sys.path:
+ sys.path.insert(0, _src_path)
+
+# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API）
+try:
+ from fc_network_apps.longCommand import CommandLongClient
+except ImportError as e:
+ import traceback
+ print(f"⚠️ 警告: 無法導入 CommandLongClient")
+ print(f" 错误: {e}")
+ print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
+ print(f" 完整堆栈跟踪:")
+ traceback.print_exc()
+ CommandLongClient = None
+
+# 導入 fc_network_apps 的 navigation（PositionTargetGlobalInt / Offboard goto）
+try:
+ from fc_network_apps.navigation import PositionTargetGlobalIntClient
+except ImportError as e:
+ import traceback
+ print(f"⚠️ 警告: 無法導入 PositionTargetGlobalIntClient")
+ print(f" 错误: {e}")
+ traceback.print_exc()
+ PositionTargetGlobalIntClient = None
+
+class DroneSignals(QObject):
+ update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
+
+class UDPMavlinkReceiver(threading.Thread):
+ """UDP MAVLink 接收器"""
+ def __init__(self, ip, port, signals, connection_name, monitor=None):
+ super().__init__(daemon=True)
+ self.ip = ip
+ self.port = port
+ self.signals = signals
+ self.connection_name = connection_name
+ self.monitor = monitor # 保存 monitor 引用
+ self.socket_id = monitor.get_next_socket_id() if monitor else 0
+ self.running = False
+ self.sock = None
+
+ def run(self):
+ """執行 UDP 接收循環"""
+ self.running = True
+ try:
+ print(f"UDP MAVLink receiver started on {self.ip}:{self.port}")
+
+ # 創建 MAVLink 連接
+ mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}')
+ while self.running:
+ try:
+ msg = mav.recv_match(blocking=True, timeout=1.0)
+ if msg is None:
+ continue
+
+ self.process_mavlink_message(msg)
+
+ except socket.timeout:
+ continue
+ except Exception as e:
+ print(f"Error receiving MAVLink message: {e}")
+
+ except Exception as e:
+ print(f"UDP receiver error: {e}")
+ finally:
+ if self.sock:
+ self.sock.close()
+
+ def process_mavlink_message(self, msg):
+ """處理 MAVLink 訊息"""
+ try:
+ msg_type = msg.get_type()
+ system_id = msg.get_srcSystem()
+ drone_id = f"s{self.socket_id}_{system_id}"
+
+ if msg_type == "HEARTBEAT":
+ # 先發送連接類型資訊
+ self.signals.update_signal.emit('connection_type', drone_id, {
+ 'type': 'UDP'
+ })
+ mode = mavutil.mode_string_v10(msg)
+ armed = bool(msg.base_mode & 128)
+ self.signals.update_signal.emit('state', drone_id, {
+ 'mode': mode,
+ 'armed': armed
+ })
+
+ elif msg_type == "BATTERY_STATUS":
+ voltage = msg.voltages[0] / 1000
+ self.signals.update_signal.emit('battery', drone_id, {
+ 'voltage': voltage
+ })
+
+ elif msg_type == "GLOBAL_POSITION_INT":
+ latitude = msg.lat / 1e7
+ longitude = msg.lon / 1e7
+ self.signals.update_signal.emit('gps', drone_id, {
+ 'lat': latitude,
+ 'lon': longitude
+ })
+
+ elif msg_type == "GPS_RAW_INT":
+ fix_type = msg.fix_type
+
+ elif msg_type == "LOCAL_POSITION_NED":
+ x = msg.y
+ y = msg.x
+ z = -msg.z
+ self.signals.update_signal.emit('local_pose', drone_id, {
+ 'x': x,
+ 'y': y,
+ 'z': z
+ })
+ self.signals.update_signal.emit('altitude', drone_id, {
+ 'altitude': z
+ })
+ self.signals.update_signal.emit('velocity', drone_id, {
+ 'vx': msg.vx,
+ 'vy': msg.vy,
+ 'vz': msg.vz
+ })
+
+ elif msg_type == "ATTITUDE":
+ # 從 MAVLink 訊息中提取並轉為角度
+ pitch = math.degrees(msg.pitch)
+ roll = math.degrees(msg.roll)
+ yaw = math.degrees(msg.yaw)
+ self.signals.update_signal.emit('attitude', drone_id, {
+ 'pitch': pitch,
+ 'roll': roll,
+ 'yaw': yaw,
+ 'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
+ })
+
+ elif msg_type == "VFR_HUD":
+ self.signals.update_signal.emit('hud', drone_id, {
+ 'airspeed': msg.airspeed,
+ 'groundspeed': msg.groundspeed,
+ 'heading': msg.heading,
+ 'throttle': msg.throttle,
+ 'alt': msg.alt,
+ 'climb': msg.climb
+ })
+
+ except Exception as e:
+ print(f"Error processing MAVLink message: {e}")
+
+ def stop(self):
+ """停止接收器"""
+ self.running = False
+
+class SerialMavlinkReceiver(threading.Thread):
+ """串口 MAVLink 接收器"""
+ def __init__(self, port, baudrate, signals, connection_name, monitor=None):
+ super().__init__(daemon=True)
+ self.port = port
+ self.baudrate = baudrate
+ self.signals = signals
+ self.connection_name = connection_name
+ self.monitor = monitor # 保存 monitor 引用
+ self.socket_id = monitor.get_next_socket_id() if monitor else 0
+ self.running = False
+ self.mav = None
+
+ def run(self):
+ """執行串口接收循環"""
+ self.running = True
+ try:
+ print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
+
+ # 創建 MAVLink 串口連接
+ self.mav = mavutil.mavlink_connection(
+ self.port,
+ baud=self.baudrate,
+ source_system=255
+ )
+
+ print(f"Waiting for heartbeat from {self.port}...")
+ self.mav.wait_heartbeat()
+ print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
+
+ while self.running:
+ try:
+ msg = self.mav.recv_match(blocking=True, timeout=1.0)
+ if msg is None:
+ continue
+
+ self.process_mavlink_message(msg)
+
+ except Exception as e:
+ if self.running:
+ print(f"Error receiving MAVLink message from serial: {e}")
+
+ except Exception as e:
+ print(f"Serial receiver error: {e}")
+ finally:
+ if self.mav:
+ try:
+ self.mav.close()
+ except:
+ pass
+
+ def process_mavlink_message(self, msg):
+ """處理 MAVLink 訊息"""
+ try:
+ msg_type = msg.get_type()
+ system_id = msg.get_srcSystem()
+ drone_id = f"s{self.socket_id}_{system_id}"
+
+ if msg_type == "HEARTBEAT":
+ # 先發送連接類型資訊
+ self.signals.update_signal.emit('connection_type', drone_id, {
+ 'type': 'Serial'
+ })
+ mode = mavutil.mode_string_v10(msg)
+ armed = bool(msg.base_mode & 128)
+ self.signals.update_signal.emit('state', drone_id, {
+ 'mode': mode,
+ 'armed': armed
+ })
+
+ elif msg_type == "BATTERY_STATUS":
+ voltage = msg.voltages[0] / 1000
+ self.signals.update_signal.emit('battery', drone_id, {
+ 'voltage': voltage
+ })
+
+ elif msg_type == "GLOBAL_POSITION_INT":
+ latitude = msg.lat / 1e7
+ longitude = msg.lon / 1e7
+ self.signals.update_signal.emit('gps', drone_id, {
+ 'lat': latitude,
+ 'lon': longitude
+ })
+
+ elif msg_type == "GPS_RAW_INT":
+ fix_type = msg.fix_type
+
+ elif msg_type == "LOCAL_POSITION_NED":
+ x = msg.y
+ y = msg.x
+ z = -msg.z
+ self.signals.update_signal.emit('local_pose', drone_id, {
+ 'x': x,
+ 'y': y,
+ 'z': z
+ })
+ self.signals.update_signal.emit('altitude', drone_id, {
+ 'altitude': z
+ })
+ self.signals.update_signal.emit('velocity', drone_id, {
+ 'vx': msg.vx,
+ 'vy': msg.vy,
+ 'vz': msg.vz
+ })
+
+ elif msg_type == "ATTITUDE":
+ # 從 MAVLink 訊息中提取並轉為角度
+ pitch = math.degrees(msg.pitch)
+ roll = math.degrees(msg.roll)
+ yaw = math.degrees(msg.yaw)
+ self.signals.update_signal.emit('attitude', drone_id, {
+ 'pitch': pitch,
+ 'roll': roll,
+ 'yaw': yaw,
+ 'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
+ })
+
+ elif msg_type == "VFR_HUD":
+ self.signals.update_signal.emit('hud', drone_id, {
+ 'airspeed': msg.airspeed,
+ 'groundspeed': msg.groundspeed,
+ 'heading': msg.heading,
+ 'throttle': msg.throttle,
+ 'alt': msg.alt,
+ 'climb': msg.climb
+ })
+
+ except Exception as e:
+ print(f"Error processing MAVLink message from serial: {e}")
+
+ def stop(self):
+ """停止接收器"""
+ self.running = False
+
+class WebSocketMavlinkReceiver(threading.Thread):
+ """WebSocket MAVLink 接收器"""
+ def __init__(self, url, signals, connection_name, monitor=None):
+ super().__init__(daemon=True)
+ self.url = url
+ self.signals = signals
+ self.connection_name = connection_name
+ self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False
+ self.max_retries = 5
+ self.base_delay = 1.0
+
+ def run(self):
+ """執行 WebSocket 接收循環"""
+ self.running = True
+ asyncio.set_event_loop(asyncio.new_event_loop())
+ asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
+
+ async def ws_client_loop(self):
+ """WebSocket 連接的主循環"""
+ retry_count = 0
+
+ print(f"Starting WebSocket client for {self.connection_name} at {self.url}")
+
+ while self.running and retry_count < self.max_retries:
+ try:
+ async with websockets.connect(self.url) as websocket:
+ print(f"WebSocket {self.connection_name} connected to {self.url}")
+ retry_count = 0 # 重置重試計數
+
+ async for message in websocket:
+ if not self.running:
+ break
+
+ try:
+ data = json.loads(message)
+ data['_connection_source'] = self.connection_name
+ self.process_websocket_message(data)
+ except json.JSONDecodeError as e:
+ print(f"WebSocket {self.connection_name} JSON decode error: {e}")
+ except Exception as e:
+ print(f"WebSocket {self.connection_name} message processing error: {e}")
+
+ except websockets.exceptions.ConnectionClosedError:
+ print(f"WebSocket {self.connection_name} connection closed")
+ if self.running:
+ retry_count += 1
+ if retry_count < self.max_retries:
+ delay = self.base_delay * (2 ** min(retry_count, 4))
+ print(f"Reconnecting in {delay}s...")
+ await asyncio.sleep(delay)
+ else:
+ break
+ except Exception as e:
+ retry_count += 1
+ if retry_count < self.max_retries and self.running:
+ delay = self.base_delay * (2 ** min(retry_count, 4))
+ print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})")
+ await asyncio.sleep(delay)
+ else:
+ break
+
+ print(f"WebSocket client {self.connection_name} stopped")
+
+ def process_websocket_message(self, data):
+ """處理 WebSocket 訊息"""
+ try:
+ system_id = data.get('system_id')
+ if not system_id:
+ return
+ drone_id = f"s{self.socket_id}_{system_id}"
+
+ # 模式
+ if 'mode' in data:
+ # 先發送連接類型資訊
+ self.signals.update_signal.emit('connection_type', drone_id, {
+ 'type': 'WS'
+ })
+ self.signals.update_signal.emit('state', drone_id, {
+ 'mode': data['mode'],
+ })
+
+ # 電池
+ if 'battery' in data:
+ self.signals.update_signal.emit('battery', drone_id, {
+ 'percentage': data['battery']
+ })
+
+ # 位置
+ if 'position' in data:
+ pos = data['position']
+ self.signals.update_signal.emit('gps', drone_id, {
+ 'lat': pos.get('lat', 0),
+ 'lon': pos.get('lon', 0)
+ })
+
+ # Local position - 設定 x, y 為 0.0
+ self.signals.update_signal.emit('local_pose', drone_id, {
+ 'x': 0.0,
+ 'y': 0.0,
+ 'z': 0.0
+ })
+
+ # Altitude - 設定為 0.0
+ self.signals.update_signal.emit('altitude', drone_id, {
+ 'altitude': 0.0
+ })
+
+ # 航向
+ if 'heading' in data:
+ self.signals.update_signal.emit('hud', drone_id, {
+ 'heading': data['heading'],
+ 'groundspeed': 0.0
+ })
+
+ except Exception as e:
+ print(f"WebSocket message processing error: {e}")
+
+ def stop(self):
+ """停止接收器"""
+ self.running = False
+
+class DroneMonitor(Node):
+ # Subscribe to drone ROS2 topics
+ _instance = None # Singleton pattern to prevent duplicate nodes
+
+ def __init__(self):
+ # Use a unique node name with timestamp to avoid conflicts on restart
+ import time
+ node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
+ super().__init__(node_name)
+ self.signals = DroneSignals()
+ self.drone_topics = {}
+ self.lock = Lock()
+
+ self.arm_clients = {}
+ self.takeoff_clients = {}
+ self.setpoint_pubs = {}
+ self.selected_drones = set()
+ self.latest_data = {}
+
+ # 定義需要過濾的模式
+ self.filtered_modes = ['Mode(0x000000c0)']
+
+ # WebSocket 接收器列表
+ self.ws_receivers = []
+
+ # 串口接收器列表
+
+ # ================================================================================
+ # 【新增】儲存 GPS 資料的字典
+ # ================================================================================
+ self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
+ # ================================================================================
+
+ # ================================================================================
+ # 【新增】Socket ID 重新分配機制 (從 0 開始)
+ # ================================================================================
+ self.socket_id_mapping = {} # {原始socket_id: 重新分配的socket_id}
+ self.socket_id_counter = 0 # 當前分配到的最大socket_id
+ self.socket_id_lock = Lock() # 線程安全鎖
+ # ================================================================================
+
+ # ================================================================================
+ # 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取)
+ # ================================================================================
+ self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'}
+ self.sys_to_socket_id = {} # {sys_id: assigned_socket_id} e.g. {'sys11': 0}
+ # ================================================================================
+ self.serial_receivers = []
+
+ # ================================================================================
+ # 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client）
+ # ================================================================================
+ # 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件
+ self.command_long_clients = {} # {drone_id: CommandLongClient}
+ self.client_lock = Lock() # 保護 clients 字典的訪問
+ self.client_counter = 0 # 用於生成唯一的 client 節點名稱
+ self.executor = None # 將在 gui.py 中設置，用於添加新的 clients
+ # ================================================================================
+
+ # ================================================================================
+ # PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto）
+ # ================================================================================
+ self.position_target_clients = {} # {drone_id: PositionTargetGlobalIntClient}
+ self.pos_client_counter = 0
+ # ================================================================================
+
+ # 主题检测定时器
+ self.create_timer(1.0, self.scan_topics)
+
+ def get_next_socket_id(self):
+ """获取下一个可用的 socket_id（从 0 开始连续分配）"""
+ with self.socket_id_lock:
+ current_id = self.socket_id_counter
+ self.socket_id_counter += 1
+ return current_id
+
+ def get_or_assign_socket_id(self, original_socket_id):
+ """根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配）
+ 同一個原始 socket_id 會得到同一個分配的 ID
+ """
+ original_socket_id = str(original_socket_id)
+
+ with self.socket_id_lock:
+ if original_socket_id not in self.socket_id_mapping:
+ # 分配新的 socket_id
+ self.socket_id_mapping[original_socket_id] = self.socket_id_counter
+ self.socket_id_counter += 1
+
+ return self.socket_id_mapping[original_socket_id]
+
+ def get_or_create_client(self, drone_id):
+ """為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件"""
+ with self.client_lock:
+ if drone_id not in self.command_long_clients:
+ try:
+ # 生成唯一的 client 節點名稱
+ self.client_counter += 1
+ unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
+ client = CommandLongClient(node_name=unique_name)
+ self.command_long_clients[drone_id] = client
+ print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
+
+ # 將新 client 添加到主執行器（這樣它的回調才能被處理）
+ if self.executor:
+ self.executor.add_node(client)
+ print(f" ✓ 將 {drone_id} 的 client 添加到主執行器")
+
+ except TypeError:
+ # 舊版 CommandLongClient 不支持 node_name 參數，使用預設
+ client = CommandLongClient()
+ self.command_long_clients[drone_id] = client
+ print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
+
+ if self.executor:
+ self.executor.add_node(client)
+ print(f" ✓ 將 {drone_id} 的 client 添加到主執行器")
+
+ except Exception as e:
+ print(f"⚠️ 無法為 {drone_id} 創建 CommandLongClient: {e}")
+ return None
+ return self.command_long_clients[drone_id]
+
+ def get_or_create_position_client(self, drone_id):
+ """為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。"""
+ if PositionTargetGlobalIntClient is None:
+ return None
+ with self.client_lock:
+ if drone_id not in self.position_target_clients:
+ try:
+ self.pos_client_counter += 1
+ unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
+ client = PositionTargetGlobalIntClient(node_name=unique_name)
+ self.position_target_clients[drone_id] = client
+ print(f" ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})")
+ if self.executor:
+ self.executor.add_node(client)
+ print(f" ✓ 將 {drone_id} 的 position client 添加到主執行器")
+ except Exception as e:
+ print(f"⚠️ 無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}")
+ return None
+ return self.position_target_clients[drone_id]
+
+ def scan_topics(self):
+ topics = self.get_topic_names_and_types()
+ drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
+
+ found_drones = set()
+ for topic_name, _ in topics:
+ if match := drone_pattern.match(topic_name):
+ sys_id, topic_type = match.groups()
+ found_drones.add(sys_id)
+ with self.lock:
+ self.drone_topics.setdefault(sys_id, set()).add(topic_type)
+
+ for sys_id in found_drones:
+ # 为每个 sys_id 分配 socket_id（如果还没有分配）
+ # 注意：如果后续 summary 提供了 socket_id，会使用 summary 的映射覆盖
+ if sys_id not in self.sys_to_socket_id:
+ # 暂时所有 ROS2 topic 共享同一个 socket_id = 0
+ self.sys_to_socket_id[sys_id] = 0
+
+ subs_attr = f'drone_{sys_id}_subs'
+ if not hasattr(self, subs_attr):
+ self.setup_drone(sys_id)
+ else:
+ # 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱）
+ subs = getattr(self, subs_attr, {})
+ if isinstance(subs, dict) and 'position_ned' not in subs:
+ base_topic = f'/fc_network/vehicle/{sys_id}'
+ try:
+ pos_ned_sub = self.create_subscription(
+ Odometry,
+ f'{base_topic}/position_ned',
+ lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+ 10
+ )
+ subs['position_ned'] = pos_ned_sub
+ setattr(self, subs_attr, subs) # 明確保存更新後的字典
+ except Exception as e:
+ pass
+
+ def setup_drone(self, sys_id):
+ # sys_id 格式： sys11, sys12, ...
+ base_topic = f'/fc_network/vehicle/{sys_id}'
+
+ # Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務)
+ self.arm_clients[sys_id] = self.create_client(
+ CommandBool,
+ f'{base_topic}/cmd/arming'
+ )
+ self.takeoff_clients[sys_id] = self.create_client(
+ CommandTOL,
+ f'{base_topic}/cmd/takeoff'
+ )
+
+ # Add setpoint publisher
+ self.setpoint_pubs[sys_id] = self.create_publisher(
+ Point,
+ f'{base_topic}/setpoint_position/local',
+ 10
+ )
+
+ subs = {
+ 'battery': self.create_subscription(
+ BatteryState,
+ f'{base_topic}/battery',
+ lambda msg, sid=sys_id: self.battery_callback(sid, msg),
+ 10
+ ),
+ 'position': self.create_subscription(
+ NavSatFix,
+ f'{base_topic}/position',
+ lambda msg, sid=sys_id: self.gps_callback(sid, msg),
+ 10
+ ),
+ 'summary': self.create_subscription(
+ String,
+ f'{base_topic}/summary',
+ lambda msg, sid=sys_id: self.summary_callback(sid, msg),
+ 10
+ ),
+ 'vfr_hud': self.create_subscription(
+ VfrHud,
+ f'{base_topic}/vfr_hud',
+ lambda msg, sid=sys_id: self.hud_callback(sid, msg),
+ 10
+ ),
+ 'position_ned': self.create_subscription(
+ Odometry,
+ f'{base_topic}/position_ned',
+ lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+ 10
+ )
+ }
+
+ setattr(self, f'drone_{sys_id}_subs', subs)
+
+ # ================================================================================
+ # 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式）
+ # ================================================================================
+ MODE_MAPPING = {
+ "STABILIZE": 0,
+ "ACRO": 1,
+ "ALT_HOLD": 2,
+ "AUTO": 3,
+ "GUIDED": 4,
+ "LOITER": 5,
+ "RTL": 6,
+ "CIRCLE": 7,
+ "POSITION": 8,
+ "LAND": 9,
+ "OF_LOITER": 10,
+ "DRIFT": 11,
+ "SPORT": 13,
+ "FLIP": 14,
+ "AUTOTUNE": 15,
+ "POSHOLD": 16,
+ "BRAKE": 17,
+ "THROW": 18,
+ "AVOID_ADSB": 19,
+ "GUIDED_NOGPS": 20,
+ "SMART_RTL": 21,
+ }
+ # ================================================================================
+
+ async def set_mode(self, drone_id, mode_name):
+ """使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
+ try:
+ # 解析 drone_id 提取 sysid
+ parts = drone_id.split('_')
+ if len(parts) < 2:
+ print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
+ return False
+ sysid = int(parts[-1])
+
+ # 獲取模式對應的 custom_mode 值
+ custom_mode = self.MODE_MAPPING.get(mode_name)
+ if custom_mode is None:
+ print(f"❌ [SET_MODE] 未知模式: {mode_name}")
+ return False
+
+ print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
+
+ # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+ client = self.get_or_create_client(drone_id)
+ if not client:
+ print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
+ return False
+
+ # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+ result = await client.change_mode_async(
+ target_sysid=sysid,
+ custom_mode=float(custom_mode),
+ target_compid=0,
+ base_mode=1.0,
+ timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
+ )
+
+ if result and result.success:
+ print(f"✅ [SET_MODE] 模式切換成功")
+ return True
+ else:
+ print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
+ return False
+ except Exception as e:
+ print(f"❌ [SET_MODE] 例外錯誤: {e}")
+ traceback.print_exc()
+ return False
+
+ async def arm_drone(self, drone_id, arm):
+ """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
+ try:
+ # 解析 drone_id 提取 sysid
+ parts = drone_id.split('_')
+ if len(parts) < 2:
+ print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
+ return False
+ sysid = int(parts[-1])
+
+ action_name = "解鎖" if arm else "上鎖"
+ print(f"\n📢 [ARM] {drone_id} → {action_name}")
+
+ # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+ client = self.get_or_create_client(drone_id)
+ if not client:
+ print(f"❌ [ARM] CommandLongClient 無法初始化")
+ return False
+
+ # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+ result = await client.arm_disarm_async(
+ target_sysid=sysid,
+ arm=arm,
+ target_compid=0,
+ timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
+ )
+
+ if result and result.success:
+ print(f"✅ [ARM] {action_name}成功")
+ return True
+ else:
+ print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
+ return False
+ except Exception as e:
+ print(f"❌ [ARM] 例外錯誤: {e}")
+ traceback.print_exc()
+ return False
+
+ async def takeoff_drone(self, drone_id, altitude):
+ """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
+ try:
+ # 解析 drone_id 提取 sysid
+ parts = drone_id.split('_')
+ if len(parts) < 2:
+ print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
+ return False
+ sysid = int(parts[-1])
+
+ print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
+
+ # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+ client = self.get_or_create_client(drone_id)
+ if not client:
+ print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
+ return False
+
+ # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+ result = await client.takeoff_async(
+ target_sysid=sysid,
+ altitude_m=float(altitude),
+ target_compid=0,
+ timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
+ )
+
+ if result and result.success:
+ print(f"✅ [TAKEOFF] 起飛成功")
+ return True
+ else:
+ print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
+ return False
+ except Exception as e:
+ print(f"❌ [TAKEOFF] 例外錯誤: {e}")
+ traceback.print_exc()
+ return False
+
+ def send_setpoint(self, drone_id, x, y, z):
+ """Send setpoint position command"""
+ if drone_id not in self.setpoint_pubs:
+ return False
+
+ msg = Point()
+ msg.x = float(x)
+ msg.y = float(y)
+ msg.z = float(z)
+
+ self.setpoint_pubs[drone_id].publish(msg)
+ return True
+
+ def quaternion_to_euler(self, q):
+ sinr_cosp = 2 * (q.w * q.x + q.y * q.z)
+ cosr_cosp = 1 - 2 * (q.x**2 + q.y**2)
+ roll = math.atan2(sinr_cosp, cosr_cosp)
+
+ sinp = 2 * (q.w * q.y - q.z * q.x)
+ pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp)
+
+ siny_cosp = 2 * (q.w * q.z + q.x * q.y)
+ cosy_cosp = 1 - 2 * (q.y**2 + q.z**2)
+ yaw = math.atan2(siny_cosp, cosy_cosp)
+
+ return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
+
+ # callbacks
+ def attitude_callback(self, drone_id, msg):
+ if hasattr(msg, 'orientation'):
+ roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
+ self.latest_data[(drone_id, 'attitude')] = {
+ 'roll': roll,
+ 'pitch': pitch,
+ 'yaw': yaw,
+ 'rates': (msg.angular_velocity.x,
+ msg.angular_velocity.y,
+ msg.angular_velocity.z)
+ }
+
+ def battery_callback(self, sys_id, msg):
+ # 使用映射獲取實際的 drone_id
+ actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+ # 如果還沒有從 summary 獲取到映射，則不處理
+ if actual_drone_id is None:
+ return
+
+ self.latest_data[(actual_drone_id, 'battery')] = {
+ 'voltage': msg.voltage
+ }
+
+ def state_callback(self, drone_id, msg):
+ mode = msg.mode
+ if mode in self.filtered_modes:
+ return
+ self.latest_data[(drone_id, 'state')] = {
+ 'mode': msg.mode,
+ 'armed': msg.armed
+ }
+
+ def summary_callback(self, sys_id, msg):
+ """處理 summary topic (JSON 格式)"""
+ try:
+ data = json.loads(msg.data)
+ mode = data.get('mode_name', 'UNKNOWN')
+ if mode in self.filtered_modes:
+ return
+
+ # 從 summary 獲取原始 socket_id，並映射到分配的 socket_id
+ original_socket_id = data.get('socket_id')
+ if original_socket_id is not None:
+ # 使用原始 socket_id 獲取或分配統一的 socket_id
+ assigned_socket_id = self.get_or_assign_socket_id(original_socket_id)
+ else:
+ # 如果沒有 socket_id，使用 sys_to_socket_id 映射
+ assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0)
+
+ sysid = data.get('sysid')
+ if sysid is not None:
+ actual_drone_id = f's{assigned_socket_id}_{sysid}'
+
+ # ================================================================================
+ # 【關鍵】保存 sys_id 到 actual_drone_id 的映射
+ # ================================================================================
+ self.sys_to_actual_id[sys_id] = actual_drone_id
+ # ================================================================================
+ else:
+ # 如果沒有 sysid，使用 sys_id 中的數字
+ sys_num = sys_id.replace('sys', '')
+ actual_drone_id = f's{assigned_socket_id}_{sys_num}'
+ self.sys_to_actual_id[sys_id] = actual_drone_id
+ print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
+
+ # 先發送連接類型資訊
+ self.signals.update_signal.emit('connection_type', actual_drone_id, {
+ 'type': 'ROS2'
+ })
+
+ self.latest_data[(actual_drone_id, 'state')] = {
+ 'mode': mode,
+ 'armed': data.get('armed', False),
+ 'socket_id': original_socket_id,
+ 'sysid': sysid,
+ 'vehicle_type': data.get('vehicle_type'),
+ 'autopilot': data.get('autopilot'),
+ 'gps_fix': data.get('gps_fix'),
+ 'gps_fix_type': data.get('gps_fix'),
+ 'connected': data.get('connected')
+ }
+ except json.JSONDecodeError as e:
+ print(f"Error parsing summary JSON for {sys_id}: {e}")
+ except Exception as e:
+ print(f"Error in summary_callback for {sys_id}: {e}")
+
+ def gps_callback(self, sys_id, msg):
+ # 使用映射獲取實際的 drone_id
+ actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+ # 如果還沒有從 summary 獲取到映射，則不處理
+ if actual_drone_id is None:
+ return
+
+ self.latest_data[(actual_drone_id, 'gps')] = {
+ 'lat': msg.latitude,
+ 'lon': msg.longitude,
+ 'alt': msg.altitude
+ }
+
+ # ================================================================================
+ # 【新增】儲存 GPS 資料到 drone_gps 字典
+ # ================================================================================
+ self.drone_gps[actual_drone_id] = {
+ 'lat': msg.latitude,
+ 'lon': msg.longitude,
+ 'alt': msg.altitude
+ }
+ # ================================================================================
+
+ def local_vel_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'velocity')] = {
+ 'vx': msg.x,
+ 'vy': msg.y,
+ 'vz': msg.z
+ }
+
+ def altitude_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'altitude')] = {
+ 'altitude': msg.data
+ }
+
+ def local_pose_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'local_pose')] = {
+ 'x': msg.x,
+ 'y': msg.y,
+ 'z': msg.z
+ }
+
+ def hud_callback(self, sys_id, msg):
+ # 使用映射獲取實際的 drone_id
+ actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+ # 如果還沒有從 summary 獲取到映射，則不處理
+ if actual_drone_id is None:
+ return
+
+ self.latest_data[(actual_drone_id, 'hud')] = {
+ 'airspeed': msg.airspeed,
+ 'groundspeed': msg.groundspeed,
+ 'heading': msg.heading,
+ 'throttle': msg.throttle,
+ 'alt': msg.altitude,
+ 'climb': msg.climb
+ }
+
+ def position_ned_callback(self, sys_id, msg):
+ """處理 position_ned topic (nav_msgs/msg/Odometry)
+ NED 座標系中的位置（在 msg.pose.pose.position）：
+ - x: 北向位移 (m)
+ - y: 東向位移 (m)
+ - z: 向下位移 (m) - 負值表示向下，轉換為高度需要 * (-1)
+ """
+ try:
+ # 使用映射獲取實際的 drone_id
+ actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+ # 如果還沒有從 summary 獲取到映射，則不處理
+ if actual_drone_id is None:
+ return
+
+ # 從 Odometry 消息中提取位置數據 (msg.pose.pose.position)
+ # Odometry 結構：header, child_frame_id, pose(包含PoseWithCovariance), twist
+ x = msg.pose.pose.position.y # NED 座標系中交換 x/y（與 local_pose 對齐）
+ y = msg.pose.pose.position.x
+ z = -msg.pose.pose.position.z # 將向下的 NED z 轉換為向上的高度（z 為負表示向下）
+
+ # 儲存高度信息
+ self.latest_data[(actual_drone_id, 'altitude')] = {
+ 'altitude': z
+ }
+
+ # 儲存本地位置信息
+ self.latest_data[(actual_drone_id, 'local_pose')] = {
+ 'x': x,
+ 'y': y,
+ 'z': z
+ }
+
+ # 發送信號給 GUI 更新高度顯示
+ self.signals.update_signal.emit('altitude', actual_drone_id, {
+ 'altitude': z
+ })
+ except Exception as e:
+ pass
+
+ def loss_rate_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'loss_rate')] = {
+ 'loss_rate': msg.data
+ }
+
+ def ping_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'ping')] = {
+ 'ping': msg.data
+ }
+
+ def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
+ """啟動串口 MAVLink 連接"""
+ connection_name = f"Serial_{port.replace('/', '_')}"
+ receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
+ receiver.start()
+ self.serial_receivers.append(receiver)
+ print(f"Started serial connection on {port} at {baudrate} baud")
+ return receiver
\ No newline at end of file
diff --git a/src/GUI/drone_panel.py b/src/GUI/drone_panel.py
new file mode 100644
index 0000000..49e560e
--- /dev/null
+++ b/src/GUI/drone_panel.py
@@ -0,0 +1,586 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox)
+from PyQt6.QtCore import pyqtSignal
+from PyQt6.QtGui import QPainter, QPen, QColor, QFont, QPolygonF
+from PyQt6.QtCore import QPointF, Qt
+import math
+
+class DronePanel(QWidget):
+ """單個無人機面板類別"""
+
+ # 定義信號
+ mode_change_requested = pyqtSignal(str) # drone_id
+ arm_requested = pyqtSignal(str) # drone_id
+ takeoff_requested = pyqtSignal(str) # drone_id
+ setpoint_requested = pyqtSignal(str) # drone_id
+ selection_changed = pyqtSignal(str, int) # drone_id, state
+
+ def __init__(self, drone_id, parent=None):
+ super().__init__(parent)
+ self.drone_id = drone_id
+
+ # 提取資訊 (格式: s{socket_seq}_{system_id}, 如 s0_11, s1_12)
+ parts = drone_id.split('_')
+ if len(parts) >= 2:
+ self.socket_seq = parts[0][1:] # socket 序號 (移除 's' 前綴)
+ self.system_id = parts[1] # system ID
+ self.display_id = f"ID:{self.system_id}" # 顯示為 ID:11, ID:12
+ else:
+ self.socket_seq = "?"
+ self.system_id = "?"
+ self.display_id = drone_id
+
+ self.attitude_indicator = None
+ self._init_ui()
+
+ def _init_ui(self):
+ """初始化UI"""
+ self.setObjectName(f"panel_{self.drone_id}")
+ self.setFixedHeight(140)
+ self.setStyleSheet("""
+ background-color: #2A2A2A;
+ border-radius: 8px;
+ """)
+
+ # 主佈局
+ main_layout = QHBoxLayout(self)
+ main_layout.setContentsMargins(8, 8, 8, 8)
+ main_layout.setSpacing(0)
+
+ # 創建內容容器（包含 info 和 control）
+ content_widget = QWidget()
+ content_widget.setStyleSheet("background-color: #333; border-radius: 6px;")
+ content_layout = QHBoxLayout(content_widget)
+ content_layout.setContentsMargins(8, 8, 8, 8)
+ content_layout.setSpacing(8)
+
+ # 左側資訊區域
+ info_widget = self._create_info_section()
+
+ # 右側態度指示器
+ attitude_widget = self._create_attitude_indicator()
+
+ # 將 info 和 attitude 加入內容容器
+ content_layout.addWidget(info_widget, 1)
+ content_layout.addWidget(attitude_widget, 0)
+
+ # 將內容容器加入主佈局
+ main_layout.addWidget(content_widget)
+
+ def _create_info_section(self):
+ """創建資訊顯示區域"""
+ info_widget = QWidget()
+ info_layout = QVBoxLayout(info_widget)
+ info_layout.setContentsMargins(0, 0, 0, 0)
+ info_layout.setSpacing(4)
+
+ # 頂部標題欄
+ header = QWidget()
+ header_layout = QHBoxLayout(header)
+ header_layout.setContentsMargins(0, 0, 0, 0)
+
+ # 勾選框
+ self.checkbox = QCheckBox()
+ self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
+ self.checkbox.setStyleSheet("""
+ QCheckBox {
+ color: #DDD;
+ }
+ QCheckBox::indicator {
+ width: 16px;
+ height: 16px;
+ border: 2px solid #888888;
+ border-radius: 3px;
+ background: transparent;
+ }
+ QCheckBox::indicator:checked {
+ background-color: #7FFFD4;
+ border: 2px solid #888888;
+ }
+ """)
+ self.checkbox.stateChanged.connect(
+ lambda state: self.selection_changed.emit(self.drone_id, state)
+ )
+
+ # ID 顯示
+ id_label = QLabel(self.display_id)
+ id_label.setStyleSheet("""
+ font-weight: bold;
+ font-size: 14px;
+ color: #7FFFD4;
+ min-width: 80px;
+ """)
+
+ header_layout.addWidget(self.checkbox)
+ header_layout.addWidget(id_label)
+ header_layout.addStretch()
+
+ info_layout.addWidget(header)
+
+ # 第一行：狀態 (模式 + ARM狀態)
+ status_row = self._create_status_row()
+ info_layout.addWidget(status_row)
+
+ # 第二行：電池（拆成百分比與電壓兩欄）
+ battery_row = self._create_battery_row()
+ info_layout.addWidget(battery_row)
+
+ # 第三行：高度
+ altitude_row = self._create_altitude_row()
+ info_layout.addWidget(altitude_row)
+
+ # 第四行：航向 + 速度
+ nav_row = self._create_nav_row()
+ info_layout.addWidget(nav_row)
+
+ return info_widget
+
+ def _create_attitude_indicator(self):
+ """創建態度指示器（ADI 人工地平儀）"""
+ self.attitude_indicator = AttitudeIndicator(self.drone_id)
+ self.attitude_indicator.setFixedSize(90, 100)
+ return self.attitude_indicator
+
+ def _create_status_row(self):
+ """創建狀態行"""
+ status_row = QWidget()
+ status_layout = QHBoxLayout(status_row)
+ status_layout.setContentsMargins(0, 0, 0, 0)
+
+ status_title = QLabel("狀態：")
+ status_title.setStyleSheet("color: #888; min-width: 50px;")
+
+ self.mode_label = QLabel("--")
+ self.mode_label.setObjectName(f"{self.drone_id}_mode")
+ self.mode_label.setStyleSheet("color: #DDD;")
+
+ self.armed_label = QLabel("--")
+ self.armed_label.setObjectName(f"{self.drone_id}_armed")
+ self.armed_label.setStyleSheet("color: #DDD;")
+
+ status_layout.addWidget(status_title)
+ status_layout.addWidget(self.mode_label)
+ status_layout.addWidget(self.armed_label)
+ status_layout.addStretch()
+
+ return status_row
+
+ def _create_connection_row(self):
+ """創建連接資訊行 (Socket Seq + 連接方式)"""
+ connection_row = QWidget()
+ connection_layout = QHBoxLayout(connection_row)
+ connection_layout.setContentsMargins(0, 0, 0, 0)
+
+ connection_title = QLabel("Socket：")
+ connection_title.setStyleSheet("color: #888; min-width: 50px;")
+
+ # 根據解析的 drone_id 資訊設定初始值
+ self.socket_seq_label = QLabel(self.socket_seq)
+ self.socket_seq_label.setObjectName(f"{self.drone_id}_socket_seq")
+ self.socket_seq_label.setStyleSheet("color: #DDD;")
+
+ connection_sep = QLabel(" - ")
+ connection_sep.setStyleSheet("color: #DDD;")
+
+ # 設定連接方式顯示
+ connection_type_map = {
+ 'r': 'ROS2',
+ 'u': 'UDP',
+ 's': 'Serial',
+ 'w': 'WS'
+ }
+ connection_type = connection_type_map.get(self.type_prefix, 'Unknown')
+
+ self.connection_type_label = QLabel(connection_type)
+ self.connection_type_label.setObjectName(f"{self.drone_id}_connection_type")
+ self.connection_type_label.setStyleSheet("color: #DDD;")
+
+ connection_layout.addWidget(connection_title)
+ connection_layout.addWidget(self.socket_seq_label)
+ connection_layout.addWidget(connection_sep)
+ connection_layout.addWidget(self.connection_type_label)
+ connection_layout.addStretch()
+
+ return connection_row
+
+ def _create_battery_row(self):
+ """創建電池行"""
+ battery_row = QWidget()
+ battery_layout = QHBoxLayout(battery_row)
+ battery_layout.setContentsMargins(0, 0, 0, 0)
+ # 顯示百分比
+ battery_title = QLabel("電池：")
+ battery_title.setStyleSheet("color: #888; min-width: 50px;")
+
+ self.battery_pct_label = QLabel("--")
+ self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
+ self.battery_pct_label.setStyleSheet("color: #DDD;")
+
+ # 分隔符
+ separator1 = QLabel(" - ")
+ separator1.setStyleSheet("color: #DDD;")
+
+ # 顯示電壓
+ self.battery_vol_label = QLabel("--")
+ self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
+ self.battery_vol_label.setStyleSheet("color: #DDD;")
+
+ # 分隔符
+ separator2 = QLabel(" - ")
+ separator2.setStyleSheet("color: #DDD;")
+
+ # 顯示電池節數 (S count)
+ self.battery_cells_label = QLabel("--")
+ self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
+ self.battery_cells_label.setStyleSheet("color: #DDD;")
+
+ battery_layout.addWidget(battery_title)
+ battery_layout.addWidget(self.battery_pct_label)
+ battery_layout.addWidget(separator1)
+ battery_layout.addWidget(self.battery_vol_label)
+ battery_layout.addWidget(separator2)
+ battery_layout.addWidget(self.battery_cells_label)
+ battery_layout.addStretch()
+
+ return battery_row
+
+ def _create_altitude_row(self):
+ """創建高度和速度行"""
+ altitude_row = QWidget()
+ altitude_layout = QHBoxLayout(altitude_row)
+ altitude_layout.setContentsMargins(0, 0, 0, 0)
+
+ altitude_title = QLabel("高度：")
+ altitude_title.setStyleSheet("color: #888; min-width: 50px;")
+
+ self.altitude_label = QLabel("--")
+ self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
+ self.altitude_label.setStyleSheet("color: #DDD;")
+
+ speed_title = QLabel("速度：")
+ speed_title.setStyleSheet("color: #888; margin-left: 10px;")
+
+ self.speed_label = QLabel("--")
+ self.speed_label.setObjectName(f"{self.drone_id}_speed")
+ self.speed_label.setStyleSheet("color: #DDD;")
+
+ altitude_layout.addWidget(altitude_title)
+ altitude_layout.addWidget(self.altitude_label)
+ altitude_layout.addWidget(speed_title)
+ altitude_layout.addWidget(self.speed_label)
+ altitude_layout.addStretch()
+
+ return altitude_row
+
+ def _create_position_row(self):
+ """位置行已移除（位置座標不再顯示於面板）。"""
+ return QWidget()
+
+ def _create_nav_row(self):
+ """創建導航行（已移除，不再顯示）"""
+ return QWidget()
+
+ def update_field(self, field, text, color=None):
+ """更新指定欄位的值"""
+ label = self.findChild(QLabel, f"{self.drone_id}_{field}")
+ if label and label.text() != text:
+ label.setText(text)
+ if color:
+ label.setStyleSheet(f"color: {color};")
+
+ def set_connection_info(self, socket_seq, connection_type):
+ """設定連接資訊（Socket Seq 和連接方式）
+ connection_type: 'UDP' | 'Serial' | 'WS'
+ """
+ self.socket_seq_label.setText(str(socket_seq))
+
+ # 顯示友善的連接方式
+ type_display = {
+ 'UDP': 'UDP',
+ 'Serial': 'Serial',
+ 'WS': 'WS'
+ }.get(connection_type, connection_type)
+
+ self.connection_type_label.setText(type_display)
+
+
+ def update_attitude(self, heading, roll, pitch):
+ """更新態度指示器"""
+ if self.attitude_indicator:
+ self.attitude_indicator.update_attitude(heading, roll, pitch)
+
+ def get_checkbox(self):
+ """獲取勾選框"""
+ return self.checkbox
+
+ def set_checked(self, checked):
+ """設置勾選狀態"""
+ self.checkbox.setChecked(checked)
+
+ def is_checked(self):
+ """獲取勾選狀態"""
+ return self.checkbox.isChecked()
+
+class SocketGroupPanel(QWidget):
+ # 定義信號
+ group_selection_changed = pyqtSignal(str, int) # socket_id, state
+
+ def __init__(self, socket_id, color='#AAAAAA', socket_type=None, parent=None):
+ super().__init__(parent)
+ self.socket_id = socket_id
+ self.color = color
+ self.socket_type = socket_type
+ self._init_ui()
+
+ def _init_ui(self):
+ """初始化UI"""
+ self.setObjectName(f"socket_group_{self.socket_id}")
+ self.setStyleSheet("""
+ background-color: #1E1E1E;
+ border-radius: 12px;
+ """)
+
+ layout = QVBoxLayout(self)
+ layout.setContentsMargins(10, 10, 10, 10)
+ layout.setSpacing(6)
+
+ # Socket 分組標題行 - 包含勾選框
+ title_row = QWidget()
+ title_layout = QHBoxLayout(title_row)
+ title_layout.setContentsMargins(0, 0, 0, 0)
+
+ # 分組勾選框
+ self.group_checkbox = QCheckBox()
+ self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
+ self.group_checkbox.setStyleSheet(f"""
+ QCheckBox {{ color: #DDD; }}
+ QCheckBox::indicator {{
+ width: 14px;
+ height: 14px;
+ border: 2px solid #888888;
+ border-radius: 3px;
+ background: transparent;
+ }}
+ QCheckBox::indicator:checked {{
+ background-color: {self.color};
+ border: 2px solid #888888;
+ }}
+ QCheckBox::indicator:indeterminate {{
+ background-color: #666;
+ border: 2px solid #888888;
+ }}
+ """)
+ self.group_checkbox.stateChanged.connect(
+ lambda state: self.group_selection_changed.emit(self.socket_id, state)
+ )
+
+ # Socket 分組標題
+ if self.socket_type:
+ title_text = f"{self.socket_type} {self.socket_id}"
+ else:
+ title_text = f"Socket {self.socket_id}"
+ self.title_label = QLabel(title_text)
+ self.title_label.setStyleSheet(f"""
+ font-weight: bold;
+ font-size: 16px;
+ color: {self.color};
+ margin-bottom: 8px;
+ padding: 4px 8px;
+ border-radius: 6px;
+ """)
+
+ title_layout.addWidget(self.group_checkbox)
+ title_layout.addWidget(self.title_label)
+ title_layout.addStretch()
+
+ layout.addWidget(title_row)
+
+ # 創建子容器用於放置該 socket 下的所有無人機面板
+ self.drones_container = QWidget()
+ self.drones_layout = QVBoxLayout(self.drones_container)
+ self.drones_layout.setContentsMargins(0, 0, 0, 0)
+ self.drones_layout.setSpacing(4)
+
+ layout.addWidget(self.drones_container)
+
+ def add_drone_panel(self, panel):
+ """添加無人機面板到分組"""
+ self.drones_layout.addWidget(panel)
+
+ def clear_drones(self):
+ """清空所有無人機面板"""
+ while self.drones_layout.count():
+ item = self.drones_layout.takeAt(0)
+ if item.widget():
+ item.widget().setParent(None)
+
+ def get_checkbox(self):
+ """獲取分組勾選框"""
+ return self.group_checkbox
+
+ def set_checked(self, checked):
+ """設置分組勾選狀態"""
+ self.group_checkbox.setChecked(checked)
+
+ def set_socket_type(self, conn_type):
+ """設置 socket 類型並更新標題"""
+ self.title_label.setText(f"{conn_type} {self.socket_id}")
+
+ def set_check_state(self, state):
+ """設置分組勾選狀態（支持半選）"""
+ self.group_checkbox.setCheckState(state)
+
+
+class AttitudeIndicator(QWidget):
+ """
+ 人工地平儀 (ADI) — 仿 Mission Planner 風格
+ 上半部顯示 roll/pitch 人工地平儀，下方細條顯示航向
+ """
+
+ def __init__(self, drone_id, parent=None):
+ super().__init__(parent)
+ self.drone_id = drone_id
+ self.heading = 0.0 # 航向 yaw (0–360)
+ self.roll = 0.0 # 滾轉 (deg, left- negative)
+ self.pitch = 0.0 # 俯仰 (deg, nose-up positive)
+ self.setStyleSheet("background-color: transparent;")
+
+ def update_attitude(self, heading, roll, pitch):
+ self.heading = heading % 360
+ self.roll = roll
+ self.pitch = pitch
+ self.update()
+
+ # ------------------------------------------------------------------ helpers
+ def _adi_rect(self):
+ """Returns the square rect used for the ADI ball."""
+ w, h = self.width(), self.height()
+ side = min(w, h - 14) # leave 14 px at bottom for heading strip
+ x = (w - side) / 2
+ y = 0
+ return x, y, side, side
+
+ # ------------------------------------------------------------------ paint
+ def paintEvent(self, event):
+ p = QPainter(self)
+ p.setRenderHint(QPainter.RenderHint.Antialiasing)
+ self._draw_adi(p)
+ self._draw_heading_strip(p)
+
+ # ---- artificial horizon ------------------------------------------------
+ def _draw_adi(self, p):
+ from PyQt6.QtGui import QPainterPath
+ x0, y0, side, _ = self._adi_rect()
+ cx = x0 + side / 2
+ cy = y0 + side / 2
+ r = side / 2 - 1
+
+ # clip to circle
+ clip_path = QPainterPath()
+ clip_path.addEllipse(QPointF(cx, cy), r, r)
+ p.setClipPath(clip_path)
+
+ # pixels-per-degree for pitch (10 deg ≈ side/5)
+ ppd = side / 50.0
+
+ # ---- rotate + translate canvas for roll & pitch
+ p.save()
+ p.translate(cx, cy)
+ p.rotate(-self.roll) # ✅ 負值：NED 坐標系轉換
+ pitch_offset = self.pitch * ppd
+
+ # sky (above horizon)
+ sky_color = QColor(30, 100, 180)
+ p.fillRect(int(-r*2), int(-r*2 + pitch_offset), int(r*4), int(r*4), sky_color)
+
+ # ground (below horizon)
+ ground_color = QColor(140, 90, 40)
+ p.fillRect(int(-r*2), int(pitch_offset), int(r*4), int(r*4), ground_color)
+
+ # horizon line
+ p.setPen(QPen(QColor("#FFFFFF"), 2))
+ p.drawLine(int(-r), int(pitch_offset), int(r), int(pitch_offset))
+
+ # pitch ladder (every 10°, ±30°)
+ p.setPen(QPen(QColor(255, 255, 255, 180), 1))
+ p.setFont(QFont("Arial", 6))
+ for deg in range(-30, 31, 10):
+ if deg == 0:
+ continue
+ yy = int(pitch_offset - deg * ppd)
+ half = int(r * (0.35 if deg % 20 == 0 else 0.22))
+ p.drawLine(-half, yy, half, yy)
+
+ p.restore()
+ p.setClipping(False)
+
+ # ---- roll arc & tick marks (outside clip, fixed frame) ----
+ p.save()
+ p.translate(cx, cy)
+
+ arc_r = r - 2
+ p.setPen(QPen(QColor("#FFFFFF"), 1))
+ # draw arc from -60° to +60° (Qt arc: 0=3o'clock, CCW, 16ths of deg)
+ p.drawArc(int(-arc_r), int(-arc_r), int(2*arc_r), int(2*arc_r),
+ (90 - 60) * 16, 120 * 16)
+
+ # tick marks at 0, ±10, ±20, ±30, ±45, ±60
+ for deg in [0, 10, 20, 30, 45, 60, -10, -20, -30, -45, -60]:
+ rad = math.radians(deg - 90)
+ tick = 6 if deg % 30 == 0 else 4
+ x1 = arc_r * math.cos(rad)
+ y1 = arc_r * math.sin(rad)
+ x2 = (arc_r - tick) * math.cos(rad)
+ y2 = (arc_r - tick) * math.sin(rad)
+ p.drawLine(QPointF(x1, y1), QPointF(x2, y2))
+
+ # roll pointer triangle (rotates with roll)
+ p.rotate(-self.roll) # ✅ 負值：NED 坐標系轉換
+ ptr_r = arc_r - 1
+ tri = QPolygonF([
+ QPointF(0, -ptr_r),
+ QPointF(-5, -ptr_r + 9),
+ QPointF(5, -ptr_r + 9),
+ ])
+ p.setBrush(QColor("#FFFFFF"))
+ p.setPen(Qt.PenStyle.NoPen)
+ p.drawPolygon(tri)
+ p.restore()
+
+ # ---- fixed aircraft symbol ----
+ p.save()
+ p.translate(cx, cy)
+ p.setPen(QPen(QColor("#FFD700"), 2))
+ # left wing
+ p.drawLine(int(-r*0.5), 0, int(-r*0.15), 0)
+ p.drawLine(int(-r*0.15), 0, int(-r*0.15), int(r*0.12))
+ # right wing
+ p.drawLine(int(r*0.15), 0, int(r*0.5), 0)
+ p.drawLine(int(r*0.15), 0, int(r*0.15), int(r*0.12))
+ # centre dot
+ p.setBrush(QColor("#FFD700"))
+ p.drawEllipse(QPointF(0, 0), 2.5, 2.5)
+ p.restore()
+
+ # ---- outer ring ----
+ p.setPen(QPen(QColor("#888888"), 1))
+ p.setBrush(Qt.BrushStyle.NoBrush)
+ p.drawEllipse(QPointF(cx, cy), r, r)
+
+ # ---- heading strip at bottom ------------------------------------------
+ def _draw_heading_strip(self, p):
+ w = self.width()
+ x0, y0, side, _ = self._adi_rect()
+ strip_y = y0 + side
+ strip_h = self.height() - strip_y
+ if strip_h < 4:
+ return
+
+ # background
+ p.fillRect(0, int(strip_y), w, strip_h, QColor(30, 30, 30))
+
+ # heading text centred (bigger)
+ p.setPen(QPen(QColor("#FFFFFF")))
+ p.setFont(QFont("Arial", 10, QFont.Weight.Bold))
+ hdg_str = f"{int(self.heading)}°"
+ p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str)
\ No newline at end of file
diff --git a/src/GUI/gui.py b/src/GUI/gui.py
new file mode 100644
index 0000000..a332ed3
--- /dev/null
+++ b/src/GUI/gui.py
@@ -0,0 +1,1930 @@
+#!/usr/bin/env python3
+import rclpy
+from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout,
+ QWidget, QLabel, QSplitter, QScrollArea,
+ QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem,
+ QHeaderView, QPushButton, QCheckBox, QLineEdit,
+ QComboBox, QDialog)
+from PyQt6.QtCore import Qt, QTimer
+from PyQt6.QtGui import QColor
+import sys
+import asyncio
+import json
+import subprocess
+import time
+import traceback
+
+# 導入分離的類別
+from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
+from map_layout import DroneMap
+from drone_panel import DronePanel, SocketGroupPanel
+from comm_panel import CommPanel
+from overview_table import OverviewTable
+
+# ================================================================================
+# 導入任務規劃器、執行器、發送器、群組
+# ================================================================================
+from mission_planner import FormationPlanner, MissionType
+from command_sender import MavlinkSender, Ros2CommandSender
+from mission_executor import MissionExecutor, MissionState
+from mission_group import (
+ MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS
+)
+# ================================================================================
+
+class ControlStationUI(QMainWindow):
+ VERSION = '2.0.8'
+
+ def __init__(self):
+ super().__init__()
+ self.setWindowTitle(f'GCS v{self.VERSION}')
+ self.resize(1400, 900)
+
+ # 初始化消息隊列（用於線程安全的 GUI 更新）
+ import queue
+ self.message_queue = queue.Queue()
+
+ # 初始化ROS2 Monitor（ROS2 本身在 main() 中已初始化）
+ self.monitor = DroneMonitor()
+ self.monitor.signals.update_signal.connect(self.update_ui)
+
+ # ROS执行器配置
+ self.executor = rclpy.executors.SingleThreadedExecutor()
+ self.executor.add_node(self.monitor)
+
+ # 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加
+ self.monitor.executor = self.executor
+
+ # 定时处理ROS事件
+ self.timer = QTimer()
+ self.timer.timeout.connect(self.spin_ros)
+ self.timer.start(10)
+
+ # 驅動 asyncio 事件循環的定時器（新增 - 關鍵！）
+ # 這允許異步任務（如 arm_drone）能夠執行
+ self.asyncio_timer = QTimer()
+ self.asyncio_timer.timeout.connect(self._spin_asyncio)
+ self.asyncio_timer.start(10) # 每 10ms 驅動一次 asyncio
+
+ # 初始化 panel 和 map 更新（10Hz）
+ self.panel_map_timer = QTimer()
+ self.panel_map_timer.timeout.connect(self._update_panel_and_map)
+ self.panel_map_timer.start(100) # 10Hz
+
+ # 消息隊列處理定時器（來自線程的 GUI 更新）
+ self.msg_queue_timer = QTimer()
+ self.msg_queue_timer.timeout.connect(self._process_message_queue)
+ self.msg_queue_timer.start(50) # 每 50ms 檢查一次
+
+ # 快取消息數據，以便在沒有新消息時使用上一次的值
+ self._message_cache = {}
+
+ # 初始化UI
+ self.drones = {}
+ self.socket_groups = {}
+ self.socket_types = {}
+
+ self.socket_colors = {
+ '0': '#00BFFF', # 天藍色 (DeepSkyBlue)
+ '1': '#FFD700', # 金色 (Gold)
+ '2': '#FF6969', # 淺紅色 (Light Red)
+ '3': '#FF69B4', # 熱粉紅 (HotPink)
+ '4': '#00FA9A', # 中春綠 (MediumSpringGreen)
+ '5': '#9370DB', # 中紫色 (MediumPurple) - 串口
+ '6': '#FFA500', # 橙色 (Orange)
+ '7': '#20B2AA', # 淺海綠 (LightSeaGreen)
+ '8': '#7CFC00', # 草綠色 (LawnGreen)
+ '9': '#FF8C00', # 深橙色 (DarkOrange)
+ 'default': '#AAAAAA' # 灰色
+ }
+
+ self.drone_positions = {}
+ self.drone_headings = {}
+ # 初始化地圖
+ self.drone_map = DroneMap()
+ # 初始化連接列表
+ self.udp_receivers = []
+ self.udp_connections = []
+ self.ws_connections = []
+ self.serial_receivers = []
+ self.serial_connections = []
+
+ # ================================================================================
+ # 初始化任務規劃器
+ # ================================================================================
+ self.mission_planner = FormationPlanner(
+ spacing=5.0, # 5 公尺間距
+ base_altitude=10.0, # 基準高度 10 公尺
+ altitude_diff=2.0 # 高低差 2 公尺
+ )
+ # ================================================================================
+
+ # ================================================================================
+ # 初始化指令發送器（所有群組共用）
+ # ================================================================================
+ # 走 ROS2 /fc_network/vehicle/pos_global_int service；per-drone client + asyncio polling
+ self.command_sender = Ros2CommandSender(monitor=self.monitor)
+ # ================================================================================
+
+ # ================================================================================
+ # 多任務群組管理
+ # ================================================================================
+ self.mission_groups = {} # group_id → MissionGroup
+ self.group_panels = {} # group_id → GroupPanel
+ self.active_group_id = None # 當前 active 的 group
+ self._group_counter = 0 # 用來產生 group_id
+ self._pending_box_assign = None # 框選後直接分配到的 group_id
+ # ================================================================================
+
+ self.init_ui()
+
+ def init_ui(self):
+ main_splitter = QSplitter(Qt.Orientation.Horizontal)
+
+ # 左側 TabWidget
+ self.left_tab = QTabWidget()
+
+ # — 分頁 1：Drone Panel
+ self.drone_panel_container = QWidget()
+ self.drone_panel_layout = QVBoxLayout(self.drone_panel_container)
+ self.drone_panel_layout.setAlignment(Qt.AlignmentFlag.AlignTop)
+ self.drone_panel_layout.setSpacing(0)
+ self.drone_panel_layout.setContentsMargins(10, 10, 10, 10)
+
+ scroll = QScrollArea()
+ scroll.setWidget(self.drone_panel_container)
+ scroll.setWidgetResizable(True)
+ self.left_tab.addTab(scroll, "無人載具")
+
+ # — 分頁 2：Overview Table
+ self.overview_table = OverviewTable()
+ self.left_tab.addTab(self.overview_table, "總覽")
+
+ # — 分頁 3：通訊設定
+ self.comm_panel = CommPanel()
+ self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added)
+ self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added)
+ self.comm_panel.serial_connection_added.connect(self.handle_serial_connection_added)
+ self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection)
+ self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection)
+ self.comm_panel.serial_connection_toggled.connect(self.toggle_serial_connection)
+ self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection)
+ self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection)
+ self.comm_panel.serial_connection_removed.connect(self.remove_serial_connection)
+ self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout))
+
+ self.left_tab.addTab(self.comm_panel, "通訊")
+
+ # 右侧容器
+ right_container = QWidget()
+ right_layout = QVBoxLayout(right_container)
+ right_layout.setContentsMargins(10, 10, 10, 10)
+ right_layout.setSpacing(10)
+
+ # ========== 任務群組 Tab ==========
+ group_header = QHBoxLayout()
+
+ # 標題 + 收起/展開按鈕
+ title_layout = QHBoxLayout()
+ group_title = QLabel("任務群組")
+ group_title.setStyleSheet(
+ "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
+ title_layout.addWidget(group_title)
+
+ # 收起/展開按鈕
+ self.toggle_group_btn = QPushButton("▼")
+ self.toggle_group_btn.setStyleSheet("""
+ QPushButton { background-color: #555; color: white; border: none;
+ padding: 3px 8px; border-radius: 3px; font-size: 12px; font-weight: bold;
+ min-width: 30px; max-width: 30px; }
+ QPushButton:hover { background-color: #666; }
+ """)
+ self.toggle_group_btn.setToolTip("收起/展開任務群組")
+ self.toggle_group_btn.clicked.connect(self._toggle_group_panel)
+ title_layout.addWidget(self.toggle_group_btn)
+ title_layout.addStretch()
+
+ group_header.addLayout(title_layout)
+ group_header.addStretch()
+
+ clear_traj_btn = QPushButton("清除軌跡")
+ clear_traj_btn.setStyleSheet("""
+ QPushButton { background-color: #EF5350; color: white; border: none;
+ padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; }
+ QPushButton:hover { background-color: #E53935; }
+ """)
+ clear_traj_btn.clicked.connect(self.drone_map.clear_trajectories)
+ group_header.addWidget(clear_traj_btn)
+
+ right_layout.addLayout(group_header)
+
+ self.group_tab_widget = QTabWidget()
+ self.group_tab_widget.setStyleSheet("""
+ QTabWidget::pane { border: 1px solid #444; background-color: #2B2B2B; }
+ QTabBar::tab {
+ background-color: #333; color: #AAA; border: 1px solid #444;
+ padding: 5px 12px; margin-right: 2px; border-top-left-radius: 4px;
+ border-top-right-radius: 4px; font-size: 12px;
+ }
+ QTabBar::tab:selected { background-color: #2B2B2B; color: #FFF; border-bottom-color: #2B2B2B; }
+ QTabBar::tab:hover { background-color: #3A3A3A; }
+ """)
+ self.group_tab_widget.setFixedHeight(150)
+ self.group_tab_widget.currentChanged.connect(self._on_group_tab_changed)
+ right_layout.addWidget(self.group_tab_widget)
+
+ # 🌟 新增：保存群組面板的展開狀態
+ self.group_panel_expanded = True
+
+ # 預設建立 Group A
+ self._add_mission_group()
+
+ # 添加地圖
+ right_layout.addWidget(self.drone_map.get_widget())
+ self.drone_map.get_gps_signal().connect(self.handle_map_click)
+ self.drone_map.get_drone_clicked_signal().connect(self.handle_drone_clicked)
+ self.drone_map.get_clear_all_drone_selection_signal().connect(self.handle_clear_all_drone_selection)
+ self.drone_map.get_toggle_select_all_drones_signal().connect(self.handle_toggle_select_all_drones)
+
+ # ================================================================================
+ # 連接地圖信號（任務模式改由 Group Tab 控制，不再從地圖下拉選單）
+ # ================================================================================
+ self.drone_map.get_rectangle_selected_signal().connect(self.handle_rectangle_selected)
+ self.drone_map.get_route_confirmed_signal().connect(self.handle_route_confirmed)
+ self.drone_map.get_drone_box_selected_signal().connect(self._handle_drone_box_selected)
+ # ================================================================================
+
+ main_splitter.addWidget(self.left_tab)
+ main_splitter.addWidget(right_container)
+ main_splitter.setSizes([400, 1000])
+
+ self.setCentralWidget(main_splitter)
+
+
+ # ================================================================================
+ # 連線管理
+ # ================================================================================
+
+ def handle_udp_connection_added(self, ip, port):
+ new_conn = {'name': f'UDP {len(self.udp_connections) + 1}', 'ip': ip, 'port': port, 'enabled': True}
+ receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'], self.monitor)
+ receiver.start()
+ self.udp_receivers.append(receiver)
+ new_conn['receiver'] = receiver
+ self.udp_connections.append(new_conn)
+ self.comm_panel.add_udp_panel(new_conn)
+ self.statusBar().showMessage(f"已添加 UDP 連接: {ip}:{port}", 3000)
+
+ def handle_ws_connection_added(self, url):
+ new_conn = {'name': f'WS {len(self.ws_connections) + 1}', 'url': url, 'enabled': True}
+ receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'], self.monitor)
+ receiver.start()
+ self.monitor.ws_receivers.append(receiver)
+ new_conn['receiver'] = receiver
+ self.ws_connections.append(new_conn)
+ self.comm_panel.add_ws_panel(new_conn)
+ self.statusBar().showMessage(f"已添加 WebSocket 連接: {url}", 3000)
+
+ def create_drone_panel(self, drone_id):
+ panel = DronePanel(drone_id)
+ panel.mode_change_requested.connect(self.handle_mode_change)
+ panel.arm_requested.connect(self.handle_arm)
+ panel.takeoff_requested.connect(self.handle_takeoff)
+ panel.setpoint_requested.connect(self.handle_single_setpoint)
+ panel.selection_changed.connect(self.handle_drone_selection)
+ return panel
+
+ def toggle_ws_connection(self, conn, btn, status_label):
+ if conn.get('enabled', False):
+ if 'receiver' in conn and conn['receiver']:
+ conn['receiver'].stop()
+ conn['enabled'] = False
+ btn.setText("啟動")
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+ self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000)
+ else:
+ receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'])
+ receiver.start()
+ self.monitor.ws_receivers.append(receiver)
+ conn['receiver'] = receiver
+ conn['enabled'] = True
+ btn.setText("停止")
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000)
+
+ def remove_ws_connection(self, conn, panel):
+ if 'receiver' in conn and conn['receiver']:
+ conn['receiver'].stop()
+ if conn['receiver'] in self.monitor.ws_receivers:
+ self.monitor.ws_receivers.remove(conn['receiver'])
+ if conn in self.ws_connections:
+ self.ws_connections.remove(conn)
+ self.comm_panel.remove_ws_connection_from_list(conn)
+ panel.setParent(None)
+ panel.deleteLater()
+ self.statusBar().showMessage(f"已移除 WebSocket 連接: {conn['url']}", 3000)
+
+ def toggle_udp_connection(self, conn, btn, status_label):
+ if conn.get('enabled', False):
+ if 'receiver' in conn and conn['receiver']:
+ conn['receiver'].stop()
+ conn['enabled'] = False
+ btn.setText("啟動")
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+ self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000)
+ else:
+ receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'])
+ receiver.start()
+ self.udp_receivers.append(receiver)
+ conn['receiver'] = receiver
+ conn['enabled'] = True
+ btn.setText("停止")
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000)
+
+ def remove_udp_connection(self, conn, panel):
+ if 'receiver' in conn and conn['receiver']:
+ conn['receiver'].stop()
+ if conn['receiver'] in self.udp_receivers:
+ self.udp_receivers.remove(conn['receiver'])
+ if conn in self.udp_connections:
+ self.udp_connections.remove(conn)
+ self.comm_panel.remove_udp_connection_from_list(conn)
+ panel.setParent(None)
+ panel.deleteLater()
+ self.statusBar().showMessage(f"已移除 UDP 連接: {conn['ip']}:{conn['port']}", 3000)
+
+ def handle_serial_connection_added(self, port, baudrate):
+ conn = {'name': 'Serial', 'port': port, 'baudrate': baudrate, 'enabled': False, 'receiver': None}
+ self.serial_connections.append(conn)
+ self.comm_panel.add_serial_panel(conn)
+ self.statusBar().showMessage(f"已添加 Serial 連接: {port} @ {baudrate}", 3000)
+
+ def toggle_serial_connection(self, conn, btn, status_label):
+ if conn.get('enabled', False):
+ if conn.get('receiver'):
+ conn['receiver'].stop()
+ conn['receiver'] = None
+ conn['enabled'] = False
+ btn.setText("啟動")
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+ self.statusBar().showMessage(f"已停止 Serial 連接: {conn['port']}", 3000)
+ else:
+ try:
+ receiver = self.monitor.start_serial_connection(conn['port'], conn['baudrate'])
+ conn['receiver'] = receiver
+ conn['enabled'] = True
+ btn.setText("停止")
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000)
+ except Exception as e:
+ print(f"❌ Serial 連接啟動失敗: {str(e)}")
+ traceback.print_exc()
+ self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000)
+
+ def remove_serial_connection(self, conn, panel):
+ if conn.get('enabled', False) and conn.get('receiver'):
+ conn['receiver'].stop()
+ if conn in self.serial_connections:
+ self.serial_connections.remove(conn)
+ self.comm_panel.remove_serial_connection_from_list(conn)
+ panel.setParent(None)
+ panel.deleteLater()
+ self.statusBar().showMessage(f"已移除 Serial 連接: {conn['port']}", 3000)
+
+ def create_socket_group_panel(self, socket_id):
+ color = self.socket_colors.get(socket_id, self.socket_colors['default'])
+ socket_type = self.socket_types.get(socket_id, None)
+ panel = SocketGroupPanel(socket_id, color, socket_type)
+ panel.group_selection_changed.connect(self.handle_group_selection)
+ return panel
+
+ # ================================================================================
+ # 無人機操作
+ # ================================================================================
+
+ def handle_mode_change(self, drone_id):
+ print(f"\n📢 [GUI] handle_mode_change 被调用")
+ print(f" drone_id: {drone_id}")
+
+ # 從 active group 的 mode_combo 讀取模式
+ group = self._get_active_group()
+ if group:
+ panel = self.group_panels.get(group.group_id)
+ mode = panel.mode_combo.currentText() if panel else "GUIDED"
+ print(f" 从群组读取模式: {mode}")
+ else:
+ mode = "GUIDED"
+ print(f" 没有活跃群组，使用默认模式: {mode}")
+
+ loop = asyncio.get_event_loop()
+ future = self.monitor.set_mode(drone_id, mode)
+ loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}"))
+
+ def handle_arm(self, drone_id):
+ print(f"\n📢 [GUI] handle_arm 被調用")
+ print(f" drone_id: {drone_id}")
+ loop = asyncio.get_event_loop()
+ print(f" 獲得事件循環: {loop}")
+ arm_state = not self.monitor.get_arm_state(drone_id)
+ print(f" 當前鎖定狀態: {not arm_state}, 目標狀態: {arm_state}")
+ print(f" 準備調用 arm_drone(drone_id={drone_id}, arm={arm_state})")
+ coro = self.monitor.arm_drone(drone_id, arm_state)
+ print(f" arm_drone() 返回類型: {type(coro)}")
+ print(f" coroutine 對象: {coro}")
+ action_text = f"{'解鎖' if arm_state else '上鎖'} {drone_id}"
+ print(f" 準備提交到事件循環: {action_text}")
+ task = loop.create_task(self.handle_service_response(coro, action_text))
+ print(f" task 已創建: {task}")
+ print(f" 已提交到事件循環\n")
+
+ def handle_takeoff(self, drone_id):
+ loop = asyncio.get_event_loop()
+ future = self.monitor.takeoff_drone(drone_id, 10.0)
+ loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}"))
+
+ def handle_setpoint_selected(self):
+ """發送位置命令到 active group 的所有選中無人機"""
+ group = self._get_active_group()
+ if not group:
+ self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+ return
+
+ try:
+ x = float(self.x_input.text() or '0')
+ y = float(self.y_input.text() or '0')
+ z = float(self.z_input.text() or '0')
+ for drone_id in group.selected_drone_ids:
+ if self.monitor.send_setpoint(drone_id, x, y, z):
+ self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000)
+ else:
+ self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000)
+ except ValueError:
+ self.statusBar().showMessage("座標格式錯誤", 3000)
+
+ def handle_single_setpoint(self, drone_id):
+ try:
+ x = float(self.x_input.text() or '0')
+ y = float(self.y_input.text() or '0')
+ z = float(self.z_input.text() or '0')
+ if self.monitor.send_setpoint(drone_id, x, y, z):
+ self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000)
+ else:
+ self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000)
+ except ValueError:
+ self.statusBar().showMessage("座標格式錯誤", 3000)
+
+ async def handle_service_response(self, future, action):
+ print(f"\n📥 [GUI] handle_service_response 開始處理: {action}")
+ print(f" Future/Coroutine 類型: {type(future)}")
+ print(f" Future/Coroutine 對象: {future}")
+ try:
+ print(f" ├─ 等待 future/coroutine 完成...")
+ print(f" └─ (這是一個阻塞等待，直到服務返回)\n")
+ result = await future
+ print(f"\n ✓ Future/Coroutine 完成，接收到返回值！")
+ print(f" ├─ 返回值類型: {type(result)}")
+ print(f" ├─ 返回值內容: {result}")
+ print(f" ├─ 返回值 == True: {result == True}")
+ print(f" ├─ 返回值 is True: {result is True}")
+ print(f" └─ bool(返回值): {bool(result)}")
+
+ # 詳細檢查 result 結構（用於調試 fc_network 回傳值）
+ if hasattr(result, '__dict__'):
+ print(f" └─ 返回值屬性: {result.__dict__}")
+
+ if result:
+ print(f"\n✅ {action} 成功 (result={result})")
+ self.statusBar().showMessage(f"{action} 成功", 3000)
+ else:
+ print(f"\n❌ {action} 失敗 (result={result})")
+ self.statusBar().showMessage(f"{action} 失敗", 3000)
+ except asyncio.CancelledError:
+ print(f"⚠️ {action} 被取消")
+ except Exception as e:
+ print(f"❌ {action} 錯誤: {str(e)}")
+ traceback.print_exc()
+ self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
+
+ def handle_arm_selected(self):
+ """解鎖 active group 的所有選中無人機"""
+ group = self._get_active_group()
+ if not group:
+ self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+ return
+
+ selected = list(group.selected_drone_ids)
+ if not selected:
+ self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000)
+ return
+
+ print(f"\n📢 [GUI] handle_arm_selected 被調用")
+ print(f" 已選擇的無人機: {selected}")
+ loop = asyncio.get_event_loop()
+ for drone_id in selected:
+ print(f" 準備批次解鎖: {drone_id}")
+ coro = self.monitor.arm_drone(drone_id, True)
+ print(f" arm_drone 返回: {coro}")
+ # 使用 run_coroutine_threadsafe() 正確地在事件循環中運行
+ asyncio.run_coroutine_threadsafe(
+ self.handle_service_response(coro, f"批次解鎖 {drone_id}"),
+ loop
+ )
+ print(f" handle_arm_selected 完成\n")
+
+ def handle_takeoff_selected(self):
+ """起飛 active group 的所有選中無人機"""
+ group = self._get_active_group()
+ if not group:
+ self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+ return
+
+ selected = list(group.selected_drone_ids)
+ if not selected:
+ self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000)
+ return
+
+ loop = asyncio.get_event_loop()
+ for drone_id in selected:
+ future = self.monitor.takeoff_drone(drone_id, 10.0)
+ loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
+
+ # ================================================================================
+ # 任務群組管理
+ # ================================================================================
+
+ def _next_group_id(self):
+ """產生下一個群組 ID (A, B, C, ...)"""
+ gid = chr(ord('A') + self._group_counter)
+ self._group_counter += 1
+ return gid
+
+ def _toggle_group_panel(self):
+ """🌟 收起/展開任務群組面板"""
+ if self.group_panel_expanded:
+ # 收起
+ self.group_tab_widget.setFixedHeight(0)
+ self.group_tab_widget.hide()
+ self.toggle_group_btn.setText("▶")
+ self.toggle_group_btn.setToolTip("展開任務群組")
+ self.group_panel_expanded = False
+ else:
+ # 展開
+ self.group_tab_widget.setFixedHeight(150)
+ self.group_tab_widget.show()
+ self.toggle_group_btn.setText("▼")
+ self.toggle_group_btn.setToolTip("收起任務群組")
+ self.group_panel_expanded = True
+
+ def _add_mission_group(self):
+ """新增一個任務群組"""
+ gid = self._next_group_id()
+ color = GROUP_COLORS[(self._group_counter - 1) % len(GROUP_COLORS)]
+ group = MissionGroup(gid, color)
+ self.mission_groups[gid] = group
+
+ panel = GroupPanel(group)
+ panel.assign_drones_requested.connect(self._handle_assign_drones)
+ panel.mission_type_changed.connect(self._handle_mission_type_changed)
+ panel.start_requested.connect(self._handle_group_start)
+ panel.pause_requested.connect(self._handle_group_pause)
+ panel.stop_requested.connect(self._handle_group_stop)
+ panel.mode_change_requested.connect(self._handle_group_mode_change)
+ panel.arm_requested.connect(self._handle_group_arm)
+ panel.takeoff_requested.connect(self._handle_group_takeoff)
+ panel.box_select_requested.connect(self._handle_box_select)
+ panel.select_all_requested.connect(self._handle_select_all_for_group)
+ panel.clear_group_requested.connect(self._handle_clear_group)
+ panel.add_group_requested.connect(self._add_mission_group)
+ panel.delete_group_requested.connect(self._handle_delete_group)
+ self.group_panels[gid] = panel
+
+ # 用帶顏色的 tab 標題
+ scroll = QScrollArea()
+ scroll.setWidget(panel)
+ scroll.setWidgetResizable(True)
+ idx = self.group_tab_widget.addTab(scroll, f"Group {gid}")
+ self.group_tab_widget.tabBar().setTabTextColor(idx, QColor(color))
+
+ # 切換到新 group 的 tab
+ self.group_tab_widget.setCurrentIndex(idx)
+
+ self.statusBar().showMessage(f"已新增 Group {gid}", 2000)
+
+ # 更新刪除按鈕的啟用/禁用狀態
+ self._update_delete_buttons_state()
+
+ def _on_group_tab_changed(self, index):
+ """切換 group tab — 只需切換 active group 並刷新 UI"""
+ if index < 0:
+ self.active_group_id = None
+ return
+
+ tab_text = self.group_tab_widget.tabText(index)
+ gid = tab_text.replace("Group ", "")
+ if gid not in self.mission_groups:
+ return
+
+ self.active_group_id = gid
+ group = self.mission_groups[gid]
+
+ # 同步地圖的任務模式
+ self.drone_map.set_mission_mode(group.mission_type)
+
+ # 統一刷新所有 UI
+ self.refresh_selection_ui()
+
+ def _get_active_group(self):
+ """取得當前 active 的 MissionGroup"""
+ if self.active_group_id and self.active_group_id in self.mission_groups:
+ return self.mission_groups[self.active_group_id]
+ return None
+
+ def refresh_selection_ui(self):
+ """統一刷新所有 UI 元素：左側 drone 勾選、左側 socket 勾選、右側 group panel"""
+ group = self._get_active_group()
+ selected = group.selected_drone_ids if group else set()
+ current_group_id = group.group_id if group else None
+
+ # 左側 drone checkbox
+ for drone_id, panel in self.drones.items():
+ checkbox = panel.get_checkbox()
+ if checkbox:
+ blocked = (
+ current_group_id is not None and
+ self._is_drone_selected_by_other_group(drone_id, current_group_id)
+ )
+ checkbox.blockSignals(True)
+ checkbox.setStyleSheet(self._get_drone_checkbox_style(blocked))
+ checkbox.setChecked(drone_id in selected)
+ checkbox.setEnabled(not blocked or drone_id in selected)
+ checkbox.blockSignals(False)
+
+ # 左側 socket checkbox
+ for socket_id in self.socket_groups.keys():
+ self.refresh_socket_checkbox(socket_id, selected, current_group_id)
+
+ # 右側 group panel
+ if group:
+ panel = self.group_panels.get(group.group_id)
+ if panel:
+ panel.update_drone_list()
+ panel.update_status()
+
+ def _get_drone_checkbox_style(self, blocked=False):
+ """回傳 drone checkbox 樣式；被其他 group 佔用時顯示紅色填滿。"""
+ blocked_style = """
+ QCheckBox::indicator:disabled {
+ background-color: #D32F2F;
+ border: 2px solid #A32020;
+ }
+ """ if blocked else ""
+ return f"""
+ QCheckBox {{
+ color: #DDD;
+ }}
+ QCheckBox::indicator {{
+ width: 16px;
+ height: 16px;
+ border: 2px solid #888888;
+ border-radius: 3px;
+ background: transparent;
+ }}
+ QCheckBox::indicator:checked {{
+ background-color: #7FFFD4;
+ border: 2px solid #888888;
+ }}
+ {blocked_style}
+ """
+
+ def refresh_socket_checkbox(self, socket_id, selected_ids, current_group_id=None):
+ """根據 selected_ids 推導並更新 socket 的勾選狀態"""
+ drone_ids = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
+ if not drone_ids:
+ return
+
+ socket_widget = self.socket_groups.get(socket_id)
+ if not socket_widget:
+ return
+
+ checkbox = socket_widget.findChild(QCheckBox, f"socket_{socket_id}_checkbox")
+ if not checkbox:
+ return
+
+ effective_drone_ids = [
+ did for did in drone_ids
+ if current_group_id is None
+ or did in selected_ids
+ or not self._is_drone_selected_by_other_group(did, current_group_id)
+ ]
+ checked_count = sum(1 for did in effective_drone_ids if did in selected_ids)
+
+ checkbox.blockSignals(True)
+ if checked_count == 0:
+ checkbox.setCheckState(Qt.CheckState.Unchecked)
+ elif checked_count == len(effective_drone_ids):
+ checkbox.setCheckState(Qt.CheckState.Checked)
+ else:
+ checkbox.setCheckState(Qt.CheckState.PartiallyChecked)
+ checkbox.setEnabled(len(effective_drone_ids) > 0)
+ checkbox.blockSignals(False)
+
+
+ # 【已遷移至 refresh_socket_checkbox()】舊的 update_group_checkbox_state() 已廢棄
+
+ def _handle_assign_drones(self, group_id):
+ """開啟無人機分配對話框 — 過濾被占用的drone"""
+ group = self.mission_groups.get(group_id)
+ if not group:
+ return
+ all_ids = list(self.drones.keys())
+
+ # 預先選中：目前 group 的 selected_drone_ids
+ pre_selected = set(group.selected_drone_ids)
+
+ dialog = DroneAssignDialog(self, all_ids, pre_selected, {})
+ if dialog.exec() == QDialog.DialogCode.Accepted:
+ requested = dialog.get_selected()
+
+ allowed = set()
+ blocked = set()
+
+ for did in requested:
+ if self._is_drone_selected_by_other_group(did, group_id):
+ blocked.add(did)
+ else:
+ allowed.add(did)
+
+ group.selected_drone_ids = allowed
+
+ # 只有當操作目標組是 active 組時，才更新 UI
+ if group_id == self.active_group_id:
+ self.refresh_selection_ui()
+
+ panel = self.group_panels.get(group_id)
+ if panel:
+ panel.update_drone_list()
+ panel.update_status()
+
+ if blocked:
+ self.statusBar().showMessage(
+ f"⚠ Group {group_id}: {len(blocked)} 台已被其他 group 勾選，未加入", 4000
+ )
+ else:
+ self.statusBar().showMessage(
+ f"Group {group_id}: 已分配 {len(group.selected_drone_ids)} 台無人機", 3000
+ )
+
+ def _handle_mission_type_changed(self, group_id, mission_type):
+ """群組任務類型切換"""
+ group = self.mission_groups.get(group_id)
+ if group:
+ group.mission_type = mission_type
+ # 如果是 active group，同步更新地圖的選擇模式
+ if group_id == self.active_group_id:
+ self.drone_map.set_mission_mode(mission_type)
+
+ def _create_executor_for_group(self, group):
+ """為群組建立 MissionExecutor"""
+ executor = MissionExecutor(
+ sender=self.command_sender,
+ drone_gps=self.monitor.drone_gps,
+ monitor=self.monitor,
+ arrival_radius=2.0,
+ tick_rate_hz=2.0,
+ )
+ executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached)
+ executor.task_status_changed.connect(self._on_task_status_changed)
+ executor.mission_completed.connect(
+ lambda gid=group.group_id: self._on_group_mission_completed(gid))
+ group.executor = executor
+
+ def _handle_group_start(self, group_id):
+ """啟動群組任務"""
+ group = self.mission_groups.get(group_id)
+ if not group:
+ return
+ if group.planned_waypoints is None:
+ self.statusBar().showMessage(f"⚠ Group {group_id}: 請先規劃任務", 3000)
+ return
+ if group.executor is None:
+ self._create_executor_for_group(group)
+ group.executor.start(group.planned_waypoints)
+ panel = self.group_panels.get(group_id)
+ if panel:
+ panel.update_status()
+ self.statusBar().showMessage(f"🚀 Group {group_id}: 任務已啟動", 3000)
+
+ def _handle_group_pause(self, group_id):
+ """暫停/恢復群組任務"""
+ group = self.mission_groups.get(group_id)
+ if not group or not group.executor:
+ return
+ if group.executor.state == MissionState.RUNNING:
+ group.executor.pause()
+ self.statusBar().showMessage(f"⏸ Group {group_id}: 任務已暫停", 3000)
+ elif group.executor.state == MissionState.PAUSED:
+ group.executor.resume()
+ self.statusBar().showMessage(f"▶ Group {group_id}: 任務已恢復", 3000)
+ panel = self.group_panels.get(group_id)
+ if panel:
+ panel.update_status()
+
+ def _handle_group_stop(self, group_id):
+ """停止群組任務"""
+ group = self.mission_groups.get(group_id)
+ if not group:
+ return
+ if group.executor:
+ group.executor.stop()
+ group.planned_waypoints = None
+ self.drone_map.clear_mission_plan_for_group(group_id)
+ panel = self.group_panels.get(group_id)
+ if panel:
+ panel.update_status()
+ panel.clear_mission_info()
+ self.statusBar().showMessage(f"■ Group {group_id}: 任務已停止", 3000)
+
+ def _handle_group_mode_change(self, group_id, mode):
+ """切換群組內所有無人機的飛行模式"""
+ print(f"\n📢 [GUI] _handle_group_mode_change 被调用", flush=True)
+ print(f" group_id: {group_id}, mode: {mode}", flush=True)
+
+ group = self.mission_groups.get(group_id)
+ if not group:
+ print(f"❌ 找不到群組: {group_id}", flush=True)
+ return
+
+ if not group.selected_drone_ids:
+ print(f"⚠️ 群組中沒有無人機", flush=True)
+ self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
+ return
+
+ print(f" 準備為 {len(group.selected_drone_ids)} 台無人機切換模式...", flush=True)
+ self.statusBar().showMessage(f"正在切換模式...", 1000)
+
+ # 使用 asyncio 執行（通過事件循環）
+ async def do_mode_changes_async():
+ print(f"\n 【異步任務】開始執行模式切換", flush=True)
+
+ for drone_id in group.selected_drone_ids:
+ print(f"\n 【切換】{drone_id} → {mode}", flush=True)
+ try:
+ result = await self.monitor.set_mode(drone_id, mode)
+
+ if result:
+ msg = f"✅ {drone_id} 切換成功"
+ print(f" {msg}", flush=True)
+ self.message_queue.put((msg, 2000))
+ else:
+ msg = f"❌ {drone_id} 切換失敗"
+ print(f" {msg}", flush=True)
+ self.message_queue.put((msg, 2000))
+
+ except Exception as e:
+ msg = f"❌ {drone_id} 錯誤: {str(e)}"
+ print(f" {msg}", flush=True)
+ traceback.print_exc()
+ self.message_queue.put((msg, 2000))
+
+ print(f"\n 【完成】模式切換任務結束\n", flush=True)
+
+ # 通過事件循環提交異步任務
+ print(f" 【排隊】將任務提交至事件循環", flush=True)
+ loop = asyncio.get_event_loop()
+ asyncio.run_coroutine_threadsafe(
+ do_mode_changes_async(),
+ loop
+ )
+
+ def _handle_group_arm(self, group_id):
+ """解鎖群組內所有無人機"""
+ print(f"\n📢 [GUI] _handle_group_arm 被調用")
+ print(f" 群組 ID: {group_id}")
+ group = self.mission_groups.get(group_id)
+ print(f" 群組存在: {group is not None}")
+ if not group:
+ print(f" ⚠️ 群組不存在，返回\n")
+ return
+ print(f" 群組內無人機: {group.selected_drone_ids}")
+ loop = asyncio.get_event_loop()
+ print(f" 事件循環: {loop}")
+
+ for drone_id in group.selected_drone_ids:
+ print(f"\n ┌─ 處理無人機: {drone_id}")
+ print(f" ├─ 準備調用 arm_drone(drone_id={drone_id}, arm=True)")
+ coro = self.monitor.arm_drone(drone_id, True)
+ print(f" ├─ arm_drone 返回類型: {type(coro)}")
+ action_text = f"解鎖 {drone_id}"
+ print(f" ├─ 準備提交到事件循環: {action_text}")
+ # 使用 run_coroutine_threadsafe() 正確地在事件循環中運行 coroutine
+ # 這是 Qt + asyncio 整合的正確方式
+ future = asyncio.run_coroutine_threadsafe(
+ self.handle_service_response(coro, action_text),
+ loop
+ )
+ print(f" ├─ Future 已創建: {future}")
+ print(f" └─ Future 將在事件循環中執行")
+
+ print(f"\n _handle_group_arm 完成（coroutine 已提交至事件循環）\n")
+
+ def _handle_group_takeoff(self, group_id, altitude):
+ """群組內所有無人機起飛"""
+ group = self.mission_groups.get(group_id)
+ if not group:
+ return
+ loop = asyncio.get_event_loop()
+ for drone_id in group.selected_drone_ids:
+ future = self.monitor.takeoff_drone(drone_id, altitude)
+ loop.create_task(self.handle_service_response(future, f"起飛 {drone_id} ({altitude}m)"))
+
+ def _handle_box_select(self, group_id):
+ """觸發地圖框選 → 框選完成後直接分配到該群組"""
+ self._pending_box_assign = group_id
+ self.drone_map.toggle_drone_box_select()
+ self.statusBar().showMessage(
+ f"請在地圖上框選要分配到 Group {group_id} 的無人機", 5000)
+
+ def _handle_drone_box_selected(self, drone_ids_json):
+ group_id = self._pending_box_assign
+ self._pending_box_assign = None
+ if not group_id:
+ return
+
+ group = self.mission_groups.get(group_id)
+ if not group:
+ return
+
+ drone_ids = json.loads(drone_ids_json)
+
+ allowed = set()
+ blocked = set()
+ for did in drone_ids:
+ if self._is_drone_selected_by_other_group(did, group_id):
+ blocked.add(did)
+ else:
+ allowed.add(did)
+
+ group.selected_drone_ids = allowed
+
+ if group_id == self.active_group_id:
+ self.refresh_selection_ui()
+
+ panel = self.group_panels.get(group_id)
+ if panel:
+ panel.update_drone_list()
+ panel.update_status()
+
+ if blocked:
+ self.statusBar().showMessage(
+ f"⚠ Group {group_id}: 部分 drone 已被其他 group 勾選，僅加入 {len(allowed)} 台", 4000
+ )
+ else:
+ self.statusBar().showMessage(
+ f"Group {group_id}: 框選分配 {len(allowed)} 台無人機", 3000
+ )
+
+ def _handle_select_all_for_group(self, group_id):
+ """全選/取消全選 — Toggle 邏輯"""
+ group = self.mission_groups.get(group_id)
+ if not group:
+ return
+
+ selectable_ids = self._get_selectable_drones_for_group(group_id) | set(group.selected_drone_ids)
+
+ # Toggle 邏輯：如果已全選，則清空；否則全選
+ if group.selected_drone_ids == selectable_ids:
+ # 已全選 → 清空
+ group.selected_drone_ids.clear()
+ msg_status = "已取消全選"
+ else:
+ # 未全選 → 全選
+ group.selected_drone_ids = set(selectable_ids)
+ msg_status = f"全選可用無人機 {len(selectable_ids)} 台"
+
+ # 只有當操作目標組是 active 組時，才更新 UI
+ if group_id == self.active_group_id:
+ self.refresh_selection_ui()
+
+ panel = self.group_panels.get(group_id)
+ if panel:
+ panel.update_drone_list()
+ panel.update_status()
+
+ self.statusBar().showMessage(
+ f"Group {group_id}: {msg_status}", 3000)
+
+ def _handle_clear_group(self, group_id):
+ """清除群組的無人機選擇"""
+ group = self.mission_groups.get(group_id)
+ if not group:
+ return
+
+ group.selected_drone_ids.clear()
+ group.planned_waypoints = None
+ if group.executor:
+ group.executor.stop()
+ self.drone_map.clear_mission_plan_for_group(group_id)
+
+ # 只有當操作目標組是 active 組時，才更新 UI
+ if group_id == self.active_group_id:
+ self.refresh_selection_ui()
+
+ panel = self.group_panels.get(group_id)
+ if panel:
+ panel.update_drone_list()
+ panel.update_status()
+ panel.clear_mission_info()
+
+ self.statusBar().showMessage(
+ f"Group {group_id}: 已清除分組", 3000)
+
+ def _handle_delete_group(self, group_id):
+ """刪除一個任務群組"""
+ # 檢查是否只有一個群組，如果是就禁止刪除
+ if len(self.mission_groups) <= 1:
+ self.statusBar().showMessage("⚠️ 至少需要保留一個群組！", 3000)
+ return
+
+ if group_id not in self.mission_groups:
+ self.statusBar().showMessage(f"Group {group_id} 不存在", 3000)
+ return
+
+ # 停止群組的執行（如果有）
+ group = self.mission_groups[group_id]
+ if group.executor:
+ group.executor.stop()
+
+ # 移除地圖上的任務計畫
+ self.drone_map.clear_mission_plan_for_group(group_id)
+
+ # 移除 tab
+ for i in range(self.group_tab_widget.count()):
+ if f"Group {group_id}" in self.group_tab_widget.tabText(i):
+ self.group_tab_widget.removeTab(i)
+ break
+
+ # 移除資料
+ del self.mission_groups[group_id]
+ if group_id in self.group_panels:
+ del self.group_panels[group_id]
+
+ # 更新 active group — 讓 currentChanged signal 自動處理
+ if self.active_group_id == group_id:
+ self.active_group_id = None
+ if self.group_tab_widget.count() > 0:
+ self.group_tab_widget.setCurrentIndex(0)
+ # _on_group_tab_changed 會自動設定新的 active group
+
+ self.statusBar().showMessage(f"已刪除 Group {group_id}", 3000)
+
+ # 更新刪除按鈕的啟用/禁用狀態
+ self._update_delete_buttons_state()
+
+ def _update_delete_buttons_state(self):
+ """根據群組數量，更新所有群組的刪除按鈕啟用/禁用狀態"""
+ # 如果只有一個群組，禁用該群組的刪除按鈕
+ # 如果有多個群組，啟用所有刪除按鈕
+ should_enable = len(self.mission_groups) > 1
+ for gid, panel in self.group_panels.items():
+ panel.set_delete_enabled(should_enable)
+
+ def _on_group_mission_completed(self, group_id):
+ """群組任務完成回呼"""
+ panel = self.group_panels.get(group_id)
+ if panel:
+ panel.update_status()
+ self.statusBar().showMessage(f"✅ Group {group_id}: 所有無人機已完成任務", 5000)
+
+ # ================================================================================
+ # UI 更新
+ # ================================================================================
+
+ def update_ui(self, msg_type, drone_id, data):
+ """只做數據快取，不在這裡更新 UI"""
+ if msg_type == 'connection_type':
+ conn_type = data.get('type', 'Unknown')
+ parts = drone_id.split('_')
+ if len(parts) >= 2 and parts[0].startswith('s'):
+ socket_id = parts[0][1:]
+ if socket_id not in self.socket_types:
+ self.socket_types[socket_id] = conn_type
+ if socket_id in self.socket_groups:
+ self.socket_groups[socket_id].set_socket_type(conn_type)
+ return
+
+ if drone_id not in self.drones:
+ self.add_drone(drone_id)
+ return
+
+ # 只做資料快取，不更新 UI - 所有 UI 更新都在 _update_panel_and_map 中進行
+ if drone_id not in self._message_cache:
+ self._message_cache[drone_id] = {}
+
+ self._message_cache[drone_id][msg_type] = data
+
+
+ # ================================================================================
+ # 勾選管理
+ # ================================================================================
+
+ def handle_group_selection(self, socket_id, state):
+ """Socket 群組勾選 — 更新該 socket 下所有 drone 的選擇狀態"""
+ group = self._get_active_group()
+ if not group:
+ return
+
+ drone_ids = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
+ selectable = []
+ blocked = []
+ for did in drone_ids:
+ if self._is_drone_selected_by_other_group(did, group.group_id):
+ blocked.append(did)
+ else:
+ selectable.append(did)
+
+ if not selectable:
+ if blocked:
+ self.statusBar().showMessage(
+ f"⚠ Socket {socket_id} 的 drone 已被其他 group 勾選，無法操作", 4000
+ )
+ self.refresh_selection_ui()
+ return
+
+ all_selectable_selected = all(did in group.selected_drone_ids for did in selectable)
+
+ if all_selectable_selected:
+ group.selected_drone_ids.difference_update(selectable)
+ else:
+ group.selected_drone_ids.update(selectable)
+ if blocked:
+ self.statusBar().showMessage(
+ f"⚠ 以下 drone 已被其他 group 勾選，略過: {', '.join(blocked)}", 4000
+ )
+
+ self.refresh_selection_ui()
+
+ def _is_drone_selected_by_other_group(self, drone_id, current_group_id):
+ """
+ 檢查該 drone 是否已被其他 group 勾選
+
+ Args:
+ drone_id: 無人機 ID
+ current_group_id: 當前 group 的 ID
+
+ Returns:
+ True 如果 drone 已被其他 group 選擇，否則 False
+ """
+ for gid, group in self.mission_groups.items():
+ # 跳過當前 group
+ if gid == current_group_id:
+ continue
+ # 檢查該 drone 是否在其他 group 中被勾選
+ if drone_id in group.selected_drone_ids:
+ return True
+ return False
+
+ def _get_selectable_drones_for_group(self, group_id):
+ """
+ 取得指定 group 可以選擇的 drone 清單（不被其他 group 佔用的）
+
+ Args:
+ group_id: group 的 ID
+
+ Returns:
+ set: 可選擇的 drone ID 集合
+ """
+ selectable = set()
+ for did in self.drones.keys():
+ if not self._is_drone_selected_by_other_group(did, group_id):
+ selectable.add(did)
+ return selectable
+
+ def handle_drone_selection(self, drone_id, state):
+ """單台 drone 勾選 — 只修改 active group 的 selected_drone_ids"""
+ group = self._get_active_group()
+ if not group:
+ return
+
+ is_checked = (state == Qt.CheckState.Checked.value)
+
+ if is_checked:
+ if self._is_drone_selected_by_other_group(drone_id, group.group_id):
+ self.statusBar().showMessage(
+ f"⚠ {drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
+ )
+ self.refresh_selection_ui()
+ return
+ group.selected_drone_ids.add(drone_id)
+ else:
+ group.selected_drone_ids.discard(drone_id)
+
+ self.refresh_selection_ui()
+
+ def handle_drone_clicked(self, drone_id):
+ """地圖上點擊 drone — 切換其選擇狀態"""
+ group = self._get_active_group()
+ if not group:
+ return
+
+ if drone_id in group.selected_drone_ids:
+ group.selected_drone_ids.remove(drone_id)
+ else:
+ # 嘗試勾選前先檢查是否被其他 group 使用
+ if self._is_drone_selected_by_other_group(drone_id, group.group_id):
+ self.statusBar().showMessage(
+ f"⚠ {drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
+ )
+ self.refresh_selection_ui()
+ return
+ group.selected_drone_ids.add(drone_id)
+
+ self.refresh_selection_ui()
+
+ def handle_toggle_select_all_drones(self):
+ """全選 / 清空 — 切換 active group 的所有 drone"""
+ group = self._get_active_group()
+ if not group:
+ return
+
+ selectable_ids = self._get_selectable_drones_for_group(group.group_id) | set(group.selected_drone_ids)
+
+ if group.selected_drone_ids == selectable_ids:
+ # 已全選 → 清空
+ group.selected_drone_ids.clear()
+ else:
+ # 未全選 → 全選
+ group.selected_drone_ids = set(selectable_ids)
+
+ self.refresh_selection_ui()
+
+ def handle_clear_all_drone_selection(self):
+ """清除 active group 的所有無人機選擇"""
+ group = self._get_active_group()
+ if not group:
+ return
+ group.selected_drone_ids.clear()
+ self.refresh_selection_ui()
+ self.statusBar().showMessage("已清除所有選擇", 2000)
+
+ # ================================================================================
+ # 任務規劃 — 點擊地圖（路由到 active group）
+ # ================================================================================
+
+ def _get_group_drones(self, group):
+ """取得群組的無人機 ID 列表（排序後）"""
+ return sorted(group.selected_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+
+ def handle_map_click(self, lat, lon):
+ """處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
+ group = self._get_active_group()
+ if not group:
+ self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+ return
+
+ mode_map = {
+ 'M_FORMATION': MissionType.M_FORMATION,
+ 'CIRCLE_FORMATION': MissionType.CIRCLE_FORMATION,
+ }
+ mission_type = mode_map.get(group.mission_type)
+ if mission_type is None:
+ return # Grid Sweep / Leader-Follower 由各自的觸發方式處理
+
+ selected_drones = self._get_group_drones(group)
+ if len(selected_drones) == 0:
+ self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
+ return
+
+ print(f"地圖點擊: {lat:.6f}, {lon:.6f} → Group {group.group_id} ({group.mission_type})")
+ panel = self.group_panels.get(group.group_id)
+ params = panel.get_mission_params() if panel else {}
+ base_alt = params.get('base_altitude', params.get('altitude', 10.0))
+ target_gps = (lat, lon, base_alt)
+ self.statusBar().showMessage(
+ f"⏳ Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台) ...", 2000)
+
+ try:
+ drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+ if drone_gps_positions is None:
+ return
+
+ waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
+ drone_gps_positions, target_gps, mission_type, params=params
+ )
+
+ group.planned_waypoints = {
+ 'drone_ids': selected_drones,
+ 'waypoints': waypoints_per_drone,
+ 'rendezvous_indices': rendezvous_indices,
+ }
+ group.center_origin = center_origin
+ self.show_planned_waypoints(group)
+
+ center_lat, center_lon, _ = center_origin
+ self.drone_map.draw_mission_plan_for_group(
+ group.group_id, group.color,
+ center_lat, center_lon, lat, lon
+ )
+
+ self._launch_verification(
+ group.mission_type, drone_gps_positions, selected_drones,
+ waypoints_per_drone, center_origin, target_gps=target_gps
+ )
+
+ panel = self.group_panels.get(group.group_id)
+ if panel:
+ panel.update_status()
+ panel.update_mission_info(center_lat, center_lon, lat, lon)
+
+ total_wps = sum(len(wps) for wps in waypoints_per_drone)
+ self.statusBar().showMessage(
+ f"✓ Group {group.group_id}: {group.mission_type} 規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
+ )
+ except Exception as e:
+ self.statusBar().showMessage(f"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
+ traceback.print_exc()
+
+ # ================================================================================
+ # 任務規劃 — 矩形選取 (Grid Sweep)
+ # ================================================================================
+
+ def handle_rectangle_selected(self, points_json):
+ group = self._get_active_group()
+ if not group:
+ self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+ return
+ if group.mission_type != 'GRID_SWEEP':
+ return # 非 Grid Sweep 模式不處理矩形選取
+
+ selected_drones = self._get_group_drones(group)
+ if len(selected_drones) == 0:
+ self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
+ return
+
+ print(f"矩形選取 → Group {group.group_id}: {points_json}")
+ try:
+ rect_corners = [(p[0], p[1]) for p in json.loads(points_json)]
+ except (json.JSONDecodeError, IndexError):
+ self.statusBar().showMessage("❌ 矩形座標解析失敗", 3000)
+ return
+
+ panel = self.group_panels.get(group.group_id)
+ params = panel.get_mission_params() if panel else {}
+ base_alt = params.get('altitude', 10.0)
+ self.statusBar().showMessage(
+ f"⏳ Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台) ...", 2000)
+ try:
+ drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+ if drone_gps_positions is None:
+ return
+
+ target_lat = sum(c[0] for c in rect_corners) / 4
+ target_lon = sum(c[1] for c in rect_corners) / 4
+ target_gps = (target_lat, target_lon, base_alt)
+
+ params['rect_corners'] = rect_corners
+ waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
+ drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
+ params=params
+ )
+
+ group.planned_waypoints = {
+ 'drone_ids': selected_drones,
+ 'waypoints': waypoints_per_drone,
+ 'rendezvous_indices': rendezvous_indices,
+ }
+ group.center_origin = center_origin
+ self.show_planned_waypoints(group)
+
+ center_lat, center_lon, _ = center_origin
+ self.drone_map.draw_mission_plan_for_group(
+ group.group_id, group.color,
+ center_lat, center_lon, target_lat, target_lon
+ )
+ self._launch_verification(
+ 'grid_sweep', drone_gps_positions, selected_drones,
+ waypoints_per_drone, center_origin, rect_corners=rect_corners
+ )
+
+ panel = self.group_panels.get(group.group_id)
+ if panel:
+ panel.update_status()
+ panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
+
+ total_wps = sum(len(wps) for wps in waypoints_per_drone)
+ self.statusBar().showMessage(
+ f"✓ Group {group.group_id}: Grid Sweep 規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
+ )
+ except Exception as e:
+ self.statusBar().showMessage(f"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
+ traceback.print_exc()
+
+ # ================================================================================
+ # 任務規劃 — 路徑確認 (Leader-Follower)
+ # ================================================================================
+
+ def handle_route_confirmed(self, points_json):
+ group = self._get_active_group()
+ if not group:
+ self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+ return
+ if group.mission_type != 'LEADER_FOLLOWER':
+ return
+
+ selected_drones = self._get_group_drones(group)
+ if len(selected_drones) == 0:
+ self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
+ return
+
+ # LEADER_FOLLOWER：把指定的領隊放到 rank 0
+ leader = getattr(group, 'leader_drone_id', None)
+ if leader and leader in selected_drones:
+ selected_drones = [leader] + [d for d in selected_drones if d != leader]
+
+ print(f"路徑確認 → Group {group.group_id}: {points_json}")
+ try:
+ route_points = json.loads(points_json)
+ route_waypoints = [(p[0], p[1]) for p in route_points]
+ except (json.JSONDecodeError, IndexError):
+ self.statusBar().showMessage("❌ 路徑座標解析失敗", 3000)
+ return
+
+ if len(route_waypoints) < 2:
+ self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000)
+ return
+
+ panel = self.group_panels.get(group.group_id)
+ params = panel.get_mission_params() if panel else {}
+ base_alt = params.get('altitude', 10.0)
+ self.statusBar().showMessage(
+ f"⏳ Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台) ...", 2000)
+
+ try:
+ drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
+ if drone_gps_positions is None:
+ return
+
+ target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints)
+ target_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints)
+ target_gps = (target_lat, target_lon, base_alt)
+
+ params['route_waypoints'] = route_waypoints
+ waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
+ drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER,
+ params=params
+ )
+
+ group.planned_waypoints = {
+ 'drone_ids': selected_drones,
+ 'waypoints': waypoints_per_drone,
+ 'rendezvous_indices': rendezvous_indices,
+ }
+ group.center_origin = center_origin
+ self.show_planned_waypoints(group)
+
+ center_lat, center_lon, _ = center_origin
+ self.drone_map.draw_mission_plan_for_group(
+ group.group_id, group.color,
+ center_lat, center_lon, target_lat, target_lon
+ )
+
+ self._launch_verification(
+ 'LEADER_FOLLOWER', drone_gps_positions, selected_drones,
+ waypoints_per_drone, center_origin,
+ target_gps=target_gps, route_waypoints=route_waypoints
+ )
+
+ panel = self.group_panels.get(group.group_id)
+ if panel:
+ panel.update_status()
+ panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
+
+ total_wps = sum(len(wps) for wps in waypoints_per_drone)
+ self.statusBar().showMessage(
+ f"✓ Group {group.group_id}: 跟隨模式規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
+ )
+ except Exception as e:
+ self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
+ traceback.print_exc()
+
+ # ================================================================================
+ # 任務執行回呼
+ # ================================================================================
+
+ def on_drone_waypoint_reached(self, drone_id, wp_index, total):
+ if wp_index >= total:
+ self.statusBar().showMessage(f"📍 {drone_id} 完成所有航點", 3000)
+ else:
+ self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000)
+
+ def _on_task_status_changed(self, drone_id, status, message):
+ """MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar"""
+ if status == "retrying":
+ self.statusBar().showMessage(f"⚠ {drone_id} {message}", 4000)
+ elif status == "fallback_loiter":
+ self.statusBar().showMessage(f"❌ {drone_id} {message}", 8000)
+ elif status == "waiting_at_barrier":
+ self.statusBar().showMessage(f"⏸ {drone_id} {message}", 3000)
+ elif status == "normal":
+ self.statusBar().showMessage(f"▶ {drone_id} {message or '已恢復'}", 2000)
+
+ # ================================================================================
+ # 輔助方法
+ # ================================================================================
+
+ def _collect_drone_gps(self, selected_drones, base_alt):
+ drone_gps_positions = []
+ for drone_id in selected_drones:
+ if hasattr(self.monitor, 'drone_gps') and drone_id in self.monitor.drone_gps:
+ gps_data = self.monitor.drone_gps[drone_id]
+ drone_gps_positions.append((gps_data['lat'], gps_data['lon'], gps_data.get('alt', base_alt)))
+ elif drone_id in self.drone_positions:
+ pos = self.drone_positions[drone_id]
+ lat_drone = 24.0 + pos[1] / 111000
+ lon_drone = 120.0 + pos[0] / (111000 * 0.9)
+ alt_drone = pos[2] if len(pos) > 2 else base_alt
+ drone_gps_positions.append((lat_drone, lon_drone, alt_drone))
+ else:
+ self.statusBar().showMessage(f"⚠ 找不到 {drone_id} 的位置資料", 3000)
+ return None
+ return drone_gps_positions
+
+ def _launch_verification(self, mission_type, drone_gps_positions,
+ selected_drones, waypoints_per_drone, origin,
+ target_gps=None, rect_corners=None, route_waypoints=None):
+ """存 JSON + 啟動 matplotlib 視覺化驗證 (獨立 process)"""
+ import os
+ data = {
+ 'mission_type': mission_type,
+ 'drone_ids': selected_drones,
+ 'drones_gps': drone_gps_positions,
+ 'waypoints': waypoints_per_drone,
+ 'origin': list(origin),
+ }
+ if target_gps:
+ data['target'] = list(target_gps)
+ if rect_corners:
+ data['rect_corners'] = rect_corners
+ if route_waypoints:
+ data['route_waypoints'] = route_waypoints
+
+ json_path = '/tmp/mission_plan.json'
+ with open(json_path, 'w') as f:
+ json.dump(data, f, indent=2)
+
+ script_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'validation', 'verify_waypoints.py')
+ subprocess.Popen([sys.executable, script_path, '--file', json_path])
+ print(f"驗證視窗已啟動: {json_path}")
+
+ def show_planned_waypoints(self, group=None):
+ pw = group.planned_waypoints if group else None
+ if not pw:
+ return
+ gid = group.group_id if group else "?"
+ print(f"\n{'=' * 60}")
+ print(f"任務規劃結果 — Group {gid}")
+ print(f"{'=' * 60}")
+ drone_ids = pw['drone_ids']
+ waypoints = pw['waypoints']
+ print(f"\n共 {len(drone_ids)} 台無人機")
+ for i, drone_id in enumerate(drone_ids):
+ wps = waypoints[i]
+ print(f"\n【{drone_id}】({len(wps)} 個航點)")
+ for j, wp in enumerate(wps):
+ print(f" WP{j}: ({wp[0]:.6f}°, {wp[1]:.6f}°, {wp[2]:.1f}m)")
+ print(f"\n{'=' * 60}")
+
+ def update_field(self, panel, drone_id, field, text, color=None):
+ if isinstance(panel, DronePanel):
+ panel.update_field(field, text, color)
+
+ def update_overview_table(self, drone_id=None, field=None, value=None):
+ if not hasattr(self, 'overview_table') or self.overview_table is None: return
+ self.overview_table.set_drones(self.drones)
+ self.overview_table.update_table(drone_id, field, value)
+
+ def get_socket_id(self, drone_id):
+ import re
+ match = re.match(r's(\d+)_(\d+)', drone_id)
+ return match.group(1) if match else 'unknown'
+
+ def add_drone(self, drone_id):
+ if drone_id in self.drones: return
+ socket_id = self.get_socket_id(drone_id)
+ panel = self.create_drone_panel(drone_id)
+ self.drones[drone_id] = panel
+ if socket_id not in self.socket_groups:
+ self.socket_groups[socket_id] = self.create_socket_group_panel(socket_id)
+ self.socket_groups[socket_id].drones_layout.addWidget(panel)
+ self.reorganize_socket_groups()
+ self.update_overview_table()
+ # 同步新 drone 到 UI
+ self.refresh_selection_ui()
+
+ def reorganize_socket_groups(self):
+ while self.drone_panel_layout.count():
+ w = self.drone_panel_layout.takeAt(0).widget()
+ if w: w.setParent(None)
+ for socket_id, group_panel in self.socket_groups.items():
+ group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
+ while group_panel.drones_layout.count():
+ w = group_panel.drones_layout.takeAt(0).widget()
+ if w: w.setParent(None)
+ def sort_key(x):
+ parts = x[1:].split('_')
+ return (int(parts[0]), int(parts[1]))
+ for did in sorted(group_drones, key=sort_key):
+ group_panel.drones_layout.addWidget(self.drones[did])
+ for socket_id in sorted(self.socket_groups.keys(), key=lambda x: int(x)):
+ self.drone_panel_layout.addWidget(self.socket_groups[socket_id])
+
+ def _update_panel_and_map(self):
+ """30Hz 定時更新 panel 和 map，批量更新 UI 以避免過度重繪"""
+ if not hasattr(self, '_message_cache') or not self._message_cache:
+ return
+
+ # 頻率監控
+ if not hasattr(self, '_map_update_time'):
+ self._map_update_time = time.time()
+ self._map_update_count = 0
+
+ self._map_update_count += 1
+ now = time.time()
+ if now - self._map_update_time >= 1.0:
+ self._map_update_time = now
+ self._map_update_count = 0
+
+ # ✅ 步驟 1: 暫停表格的即時重繪
+ if hasattr(self, 'overview_table') and self.overview_table:
+ self.overview_table.setUpdatesEnabled(False)
+
+ try:
+ start_time = time.time()
+
+ # ✅ 步驟 2: 遍歷快取中最新的資料來更新 UI
+ for drone_id in list(self._message_cache.keys()):
+ if drone_id not in self.drones:
+ continue
+
+ panel = self.drones[drone_id]
+ cached_data = self._message_cache[drone_id]
+
+ # 處理所有快取的消息類型
+ for msg_type, data in cached_data.items():
+ if msg_type == 'state':
+ mode = data.get('mode', 'UNKNOWN')
+ armed = data.get('armed', None)
+ mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55'
+ if armed is True:
+ arm_text, arm_color = "ARMED", '#55FF55'
+ elif armed is False:
+ arm_text, arm_color = "DISARMED", '#FF5555'
+ else:
+ arm_text, arm_color = "--", '#AAAAAA'
+ self.update_field(panel, drone_id, 'mode', mode, mode_color)
+ self.update_field(panel, drone_id, 'armed', arm_text, arm_color)
+ self.update_overview_table(drone_id, 'mode', mode)
+ self.update_overview_table(drone_id, 'armed', arm_text)
+
+ elif msg_type == 'battery':
+ voltage = data.get('voltage', 16)
+ cells = round(voltage / 3.95)
+ percentage = (voltage / cells - 3.7) / 0.5 * 100 if cells > 0 else 0
+ if percentage < 20: voltage_color = '#FF6464'
+ elif percentage < 50: voltage_color = '#FFA500'
+ else: voltage_color = '#FFFFFF'
+ percentage = data.get('percentage', percentage)
+ self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color)
+ self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V")
+ self.update_field(panel, drone_id, 'battery_cells', f"{cells}S")
+ self.update_overview_table(drone_id, 'battery', f"{voltage:.2f}V")
+
+ elif msg_type == 'altitude':
+ altitude = data.get('altitude', 0)
+ text = f"{altitude:.1f} m"
+ self.update_field(panel, drone_id, 'altitude', text)
+ self.update_overview_table(drone_id, 'altitude', text)
+
+ elif msg_type == 'local_pose':
+ x, y = data.get('x', 0), data.get('y', 0)
+ if not hasattr(self.monitor, 'drone_local'):
+ self.monitor.drone_local = {}
+ self.monitor.drone_local[drone_id] = {'x': x, 'y': y}
+ self.update_overview_table(drone_id, 'local', f"{x:.1f}, {y:.1f}")
+
+ elif msg_type == 'loss_rate':
+ text = f"{data.get('loss_rate', 0):.1f}%"
+ self.update_field(panel, drone_id, 'loss_rate', text)
+ self.update_overview_table(drone_id, 'loss_rate', text)
+
+ elif msg_type == 'ping':
+ text = f"{data.get('ping', 0):.1f} ms"
+ self.update_field(panel, drone_id, 'ping', text)
+ self.update_overview_table(drone_id, 'ping', text)
+
+ elif msg_type == 'velocity':
+ self.update_overview_table(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}")
+
+ elif msg_type == 'attitude':
+ roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)
+ self.update_overview_table(drone_id, 'roll', f"{roll:.1f}°")
+ self.update_overview_table(drone_id, 'pitch', f"{pitch:.1f}°")
+ self.update_overview_table(drone_id, 'yaw', f"{yaw:.1f}°")
+ panel._last_roll = roll
+ panel._last_pitch = pitch
+ if hasattr(panel, 'update_attitude'):
+ heading = self.drone_headings.get(drone_id, 0)
+ panel.update_attitude(heading, roll, pitch)
+
+ elif msg_type == 'gps':
+ gps_data = data
+ lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0)
+ self.drone_positions[drone_id] = (lat, lon)
+ alt = gps_data.get('alt', 0)
+ if not hasattr(self.monitor, 'drone_gps'):
+ self.monitor.drone_gps = {}
+ self.monitor.drone_gps[drone_id] = {'lat': lat, 'lon': lon, 'alt': alt}
+ self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°")
+ self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°")
+
+ elif msg_type == 'hud':
+ hud_data = data
+ heading = hud_data.get('heading', 0)
+ self.drone_headings[drone_id] = heading
+ groundspeed = hud_data.get('groundspeed', 0)
+ airspeed = hud_data.get('airspeed', 0)
+ throttle = hud_data.get('throttle', 0)
+ hud_alt = hud_data.get('alt', 0)
+ climb = hud_data.get('climb', 0)
+
+ self.update_overview_table(drone_id, 'heading', f"{heading:.1f}°")
+ self.update_overview_table(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
+ self.update_overview_table(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--")
+ self.update_overview_table(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--")
+ self.update_overview_table(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--")
+ self.update_overview_table(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--")
+
+ self.update_field(panel, drone_id, 'heading', f"{heading:.1f}°")
+ self.update_field(panel, drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
+ self.update_field(panel, drone_id, 'speed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
+
+ if drone_id in self.drone_positions:
+ lat, lon = self.drone_positions[drone_id]
+ self.drone_map.update_drone_position(drone_id, lat, lon, heading)
+
+ elapsed = (time.time() - start_time) * 1000
+ if elapsed > 33:
+ print(f"[WARNING] UI update took {elapsed:.1f}ms (target: <33ms)")
+
+ finally:
+ # ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪）
+ if hasattr(self, 'overview_table') and self.overview_table:
+ self.overview_table.setUpdatesEnabled(True)
+ self.overview_table.viewport().update()
+
+
+
+ def _process_message_queue(self):
+ """處理來自後台線程的消息隊列（更新 GUI 狀態欄）"""
+ try:
+ while True:
+ try:
+ message, duration = self.message_queue.get_nowait()
+ self.statusBar().showMessage(message, duration)
+ except:
+ break
+ except Exception as e:
+ print(f"❌ 消息隊列處理錯誤: {e}", flush=True)
+ traceback.print_exc()
+
+ def _spin_asyncio(self):
+ """驅動 asyncio 事件循環，允許異步任務執行
+
+ 關鍵修復：asyncio 事件循環不會自動運行。
+ 這個定時器會定期執行事件循環，讓 run_coroutine_threadsafe() 提交的任務能夠執行。
+ """
+ try:
+ loop = asyncio.get_event_loop()
+ if loop and not loop.is_closed() and not loop.is_running():
+ # 執行事件循環直到沒有掛起的任務或超時
+ # 使用 run_until_complete() 配合一個快速返回的 coroutine
+ loop.run_until_complete(asyncio.sleep(0))
+ except Exception as e:
+ # 靜默忽略任何錯誤，防止 Qt 定時器出現異常
+ # 但仍然打印詳細的堆棧跟踪以便調試
+ traceback.print_exc()
+
+ def spin_ros(self):
+ try:
+ # 仅在 ROS2 正常工作时才尝试 spin
+ if rclpy.ok():
+ self.executor.spin_once(timeout_sec=0.01)
+ for (drone_id, msg_type), data in self.monitor.latest_data.items():
+ self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
+ self.monitor.latest_data.clear()
+ except RuntimeError as e:
+ # ROS2 context 被破坏或不可用
+ if "Context" in str(e) or "context" in str(e).lower():
+ print(f"⚠️ ROS2 context 錯誤（忽略）: {e}", flush=True)
+ else:
+ print(f"ROS spin error: {e}", flush=True)
+ traceback.print_exc()
+ except Exception as e:
+ print(f"ROS spin error: {e}", flush=True)
+ traceback.print_exc()
+
+ def closeEvent(self, event):
+ try:
+ for group in self.mission_groups.values():
+ if group.executor:
+ group.executor.stop()
+ self.command_sender.close()
+ for receiver in self.udp_receivers:
+ receiver.stop()
+ for receiver in self.monitor.ws_receivers:
+ receiver.stop()
+ # Clean up serial receivers
+ for receiver in self.monitor.serial_receivers:
+ receiver.stop()
+ # Clean up all CommandLongClient instances
+ for drone_id, client in self.monitor.command_long_clients.items():
+ try:
+ client.destroy_node()
+ except:
+ pass
+ # Clean up all PositionTargetGlobalIntClient instances
+ for drone_id, client in getattr(self.monitor, 'position_target_clients', {}).items():
+ try:
+ client.destroy_node()
+ except:
+ pass
+ self.monitor.destroy_node()
+ self.executor.shutdown()
+ except Exception as e:
+ print(f"⚠️ 清理資源時出錯: {e}", flush=True)
+ traceback.print_exc()
+
+ # 安全地 shutdown ROS2
+ try:
+ if rclpy.ok():
+ rclpy.shutdown()
+ except RuntimeError as e:
+ print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True)
+ traceback.print_exc()
+
+ event.accept()
+
+
+def main():
+ """
+ GUI 應用程式的主入口點
+
+ 標準 ROS2 + Qt 架構：
+ 1. 在最外層/最前面只做一次 rclpy.init()
+ 2. 啟動 Qt 應用程式
+ 3. 在 finally 中做 rclpy.shutdown()
+
+ 這樣可以確保所有 ROS2 相關操作都共享同一個初始化狀態
+ """
+ # 第一步：在最外層只初始化一次 ROS2（終極防護）
+ # 添加 rclpy.ok() 檢查，防止重複初始化導致 "Context.init() must only be called once" 錯誤
+ print("🚀 [GUI 主程式] 檢查 ROS2 初始化狀態...", flush=True)
+ if not rclpy.ok():
+ print("🚀 [GUI 主程式] ROS2 未初始化，開始初始化...", flush=True)
+ rclpy.init()
+ print("✅ [GUI 主程式] ROS2 已全局初始化（由 GUI 主程式霸佔）", flush=True)
+ else:
+ print("ℹ️ [GUI 主程式] ROS2 已初始化，跳過重複初始化", flush=True)
+
+ try:
+ # 第二步：啟動 Qt 應用程式
+ print("🚀 [GUI 主程式] 啟動 Qt 應用程式...", flush=True)
+ app = QApplication(sys.argv)
+ station = ControlStationUI()
+ station.show()
+ print("✓ [GUI 主程式] 應用程式已啟動", flush=True)
+
+ # 第三步：進入 Qt 事件循環（阻塞直到用戶關閉應用）
+ print("🎯 [GUI 主程式] 進入主事件循環，等待用戶操作...", flush=True)
+ sys.exit(app.exec())
+
+ finally:
+ # 第四步：只有當 GUI 視窗被關閉時，才做 ROS2 cleanup（終極防護）
+ # 這裡確保 ROS2 被正確且安全地關閉
+ print("\n🛑 [GUI 主程式] 應用程式關閉，正在清理 ROS2 資源...", flush=True)
+ if rclpy.ok():
+ rclpy.shutdown()
+ print("✓ [GUI 主程式] ROS2 已安全關閉", flush=True)
+ else:
+ print("ℹ️ [GUI 主程式] ROS2 已關閉或不可用，無需重複 shutdown", flush=True)
+ print("✓ [GUI 主程式] 應用程式完全退出", flush=True)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py
new file mode 100644
index 0000000..2be9cb9
--- /dev/null
+++ b/src/GUI/map_layout.py
@@ -0,0 +1,1084 @@
+#!/usr/bin/env python3
+from PyQt6.QtWebEngineWidgets import QWebEngineView
+from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
+from PyQt6.QtWebChannel import QWebChannel
+
+class DroneMap:
+ """無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
+
+ def __init__(self):
+ """初始化地圖"""
+ self.map_view = QWebEngineView()
+ self.map_loaded = False
+ self.pending_map_updates = {}
+
+ # 創建橋接對象
+ self.bridge = MapBridge()
+
+ # 設置 QWebChannel
+ self.channel = QWebChannel()
+ self.channel.registerObject('bridge', self.bridge)
+ self.map_view.page().setWebChannel(self.channel)
+
+ # 設置地圖 HTML
+ inline_html = '''
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ '''
+
+ self.map_view.setHtml(inline_html)
+ self.map_view.loadFinished.connect(self._on_map_loaded)
+
+ # 設置地圖更新計時器
+ self.map_update_timer = QTimer()
+ self.map_update_timer.timeout.connect(self.update_map_positions)
+ self.map_update_timer.start(200) # 每 200ms 更新一次
+
+ def _on_map_loaded(self, ok: bool):
+ """地圖加載完成回調"""
+ if ok:
+ self.map_loaded = True
+ else:
+ print("⚠️ 地圖加載失敗")
+
+ def update_drone_position(self, drone_id, lat, lon, heading):
+ """更新無人機位置（加入待處理隊列）"""
+ self.pending_map_updates[drone_id] = (lat, lon, heading)
+
+ def update_map_positions(self):
+ """批量更新地圖上的無人機位置"""
+ if not self.map_loaded or not self.pending_map_updates:
+ return
+
+ js_commands = []
+ for drone_id, (lat, lon, heading) in self.pending_map_updates.items():
+ js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});")
+
+ if js_commands:
+ combined_js = "\n".join(js_commands)
+ self.map_view.page().runJavaScript(combined_js)
+
+ self.pending_map_updates.clear()
+
+ def clear_trajectories(self):
+ """清除所有軌跡"""
+ if self.map_loaded:
+ self.map_view.page().runJavaScript("clearAllTrajectories();")
+
+ def focus_on_drone(self, drone_id):
+ """聚焦到指定無人機"""
+ if self.map_loaded:
+ self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
+
+ # ================================================================================
+ # 任務規劃視覺化方法
+ # ================================================================================
+ def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon):
+ """在地圖上繪製任務規劃（舊介面，相容用）"""
+ self.draw_mission_plan_for_group('_default', '#FF4444',
+ center_lat, center_lon, target_lat, target_lon)
+
+ def draw_mission_plan_for_group(self, group_id, color,
+ center_lat, center_lon, target_lat, target_lon):
+ """在地圖上繪製指定群組的任務規劃（帶顏色區分）"""
+ if self.map_loaded:
+ js_code = (
+ f"drawMissionPlanForGroup("
+ f"'{group_id}', '{color}', "
+ f"{center_lat:.6f}, {center_lon:.6f}, "
+ f"{target_lat:.6f}, {target_lon:.6f});"
+ )
+ self.map_view.page().runJavaScript(js_code)
+ print(f"📍 地圖已繪製 Group {group_id} 任務規劃: "
+ f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})")
+
+ def clear_mission_plan(self):
+ """清除地圖上所有任務規劃標記"""
+ if self.map_loaded:
+ self.map_view.page().runJavaScript("clearAllMissionPlans();")
+ print("🗑️ 地圖已清除所有任務規劃")
+
+ def clear_mission_plan_for_group(self, group_id):
+ """清除指定群組的任務規劃標記"""
+ if self.map_loaded:
+ self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');")
+ print(f"🗑️ 地圖已清除 Group {group_id} 任務規劃")
+
+ def set_mission_mode(self, mode):
+ """從 Python 端切換地圖的任務模式（觸發框選/路徑標記等）"""
+ if self.map_loaded:
+ self.map_view.page().runJavaScript(f"onMissionModeChanged('{mode}');")
+
+ def toggle_drone_box_select(self):
+ """切換地圖上的無人機框選模式"""
+ if self.map_loaded:
+ self.map_view.page().runJavaScript("toggleDroneSelection();")
+ # ================================================================================
+
+ def get_widget(self):
+ """獲取地圖 widget"""
+ return self.map_view
+
+ def get_gps_signal(self):
+ """獲取 GPS 信號"""
+ return self.bridge.gps_signal
+
+ def get_drone_clicked_signal(self):
+ """獲取無人機點擊信號"""
+ return self.bridge.drone_clicked
+
+ def get_clear_all_drone_selection_signal(self):
+ """獲取清除所有無人機選擇信號"""
+ return self.bridge.clear_all_drone_selection
+
+ def get_toggle_select_all_drones_signal(self):
+ """獲取切換全選所有無人機信號"""
+ return self.bridge.select_all_drones
+
+ def get_select_all_drones_signal(self):
+ """獲取全選所有無人機信號"""
+ return self.bridge.select_all_drones
+
+ def get_start_mission_signal(self):
+ """獲取開始任務信號"""
+ return self.bridge.start_mission_signal
+
+ def get_pause_mission_signal(self):
+ """獲取暫停任務信號"""
+ return self.bridge.pause_mission_signal
+
+ def get_rectangle_selected_signal(self):
+ """獲取矩形選擇信號"""
+ return self.bridge.rectangle_selected
+
+ def get_polygon_selected_signal(self):
+ """獲取多邊形選擇信號"""
+ return self.bridge.polygon_selected
+
+ def get_mission_mode_changed_signal(self):
+ """獲取任務模式切換信號"""
+ return self.bridge.mission_mode_changed
+
+ def get_route_confirmed_signal(self):
+ """獲取路徑確認信號"""
+ return self.bridge.route_confirmed
+
+ def get_drone_box_selected_signal(self):
+ """獲取框選無人機完成信號"""
+ return self.bridge.drone_box_selected
+
+class MapBridge(QObject):
+ """JavaScript 和 Python 之間的橋接類"""
+ gps_signal = pyqtSignal(float, float)
+ drone_clicked = pyqtSignal(str)
+ clear_all_drone_selection = pyqtSignal()
+ select_all_drones = pyqtSignal()
+ start_mission_signal = pyqtSignal(float, float, float, float)
+ pause_mission_signal = pyqtSignal()
+ rectangle_selected = pyqtSignal(str)
+ polygon_selected = pyqtSignal(str)
+ mission_mode_changed = pyqtSignal(str)
+ route_confirmed = pyqtSignal(str) # 路徑確認 (JSON 字串)
+ drone_box_selected = pyqtSignal(str) # 框選無人機完成 (JSON 字串)
+
+ def __init__(self):
+ super().__init__()
+
+ @pyqtSlot(float, float)
+ def emitGpsSignal(self, lat, lon):
+ """供 JavaScript 調用的方法"""
+ self.gps_signal.emit(lat, lon)
+
+ @pyqtSlot(str)
+ def emitDroneClicked(self, drone_id):
+ """供 JavaScript 調用的方法 - 當無人機被點擊時"""
+ self.drone_clicked.emit(drone_id)
+
+ @pyqtSlot()
+ def clearAllDroneSelection(self):
+ """供 JavaScript 調用的方法 - 清除所有無人機選擇"""
+ self.clear_all_drone_selection.emit()
+ print("🗑️ 清除所有無人機選擇")
+
+ @pyqtSlot()
+ def toggleSelectAllDrones(self):
+ """供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機"""
+ self.select_all_drones.emit()
+ print("🔄 切換全選無人機")
+
+ @pyqtSlot(float, float, float, float)
+ def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon):
+ """供 JavaScript 調用的方法 - 開始任務"""
+ self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon)
+ print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})")
+
+ @pyqtSlot()
+ def pauseMissionSignal(self):
+ """供 JavaScript 調用的方法 - 暫停任務"""
+ self.pause_mission_signal.emit()
+ print("⏸️ 暫停任務信號已發出")
+
+ @pyqtSlot(str)
+ def rectangleSelected(self, points_json):
+ """供 JavaScript 調用的方法 - 矩形選擇完成"""
+ self.rectangle_selected.emit(points_json)
+ print(f"📦 矩形區域已選擇: {points_json}")
+
+ @pyqtSlot(str)
+ def polygonSelected(self, points_json):
+ """供 JavaScript 調用的方法 - 多邊形選擇完成"""
+ self.polygon_selected.emit(points_json)
+ print(f"🔷 多邊形區域已選擇: {points_json}")
+
+ @pyqtSlot(str)
+ def missionModeChanged(self, mode):
+ """供 JavaScript 調用的方法 - 任務模式切換"""
+ self.mission_mode_changed.emit(mode)
+ print(f"🔄 任務模式切換: {mode}")
+
+ @pyqtSlot(str)
+ def routeConfirmed(self, points_json):
+ """供 JavaScript 調用的方法 - 路徑確認"""
+ self.route_confirmed.emit(points_json)
+ print(f"📍 路徑已確認: {points_json}")
+
+ @pyqtSlot(str)
+ def droneBoxSelected(self, drone_ids_json):
+ """供 JavaScript 調用的方法 - 框選無人機完成"""
+ self.drone_box_selected.emit(drone_ids_json)
+ print(f"📦 框選無人機: {drone_ids_json}")
\ No newline at end of file
diff --git a/src/GUI/mission_executor.py b/src/GUI/mission_executor.py
new file mode 100644
index 0000000..e5bd11f
--- /dev/null
+++ b/src/GUI/mission_executor.py
@@ -0,0 +1,370 @@
+#!/usr/bin/env python3
+"""
+任務執行模組
+管理多架無人機的 GUIDED 模式飛行控制迴圈
+
+設計:
+ - 每架無人機持有一個航點序列，逐點推進
+ - 各自到達就各自切換到下一個航點
+ - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
+ - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
+ - Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
+ - 用 QTimer 驅動到達判定，在 Qt 主線程執行
+ - 暫停 = 停止到達判定 + 停止新指令
+"""
+import asyncio
+import math
+import time
+from enum import Enum
+from PyQt6.QtCore import QObject, QTimer, pyqtSignal
+
+
+class MissionState(Enum):
+ """整體任務狀態"""
+ IDLE = "idle"
+ RUNNING = "running"
+ PAUSED = "paused"
+
+
+class TaskStatus(Enum):
+ """單機任務狀態"""
+ NORMAL = "normal"
+ RETRYING = "retrying"
+ WAITING_AT_BARRIER = "waiting_at_barrier"
+ FALLBACK_LOITER = "fallback_loiter"
+
+
+class DroneTask:
+ """單架無人機的任務資料"""
+ __slots__ = (
+ 'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
+ 'sent_current_wp', 'fail_count', 'status', 'waiting_since',
+ )
+
+ def __init__(self, drone_id, sysid, waypoints):
+ self.drone_id = drone_id
+ self.sysid = sysid
+ self.waypoints = waypoints
+ self.wp_index = 0
+ self.done = len(waypoints) == 0
+ self.sent_current_wp = False
+ self.fail_count = 0
+ self.status = TaskStatus.NORMAL
+ self.waiting_since = 0.0 # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
+
+ @property
+ def current_target(self):
+ if self.done or self.wp_index >= len(self.waypoints):
+ return None
+ return self.waypoints[self.wp_index]
+
+ @property
+ def total_waypoints(self):
+ return len(self.waypoints)
+
+
+class MissionExecutor(QObject):
+ """
+ 任務執行器 (事件驅動版 + rendezvous barrier)
+
+ planned_waypoints 格式:
+ {
+ 'drone_ids': ['s0_1', 's0_2', ...],
+ 'waypoints': [
+ [(lat,lon,alt), ...], # drone 0
+ [(lat,lon,alt), ...], # drone 1
+ ],
+ 'rendezvous_indices': [3, 7, ...], # 選填；缺省 = 全程不同步（各自跑）
+ }
+ """
+
+ MAX_RETRY = 3
+
+ drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total)
+ task_status_changed = pyqtSignal(str, str, str) # (drone_id, status, message)
+ mission_completed = pyqtSignal()
+
+ def __init__(self, sender, drone_gps, monitor=None,
+ arrival_radius=2.0, tick_rate_hz=2.0,
+ barrier_timeout_sec=20.0):
+ super().__init__()
+ self.sender = sender
+ self.drone_gps = drone_gps
+ self.monitor = monitor # 用於失敗 fallback 到 LOITER
+ self.arrival_radius = arrival_radius
+ self.barrier_timeout_sec = barrier_timeout_sec
+ self.state = MissionState.IDLE
+ self.tasks = {}
+ self.rendezvous_indices = set()
+
+ self._interval_ms = int(1000 / tick_rate_hz)
+ self._timer = QTimer()
+ self._timer.timeout.connect(self._tick)
+
+ # 只有 Ros2CommandSender 有 send_result signal；MavlinkSender 沒有
+ if hasattr(sender, 'send_result'):
+ sender.send_result.connect(self._on_send_result)
+
+ # ------------------------------------------------------------------ 公開方法
+
+ def start(self, planned_waypoints):
+ if self.state == MissionState.RUNNING:
+ print("任務已在執行中")
+ return
+
+ self.tasks.clear()
+
+ drone_ids = planned_waypoints['drone_ids']
+ waypoints_list = planned_waypoints['waypoints']
+ self.rendezvous_indices = set(
+ planned_waypoints.get('rendezvous_indices', []) or []
+ )
+
+ for i, drone_id in enumerate(drone_ids):
+ sysid = int(drone_id.split('_')[1])
+ self.tasks[drone_id] = DroneTask(drone_id, sysid, waypoints_list[i])
+
+ self.state = MissionState.RUNNING
+ self._timer.start(self._interval_ms)
+
+ total_wps = sum(t.total_waypoints for t in self.tasks.values())
+ rv_info = (
+ f"rendezvous WP={sorted(self.rendezvous_indices)}"
+ if self.rendezvous_indices else "無 rendezvous (各自跑)"
+ )
+ print(f"任務啟動: {len(self.tasks)} 架無人機, "
+ f"共 {total_wps} 個航點, "
+ f"到達半徑={self.arrival_radius}m, "
+ f"tick 週期={self._interval_ms}ms, "
+ f"barrier timeout={self.barrier_timeout_sec}s, "
+ f"{rv_info}")
+
+ def pause(self):
+ if self.state == MissionState.RUNNING:
+ self._timer.stop()
+ self.state = MissionState.PAUSED
+ print("任務暫停")
+
+ def resume(self):
+ if self.state == MissionState.PAUSED:
+ self._timer.start(self._interval_ms)
+ self.state = MissionState.RUNNING
+ print("任務恢復")
+
+ def stop(self):
+ self._timer.stop()
+ self.tasks.clear()
+ self.rendezvous_indices = set()
+ self.state = MissionState.IDLE
+ print("任務停止")
+
+ # ------------------------------------------------------------------ 控制迴圈
+
+ def _tick(self):
+ """
+ 控制 tick：
+ Phase 1: 每台機做到達判定；非 barrier 點直接推進，barrier 點改成 WAITING
+ Phase 2: 檢查 barrier 能不能釋放（全員到齊或 timeout）
+ Phase 3: 每台未送過當前航點的機送一次
+ Phase 4: 全部完成檢查
+ """
+ now = time.monotonic()
+
+ # ---- Phase 1: 到達判定 ----
+ for task in self.tasks.values():
+ if task.done or task.status == TaskStatus.FALLBACK_LOITER:
+ continue
+ # 已在 barrier 等待的不再跑到達判定
+ if task.status == TaskStatus.WAITING_AT_BARRIER:
+ continue
+
+ target = task.current_target
+ if target is None:
+ continue
+
+ gps = self.drone_gps.get(task.drone_id)
+ if gps is None:
+ continue
+
+ tgt_lat, tgt_lon, _ = target
+ distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
+ if distance >= self.arrival_radius:
+ continue
+
+ # 到達當前 wp_index
+ if (task.wp_index in self.rendezvous_indices
+ and task.wp_index < task.total_waypoints - 1):
+ # rendezvous 點 → 不推進，進入 barrier 等待
+ task.status = TaskStatus.WAITING_AT_BARRIER
+ task.waiting_since = now
+ print(f" {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
+ self.task_status_changed.emit(
+ task.drone_id, task.status.value,
+ f"waiting at WP {task.wp_index}"
+ )
+ continue
+
+ # 一般點 → 直接推進
+ self._advance_waypoint(task, distance)
+
+ # ---- Phase 2: barrier 釋放檢查 ----
+ self._check_barriers(now)
+
+ # ---- Phase 3: 發送未送過的目標 ----
+ for task in self.tasks.values():
+ if task.done or task.status in (
+ TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
+ ):
+ continue
+ if task.sent_current_wp:
+ continue
+ target = task.current_target
+ if target is None:
+ continue
+ tgt_lat, tgt_lon, tgt_alt = target
+ task.sent_current_wp = True
+ self.sender.send_position_global(
+ task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
+ )
+
+ # ---- Phase 4: 完成檢查 ----
+ all_done = all(
+ t.done or t.status == TaskStatus.FALLBACK_LOITER
+ for t in self.tasks.values()
+ )
+ if all_done and self.tasks:
+ self._timer.stop()
+ self.state = MissionState.IDLE
+ self.mission_completed.emit()
+ print("===== 任務全部完成 =====")
+
+ def _advance_waypoint(self, task, arrived_distance):
+ """把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
+ task.wp_index += 1
+ task.sent_current_wp = False
+ task.fail_count = 0
+ if task.wp_index >= task.total_waypoints:
+ task.done = True
+ self.drone_waypoint_reached.emit(
+ task.drone_id, task.wp_index, task.total_waypoints
+ )
+ print(f" {task.drone_id} → DONE "
+ f"({task.total_waypoints}/{task.total_waypoints})")
+ return
+ self.drone_waypoint_reached.emit(
+ task.drone_id, task.wp_index, task.total_waypoints
+ )
+ print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
+ f"(到達距離: {arrived_distance:.1f}m)")
+
+ def _check_barriers(self, now):
+ """檢查每個有人在等的 barrier 能不能釋放。"""
+ # 蒐集目前有哪些 barrier 有人卡著
+ waiting_groups = {} # barrier_idx -> [task, ...]
+ for task in self.tasks.values():
+ if task.status == TaskStatus.WAITING_AT_BARRIER:
+ waiting_groups.setdefault(task.wp_index, []).append(task)
+
+ if not waiting_groups:
+ return
+
+ for barrier_idx, waiting_tasks in waiting_groups.items():
+ # 判斷是否「全員到齊」：所有仍活著的機都要麼在此 barrier 等，要麼早已越過此 idx
+ everyone_arrived = True
+ for task in self.tasks.values():
+ if task.done or task.status == TaskStatus.FALLBACK_LOITER:
+ continue # 失敗機/完成機不擋
+ if (task.status == TaskStatus.WAITING_AT_BARRIER
+ and task.wp_index == barrier_idx):
+ continue # 在這裡等
+ if task.wp_index > barrier_idx:
+ continue # 早已越過（barrier 先前已對他釋放）
+ # 還在路上
+ everyone_arrived = False
+ break
+
+ force_reason = None
+ if not everyone_arrived:
+ # 檢查 timeout：以該 barrier 上最早進入等待的時間為準
+ oldest_wait = min(t.waiting_since for t in waiting_tasks)
+ if now - oldest_wait >= self.barrier_timeout_sec:
+ force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
+ print(f"⚠️ barrier WP {barrier_idx} {force_reason}，強制放行")
+ else:
+ continue # 還沒到齊、也還沒 timeout → 繼續等
+
+ # 釋放：把所有在此 barrier 等待的機一起推進
+ tag = "全員到齊" if force_reason is None else force_reason
+ print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，"
+ f"推進 {len(waiting_tasks)} 架")
+ for task in waiting_tasks:
+ task.status = TaskStatus.NORMAL
+ msg = f"barrier WP {barrier_idx} released ({tag})"
+ self.task_status_changed.emit(
+ task.drone_id, task.status.value, msg
+ )
+ self._advance_waypoint(task, self.arrival_radius)
+
+ # ------------------------------------------------------------------ 結果回呼
+
+ def _on_send_result(self, drone_id, sysid, success, message):
+ """Ros2CommandSender.send_result 的 slot"""
+ task = self.tasks.get(drone_id)
+ if task is None or task.done:
+ return
+ # 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
+ if task.status == TaskStatus.WAITING_AT_BARRIER:
+ return
+
+ if success:
+ if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
+ task.status = TaskStatus.NORMAL
+ self.task_status_changed.emit(drone_id, task.status.value, "recovered")
+ task.fail_count = 0
+ return
+
+ task.fail_count += 1
+ print(f"⚠️ {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
+
+ if task.fail_count < self.MAX_RETRY:
+ task.status = TaskStatus.RETRYING
+ task.sent_current_wp = False # 下個 tick 會重送
+ self.task_status_changed.emit(
+ drone_id, task.status.value,
+ f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
+ )
+ else:
+ task.status = TaskStatus.FALLBACK_LOITER
+ self.task_status_changed.emit(
+ drone_id, task.status.value,
+ f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
+ )
+ print(f"❌ {drone_id} 連續失敗 {self.MAX_RETRY} 次 → 切換 LOITER")
+ self._fallback_to_loiter(drone_id)
+
+ def _fallback_to_loiter(self, drone_id):
+ """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
+ if self.monitor is None:
+ print(f"⚠️ 無 monitor，無法將 {drone_id} 切到 LOITER")
+ return
+ try:
+ loop = asyncio.get_event_loop()
+ except RuntimeError:
+ loop = asyncio.new_event_loop()
+ asyncio.set_event_loop(loop)
+ loop.create_task(self.monitor.set_mode(drone_id, "LOITER"))
+
+
+# ------------------------------------------------------------------ 工具函式
+
+def _haversine(lat1, lon1, lat2, lon2):
+ """計算兩個 GPS 座標間的水平距離 (公尺)"""
+ R = 6371000.0
+ lat1_r = math.radians(lat1)
+ lat2_r = math.radians(lat2)
+ dlat = math.radians(lat2 - lat1)
+ dlon = math.radians(lon2 - lon1)
+
+ a = (math.sin(dlat / 2) ** 2 +
+ math.cos(lat1_r) * math.cos(lat2_r) * math.sin(dlon / 2) ** 2)
+ return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
diff --git a/src/GUI/mission_group.py b/src/GUI/mission_group.py
new file mode 100644
index 0000000..61620e4
--- /dev/null
+++ b/src/GUI/mission_group.py
@@ -0,0 +1,556 @@
+#!/usr/bin/env python3
+"""
+任務群組模組
+管理多任務群組的資料結構與無人機分配對話框
+"""
+from PyQt6.QtWidgets import (
+ QWidget, QVBoxLayout, QHBoxLayout, QLabel, QPushButton,
+ QComboBox, QDialog, QCheckBox, QScrollArea, QFrame, QLineEdit
+)
+from PyQt6.QtCore import Qt, pyqtSignal
+from mission_executor import MissionExecutor, MissionState
+
+
+# 群組顏色（循環使用）
+GROUP_COLORS = [
+ '#4A9EFF', # 藍
+ '#FF8C42', # 橘
+ '#56C87A', # 綠
+ '#E85D75', # 紅
+ '#B07CED', # 紫
+ '#F5C542', # 黃
+ '#42C9C9', # 青
+ '#FF6B9D', # 粉
+]
+
+
+class MissionGroup:
+ """單一任務群組的資料"""
+
+ def __init__(self, group_id, color):
+ self.group_id = group_id # 'A', 'B', 'C', ...
+ self.color = color # 顏色 hex
+
+ # 唯一真實資料來源：該 group 選中的 drone ID
+ # 代表：group 擁有的 drone、被選中的 drone、可以操作的 drone
+ self.selected_drone_ids = set()
+
+ self.mission_type = 'M_FORMATION' # 預設任務類型
+ self.planned_waypoints = None # 規劃結果 dict
+ self.executor = None # MissionExecutor 實例（延遲建立）
+ self.center_origin = None # 規劃原點
+ self.leader_drone_id = None # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
+
+ @property
+ def state(self):
+ if self.executor is None:
+ return MissionState.IDLE
+ return self.executor.state
+
+ @property
+ def display_name(self):
+ return f"Group {self.group_id}"
+
+
+class DroneAssignDialog(QDialog):
+ """無人機分配對話框"""
+
+ def __init__(self, parent, all_drone_ids, current_assigned, other_assigned=None):
+ """
+ Args:
+ parent: 父 widget
+ all_drone_ids: 所有可用無人機 ID 列表
+ current_assigned: 當前群組已分配的無人機 set
+ other_assigned: (忽略 — 現在允許多 group 分配)
+ """
+ super().__init__(parent)
+ self.setWindowTitle("分配無人機")
+ self.setMinimumWidth(280)
+ self.setStyleSheet("""
+ QDialog { background-color: #2B2B2B; }
+ QLabel { color: #DDD; }
+ QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
+ QCheckBox::indicator { width: 16px; height: 16px; }
+ """)
+
+ layout = QVBoxLayout(self)
+
+ title = QLabel("選擇要分配到此群組的無人機：")
+ title.setStyleSheet("font-size: 13px; font-weight: bold; padding-bottom: 6px;")
+ layout.addWidget(title)
+
+ # 滾動區域
+ scroll = QScrollArea()
+ scroll.setWidgetResizable(True)
+ scroll.setMaximumHeight(300)
+ scroll_widget = QWidget()
+ scroll_layout = QVBoxLayout(scroll_widget)
+ scroll_layout.setSpacing(2)
+
+ self.checkboxes = {}
+ sorted_ids = sorted(all_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+
+ for drone_id in sorted_ids:
+ cb = QCheckBox(drone_id)
+
+ # 所有 drone 都能被選摘（支持多 group 分配）
+ if drone_id in current_assigned:
+ cb.setChecked(True)
+
+ self.checkboxes[drone_id] = cb
+ scroll_layout.addWidget(cb)
+
+ scroll_layout.addStretch()
+ scroll.setWidget(scroll_widget)
+ layout.addWidget(scroll)
+
+ # 按鈕
+ btn_layout = QHBoxLayout()
+ ok_btn = QPushButton("確定")
+ ok_btn.setStyleSheet("""
+ QPushButton { background-color: #4A9EFF; color: white; border: none;
+ padding: 8px 20px; border-radius: 4px; font-weight: bold; }
+ QPushButton:hover { background-color: #3A8EEF; }
+ """)
+ ok_btn.clicked.connect(self.accept)
+
+ cancel_btn = QPushButton("取消")
+ cancel_btn.setStyleSheet("""
+ QPushButton { background-color: #555; color: #DDD; border: none;
+ padding: 8px 20px; border-radius: 4px; }
+ QPushButton:hover { background-color: #666; }
+ """)
+ cancel_btn.clicked.connect(self.reject)
+
+ btn_layout.addStretch()
+ btn_layout.addWidget(cancel_btn)
+ btn_layout.addWidget(ok_btn)
+ layout.addLayout(btn_layout)
+
+ def get_selected(self):
+ """回傳勾選的無人機 ID set"""
+ return {did for did, cb in self.checkboxes.items() if cb.isChecked() and cb.isEnabled()}
+
+
+class GroupPanel(QWidget):
+ """單一群組的 UI 面板（嵌入到 tab 中）— 三欄式佈局"""
+
+ # 信號
+ assign_drones_requested = pyqtSignal(str) # group_id
+ mission_type_changed = pyqtSignal(str, str) # group_id, mission_type
+ start_requested = pyqtSignal(str) # group_id
+ pause_requested = pyqtSignal(str) # group_id
+ stop_requested = pyqtSignal(str) # group_id
+ mode_change_requested = pyqtSignal(str, str) # group_id, mode
+ arm_requested = pyqtSignal(str) # group_id
+ takeoff_requested = pyqtSignal(str, float) # group_id, altitude
+ box_select_requested = pyqtSignal(str) # group_id — 框選直接分配
+ select_all_requested = pyqtSignal(str) # group_id — 全選直接分配
+ clear_group_requested = pyqtSignal(str) # group_id — 清除分組
+ add_group_requested = pyqtSignal() # 新增群組
+ delete_group_requested = pyqtSignal(str) # group_id — 刪除群組
+
+ BUTTON_STYLE = """
+ QPushButton {{ background-color: {bg}; color: {fg}; border: none;
+ padding: 5px 8px; border-radius: 4px; font-size: 11px; }}
+ QPushButton:hover {{ background-color: {hover}; }}
+ QPushButton:disabled {{ background-color: #444; color: #666; }}
+ """
+
+ def __init__(self, group: MissionGroup, parent=None):
+ super().__init__(parent)
+ self.group = group
+ self.all_btn_ref = None # 保存全選按鈕的參考
+ self._build_ui()
+
+ def _make_sep(self):
+ """建立垂直分隔線"""
+ sep = QFrame()
+ sep.setFrameShape(QFrame.Shape.VLine)
+ sep.setStyleSheet("color: #444;")
+ return sep
+
+ def _build_ui(self):
+ layout = QVBoxLayout(self)
+ layout.setContentsMargins(6, 4, 6, 4)
+ layout.setSpacing(0)
+
+ COMBO = ("QComboBox { background-color: #333; color: #DDD; "
+ "border-radius: 3px; padding: 2px 6px; font-size: 11px; }")
+ BTN = self.BUTTON_STYLE
+ LBL = "color: #AAA; font-size: 11px;"
+ TITLE = "color: #DDD; font-size: 11px; font-weight: bold; padding-bottom: 2px;"
+
+ # ── 三欄主佈局 ──
+ cols = QHBoxLayout()
+ cols.setSpacing(6)
+
+ # ============================
+ # 左欄：控制指令
+ # ============================
+ left = QVBoxLayout()
+ left.setSpacing(3)
+
+ left_title = QLabel("控制指令")
+ left_title.setStyleSheet(TITLE)
+ left.addWidget(left_title)
+
+ # 模式切換
+ mode_row = QHBoxLayout()
+ mode_row.setSpacing(3)
+ self.mode_combo = QComboBox()
+ self.mode_combo.addItems([
+ "GUIDED", "AUTO", "LAND", "LOITER",
+ "STABILIZE", "ALT_HOLD", "RTL", "POSHOLD", "SMART_RTL"
+ ])
+ self.mode_combo.setStyleSheet(COMBO)
+ mode_btn = QPushButton("切換")
+ mode_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+ mode_btn.clicked.connect(
+ lambda: self.mode_change_requested.emit(
+ self.group.group_id, self.mode_combo.currentText()))
+ mode_row.addWidget(self.mode_combo, 1)
+ mode_row.addWidget(mode_btn)
+ left.addLayout(mode_row)
+
+ # 解鎖
+ arm_btn = QPushButton("解鎖")
+ arm_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+ arm_btn.clicked.connect(
+ lambda: self.arm_requested.emit(self.group.group_id))
+ left.addWidget(arm_btn)
+
+ # 起飛
+ takeoff_row = QHBoxLayout()
+ takeoff_row.setSpacing(3)
+ self.alt_input = QComboBox()
+ self.alt_input.setEditable(True)
+ self.alt_input.addItems(["5", "10", "15", "20"])
+ self.alt_input.setCurrentText("10")
+ self.alt_input.setStyleSheet(COMBO)
+ alt_lbl = QLabel("m")
+ alt_lbl.setStyleSheet(LBL)
+ takeoff_btn = QPushButton("起飛")
+ takeoff_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+ takeoff_btn.clicked.connect(self._on_takeoff)
+ takeoff_row.addWidget(self.alt_input, 1)
+ takeoff_row.addWidget(alt_lbl)
+ takeoff_row.addWidget(takeoff_btn)
+ left.addLayout(takeoff_row)
+
+ left.addStretch()
+
+ # ============================
+ # 中欄：任務規劃（左右分割）
+ # ============================
+ mid = QVBoxLayout()
+ mid.setSpacing(2)
+
+ mid_title = QLabel("任務規劃")
+ mid_title.setStyleSheet(TITLE)
+ mid.addWidget(mid_title)
+
+ mid_body = QHBoxLayout()
+ mid_body.setSpacing(4)
+
+ # 左側：類型 + 狀態 + 座標
+ mid_left = QVBoxLayout()
+ mid_left.setSpacing(2)
+ self.type_combo = QComboBox()
+ self.type_combo.addItems([
+ "M_FORMATION", "CIRCLE_FORMATION", "LEADER_FOLLOWER", "GRID_SWEEP"
+ ])
+ self.type_combo.setStyleSheet(COMBO)
+ self.type_combo.currentTextChanged.connect(
+ lambda t: self.mission_type_changed.emit(self.group.group_id, t))
+ mid_left.addWidget(self.type_combo)
+ self.status_label = QLabel("○ 未規劃")
+ self.status_label.setStyleSheet("color: #888; font-size: 11px;")
+ mid_left.addWidget(self.status_label)
+ self.center_label = QLabel("中心: --")
+ self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
+ mid_left.addWidget(self.center_label)
+ self.target_label = QLabel("目標: --")
+ self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
+ mid_left.addWidget(self.target_label)
+ mid_left.addStretch()
+
+ # 右側：執行 / 暫停 / 停止（垂直排列）
+ mid_right = QVBoxLayout()
+ mid_right.setSpacing(3)
+ self.start_btn = QPushButton("▶ 執行")
+ self.start_btn.setStyleSheet(BTN.format(bg='#2E7D32', fg='white', hover='#388E3C'))
+ self.start_btn.clicked.connect(
+ lambda: self.start_requested.emit(self.group.group_id))
+ self.pause_btn = QPushButton("⏸ 暫停")
+ self.pause_btn.setStyleSheet(BTN.format(bg='#F57F17', fg='white', hover='#F9A825'))
+ self.pause_btn.clicked.connect(
+ lambda: self.pause_requested.emit(self.group.group_id))
+ self.stop_btn = QPushButton("■ 停止")
+ self.stop_btn.setStyleSheet(BTN.format(bg='#C62828', fg='white', hover='#D32F2F'))
+ self.stop_btn.clicked.connect(
+ lambda: self.stop_requested.emit(self.group.group_id))
+ mid_right.addWidget(self.start_btn)
+ mid_right.addWidget(self.pause_btn)
+ mid_right.addWidget(self.stop_btn)
+ mid_right.addStretch()
+
+ mid_body.addLayout(mid_left, 1)
+ mid_body.addLayout(mid_right)
+ mid.addLayout(mid_body)
+
+ # ============================
+ # 選取與分組（3x2 按鈕）
+ # ============================
+ right = QVBoxLayout()
+ right.setSpacing(3)
+
+ right_title = QLabel("選取與分組")
+ right_title.setStyleSheet(TITLE)
+ right.addWidget(right_title)
+
+ self.drone_list_label = QLabel("尚未分配")
+ self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+ self.drone_list_label.setWordWrap(True)
+ right.addWidget(self.drone_list_label)
+
+ # 3x2 按鈕網格：第一行 框選 全選 新增群組
+ grid_r1 = QHBoxLayout()
+ grid_r1.setSpacing(3)
+ box_btn = QPushButton("框選")
+ box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+ box_btn.clicked.connect(
+ lambda: self.box_select_requested.emit(self.group.group_id))
+ all_btn = QPushButton("全選/取消")
+ all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+ all_btn.clicked.connect(self._on_all_select_clicked)
+ self.all_btn_ref = all_btn # 保存按鈕參考（備用）
+ add_group_btn = QPushButton("+ 新增群組")
+ add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
+ add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
+ grid_r1.addWidget(box_btn)
+ grid_r1.addWidget(all_btn)
+ grid_r1.addWidget(add_group_btn)
+ right.addLayout(grid_r1)
+
+ # 第二行 編輯分配 清除分組 刪除群組
+ grid_r2 = QHBoxLayout()
+ grid_r2.setSpacing(3)
+ assign_btn = QPushButton("編輯分配")
+ assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
+ assign_btn.clicked.connect(
+ lambda: self.assign_drones_requested.emit(self.group.group_id))
+ clear_btn = QPushButton("清除分組")
+ clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
+ clear_btn.clicked.connect(
+ lambda: self.clear_group_requested.emit(self.group.group_id))
+ self.delete_group_btn = QPushButton("刪除群組")
+ self.delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+ self.delete_group_btn.clicked.connect(
+ lambda: self.delete_group_requested.emit(self.group.group_id))
+ grid_r2.addWidget(assign_btn)
+ grid_r2.addWidget(clear_btn)
+ grid_r2.addWidget(self.delete_group_btn)
+ right.addLayout(grid_r2)
+
+ right.addStretch()
+
+ # ============================
+ # 第四欄：任務參數
+ # ============================
+ param_col = QVBoxLayout()
+ param_col.setSpacing(2)
+
+ param_title = QLabel("任務參數")
+ param_title.setStyleSheet(TITLE)
+ param_col.addWidget(param_title)
+
+ INPUT = ("QLineEdit { background-color: #333; color: #DDD; "
+ "border: 1px solid #555; border-radius: 3px; "
+ "padding: 1px 4px; font-size: 11px; }")
+
+ # 每種任務類型的參數定義: (key, label, default_value)
+ self._param_defs = {
+ 'M_FORMATION': [
+ ('spacing', '間距 (m)', '5.0'),
+ ('base_altitude', '基準高度 (m)', '10.0'),
+ ('altitude_diff', '高低差 (m)', '2.0'),
+ ],
+ 'CIRCLE_FORMATION': [
+ ('radius', '半徑 (m)', '10.0'),
+ ('altitude', '高度 (m)', '10.0'),
+ ('start_angle', '起始角 (°)', '0'),
+ ],
+ 'LEADER_FOLLOWER': [
+ ('lateral_offset', '橫向偏移 (m)', '3.0'),
+ ('longitudinal_spacing', '縱向間距 (m)', '5.0'),
+ ('altitude', '高度 (m)', '10.0'),
+ ],
+ 'GRID_SWEEP': [
+ ('line_spacing', '掃描線距 (m)', '5.0'),
+ ('altitude', '高度 (m)', '10.0'),
+ ],
+ }
+
+ # 建立所有參數列的 widget（先全部建好，切換時顯示/隱藏）
+ self._param_widgets = {} # key → (label_widget, input_widget)
+ self._param_rows = [] # 所有 row layout 對應的 container widget
+
+ for mission_type, defs in self._param_defs.items():
+ for key, label_text, default in defs:
+ if key in self._param_widgets:
+ continue # 同名參數只建一次
+ row_w = QWidget()
+ row_l = QHBoxLayout(row_w)
+ row_l.setContentsMargins(0, 0, 0, 0)
+ row_l.setSpacing(3)
+ lbl = QLabel(label_text)
+ lbl.setStyleSheet(LBL)
+ inp = QLineEdit(default)
+ inp.setStyleSheet(INPUT)
+ inp.setFixedWidth(50)
+ row_l.addWidget(lbl, 1)
+ row_l.addWidget(inp)
+ param_col.addWidget(row_w)
+ self._param_widgets[key] = (row_w, inp)
+ self._param_rows.append(row_w)
+
+ # LEADER_FOLLOWER 專用：領隊下拉選單
+ self._leader_row = QWidget()
+ leader_layout = QHBoxLayout(self._leader_row)
+ leader_layout.setContentsMargins(0, 0, 0, 0)
+ leader_layout.setSpacing(3)
+ leader_lbl = QLabel("領隊")
+ leader_lbl.setStyleSheet(LBL)
+ self._leader_combo = QComboBox()
+ self._leader_combo.setStyleSheet(COMBO)
+ self._leader_combo.setFixedWidth(70)
+ self._leader_combo.currentTextChanged.connect(self._on_leader_changed)
+ leader_layout.addWidget(leader_lbl, 1)
+ leader_layout.addWidget(self._leader_combo)
+ param_col.addWidget(self._leader_row)
+
+ param_col.addStretch()
+
+ # 初始顯示對應的參數
+ self._update_param_visibility()
+
+ # 當任務類型切換時更新參數顯示
+ self.type_combo.currentTextChanged.connect(self._update_param_visibility)
+
+ # ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
+ # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
+ cols.addLayout(left, 0)
+ cols.addWidget(self._make_sep())
+ cols.addLayout(mid, 0)
+ cols.addWidget(self._make_sep())
+ cols.addLayout(param_col, 0)
+ cols.addWidget(self._make_sep())
+ cols.addLayout(right, 0)
+ cols.addStretch() # 填充剩餘空間，使四列置左
+
+ layout.addLayout(cols)
+
+ def update_drone_list(self):
+ """更新無人機列表顯示"""
+ if not self.group.selected_drone_ids:
+ self.drone_list_label.setText("尚未分配")
+ self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
+ else:
+ sorted_ids = sorted(self.group.selected_drone_ids,
+ key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+ self.drone_list_label.setText("、".join(sorted_ids))
+ self.drone_list_label.setStyleSheet(
+ f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
+ self._refresh_leader_options()
+
+ def _refresh_leader_options(self):
+ """依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效"""
+ sorted_ids = sorted(self.group.selected_drone_ids,
+ key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+ current = self.group.leader_drone_id
+ self._leader_combo.blockSignals(True)
+ self._leader_combo.clear()
+ self._leader_combo.addItems(sorted_ids)
+ if current and current in sorted_ids:
+ self._leader_combo.setCurrentText(current)
+ elif sorted_ids:
+ self._leader_combo.setCurrentText(sorted_ids[0])
+ self.group.leader_drone_id = sorted_ids[0]
+ else:
+ self.group.leader_drone_id = None
+ self._leader_combo.blockSignals(False)
+
+ def _on_leader_changed(self, drone_id):
+ self.group.leader_drone_id = drone_id if drone_id else None
+
+ def update_status(self):
+ """更新任務狀態顯示"""
+ state = self.group.state
+ if self.group.planned_waypoints is None:
+ self.status_label.setText("○ 未規劃")
+ self.status_label.setStyleSheet("color: #888; font-size: 11px;")
+ elif state == MissionState.RUNNING:
+ self.status_label.setText("▶ 執行中")
+ self.status_label.setStyleSheet("color: #4CAF50; font-size: 11px; font-weight: bold;")
+ elif state == MissionState.PAUSED:
+ self.status_label.setText("⏸ 已暫停")
+ self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
+ else:
+ n = len(self.group.selected_drone_ids)
+ total_wps = sum(len(wps) for wps in self.group.planned_waypoints['waypoints'])
+ self.status_label.setText(f"● 已規劃 ({n}架/{total_wps}點)")
+ self.status_label.setStyleSheet(
+ f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
+
+ def _on_all_select_clicked(self):
+ """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
+ self.select_all_requested.emit(self.group.group_id)
+
+ def set_delete_enabled(self, enabled):
+ """啟用或禁用刪除群組按鈕"""
+ self.delete_group_btn.setEnabled(enabled)
+
+ def _update_param_visibility(self, _=None):
+ """根據當前任務類型，顯示/隱藏對應的參數列"""
+ mission_type = self.type_combo.currentText()
+ visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
+ for key, (row_w, _inp) in self._param_widgets.items():
+ row_w.setVisible(key in visible_keys)
+ self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
+
+ def get_mission_params(self):
+ """讀取當前顯示的參數值，回傳 dict"""
+ mission_type = self.type_combo.currentText()
+ params = {}
+ for key, _label, default in self._param_defs.get(mission_type, []):
+ if key in self._param_widgets:
+ _row_w, inp = self._param_widgets[key]
+ try:
+ params[key] = float(inp.text())
+ except ValueError:
+ params[key] = float(default)
+ return params
+
+ def update_mission_info(self, center_lat, center_lon, target_lat, target_lon):
+ """更新中心點 / 目標點顯示"""
+ info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;"
+ self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°")
+ self.center_label.setStyleSheet(info_style)
+ self.target_label.setText(f"目標: {target_lat:.6f}°, {target_lon:.6f}°")
+ self.target_label.setStyleSheet(info_style)
+
+ def clear_mission_info(self):
+ """清除中心點 / 目標點顯示"""
+ self.center_label.setText("中心: --")
+ self.center_label.setStyleSheet("color: #AAA; font-size: 11px;")
+ self.target_label.setText("目標: --")
+ self.target_label.setStyleSheet("color: #AAA; font-size: 11px;")
+
+ def _on_takeoff(self):
+ try:
+ alt = float(self.alt_input.currentText())
+ except ValueError:
+ alt = 10.0
+ self.takeoff_requested.emit(self.group.group_id, alt)
+
diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py
new file mode 100644
index 0000000..29ed44e
--- /dev/null
+++ b/src/GUI/mission_planner.py
@@ -0,0 +1,703 @@
+#!/usr/bin/env python3
+"""
+飛行任務規劃模組
+支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
+"""
+import math
+from typing import List, Tuple, Optional, Dict, Any
+from enum import Enum
+
+
+class MissionType(Enum):
+ """任務類型"""
+ M_FORMATION = "m_formation"
+ CIRCLE_FORMATION = "circle_formation"
+ LEADER_FOLLOWER = "leader_follower"
+ GRID_SWEEP = "grid_sweep"
+
+
+class CoordinateConverter:
+ """GPS 座標與 Local 座標的轉換器"""
+
+ def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
+ self.origin_lat = origin_lat
+ self.origin_lon = origin_lon
+ self.origin_alt = origin_alt
+ self.R = 6371000.0
+
+ def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
+ lat_rad = math.radians(lat)
+ lon_rad = math.radians(lon)
+ origin_lat_rad = math.radians(self.origin_lat)
+ origin_lon_rad = math.radians(self.origin_lon)
+
+ dlat = lat_rad - origin_lat_rad
+ dlon = lon_rad - origin_lon_rad
+
+ x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
+ y = self.R * dlat
+ z = alt - self.origin_alt
+
+ return x, y, z
+
+ def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
+ origin_lat_rad = math.radians(self.origin_lat)
+ origin_lon_rad = math.radians(self.origin_lon)
+
+ lat_rad = origin_lat_rad + (y / self.R)
+ lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))
+
+ lat = math.degrees(lat_rad)
+ lon = math.degrees(lon_rad)
+ alt = self.origin_alt + z
+
+ return lat, lon, alt
+
+
+class FormationPlanner:
+ """隊形規劃器"""
+
+ def __init__(self, spacing: float = 5.0,
+ base_altitude: float = 10.0,
+ altitude_diff: float = 2.0):
+ self.spacing = spacing
+ self.base_altitude = base_altitude
+ self.altitude_diff = altitude_diff
+ self.current_origin = None
+ self.converter = None
+
+ def plan_formation_mission(self,
+ drone_gps_positions: List[Tuple[float, float, float]],
+ target_gps: Tuple[float, float, float],
+ mission_type: MissionType = MissionType.M_FORMATION,
+ params: Optional[Dict[str, Any]] = None
+ ) -> Tuple[List[List[Tuple[float, float, float]]],
+ Tuple[float, float, float],
+ List[int]]:
+ """
+ 回傳 (waypoints_gps, origin, rendezvous_indices)。
+ rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。
+ """
+ if len(drone_gps_positions) == 0:
+ raise ValueError("無人機位置列表不能為空")
+
+ center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
+ center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
+ center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
+
+ self.current_origin = (center_lat, center_lon, center_alt)
+ self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
+
+ drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
+ target_local = self.converter.gps_to_local(*target_gps)
+
+ if mission_type == MissionType.M_FORMATION:
+ s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
+ waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
+ # 起點集合後一起出發到 stage2
+ rendezvous_indices = [0]
+
+ elif mission_type == MissionType.CIRCLE_FORMATION:
+ s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
+ waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
+ # 半程集合後一起進最終圓環位置
+ rendezvous_indices = [0]
+
+ elif mission_type == MissionType.LEADER_FOLLOWER:
+ params = params or {}
+ route_wps_gps = params.get('route_waypoints')
+ if route_wps_gps is None or len(route_wps_gps) < 2:
+ raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點")
+ route_wps_local = [
+ self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
+ for wp in route_wps_gps
+ ]
+ waypoints_local, rendezvous_indices = self._calculate_leader_follower(
+ drone_local, route_wps_local, params
+ )
+
+ elif mission_type == MissionType.GRID_SWEEP:
+ params = params or {}
+ rect_corners_gps = params.get('rect_corners')
+ if rect_corners_gps is None or len(rect_corners_gps) != 4:
+ raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點")
+ rect_corners_local = [
+ self.converter.gps_to_local(c[0], c[1], 0)[:2]
+ for c in rect_corners_gps
+ ]
+ waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
+ drone_local, rect_corners_local, params
+ )
+ else:
+ raise ValueError(f"不支援的任務類型: {mission_type}")
+
+ waypoints_gps = []
+ for drone_wps in waypoints_local:
+ gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
+ waypoints_gps.append(gps_wps)
+
+ return waypoints_gps, self.current_origin, rendezvous_indices
+
+ # ------------------------------------------------------------------ M-Formation
+
+ def _calculate_m_formation(self, drone_positions, target_point, params):
+ params = params or {}
+ N = len(drone_positions)
+ spacing = params.get('spacing', self.spacing)
+ base_altitude = params.get('base_altitude', self.base_altitude)
+ altitude_diff = params.get('altitude_diff', self.altitude_diff)
+
+ C_x = sum(pos[0] for pos in drone_positions) / N
+ C_y = sum(pos[1] for pos in drone_positions) / N
+
+ T_x, T_y, T_z = target_point
+ V_x, V_y = T_x - C_x, T_y - C_y
+ P_x, P_y = -V_y, V_x
+
+ length = math.sqrt(P_x ** 2 + P_y ** 2)
+ P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)
+
+ projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i)
+ for i, pos in enumerate(drone_positions)]
+ projections.sort()
+
+ stage1_positions = [None] * N
+ stage2_positions = [None] * N
+
+ for rank, (_, original_idx) in enumerate(projections):
+ offset = (rank - (N - 1) / 2) * spacing
+ altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
+ stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
+ stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)
+
+ return stage1_positions, stage2_positions
+
+ # ------------------------------------------------------------------ Circle
+
+ def _calculate_circle_formation(self, drone_positions, target_point, params):
+ params = params or {}
+ N = len(drone_positions)
+ radius = params.get('radius', 10.0)
+ altitude = params.get('altitude', 10.0)
+ start_angle = params.get('start_angle', 0.0)
+
+ center_x, center_y, center_z = target_point
+ stage1_positions = []
+ stage2_positions = []
+ angle_step = 360.0 / N
+
+ for i in range(N):
+ angle_rad = math.radians(start_angle + angle_step * i)
+ final_x = center_x + radius * math.cos(angle_rad)
+ final_y = center_y + radius * math.sin(angle_rad)
+ final_z = altitude
+
+ current_x, current_y, current_z = drone_positions[i]
+ stage1_positions.append((
+ current_x + (final_x - current_x) * 0.5,
+ current_y + (final_y - current_y) * 0.5,
+ current_z + (final_z - current_z) * 0.5
+ ))
+ stage2_positions.append((final_x, final_y, final_z))
+
+ return stage1_positions, stage2_positions
+
+ # ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊)
+
+ def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
+ """
+ 路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
+
+ 核心想法：
+ - center_path 建好後計算每個 sample 的累積弧長 s
+ - 每架 drone 給 (lat_k, lon_k) 的隊形偏移
+ - 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k
+ - follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent)
+ - 超過 path 起點/終點以 clip 處理，避免倒退或衝出
+
+ 解決前版「空間偏移」的三個 bug：
+ 1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退
+ 2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup
+ 3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線
+
+ Rank 定序規則：
+ **以 drone_positions 的輸入順序為準**（= GUI 選取順序）。
+ drone_positions[0] = rank 0 = leader (lon=0)，
+ drone_positions[1] = rank 1 (lon=5)，依此類推。
+ 刻意「不」用位置投影自動排序，避免浮點噪音造成 run-to-run
+ leader 漂移，以確保整個 mission 像軍隊縱隊那樣順序固定。
+
+ 隊形 (俯視, 以路徑行進方向為前):
+ D0 (lat=-L, lon=0) ← front-right (leader)
+ D1 (lat=+L, lon=5)
+ D2 (lat=-L, lon=10)
+ D3 (lat=+L, lon=15)
+ """
+ N = len(drone_positions)
+ lateral_offset = params.get('lateral_offset', 3.0)
+ longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
+ altitude = params.get('altitude', self.base_altitude)
+ min_inner_radius = params.get('min_inner_radius', 3.0)
+ arc_resolution = params.get('arc_resolution', 12)
+ sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0) # cos(90°)=0
+
+ # Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s）
+ center_path, barrier_indices = self._build_center_path(
+ route_wps_local,
+ formation_max_lateral=lateral_offset,
+ min_inner_radius=min_inner_radius,
+ arc_resolution=arc_resolution,
+ sharp_turn_cos_threshold=sharp_turn_cos,
+ )
+
+ s_list = [pt['s'] for pt in center_path]
+ s_max = s_list[-1]
+ n_pts = len(center_path)
+
+ def lookup(s_target):
+ """
+ 在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
+ s<0 → clip 到 start；s>s_max → clip 到 end。
+ tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
+ """
+ if n_pts == 1:
+ pt = center_path[0]
+ return pt['x'], pt['y'], pt['dir']
+
+ if s_target <= 0.0:
+ a = center_path[0]
+ b = center_path[1]
+ sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+ slen = math.hypot(sdx, sdy)
+ if slen > 1e-6:
+ return a['x'], a['y'], (sdx / slen, sdy / slen)
+ return a['x'], a['y'], a['dir']
+
+ if s_target >= s_max:
+ a = center_path[-2]
+ b = center_path[-1]
+ sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+ slen = math.hypot(sdx, sdy)
+ if slen > 1e-6:
+ return b['x'], b['y'], (sdx / slen, sdy / slen)
+ return b['x'], b['y'], b['dir']
+
+ # Binary search：s_list[lo] <= s_target < s_list[hi]
+ lo, hi = 0, n_pts - 1
+ while lo + 1 < hi:
+ mid = (lo + hi) // 2
+ if s_list[mid] <= s_target:
+ lo = mid
+ else:
+ hi = mid
+ a = center_path[lo]
+ b = center_path[hi]
+ ds = s_list[hi] - s_list[lo]
+ if ds < 1e-9:
+ return a['x'], a['y'], a['dir']
+ t = (s_target - s_list[lo]) / ds
+ x = a['x'] + t * (b['x'] - a['x'])
+ y = a['y'] + t * (b['y'] - a['y'])
+ sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
+ slen = math.hypot(sdx, sdy)
+ if slen > 1e-6:
+ return x, y, (sdx / slen, sdy / slen)
+ return x, y, a['dir']
+
+ # Step 2: rank = 輸入順序（GUI 選取順序），不再做 projection sort
+ # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
+ all_waypoints = [None] * N
+
+ # 預先算好路徑終點的切線（給收尾點用）
+ end = center_path[-1]
+ end_dx, end_dy = end['dir']
+ if n_pts >= 2:
+ a = center_path[-2]
+ sdx, sdy = end['x'] - a['x'], end['y'] - a['y']
+ slen = math.hypot(sdx, sdy)
+ if slen > 1e-6:
+ end_dx, end_dy = sdx / slen, sdy / slen
+
+ for rank in range(N):
+ lat_sign = -1 if rank % 2 == 0 else 1
+ lat = lat_sign * lateral_offset
+ lon = rank * longitudinal_spacing
+
+ waypoints = []
+ for i in range(n_pts):
+ s_follower = s_list[i] - lon
+ x, y, (dx, dy) = lookup(s_follower)
+ perp_x, perp_y = -dy, dx
+ waypoints.append((
+ x + lat * perp_x,
+ y + lat * perp_y,
+ altitude,
+ ))
+
+ # 收尾：全員到 path 終點的 rank 位置（lon 歸零）
+ waypoints.append((
+ end['x'] + lat * (-end_dy),
+ end['y'] + lat * end_dx,
+ altitude,
+ ))
+
+ all_waypoints[rank] = waypoints
+
+ # barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
+ # 不觸及末尾收尾點，直接沿用即可
+ return all_waypoints, barrier_indices
+
+ def _build_center_path(self, waypoints,
+ formation_max_lateral,
+ min_inner_radius,
+ arc_resolution,
+ sharp_turn_cos_threshold):
+ """
+ 建立以「圓弧」連接直線段的中心路徑。
+
+ 每個中間 waypoint 的處理：
+ 1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out)
+ 2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)，
+ barrier 放在該點，讓隊伍整體停下再繼續
+ 3. 否則 → 平滑弧：
+ R_base = formation_max_lateral + min_inner_radius
+ d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限
+ R_actual = d / tan(β/2)
+ tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
+ arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW
+ arc 由 theta_in 到 theta_out 以 arc_resolution 等分
+ path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)]
+ barrier 放在 T_out (轉完彎後的集合點)
+
+ path 是一個 list[dict]，每個 dict 至少含:
+ {'seg': 'straight' | 'arc',
+ 'x': float, 'y': float,
+ 'dir': (dx, dy)}
+ 'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta'
+
+ Returns:
+ (path, barrier_indices)
+ """
+ num_wps = len(waypoints)
+
+ if num_wps < 2:
+ return [{
+ 'seg': 'straight',
+ 'x': waypoints[0][0], 'y': waypoints[0][1],
+ 'dir': (0.0, 1.0),
+ }], []
+
+ segments = []
+ for i in range(num_wps - 1):
+ dx = waypoints[i + 1][0] - waypoints[i][0]
+ dy = waypoints[i + 1][1] - waypoints[i][1]
+ length = math.hypot(dx, dy)
+ if length < 0.01:
+ segments.append(((0.0, 1.0), length))
+ else:
+ segments.append(((dx / length, dy / length), length))
+
+ R_base = formation_max_lateral + min_inner_radius
+ path = []
+ barrier_indices = []
+
+ # 起點
+ path.append({
+ 'seg': 'straight',
+ 'x': waypoints[0][0], 'y': waypoints[0][1],
+ 'dir': segments[0][0],
+ })
+
+ for i in range(1, num_wps - 1):
+ u_in, len_in = segments[i - 1]
+ u_out, len_out = segments[i]
+ wp = waypoints[i]
+
+ dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
+ dot = max(-1.0, min(1.0, dot))
+
+ # 銳角：hover + 原地轉向
+ if dot < sharp_turn_cos_threshold:
+ avg_dx = u_in[0] + u_out[0]
+ avg_dy = u_in[1] + u_out[1]
+ avg_len = math.hypot(avg_dx, avg_dy)
+ if avg_len > 0.01:
+ avg_dir = (avg_dx / avg_len, avg_dy / avg_len)
+ else:
+ avg_dir = u_in
+ path.append({
+ 'seg': 'straight',
+ 'x': wp[0], 'y': wp[1],
+ 'dir': avg_dir,
+ })
+ barrier_indices.append(len(path) - 1)
+ continue
+
+ # 平滑弧
+ beta = math.acos(dot)
+ if beta < 1e-4:
+ # 幾乎直線，不插弧
+ path.append({
+ 'seg': 'straight',
+ 'x': wp[0], 'y': wp[1],
+ 'dir': u_in,
+ })
+ continue
+
+ half_tan = math.tan(beta / 2.0)
+ d_ideal = R_base * half_tan
+ d_max = 0.45 * min(len_in, len_out)
+ d = min(d_ideal, d_max)
+ R_actual = d / half_tan
+
+ T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d)
+ T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d)
+
+ # +1 CCW (左轉) / -1 CW (右轉)
+ cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
+ sign = 1 if cross > 0 else -1
+
+ # 入向左法線
+ n_left = (-u_in[1], u_in[0])
+ center = (
+ T_in[0] + R_actual * sign * n_left[0],
+ T_in[1] + R_actual * sign * n_left[1],
+ )
+
+ theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
+
+ # T_in (straight, 方向 u_in)
+ path.append({
+ 'seg': 'straight',
+ 'x': T_in[0], 'y': T_in[1],
+ 'dir': u_in,
+ })
+
+ # 弧段採樣 k=1..arc_resolution-1（不含兩端）
+ for k in range(1, arc_resolution):
+ t = k / arc_resolution
+ theta = theta_in + sign * beta * t
+ px = center[0] + R_actual * math.cos(theta)
+ py = center[1] + R_actual * math.sin(theta)
+ # 切線: sign × (-sin θ, cos θ)
+ tx = sign * (-math.sin(theta))
+ ty = sign * math.cos(theta)
+ path.append({
+ 'seg': 'arc',
+ 'x': px, 'y': py,
+ 'dir': (tx, ty),
+ 'arc_center': center,
+ 'arc_R': R_actual,
+ 'arc_sign': sign,
+ 'theta': theta,
+ })
+
+ # T_out (straight, 方向 u_out)，barrier 放這
+ path.append({
+ 'seg': 'straight',
+ 'x': T_out[0], 'y': T_out[1],
+ 'dir': u_out,
+ })
+ barrier_indices.append(len(path) - 1)
+
+ # 終點
+ path.append({
+ 'seg': 'straight',
+ 'x': waypoints[-1][0], 'y': waypoints[-1][1],
+ 'dir': segments[-1][0],
+ })
+
+ # 後處理：為每個 sample 加上累積 polyline 弧長 s
+ path[0]['s'] = 0.0
+ for i in range(1, len(path)):
+ prev = path[i - 1]
+ cur = path[i]
+ cur['s'] = prev['s'] + math.hypot(
+ cur['x'] - prev['x'], cur['y'] - prev['y']
+ )
+
+ return path, barrier_indices
+
+ # ------------------------------------------------------------------ 柵狀掃描
+
+ def _calculate_grid_sweep(self, drone_positions, rect_corners_local, params):
+ """柵狀掃描：掃描方向沿矩形長邊，切割方向沿短邊"""
+ N = len(drone_positions)
+ line_spacing = params.get('line_spacing', 5.0)
+ altitude = params.get('altitude', self.base_altitude)
+
+ c0, c1, c2, c3 = rect_corners_local
+
+ edge_a = (c1[0] - c0[0], c1[1] - c0[1])
+ len_a = math.sqrt(edge_a[0] ** 2 + edge_a[1] ** 2)
+ edge_b = (c3[0] - c0[0], c3[1] - c0[1])
+ len_b = math.sqrt(edge_b[0] ** 2 + edge_b[1] ** 2)
+
+ if len_a >= len_b:
+ sweep_vec = edge_a
+ sweep_len = len_a
+ cut_vec = edge_b
+ cut_len = len_b
+ sweep_start_mid = ((c0[0] + c3[0]) / 2, (c0[1] + c3[1]) / 2)
+ sweep_end_mid = ((c1[0] + c2[0]) / 2, (c1[1] + c2[1]) / 2)
+ cut_start_corner = c0
+ else:
+ sweep_vec = edge_b
+ sweep_len = len_b
+ cut_vec = edge_a
+ cut_len = len_a
+ sweep_start_mid = ((c0[0] + c1[0]) / 2, (c0[1] + c1[1]) / 2)
+ sweep_end_mid = ((c3[0] + c2[0]) / 2, (c3[1] + c2[1]) / 2)
+ cut_start_corner = c0
+
+ sweep_dir = (sweep_vec[0] / sweep_len, sweep_vec[1] / sweep_len)
+ cut_dir = (cut_vec[0] / cut_len, cut_vec[1] / cut_len)
+
+ C_x = sum(p[0] for p in drone_positions) / N
+ C_y = sum(p[1] for p in drone_positions) / N
+
+ dist_to_start = math.sqrt(
+ (C_x - sweep_start_mid[0]) ** 2 + (C_y - sweep_start_mid[1]) ** 2
+ )
+ dist_to_end = math.sqrt(
+ (C_x - sweep_end_mid[0]) ** 2 + (C_y - sweep_end_mid[1]) ** 2
+ )
+
+ if dist_to_start <= dist_to_end:
+ near_corner_a = cut_start_corner
+ else:
+ sweep_dir = (-sweep_dir[0], -sweep_dir[1])
+ near_corner_a = (cut_start_corner[0] + sweep_vec[0],
+ cut_start_corner[1] + sweep_vec[1])
+
+ projections = [
+ ((pos[0] - C_x) * cut_dir[0] + (pos[1] - C_y) * cut_dir[1], i)
+ for i, pos in enumerate(drone_positions)
+ ]
+ projections.sort()
+
+ strip_width = cut_len / N
+ drone_sweep_proj = C_x * sweep_dir[0] + C_y * sweep_dir[1]
+ near_sweep_proj = near_corner_a[0] * sweep_dir[0] + near_corner_a[1] * sweep_dir[1]
+ gather_sweep_proj = (drone_sweep_proj + near_sweep_proj) / 2
+
+ all_waypoints = [None] * N
+
+ for rank, (_, original_idx) in enumerate(projections):
+ strip_center_offset = (rank + 0.5) * strip_width
+ base_x = near_corner_a[0] + cut_dir[0] * strip_center_offset
+ base_y = near_corner_a[1] + cut_dir[1] * strip_center_offset
+
+ waypoints_list = []
+
+ gather_offset = gather_sweep_proj - near_sweep_proj
+ gx = base_x + sweep_dir[0] * gather_offset
+ gy = base_y + sweep_dir[1] * gather_offset
+ waypoints_list.append((gx, gy, altitude))
+
+ num_lines = max(1, int(strip_width / line_spacing))
+ actual_spacing = strip_width / num_lines
+ first_line_offset = (rank * strip_width) + actual_spacing / 2
+
+ entry_x = near_corner_a[0] + cut_dir[0] * first_line_offset
+ entry_y = near_corner_a[1] + cut_dir[1] * first_line_offset
+ waypoints_list.append((entry_x, entry_y, altitude))
+
+ current_cut_offset = first_line_offset
+
+ for line_idx in range(num_lines):
+ line_near_x = near_corner_a[0] + cut_dir[0] * current_cut_offset
+ line_near_y = near_corner_a[1] + cut_dir[1] * current_cut_offset
+ line_far_x = line_near_x + sweep_dir[0] * sweep_len
+ line_far_y = line_near_y + sweep_dir[1] * sweep_len
+
+ if line_idx % 2 == 0:
+ waypoints_list.append((line_far_x, line_far_y, altitude))
+ else:
+ waypoints_list.append((line_near_x, line_near_y, altitude))
+
+ if line_idx < num_lines - 1:
+ next_cut_offset = current_cut_offset + actual_spacing
+ next_near_x = near_corner_a[0] + cut_dir[0] * next_cut_offset
+ next_near_y = near_corner_a[1] + cut_dir[1] * next_cut_offset
+ next_far_x = next_near_x + sweep_dir[0] * sweep_len
+ next_far_y = next_near_y + sweep_dir[1] * sweep_len
+
+ if line_idx % 2 == 0:
+ waypoints_list.append((next_far_x, next_far_y, altitude))
+ else:
+ waypoints_list.append((next_near_x, next_near_y, altitude))
+ current_cut_offset = next_cut_offset
+
+ all_waypoints[original_idx] = waypoints_list
+
+ # 每一條掃描線的「起點」都做一次同步：
+ # wp 0 = gather, wp 1 = 第一條線起點, wp 2 = 第一條線終點,
+ # wp 3 = 第二條線起點, wp 4 = 第二條線終點, ...
+ # 每條線的「起點 index」= 1, 3, 5, ... = 2*i + 1（i 從 0 開始）
+ # 所有 drone 的 waypoint 數量相同（num_lines 對所有 strip 都一致），
+ # 所以用同一份 rendezvous_indices
+ rendezvous_indices = [2 * i + 1 for i in range(num_lines)]
+
+ return all_waypoints, rendezvous_indices
+
+
+# ================================================================================
+# 測試
+# ================================================================================
+if __name__ == "__main__":
+ planner = FormationPlanner()
+
+ drones = [
+ (24.123450, 120.567800, 0.0),
+ (24.123470, 120.567820, 0.0),
+ (24.123440, 120.567810, 0.0),
+ (24.123460, 120.567830, 0.0),
+ ]
+ target = (24.12400, 120.56795, 10.0)
+
+ # M-Formation
+ wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
+ print("M-Formation:")
+ for i, wp_list in enumerate(wps):
+ print(f" Drone {i}: {len(wp_list)} waypoints")
+
+ # Leader-Follower (虛擬領隊 / 弧長參數化)
+ route = [
+ (24.12345, 120.56780),
+ (24.12370, 120.56800),
+ (24.12390, 120.56820),
+ (24.12400, 120.56800),
+ (24.12420, 120.56790),
+ ]
+ wps_lf, origin_lf, rv_lf = planner.plan_formation_mission(
+ drones, target, MissionType.LEADER_FOLLOWER,
+ params={
+ 'route_waypoints': route,
+ 'lateral_offset': 3.0,
+ 'longitudinal_spacing': 5.0,
+ 'min_inner_radius': 3.0,
+ 'arc_resolution': 8,
+ 'altitude': 10.0
+ }
+ )
+ print(f"\nLeader-Follower (arc-length virtual leader):")
+ for i, wp_list in enumerate(wps_lf):
+ print(f" Drone {i}: {len(wp_list)} waypoints")
+ for j, wp in enumerate(wp_list):
+ print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})")
+
+ # Grid Sweep
+ rect = [
+ (24.1237, 120.5679),
+ (24.1237, 120.5683),
+ (24.1240, 120.5683),
+ (24.1240, 120.5679)
+ ]
+ wps2, origin2, rv2 = planner.plan_formation_mission(
+ drones, target, MissionType.GRID_SWEEP,
+ params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
+ )
+ print(f"\nGrid Sweep:")
+ for i, wp_list in enumerate(wps2):
+ print(f" Drone {i}: {len(wp_list)} waypoints")
\ No newline at end of file
diff --git a/src/GUI/overview_table.py b/src/GUI/overview_table.py
new file mode 100644
index 0000000..1b739e3
--- /dev/null
+++ b/src/GUI/overview_table.py
@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import QTableWidget, QTableWidgetItem, QHeaderView, QLabel
+from PyQt6.QtCore import Qt
+
+class OverviewTable(QTableWidget):
+ """總覽表格，顯示所有無人機的狀態資訊"""
+
+ # 默認的資訊類型和映射
+ DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
+ "空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+
+ DEFAULT_INFO_TYPE_MAP = {
+ "mode": 0,
+ "armed": 1,
+ "battery": 2,
+ "longitude": 3,
+ "latitude": 4,
+ "altitude": 5,
+ "local": 6,
+ "velocity": 7,
+ "groundspeed": 8,
+ "heading": 9,
+ "airspeed": 10,
+ "throttle": 11,
+ "hud_alt": 12,
+ "climb": 13,
+ "roll": 14,
+ "pitch": 15,
+ "yaw": 16,
+ "loss_rate": 17,
+ "ping": 18
+ }
+
+ def __init__(self, info_types=None, info_type_map=None, parent=None):
+ super().__init__(parent)
+
+ # 使用提供的或默認的資訊類型
+ self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES
+ self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP
+ self.drones = {} # 存儲無人機面板的引用
+
+ # 初始化表格
+ self.setColumnCount(1)
+ self.setRowCount(len(self.info_types))
+ self.setHorizontalHeaderLabels(["資訊"])
+ header = self.horizontalHeader()
+ header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents)
+ self.verticalHeader().setVisible(False)
+
+ # 設置第一列的資訊類型
+ for i, txt in enumerate(self.info_types):
+ item = QTableWidgetItem(txt)
+ item.setFlags(Qt.ItemFlag.ItemIsEnabled)
+ item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
+ self.setItem(i, 0, item)
+
+ def set_drones(self, drones):
+ """設置無人機面板字典的引用"""
+ self.drones = drones
+
+ def update_table(self, drone_id=None, field=None, value=None):
+ """更新總覽表格
+
+ Args:
+ drone_id: 無人機 ID
+ field: 欄位名稱 (如 'mode', 'altitude' 等)
+ value: 要更新的值
+ """
+ # 更新特定儲存格
+ if drone_id and field and value:
+ if drone_id not in self.drones:
+ return
+
+ col = 1 + list(self.drones.keys()).index(drone_id)
+ row = self.info_type_map.get(field, -1)
+
+ if row == -1:
+ return # 無效的欄位
+
+ item = self.item(row, col)
+ if not item:
+ item = QTableWidgetItem()
+ self.setItem(row, col, item)
+
+ item.setText(value)
+ item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
+
+ # 如果沒有指定更新，刷新整個表格
+ if drone_id is None:
+ self.refresh_all()
+
+ def refresh_all(self):
+ """刷新整個表格"""
+ cols = 1 + len(self.drones)
+ self.setColumnCount(cols)
+ headers = ["資訊"] + list(self.drones.keys())
+ self.setHorizontalHeaderLabels(headers)
+
+ for col, did in enumerate(self.drones, start=1):
+ panel = self.drones[did]
+ for field, row in self.info_type_map.items():
+ lbl = panel.findChild(QLabel, f"{did}_{field}")
+ val = lbl.text() if lbl else "--"
+ val_item = QTableWidgetItem(val)
+ val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
+ self.setItem(row, col, val_item)
+
+ def add_drone_column(self, drone_id):
+ """當新增無人機時，添加一列"""
+ if drone_id in self.drones:
+ self.refresh_all()
+
+ def remove_drone_column(self, drone_id):
+ """當移除無人機時，刷新表格"""
+ if drone_id in self.drones:
+ self.refresh_all()
diff --git a/src/GUI/validation/test_mission.py b/src/GUI/validation/test_mission.py
new file mode 100644
index 0000000..7693789
--- /dev/null
+++ b/src/GUI/validation/test_mission.py
@@ -0,0 +1,299 @@
+#!/usr/bin/env python3
+"""
+獨立測試腳本 — 驗證 MissionExecutor + MavlinkSender 在 SITL 環境下的運作
+
+使用方式:
+ 1. 啟動 SITL
+ 2. 修改下方 CONFIG 區塊
+ 3. python3 test_mission.py
+"""
+import sys, os
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..'))
+
+import time
+from pymavlink import mavutil
+from PyQt6.QtWidgets import QApplication
+from PyQt6.QtCore import QTimer
+
+from mission_planner import FormationPlanner, MissionType
+from command_sender import MavlinkSender
+from mission_executor import MissionExecutor, MissionState
+
+
+# ================================================================================
+# CONFIG
+# ================================================================================
+
+# 接收用連線 (讀取無人機狀態)
+RECV_CONNECTION = "udp:127.0.0.1:14550"
+
+# 發送用連線 (發送 setpoint 指令)
+SEND_CONNECTION = "udpout:127.0.0.1:14550"
+
+# 要控制的無人機 sysid 列表
+DRONE_SYSIDS = [1]
+
+# 起飛高度 (公尺)
+TAKEOFF_ALT = 10.0
+
+# 任務規劃參數
+FORMATION_SPACING = 5.0
+BASE_ALTITUDE = 10.0
+ALTITUDE_DIFF = 2.0
+ARRIVAL_RADIUS = 2.0
+
+# 測試模式: "formation" 或 "grid_sweep"
+TEST_MODE = "formation"
+
+# Grid Sweep 專用設定
+GRID_LINE_SPACING = 5.0
+
+# ================================================================================
+
+
+class SITLDroneManager:
+ """管理 SITL 無人機的連線、起飛前置作業"""
+
+ def __init__(self, connection_string, sysids):
+ self.connection_string = connection_string
+ self.sysids = sysids
+ self.mav = None
+ self.drone_gps = {}
+
+ def connect(self):
+ """建立 MAVLink 連線並等待心跳"""
+ print(f"連線到 {self.connection_string} ...")
+ self.mav = mavutil.mavlink_connection(self.connection_string)
+ self.mav.wait_heartbeat()
+ print(f"已收到心跳: sysid={self.mav.target_system}, compid={self.mav.target_component}")
+
+ def set_guided_and_arm(self, sysid):
+ """切換到 GUIDED 模式並解鎖"""
+ print(f"\n--- sysid={sysid}: 切換 GUIDED + 解鎖 ---")
+
+ # 切換 GUIDED 模式
+ self.mav.mav.command_long_send(
+ sysid, 1,
+ mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+ 0,
+ mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+ 4, # GUIDED = 4
+ 0, 0, 0, 0, 0
+ )
+ time.sleep(1)
+
+ # 解鎖
+ self.mav.mav.command_long_send(
+ sysid, 1,
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+ 0, 1, 0, 0, 0, 0, 0, 0
+ )
+ time.sleep(1)
+ print(f" sysid={sysid}: GUIDED + ARMED")
+
+ def takeoff(self, sysid, altitude):
+ """起飛到指定高度"""
+ print(f" sysid={sysid}: 起飛到 {altitude}m ...")
+ self.mav.mav.command_long_send(
+ sysid, 1,
+ mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+ 0, 0, 0, 0, 0, 0, 0, altitude
+ )
+
+ def wait_for_altitude(self, sysid, target_alt, timeout=30):
+ """等待無人機到達指定高度"""
+ print(f" sysid={sysid}: 等待到達 {target_alt}m ...")
+ start = time.time()
+ while time.time() - start < timeout:
+ msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+ if msg and msg.get_srcSystem() == sysid:
+ alt = msg.relative_alt / 1000.0
+ if alt >= target_alt * 0.9:
+ print(f" sysid={sysid}: 已到達 {alt:.1f}m")
+ return True
+ print(f" sysid={sysid}: 等待超時!")
+ return False
+
+ def update_gps_once(self):
+ """讀取一輪 GPS 資料更新 drone_gps"""
+ deadline = time.time() + 3
+ received = set()
+ while time.time() < deadline and len(received) < len(self.sysids):
+ msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+ if msg is None:
+ continue
+ sid = msg.get_srcSystem()
+ if sid in self.sysids:
+ drone_id = f"s0_{sid}"
+ self.drone_gps[drone_id] = {
+ 'lat': msg.lat / 1e7,
+ 'lon': msg.lon / 1e7,
+ 'alt': msg.relative_alt / 1000.0
+ }
+ received.add(sid)
+
+ for sid in self.sysids:
+ drone_id = f"s0_{sid}"
+ if drone_id in self.drone_gps:
+ gps = self.drone_gps[drone_id]
+ print(f" {drone_id}: ({gps['lat']:.6f}, {gps['lon']:.6f}, {gps['alt']:.1f}m)")
+ else:
+ print(f" {drone_id}: 尚未收到 GPS")
+
+ def start_gps_polling(self, interval_ms=200):
+ """啟動定時 GPS 輪詢 (用 QTimer)"""
+ self._gps_timer = QTimer()
+ self._gps_timer.timeout.connect(self._poll_gps)
+ self._gps_timer.start(interval_ms)
+
+ def _poll_gps(self):
+ """非阻塞方式讀取最新 GPS"""
+ while True:
+ msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=False)
+ if msg is None:
+ break
+ sid = msg.get_srcSystem()
+ if sid in self.sysids:
+ drone_id = f"s0_{sid}"
+ self.drone_gps[drone_id] = {
+ 'lat': msg.lat / 1e7,
+ 'lon': msg.lon / 1e7,
+ 'alt': msg.relative_alt / 1000.0
+ }
+
+
+def main():
+ # 建立 Qt 應用 (MissionExecutor 需要 QTimer)
+ app = QApplication(sys.argv)
+
+ # 連線 + 起飛前置作業
+ manager = SITLDroneManager(RECV_CONNECTION, DRONE_SYSIDS)
+ manager.connect()
+
+ for sysid in DRONE_SYSIDS:
+ manager.set_guided_and_arm(sysid)
+ manager.takeoff(sysid, TAKEOFF_ALT)
+
+ # 等待所有無人機到達起飛高度
+ for sysid in DRONE_SYSIDS:
+ manager.wait_for_altitude(sysid, TAKEOFF_ALT)
+
+ time.sleep(2)
+
+ # 讀取當前 GPS 位置
+ print("\n讀取當前 GPS 位置 ...")
+ manager.update_gps_once()
+
+ drone_ids = [f"s0_{sid}" for sid in DRONE_SYSIDS]
+ drone_gps_positions = []
+ for drone_id in drone_ids:
+ gps = manager.drone_gps.get(drone_id)
+ if gps is None:
+ print(f"錯誤: 讀不到 {drone_id} 的 GPS")
+ return
+ drone_gps_positions.append((gps['lat'], gps['lon'], gps['alt']))
+
+ # 規劃任務
+ print(f"\n規劃任務 (模式: {TEST_MODE}) ...")
+ planner = FormationPlanner(
+ spacing=FORMATION_SPACING,
+ base_altitude=BASE_ALTITUDE,
+ altitude_diff=ALTITUDE_DIFF
+ )
+
+ center_lat = drone_gps_positions[0][0]
+ center_lon = drone_gps_positions[0][1]
+
+ if TEST_MODE == "formation":
+ target_lat = center_lat + 30.0 / 111000.0
+ target_lon = center_lon
+ target_gps = (target_lat, target_lon, BASE_ALTITUDE)
+ print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})")
+
+ waypoints_per_drone, origin, _ = planner.plan_formation_mission(
+ drone_gps_positions, target_gps, MissionType.M_FORMATION
+ )
+
+ elif TEST_MODE == "grid_sweep":
+ # 在無人機前方 30m 處建立 40m x 30m 的矩形
+ offset_lat = 30.0 / 111000.0
+ half_w = 20.0 / (111000.0 * 0.9)
+ half_h = 15.0 / 111000.0
+
+ rect_center_lat = center_lat + offset_lat
+ rect_center_lon = center_lon
+
+ rect_corners = [
+ (rect_center_lat - half_h, rect_center_lon - half_w),
+ (rect_center_lat - half_h, rect_center_lon + half_w),
+ (rect_center_lat + half_h, rect_center_lon + half_w),
+ (rect_center_lat + half_h, rect_center_lon - half_w),
+ ]
+ target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE)
+ print(f" 矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})")
+
+ waypoints_per_drone, origin, _ = planner.plan_formation_mission(
+ drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
+ params={
+ 'rect_corners': rect_corners,
+ 'line_spacing': GRID_LINE_SPACING,
+ 'altitude': BASE_ALTITUDE
+ }
+ )
+ else:
+ print(f"未知測試模式: {TEST_MODE}")
+ return
+
+ planned_waypoints = {
+ 'drone_ids': drone_ids,
+ 'waypoints': waypoints_per_drone
+ }
+
+ # 印出規劃結果
+ for i, did in enumerate(drone_ids):
+ wps = waypoints_per_drone[i]
+ print(f" {did}: {len(wps)} 個航點")
+ for j, wp in enumerate(wps):
+ print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)")
+
+ # 啟動任務
+ print("\n啟動任務 ...")
+ manager.start_gps_polling(interval_ms=200)
+
+ sender = MavlinkSender(SEND_CONNECTION)
+ executor = MissionExecutor(
+ sender=sender,
+ drone_gps=manager.drone_gps,
+ arrival_radius=ARRIVAL_RADIUS,
+ send_rate_hz=2.0
+ )
+
+ executor.drone_waypoint_reached.connect(
+ lambda did, idx, total: print(f"\n >> {did} 到達 WP {idx}/{total}")
+ )
+ executor.mission_completed.connect(
+ lambda: (print("\n===== 任務全部完成 ====="), app.quit())
+ )
+
+ # 設定超時自動退出
+ timeout_timer = QTimer()
+ timeout_timer.setSingleShot(True)
+ timeout_timer.timeout.connect(lambda: (
+ print("\n⚠ 測試超時，強制退出"),
+ executor.stop(),
+ app.quit()
+ ))
+ timeout_timer.start(180_000) # 180 秒超時
+
+ executor.start(planned_waypoints)
+
+ print("進入事件迴圈 (等待任務完成或 180 秒超時) ...\n")
+ app.exec()
+
+ executor.stop()
+ sender.close()
+ print("測試結束")
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/GUI/validation/verify_waypoints.py b/src/GUI/validation/verify_waypoints.py
new file mode 100644
index 0000000..15e9356
--- /dev/null
+++ b/src/GUI/validation/verify_waypoints.py
@@ -0,0 +1,482 @@
+#!/usr/bin/env python3
+"""
+任務規劃視覺化驗證工具（含動畫模擬）
+
+使用方式:
+ 1. 由 GUI 自動觸發: python3 verify_waypoints.py --file /tmp/mission_plan.json
+ 2. 獨立手動執行: python3 verify_waypoints.py
+
+操作:
+ - 啟動後先顯示靜態航點圖
+ - 點擊圖下方的按鈕控制動畫
+"""
+import sys, os
+sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..'))
+
+import json
+import argparse
+import math
+import matplotlib
+matplotlib.use('TkAgg')
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+from matplotlib.widgets import Button
+from mpl_toolkits.mplot3d import Axes3D
+import numpy as np
+from mission_planner import FormationPlanner, MissionType, CoordinateConverter
+
+
+# ================================================================================
+# 色彩定義
+# ================================================================================
+COLORS = ['#378ADD', '#1D9E75', '#BA7517', '#D85A30', '#7F77DD', '#D4537E',
+ '#E24B4A', '#639922', '#00BFFF', '#FF69B4']
+
+# 動畫參數
+FRAMES_PER_SEGMENT = 40 # 每段航點之間的畫面數
+TRAIL_LENGTH = 60 # 拖尾長度（畫面數）
+INTERVAL_MS = 50 # 每幀間隔 (ms)
+
+
+# ================================================================================
+# 靜態繪圖
+# ================================================================================
+
+def plot_grid_sweep(ax, data, conv):
+ """畫 Grid Sweep 俯視圖"""
+ if 'rect_corners' in data:
+ rect_local = [conv.gps_to_local(c[0], c[1], 0)[:2] for c in data['rect_corners']]
+ xs = [p[0] for p in rect_local] + [rect_local[0][0]]
+ ys = [p[1] for p in rect_local] + [rect_local[0][1]]
+ ax.plot(xs, ys, 'k--', linewidth=1.5, label='Task area')
+ ax.fill(xs, ys, alpha=0.05, color='gray')
+
+ _draw_waypoint_paths(ax, data, conv, show_sweep_labels=True)
+
+ total_wps = sum(len(wps) for wps in data['waypoints'])
+ ax.set_title(f'Grid Sweep - {len(data["drone_ids"])} drones, {total_wps} waypoints')
+ ax.set_xlabel('East (m)')
+ ax.set_ylabel('North (m)')
+ ax.set_aspect('equal')
+ ax.grid(True, alpha=0.3)
+
+
+def plot_formation(ax, data, conv):
+ """畫 M-Formation / Circle / Leader-Follower 俯視圖"""
+ _draw_waypoint_paths(ax, data, conv, show_sweep_labels=False)
+
+ if 'target' in data:
+ t = data['target']
+ tx, ty, _ = conv.gps_to_local(t[0], t[1], t[2] if len(t) > 2 else 0)
+ ax.plot(tx, ty, 'r*', markersize=18, zorder=5)
+ ax.annotate('Target', (tx, ty + 1), fontsize=9, color='red', ha='center', va='bottom')
+
+ if 'route_waypoints' in data:
+ route_local = [conv.gps_to_local(wp[0], wp[1], 0)[:2] for wp in data['route_waypoints']]
+ rxs = [p[0] for p in route_local]
+ rys = [p[1] for p in route_local]
+ ax.plot(rxs, rys, 'r--', linewidth=1.5, alpha=0.5, label='Route center')
+ for i, (rx, ry) in enumerate(route_local):
+ ax.plot(rx, ry, 'ro', markersize=6, alpha=0.5)
+ ax.annotate(f'R{i+1}', (rx, ry + 0.8), fontsize=7, color='red',
+ ha='center', va='bottom', alpha=0.7)
+
+ mission_type = data.get('mission_type', 'formation')
+ total_wps = sum(len(wps) for wps in data['waypoints'])
+ ax.set_title(f'{mission_type} - {len(data["drone_ids"])} drones, {total_wps} waypoints')
+ ax.set_xlabel('East (m)')
+ ax.set_ylabel('North (m)')
+ ax.set_aspect('equal')
+ ax.grid(True, alpha=0.3)
+
+
+def _draw_waypoint_paths(ax, data, conv, show_sweep_labels=False):
+ """共用的航點路徑繪圖"""
+ drone_ids = data['drone_ids']
+ waypoints = data['waypoints']
+ drones_gps = data.get('drones_gps', [])
+
+ for i, pos in enumerate(drones_gps):
+ x, y, _ = conv.gps_to_local(pos[0], pos[1], pos[2] if len(pos) > 2 else 0)
+ c = COLORS[i % len(COLORS)]
+ ax.plot(x, y, 'o', color=c, markersize=10, zorder=5)
+ ax.annotate(f'{drone_ids[i]}', (x, y + 1), fontsize=8, fontweight='bold',
+ ha='center', va='bottom', color=c)
+
+ for i, wps in enumerate(waypoints):
+ c = COLORS[i % len(COLORS)]
+ local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
+ xs = [p[0] for p in local_wps]
+ ys = [p[1] for p in local_wps]
+ ax.plot(xs, ys, '-', color=c, linewidth=1.5, alpha=0.7)
+
+ for j, (x, y, z) in enumerate(local_wps):
+ if show_sweep_labels:
+ if j == 0:
+ ax.plot(x, y, 's', color=c, markersize=8, zorder=4)
+ ax.annotate('gather', (x, y), fontsize=6, ha='right', va='top')
+ elif j == 1:
+ ax.plot(x, y, '^', color=c, markersize=8, zorder=4)
+ ax.annotate('entry', (x, y), fontsize=6, ha='right', va='top')
+ else:
+ ax.plot(x, y, '.', color=c, markersize=4)
+ else:
+ marker = 's' if j == 0 else '*'
+ ax.plot(x, y, marker, color=c, markersize=10, zorder=4)
+ ax.annotate(f'WP{j}\n({z:.0f}m)', (x, y), fontsize=6, ha='center', va='bottom')
+
+ if local_wps:
+ lx, ly, _ = local_wps[-1]
+ ax.plot(lx, ly, 'X', color=c, markersize=10, markeredgewidth=2, zorder=4)
+
+
+# ================================================================================
+# 動畫模擬
+# ================================================================================
+
+class MissionAnimator:
+ """任務動畫控制器"""
+
+ def __init__(self, fig, ax, data, conv):
+ self.fig = fig
+ self.ax = ax
+ self.data = data
+ self.conv = conv
+ self.is_playing = False
+ self.anim = None
+ self.current_frame = 0
+
+ drone_ids = data['drone_ids']
+ waypoints = data['waypoints']
+ drones_gps = data.get('drones_gps', [])
+ self.num_drones = len(drone_ids)
+
+ # 建立完整航點序列：初始位置 → WP0 → WP1 → ...
+ self.all_local_wps = []
+ for i, wps in enumerate(waypoints):
+ local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
+
+ # 把初始位置插入最前面
+ if i < len(drones_gps):
+ pos = drones_gps[i]
+ start = conv.gps_to_local(pos[0], pos[1], pos[2] if len(pos) > 2 else 0)
+ local_wps.insert(0, start)
+
+ self.all_local_wps.append(local_wps)
+
+ # 計算最大航點段數
+ self.max_segments = max(len(wps) - 1 for wps in self.all_local_wps) if self.all_local_wps else 0
+ self.total_frames = self.max_segments * FRAMES_PER_SEGMENT
+
+ # 預計算位置
+ self.positions = self._precompute_positions()
+
+ # 動畫元素
+ self.drone_dots = []
+ self.trail_lines = []
+ self.trail_data = [[] for _ in range(self.num_drones)]
+ self.status_text = None
+
+ def _precompute_positions(self):
+ """預計算所有幀的位置 — 等時間步進"""
+ positions = []
+
+ for frame in range(self.total_frames + 1):
+ seg_idx = frame // FRAMES_PER_SEGMENT
+ seg_progress = (frame % FRAMES_PER_SEGMENT) / FRAMES_PER_SEGMENT
+
+ frame_positions = []
+ for drone_idx in range(self.num_drones):
+ wps = self.all_local_wps[drone_idx]
+ num_segs = len(wps) - 1
+
+ if seg_idx >= num_segs:
+ frame_positions.append((wps[-1][0], wps[-1][1]))
+ else:
+ x0, y0, _ = wps[seg_idx]
+ x1, y1, _ = wps[seg_idx + 1]
+ x = x0 + (x1 - x0) * seg_progress
+ y = y0 + (y1 - y0) * seg_progress
+ frame_positions.append((x, y))
+
+ positions.append(frame_positions)
+
+ return positions
+
+ def setup(self):
+ """建立動畫元素和按鈕"""
+ for i in range(self.num_drones):
+ c = COLORS[i % len(COLORS)]
+ dot, = self.ax.plot([], [], 'o', color=c, markersize=12,
+ markeredgecolor='white', markeredgewidth=1.5, zorder=10)
+ self.drone_dots.append(dot)
+ trail, = self.ax.plot([], [], '-', color=c, linewidth=2.5, alpha=0.4, zorder=9)
+ self.trail_lines.append(trail)
+
+ self.status_text = self.ax.text(
+ 0.02, 0.98, 'Ready',
+ transform=self.ax.transAxes, fontsize=10,
+ verticalalignment='top',
+ bbox=dict(boxstyle='round,pad=0.3', facecolor='white', alpha=0.8)
+ )
+
+ # 按鈕放在右上角圖內，避免擋到軸標籤
+ ax_play = self.fig.add_axes([0.72, 0.91, 0.08, 0.035])
+ self.btn_play = Button(ax_play, 'Play', color='#4CAF50', hovercolor='#66BB6A')
+ self.btn_play.label.set_color('white')
+ self.btn_play.label.set_fontweight('bold')
+ self.btn_play.on_clicked(self._on_play)
+
+ ax_pause = self.fig.add_axes([0.81, 0.91, 0.08, 0.035])
+ self.btn_pause = Button(ax_pause, 'Pause', color='#FF9800', hovercolor='#FFB74D')
+ self.btn_pause.label.set_color('white')
+ self.btn_pause.label.set_fontweight('bold')
+ self.btn_pause.on_clicked(self._on_pause)
+
+ ax_reset = self.fig.add_axes([0.90, 0.91, 0.08, 0.035])
+ self.btn_reset = Button(ax_reset, 'Reset', color='#F44336', hovercolor='#EF5350')
+ self.btn_reset.label.set_color('white')
+ self.btn_reset.label.set_fontweight('bold')
+ self.btn_reset.on_clicked(self._on_reset)
+
+ def _on_play(self, event):
+ if self.is_playing:
+ return
+ if self.anim is None:
+ self.anim = animation.FuncAnimation(
+ self.fig, self._update_frame,
+ frames=range(self.current_frame, self.total_frames + 1),
+ interval=INTERVAL_MS,
+ blit=False,
+ repeat=False
+ )
+ else:
+ self.anim.resume()
+ self.is_playing = True
+ self.fig.canvas.draw_idle()
+
+ def _on_pause(self, event):
+ if not self.is_playing or self.anim is None:
+ return
+ self.anim.pause()
+ self.is_playing = False
+ self.status_text.set_text(f'Paused (frame {self.current_frame}/{self.total_frames})')
+ self.fig.canvas.draw_idle()
+
+ def _on_reset(self, event):
+ if self.anim is not None:
+ self.anim.event_source.stop()
+ self.anim = None
+ self.is_playing = False
+ self.current_frame = 0
+ self.trail_data = [[] for _ in range(self.num_drones)]
+ for dot in self.drone_dots:
+ dot.set_data([], [])
+ for trail in self.trail_lines:
+ trail.set_data([], [])
+ self.status_text.set_text('Ready')
+ self.fig.canvas.draw_idle()
+
+ def _update_frame(self, frame):
+ self.current_frame = frame
+
+ if frame >= len(self.positions):
+ self.is_playing = False
+ self.status_text.set_text('Done')
+ return self.drone_dots + self.trail_lines + [self.status_text]
+
+ # seg_idx - 1 是因為第 0 段是 start→WP0
+ seg_idx = frame // FRAMES_PER_SEGMENT
+ progress = (frame % FRAMES_PER_SEGMENT) / FRAMES_PER_SEGMENT
+
+ # 顯示時把第 0 段標為 "Start -> WP0"
+ if seg_idx == 0:
+ label = f'Start -> WP0 Progress {progress:.0%}'
+ else:
+ label = f'WP{seg_idx-1} -> WP{seg_idx} Progress {progress:.0%}'
+ self.status_text.set_text(
+ f'{label} Frame {frame}/{self.total_frames}'
+ )
+
+ for i in range(self.num_drones):
+ x, y = self.positions[frame][i]
+ self.drone_dots[i].set_data([x], [y])
+
+ self.trail_data[i].append((x, y))
+ if len(self.trail_data[i]) > TRAIL_LENGTH:
+ self.trail_data[i] = self.trail_data[i][-TRAIL_LENGTH:]
+ trail_x = [p[0] for p in self.trail_data[i]]
+ trail_y = [p[1] for p in self.trail_data[i]]
+ self.trail_lines[i].set_data(trail_x, trail_y)
+
+ return self.drone_dots + self.trail_lines + [self.status_text]
+
+
+# ================================================================================
+# 主流程
+# ================================================================================
+
+def visualize_from_file(filepath):
+ """從 JSON 檔案讀取並視覺化"""
+ with open(filepath, 'r') as f:
+ data = json.load(f)
+
+ origin = data['origin']
+ conv = CoordinateConverter(origin[0], origin[1], 0)
+ mission_type = data.get('mission_type', 'formation')
+ is_sweep = mission_type == 'grid_sweep'
+
+ fig, ax = plt.subplots(1, 1, figsize=(10, 8))
+ fig.suptitle(f'Mission Verification - {mission_type}', fontsize=13, fontweight='bold')
+
+ if is_sweep:
+ plot_grid_sweep(ax, data, conv)
+ else:
+ plot_formation(ax, data, conv)
+
+ _print_summary(data)
+
+ animator = MissionAnimator(fig, ax, data, conv)
+ animator.setup()
+
+ plt.tight_layout(rect=[0, 0, 1, 0.95])
+ plt.show()
+
+
+def visualize_standalone():
+ """獨立執行：使用內建測試資料"""
+ drones = [
+ (24.123450, 120.567800, 0.0),
+ (24.123470, 120.567820, 0.0),
+ (24.123440, 120.567810, 0.0),
+ (24.123460, 120.567830, 0.0),
+ ]
+ rect_corners = [
+ (24.12380, 120.56775),
+ (24.12380, 120.56810),
+ (24.12420, 120.56810),
+ (24.12420, 120.56775),
+ ]
+ target = (24.12400, 120.56795, 10.0)
+
+ planner = FormationPlanner(spacing=5.0, base_altitude=10.0, altitude_diff=2.0)
+
+ fig = plt.figure(figsize=(16, 10))
+ fig.suptitle('Mission Planner Verification (standalone)', fontsize=14, fontweight='bold')
+
+ # M-Formation
+ wps_m, origin_m, _ = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
+ conv_m = CoordinateConverter(origin_m[0], origin_m[1], 0)
+ data_m = {
+ 'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
+ 'waypoints': wps_m,
+ 'drones_gps': drones,
+ 'target': target,
+ 'mission_type': 'M_FORMATION'
+ }
+ ax1 = fig.add_subplot(2, 2, 1)
+ plot_formation(ax1, data_m, conv_m)
+
+ # Grid Sweep 5m
+ target_gs = (sum(c[0] for c in rect_corners) / 4,
+ sum(c[1] for c in rect_corners) / 4, 10.0)
+ wps_g, origin_g, _ = planner.plan_formation_mission(
+ drones, target_gs, MissionType.GRID_SWEEP,
+ params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': 10.0}
+ )
+ conv_g = CoordinateConverter(origin_g[0], origin_g[1], 0)
+ data_g = {
+ 'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
+ 'waypoints': wps_g,
+ 'drones_gps': drones,
+ 'rect_corners': rect_corners,
+ 'mission_type': 'grid_sweep'
+ }
+ ax2 = fig.add_subplot(2, 2, 2)
+ plot_grid_sweep(ax2, data_g, conv_g)
+
+ # Leader-Follower
+ route = [
+ (24.12360, 120.56780),
+ (24.12380, 120.56800),
+ (24.12400, 120.56820),
+ (24.12410, 120.56800),
+ (24.12420, 120.56790),
+ ]
+ wps_lf, origin_lf, _ = planner.plan_formation_mission(
+ drones, target, MissionType.LEADER_FOLLOWER,
+ params={'route_waypoints': route, 'lateral_offset': 3.0,
+ 'longitudinal_spacing': 5.0, 'altitude': 10.0}
+ )
+ conv_lf = CoordinateConverter(origin_lf[0], origin_lf[1], 0)
+ data_lf = {
+ 'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
+ 'waypoints': wps_lf,
+ 'drones_gps': drones,
+ 'route_waypoints': route,
+ 'target': target,
+ 'mission_type': 'LEADER_FOLLOWER'
+ }
+ ax3 = fig.add_subplot(2, 2, 3)
+ plot_formation(ax3, data_lf, conv_lf)
+
+ # 3D
+ ax4 = fig.add_subplot(2, 2, 4, projection='3d')
+ _plot_3d(ax4, data_g, conv_g)
+
+ plt.tight_layout()
+ plt.show()
+
+
+def _plot_3d(ax, data, conv):
+ """3D 視角"""
+ if 'rect_corners' in data:
+ rect_local = [conv.gps_to_local(c[0], c[1], 0)[:2] for c in data['rect_corners']]
+ xs = [p[0] for p in rect_local] + [rect_local[0][0]]
+ ys = [p[1] for p in rect_local] + [rect_local[0][1]]
+ ax.plot(xs, ys, [0] * len(xs), 'k--', linewidth=1, alpha=0.4)
+
+ for i, wps in enumerate(data['waypoints']):
+ c = COLORS[i % len(COLORS)]
+ local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
+ xs = [p[0] for p in local_wps]
+ ys = [p[1] for p in local_wps]
+ zs = [p[2] for p in local_wps]
+ ax.plot(xs, ys, zs, '-', color=c, linewidth=1.5)
+ if local_wps:
+ ax.scatter(xs[0], ys[0], zs[0], color=c, s=50, marker='s')
+ ax.scatter(xs[-1], ys[-1], zs[-1], color=c, s=50, marker='X')
+
+ ax.set_title('3D view')
+ ax.set_xlabel('East (m)')
+ ax.set_ylabel('North (m)')
+ ax.set_zlabel('Alt (m)')
+
+
+def _print_summary(data):
+ """終端印出摘要"""
+ drone_ids = data['drone_ids']
+ waypoints = data['waypoints']
+ mission_type = data.get('mission_type', 'unknown')
+ print(f"\n{'=' * 50}")
+ print(f"Mission: {mission_type}")
+ print(f"Drones: {len(drone_ids)}")
+ print(f"{'=' * 50}")
+ for i, did in enumerate(drone_ids):
+ wps = waypoints[i]
+ print(f" {did}: {len(wps)} waypoints")
+ for j, wp in enumerate(wps):
+ print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)")
+ print(f"{'=' * 50}\n")
+
+
+if __name__ == "__main__":
+ parser = argparse.ArgumentParser(description='Mission waypoint visualizer')
+ parser.add_argument('--file', '-f', type=str, default=None,
+ help='JSON file from GUI (auto-generated)')
+ args = parser.parse_args()
+
+ if args.file:
+ visualize_from_file(args.file)
+ else:
+ visualize_standalone()
\ No newline at end of file
diff --git a/src/external/angles b/src/external/angles
new file mode 160000
index 0000000..a96224f
--- /dev/null
+++ b/src/external/angles
@@ -0,0 +1 @@
+Subproject commit a96224f9ab3ac51fe8fd981c1e1554528dc4345a
diff --git a/src/external/geographic_info b/src/external/geographic_info
new file mode 160000
index 0000000..24806ad
--- /dev/null
+++ b/src/external/geographic_info
@@ -0,0 +1 @@
+Subproject commit 24806adc767414eb3a34a58aefeb648ee415b09a
diff --git a/src/external/mavros_msgs/CHANGELOG.rst b/src/external/mavros_msgs/CHANGELOG.rst
new file mode 100644
index 0000000..3cbecbe
--- /dev/null
+++ b/src/external/mavros_msgs/CHANGELOG.rst
@@ -0,0 +1,1009 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package mavros_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.9.0 (2024-10-10)
+------------------
+
+2.8.0 (2024-06-07)
+------------------
+* regenerate all using cogall.sh
+* Merge branch 'master' into ros2
+ * master:
+ 1.19.0
+ update changelog
+ gps_global_origin: remove LLA to ECEF conversion
+* 1.19.0
+* update changelog
+* removed prefix in enums in messages and changed to use existing functions for string and quaternion convert
+* Final touches
+ Added functionality that was overlooked for camera tracking if supported, added copyright info, added custom exception thrown when mode enumerator is not understood
+* Added gimbal_control plugin
+ Added all functionality to support a plugin to enable compatibility with MAVLink Gimbal Protocol v2
+* Contributors: Frederik Mazur Andersen, Mark-Beaty, Vladimir Ermakov
+
+1.19.0 (2024-06-06)
+-------------------
+
+2.7.0 (2024-03-03)
+------------------
+* re-generate with cogall.sh
+* Merge branch 'master' into ros2
+ * master:
+ 1.18.0
+ update changelog
+ sys_status.cpp: improve timeout code
+ sys_status.cpp: Add a SYS_STATUS message publisher
+ [camera plugin] Fix image_index and capture_result not properly filled
+ Fix missing semi-colon
+ GPS_STATUS Plugin: Fill in available messages for ROS1 legacy
+* 1.18.0
+* update changelog
+* sys_status.cpp: Add a SYS_STATUS message publisher
+* cog checksum
+* remove event_time_boot_ms, fill stamp instead
+* handle events
+* Fix errata in GPSRAW.msg
+* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Seunghwan Jo, Vladimir Ermakov, victor
+
+1.18.0 (2024-03-03)
+-------------------
+* sys_status.cpp: Add a SYS_STATUS message publisher
+* Contributors: Dr.-Ing. Amilcar do Carmo Lucas
+
+2.6.0 (2023-09-09)
+------------------
+* msgs: move generator code
+* cog: regenerate all
+* Merge branch 'master' into ros2
+ * master:
+ 1.17.0
+ update changelog
+ cog: regenerate all
+ Bugfix/update map origin with home position (`#1892 `_)
+ mavros: Remove extra ';'
+ mavros_extras: Fix some init order warnings
+ Suppress warnings from included headers
+ 1.16.0
+ update changelog
+ made it such that the gp_origin topic published latched.
+ use hpp instead of deprecated .h pluginlib headers
+* 1.17.0
+* update changelog
+* cog: regenerate all
+* local takeoff and land topics (`#1890 `_)
+ * local takeoff and land topics
+ * vector3 position type, rename to TOLLocal
+ * remove auto include line
+* Merge pull request `#1871 `_ from Vladislavert/feature/optical_flow_msg
+ Addition of New OpticalFlow.msg
+* Added geometry_msgs/Vector3 to OpticalFlow.msg
+* Added vectors to the message OpticalFlow.msg
+* Added message optical flow
+* 1.16.0
+* update changelog
+* Contributors: Ido Guzi, Vladimir Ermakov, Vladislavert
+
+2.5.0 (2023-05-05)
+------------------
+
+2.4.0 (2022-12-30)
+------------------
+* msgs: re-generate
+* Merge branch 'master' into ros2
+ * master:
+ 1.15.0
+ update changelog
+ ci: update actions
+ Implement debug float array handler
+ mavros_extras: Fix a sequence point warning
+ mavros_extras: Fix a comparison that shouldn't be bitwise
+ mavros: Fix some warnings
+ mavros_extras: Fix buggy check for lat/lon ignored
+ libmavconn: fix MAVLink v1.0 output selection
+* 1.15.0
+* update changelog
+* Merge pull request `#1811 `_ from scoutdi/debug-float-array
+ Implement debug float array handler
+* Implement debug float array handler
+ Co-authored-by: Morten Fyhn Amundsen
+* Contributors: Sverre Velten Rothmund, Vladimir Ermakov
+
+2.3.0 (2022-09-24)
+------------------
+* Merge branch 'master' into ros2
+ * master:
+ 1.14.0
+ update changelog
+ scripts: waypoint and param files are text, not binary
+ libmavconn: fix MAVLink v1.0 output selection
+ plugins: add guided_target to accept offboard position targets
+ add cmake module path for geographiclib on debian based systems
+ use already installed FindGeographicLib.cmake
+* 1.14.0
+* update changelog
+* Contributors: Vladimir Ermakov
+
+2.2.0 (2022-06-27)
+------------------
+* Merge branch 'master' into ros2
+ * master:
+ mount_control.cpp: detect MOUNT_ORIENTATION stale messages
+ ESCTelemetryItem.msg: correct RPM units
+ apm_config.yaml: add mount configuration
+ sys_status.cpp fix free memory for values > 64KiB
+ uncrustify cellular_status.cpp
+ Add CellularStatus plugin and message
+ *_config.yaml: document usage of multiple batteries diagnostics
+ sys_status.cpp: fix compilation
+ sys_status.cpp: support diagnostics on up-to 10 batteries
+ sys_status.cpp: do not use harcoded constants
+ sys_status.cpp: Timeout on MEMINFO and HWSTATUS mavlink messages and publish on the diagnostics
+ sys_status.cpp: fix enabling of mem_diag and hwst_diag
+ sys_status.cpp: Do not use battery1 voltage as voltage for all other batteries (bugfix).
+ sys_status.cpp: ignore sys_status mavlink messages from gimbals
+ mount_control.cpp: use mount_nh for params to keep similarities with other plugins set diag settings before add()
+ sys_status.cpp: remove deprecated BATTERY2 mavlink message support
+ Mount control plugin: add configurable diagnostics
+ Bugfix: increment_f had no value asigned when input LaserScan was bigger than obstacle.distances.size()
+ Bugfix: wrong interpolation when the reduction ratio (scale_factor) is not integer.
+ Disable startup_px4_usb_quirk in px4_config.yaml
+* msgs: support humble
+
+2.1.1 (2022-03-02)
+------------------
+
+2.1.0 (2022-02-02)
+------------------
+* Merge branch 'master' into ros2
+ * master:
+ 1.13.0
+ update changelog
+ py-lib: fix compatibility with py3 for Noetic
+ re-generate all coglets
+ test: add checks for ROTATION_CUSTOM
+ lib: Fix rotation search for CUSTOM
+ Removed CamelCase for class members. Publish to "report"
+ More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages
+ Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention
+ Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html
+ Fixed topic names to match more closely what other plugins use. Fixed a typo.
+ Add plugin for reporting terrain height estimate from FCU
+ 1.12.2
+ update changelog
+ Set time/publish_sim_time to false by default
+ plugin: setpoint_raw: move getParam to initializer
+ extras: trajectory: backport `#1667 `_
+* 1.13.0
+* update changelog
+* Merge pull request `#1690 `_ from mavlink/fix-enum_sensor_orientation
+ Fix enum sensor_orientation
+* re-generate all coglets
+* Merge pull request `#1680 `_ from AndersonRayner/new_mav_frames
+ Add extra MAV_FRAMES to waypoint message
+* Merge pull request `#1677 `_ from AndersonRayner/add_terrain
+ Add plugin for reporting terrain height estimate from the FCU
+* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages
+* Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html
+* Add plugin for reporting terrain height estimate from FCU
+* 1.12.2
+* update changelog
+* Merge branch 'master' into ros2
+ * master:
+ 1.12.1
+ update changelog
+ mavconn: fix connection issue introduced by `#1658 `_
+ mavros_extras: Fix some warnings
+ mavros: Fix some warnings
+* 1.12.1
+* update changelog
+* Contributors: Vladimir Ermakov, matt
+
+2.0.5 (2021-11-28)
+------------------
+* Merge branch 'master' into ros2
+ * master:
+ 1.12.0
+ update changelog
+ Fix multiple bugs
+ lib: fix mission frame debug print
+ extras: distance_sensor: revert back to zero quaternion
+* 1.12.0
+* update changelog
+* extras: fix some more lint warns
+* msgs: update conversion header
+* Merge branch 'master' into ros2
+ * master:
+ 1.11.1
+ update changelog
+ lib: fix build
+* 1.11.1
+* update changelog
+* Merge branch 'master' into ros2
+ * master:
+ 1.11.0
+ update changelog
+ lib: fix ftf warnings
+ msgs: use pragmas to ignore unaligned pointer warnings
+ extras: landing_target: fix misprint
+ msgs: fix convert const
+ plugin: setpoint_raw: fix misprint
+ msgs: try to hide 'unaligned pointer' warning
+ plugin: sys: fix compillation error
+ plugin: initialize quaternions with identity
+ plugin: sys: Use wall timers for connection management
+ Use meters for relative altitude
+ distance_sensor: Initialize sensor orientation quaternion to zero
+ Address review comments
+ Add camera plugin for interfacing with mavlink camera protocol
+* 1.11.0
+* update changelog
+* msgs: use pragmas to ignore unaligned pointer warnings
+* msgs: fix convert const
+* msgs: try to hide 'unaligned pointer' warning
+* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin
+ Add camera plugin for interfacing with mavlink camera protocol
+* Address review comments
+* Add camera plugin for interfacing with mavlink camera protocol
+ Add camera image captured message for handling camera trigger information
+* Merge branch 'master' into ros2
+ * master:
+ msgs: add yaw field to GPS_INPUT
+* msgs: add yaw field to GPS_INPUT
+* Contributors: Jaeyoung-Lim, Vladimir Ermakov
+
+2.0.4 (2021-11-04)
+------------------
+* Merge branch 'master' into ros2
+ * master:
+ 1.10.0
+ prepare release
+* 1.10.0
+* prepare release
+* Merge branch 'master' into ros2
+ * master:
+ msgs: update gpsraw to have yaw field
+* msgs: update gpsraw to have yaw field
+* Merge branch 'master' into ros2
+ * master: (25 commits)
+ Remove reference
+ Catch std::length_error in send_message
+ Show ENOTCONN error instead of crash
+ Tunnel: Check for invalid payload length
+ Tunnel.msg: Generate enum with cog
+ mavros_extras: Create tunnel plugin
+ mavros_msgs: Add Tunnel message
+ MountControl.msg: fix copy-paste
+ sys_time.cpp: typo
+ sys_time: publish /clock for simulation times
+ 1.9.0
+ update changelog
+ Spelling corrections
+ Changed OverrideRCIn to 18 channels
+ This adds functionality to erase all logs on the SD card via mavlink
+ publish BATTERY2 message as /mavros/battery2 topic
+ Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0
+ Added NAV_CONTROLLER_OUTPUT Plugin
+ Added GPS_INPUT plugin
+ Update esc_status plugin with datatype change on MAVLink.
+ ...
+* Merge pull request `#1625 `_ from scoutdi/tunnel-plugin
+ Plugin for TUNNEL messages
+* Tunnel.msg: Generate enum with cog
+* mavros_msgs: Add Tunnel message
+* Merge pull request `#1623 `_ from amilcarlucas/pr/more-typo-fixes
+ More typo fixes
+* MountControl.msg: fix copy-paste
+* 1.9.0
+* update changelog
+* Merge pull request `#1616 `_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions
+ Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.…
+* Changed OverrideRCIn to 18 channels
+* Merge pull request `#1617 `_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin
+ Added NAV_CONTROLLER_OUTPUT Plugin
+* Merge pull request `#1618 `_ from amilcarlucas/pr/GPS_INPUT-plugin
+ Added GPS_INPUT plugin
+* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0
+* Added NAV_CONTROLLER_OUTPUT Plugin
+* Added GPS_INPUT plugin
+* Merge branch 'master' into master
+* Update esc_status plugin with datatype change on MAVLink.
+ ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side.
+* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message.
+* msgs: re-generate file lists
+* Merge branch 'master' into ros2
+ * master:
+ extras: esc_telemetry: fix build
+ extras: fix esc_telemetry centi-volt/amp conversion
+ extras: uncrustify all plugins
+ plugins: reformat xml
+ extras: reformat plugins xml
+ extras: fix apm esc_telemetry
+ msgs: fix types for apm's esc telemetry
+ actually allocate memory for the telemetry information
+ fixed some compile errors
+ added esc_telemetry plugin
+ Reset calibration flag when re-calibrating. Prevent wrong data output.
+ Exclude changes to launch files.
+ Delete debug files.
+ Apply uncrustify changes.
+ Set progress array to global to prevent erasing data.
+ Move Compass calibration report to extras. Rewrite code based on instructions.
+ Remove extra message from CMakeLists.
+ Add message and service definition.
+ Add compass calibration feedback status. Add service to call the 'Next' button in calibrations.
+* msgs: fix types for apm's esc telemetry
+* actually allocate memory for the telemetry information
+* added esc_telemetry plugin
+* Add Mount angles message for communications with ardupilotmega.
+* Remove extra message from CMakeLists.
+* Add message and service definition.
+* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Morten Fyhn Amundsen, Ricardo Marques, Russell, Vladimir Ermakov
+
+2.0.3 (2021-06-20)
+------------------
+
+2.0.2 (2021-06-20)
+------------------
+
+2.0.1 (2021-06-06)
+------------------
+* Add rcl_interfaces dependency
+* Merge branch 'master' into ros2
+ * master:
+ readme: update
+ 1.8.0
+ update changelog
+ Create semgrep-analysis.yml
+ Create codeql-analysis.yml
+* 1.8.0
+* update changelog
+* Contributors: Rob Clarke, Vladimir Ermakov
+
+2.0.0 (2021-05-28)
+------------------
+* msgs: update command codes
+* msgs: update param services
+* plugins: setpoint_velocity: port to ros2
+* Merge branch 'master' into ros2
+ * master:
+ 1.7.1
+ update changelog
+ re-generate all pymavlink enums
+ 1.7.0
+ update changelog
+* mavros: generate plugin list
+* Merge branch 'master' into ros2
+ * master:
+ msgs: re-generate the code
+ lib: re-generate the code
+ plugins: mission: re-generate the code
+ MissionBase: correction to file information
+ MissionBase: add copyright from origional waypoint.cpp
+ uncrustify
+ whitespace
+ add rallypoint and geofence plugins to mavros plugins xml
+ add rallypoint and geofence plugins to CMakeList
+ Geofence: add geofence plugin
+ Rallypoint: add rallypoint plugin
+ Waypoint: inherit MissionBase class for mission protocol
+ MissionBase: breakout mission protocol from waypoint.cpp
+ README: Update PX4 Autopilot references
+ Fix https://github.com/mavlink/mavros/issues/849
+* router: catch DeviceError
+* router: weak_ptr segfaults, replace with shared_ptr
+* router: implement params handler
+* mavros: router decl done
+* lib: port enum_to_string
+* lib: update sensor_orientation
+* msgs: add linter
+* libmavconn: start porintg, will use plain asio, without boost
+* msgs: remove redundant dependency which result in colcon warning
+* msgs: cogify file lists
+* Merge pull request `#1186 `_ from PickNikRobotics/ros2
+ mavros_msgs Ros2
+* Merge branch 'ros2' into ros2
+* msgs: start porting to ROS2
+* fixing cmakelists
+* updating msg and srv list
+* reenable VfrHud once renamed to match ROS2 conventions
+ add ros1_bridge mapping rule for renamed VfrHud message
+* make mavro_msgs compile in ROS 2
+* Contributors: Mikael Arguedas, Mike Lautman, Vladimir Ermakov
+
+1.17.0 (2023-09-09)
+-------------------
+* cog: regenerate all
+* Contributors: Vladimir Ermakov
+
+1.16.0 (2023-05-05)
+-------------------
+
+1.15.0 (2022-12-30)
+-------------------
+* Merge pull request `#1811 `_ from scoutdi/debug-float-array
+ Implement debug float array handler
+* Implement debug float array handler
+ Co-authored-by: Morten Fyhn Amundsen
+* Contributors: Sverre Velten Rothmund, Vladimir Ermakov
+
+1.14.0 (2022-09-24)
+-------------------
+* Merge pull request `#1742 `_ from amilcarlucas/correct_rpm_units
+ ESCTelemetryItem.msg: correct RPM units
+* ESCTelemetryItem.msg: correct RPM units
+* Merge pull request `#1727 `_ from BV-OpenSource/pr-cellular-status
+ Pr cellular status
+* Add CellularStatus plugin and message
+* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Rui Mendes, Vladimir Ermakov
+
+1.13.0 (2022-01-13)
+-------------------
+* Merge pull request `#1690 `_ from mavlink/fix-enum_sensor_orientation
+ Fix enum sensor_orientation
+* re-generate all coglets
+* Merge pull request `#1680 `_ from AndersonRayner/new_mav_frames
+ Add extra MAV_FRAMES to waypoint message
+* Merge pull request `#1677 `_ from AndersonRayner/add_terrain
+ Add plugin for reporting terrain height estimate from the FCU
+* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages
+* Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html
+* Add plugin for reporting terrain height estimate from FCU
+* Contributors: Vladimir Ermakov, matt
+
+1.12.2 (2021-12-12)
+-------------------
+
+1.12.1 (2021-11-29)
+-------------------
+
+1.12.0 (2021-11-27)
+-------------------
+
+1.11.1 (2021-11-24)
+-------------------
+
+1.11.0 (2021-11-24)
+-------------------
+* msgs: use pragmas to ignore unaligned pointer warnings
+* msgs: fix convert const
+* msgs: try to hide 'unaligned pointer' warning
+* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin
+ Add camera plugin for interfacing with mavlink camera protocol
+* Address review comments
+* Add camera plugin for interfacing with mavlink camera protocol
+ Add camera image captured message for handling camera trigger information
+* msgs: add yaw field to GPS_INPUT
+* Contributors: Jaeyoung-Lim, Vladimir Ermakov
+
+1.10.0 (2021-11-04)
+-------------------
+* msgs: update gpsraw to have yaw field
+* Merge pull request `#1625 `_ from scoutdi/tunnel-plugin
+ Plugin for TUNNEL messages
+* Tunnel.msg: Generate enum with cog
+* mavros_msgs: Add Tunnel message
+* Merge pull request `#1623 `_ from amilcarlucas/pr/more-typo-fixes
+ More typo fixes
+* MountControl.msg: fix copy-paste
+* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Morten Fyhn Amundsen, Vladimir Ermakov
+
+1.9.0 (2021-09-09)
+------------------
+* Merge pull request `#1616 `_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions
+ Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.…
+* Changed OverrideRCIn to 18 channels
+* Merge pull request `#1617 `_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin
+ Added NAV_CONTROLLER_OUTPUT Plugin
+* Merge pull request `#1618 `_ from amilcarlucas/pr/GPS_INPUT-plugin
+ Added GPS_INPUT plugin
+* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0
+* Added NAV_CONTROLLER_OUTPUT Plugin
+* Added GPS_INPUT plugin
+* Merge branch 'master' into master
+* Update esc_status plugin with datatype change on MAVLink.
+ ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side.
+* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message.
+* msgs: fix types for apm's esc telemetry
+* actually allocate memory for the telemetry information
+* added esc_telemetry plugin
+* Add Mount angles message for communications with ardupilotmega.
+* Remove extra message from CMakeLists.
+* Add message and service definition.
+* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Ricardo Marques, Russell, Vladimir Ermakov
+
+1.8.0 (2021-05-05)
+------------------
+
+1.7.1 (2021-04-05)
+------------------
+* re-generate all pymavlink enums
+* Contributors: Vladimir Ermakov
+
+1.7.0 (2021-04-05)
+------------------
+* msgs: re-generate the code
+* Contributors: Vladimir Ermakov
+
+1.6.0 (2021-02-15)
+------------------
+
+1.5.2 (2021-02-02)
+------------------
+
+1.5.1 (2021-01-04)
+------------------
+
+1.5.0 (2020-11-11)
+------------------
+* mavros_msgs/VehicleInfo: Add flight_custom_version field
+ Mirroring the field in the corresponding MAVLink message.
+* mavros_msgs/State: Fix PX4 flight mode constants
+ Turns out ROS message string literals don't need quotes,
+ so adding quotes creates strings including the quotes.
+* mavros_msgs/State: Add flight mode constants
+* mavros_msgs: Don't move temporary objects
+* Contributors: Morten Fyhn Amundsen
+
+1.4.0 (2020-09-11)
+------------------
+* play_tune: Assign tune format directly
+* play_tune: Write new plugin
+* Contributors: Morten Fyhn Amundsen
+
+1.3.0 (2020-08-08)
+------------------
+* Add esc_status plugin.
+* Add gps_status plugin to publish GPS_RAW and GPS_RTK messages from FCU.
+ The timestamps for the gps_status topics take into account the mavlink time and uses the convienence function
+* adding support for publishing rtkbaseline msgs over ROS
+* Contributors: CSCE439, Dr.-Ing. Amilcar do Carmo Lucas, Ricardo Marques
+
+1.2.0 (2020-05-22)
+------------------
+* add yaw to CMD_DO_SET_HOME
+* Contributors: David Jablonski
+
+1.1.0 (2020-04-04)
+------------------
+
+1.0.0 (2020-01-01)
+------------------
+
+0.33.4 (2019-12-12)
+-------------------
+* Splitted the message fields.
+* Updated esimator status msg according to the new cog based definition of estimator status.
+* Added comments to msg.
+* Added new line char at end of message.
+* Added a publisher for estimator status message received from mavlink in sys_status.
+* Contributors: saifullah3396
+
+0.33.3 (2019-11-13)
+-------------------
+
+0.33.2 (2019-11-13)
+-------------------
+
+0.33.1 (2019-11-11)
+-------------------
+* resolved merge conflict
+* Contributors: David Jablonski
+
+0.33.0 (2019-10-10)
+-------------------
+* Add vtol transition service
+* Apply comments
+* Add mount configure service message
+* cog: Update all generated code
+* added manual flag to mavros/state
+* use header.stamp to fill mavlink msg field time_usec
+* use cog for copy
+* adapt message and plugin after mavlink message merge
+* rename message and adjust fields
+* add component id to mavros message to distinguish ROS msgs from different systems
+* component_status message and plugin draft
+* Contributors: David Jablonski, Jaeyoung-Lim, Vladimir Ermakov, baumanta
+
+0.32.2 (2019-09-09)
+-------------------
+
+0.32.1 (2019-08-08)
+-------------------
+
+0.32.0 (2019-07-06)
+-------------------
+* add mav_cmd associated with each point in trajectory plugin
+* Use MountControl Msg
+* Define new MountControl.msg
+* Contributors: Jaeyoung-Lim, Martina Rivizzigno
+
+0.31.0 (2019-06-07)
+-------------------
+* mavros_msgs: LandingTarget: update msg description link
+* extras: landing target: improve usability and flexibility
+* Contributors: TSC21
+
+0.30.0 (2019-05-20)
+-------------------
+
+0.29.2 (2019-03-06)
+-------------------
+
+0.29.1 (2019-03-03)
+-------------------
+* All: catkin lint files
+* mavros_msgs: Fix line endings for OpticalFlowRad message
+* Contributors: Pierre Kancir, sfalexrog
+
+0.29.0 (2019-02-02)
+-------------------
+* Fix broken documentation URLs
+* Merge branch 'master' into param-timeout
+* mavros_extras: Wheel odometry plugin updated according to the final mavlink WHEEL_DISTANCE message.
+* mavros_msgs: Float32ArrayStamped replaced by WheelOdomStamped.
+* mavros_msgs: Float32ArrayStamped message added.
+ For streaming timestamped data from FCU sensors (RPM, WHEEL_DISTANCE, etc.)
+* msgs: Fix message id type, mavlink v2 uses 24 bit msg ids
+* mavros_msgs: add MessageInterval.srv to CMakeLists
+* sys_status: add set_message_interval service
+* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Pavlo Kolomiiets, Randy Mackay, Vladimir Ermakov
+
+0.28.0 (2019-01-03)
+-------------------
+* plugin:param: publish new param value
+* Merge pull request `#1148 `_ from Kiwa21/pr-param-value
+ param plugin : add msg and publisher to catch latest param value
+* msgs: update Header
+* sys_state: Small cleanup of `#1150 `_
+* VehicleInfo : add srv into sys_status plugin to request basic info from vehicle
+* mavros_msgs/msg/LogData.msg: Define "offset" field to be of type uint32
+* param plugin : add msg and publisher to catch latest param value
+* style clean up
+* Use component_id to determine message sender
+* change message name from COMPANION_STATUS to COMPANION_PROCESS_STATUS
+* change message to include pid
+* Change from specific avoidance status message to a more generic companion status message
+* Add message for avoidance status
+* Contributors: Gregoire Linard, Vladimir Ermakov, baumanta, mlvov
+
+0.27.0 (2018-11-12)
+-------------------
+* Add service to send mavlink TRIGG_INTERVAL commands
+ Adapt trigger_control service to current mavlink cmd spec. Add a new service to change trigger interval and integration time
+* Contributors: Moritz Zimmermann
+
+0.26.3 (2018-08-21)
+-------------------
+* fixup! 5a4344a2dcedc157f93b620cebd2e0b273ec24be
+* mavros_msgs: Add msg and srv files related to log transfer
+* Contributors: mlvov
+
+0.26.2 (2018-08-08)
+-------------------
+* Updating the gps_rtk plugin to fit mavros guidelines:
+ - Updating max_frag_len to allow changes in size in MAVLink seamlessly
+ - Using std::copy instead of memset
+ - Zero fill with std::fill
+ - Preapply the sequence flags
+ - Use of std iterators
+ - Add the maximal data size in the mavros_msgs
+* Renaming the GPS RTK module, Adding fragmentation, Changing the RTCM message
+* RTK Plugin; to forward RTCM messages
+ Signed-off-by: Alexis Paques
+* Contributors: Alexis Paques
+
+0.26.1 (2018-07-19)
+-------------------
+
+0.26.0 (2018-06-06)
+-------------------
+* mavros_msgs : add timesync status message
+* Contributors: Mohammed Kabir
+
+0.25.1 (2018-05-14)
+-------------------
+
+0.25.0 (2018-05-11)
+-------------------
+* trajectory: add time_horizon field
+* change message name from ObstacleAvoidance to Trajectory since it is
+ general enough to support any type of trajectory
+* CMakeLists: add ObstacleAvoidance message
+* add ObstacleAvoidance message
+* msgs: Update message doc link
+* CommandCode: update list of available commands on MAV_CMD enum (`#995 `_)
+* Contributors: Martina, Nuno Marques, Vladimir Ermakov
+
+0.24.0 (2018-04-05)
+-------------------
+* Add ability to send STATUSTEXT messages
+* Contributors: Anass Al
+
+0.23.3 (2018-03-09)
+-------------------
+
+0.23.2 (2018-03-07)
+-------------------
+
+0.23.1 (2018-02-27)
+-------------------
+
+0.23.0 (2018-02-03)
+-------------------
+
+0.22.0 (2017-12-11)
+-------------------
+* SetMavFrame.srv: add FRAME\_ prefix
+* Add cog for SetMavFrame.srv
+* Setpoints: add service to specify frame
+* Contributors: Pierre Kancir, khancyr
+
+0.21.5 (2017-11-16)
+-------------------
+
+0.21.4 (2017-11-01)
+-------------------
+
+0.21.3 (2017-10-28)
+-------------------
+* plugin waypoints: Use stamped message
+* add debug plugin
+* Contributors: TSC21, Vladimir Ermakov
+
+0.21.2 (2017-09-25)
+-------------------
+
+0.21.1 (2017-09-22)
+-------------------
+
+0.21.0 (2017-09-14)
+-------------------
+* plugin waypoint: Rename current seq in wp list message
+* waypoint: Publish current waypoint seq
+* waypoint partial: code style cleanup
+* waypoint partial: extend existing service
+* Partial waypoint: added wp_transfered to push partial service response
+* Partial waypoint: added partial updating to mavwp
+* Contributors: James Mare, James Stewart, Vladimir Ermakov
+
+0.20.1 (2017-08-28)
+-------------------
+
+0.20.0 (2017-08-23)
+-------------------
+* HIL Plugin
+ * add HilSensor.msg, HilStateQuaternion.msg, and add them in CMakeLists.txt
+ * Add hil_sensor.cpp plugin to send HIL_SENSOR mavlink message to FCU.
+ * fix HilSensor.msg. Make it more compact.
+ * Fix HilStateQuaternion.msg. Make it more compact.
+ * Add hil_state_quaternion plugin
+ * fix files: some variable names were wrong+some syntax problems
+ * fix syntax error in plugin .cpp files, make msg files match corresponding mavlink definitions
+ * fix plugin source files
+ * fix syntax
+ * fix function name. It was wrong.
+ * add HIL_GPS plugin
+ * add HilGPS.msg to CMakeList
+ * fix missing semicolon
+ * fix call of class name
+ * Add ACTUATOR_CONTROL_TARGET MAVLink message
+ * fix code
+ * increase number of fake satellites
+ * control sensor and control rates
+ * change control rate
+ * change control rate
+ * fix fake gps rate
+ * fix
+ * fix plugin_list
+ * fix
+ * remove unnecessary hil_sensor_mixin
+ * update HilSensor.msg and usage
+ * update HilStateQuaterion.msg and usage
+ * redo some changes; update HilGPS.msg and usage
+ * update hil_controls msg - use array of floats for aux channels
+ * merge actuator_control with actuator_control_target
+ * remove hil_sensor_mixin.h
+ * update actuator_control logic
+ * merge all plugins into a single one
+ * delete the remaining plugin files
+ * update description
+ * redo some changes; reduce LOC
+ * fix type cast on gps coord
+ * add HIL_OPTICAL_FLOW send based on OpticalFlowRad sub
+ * update authors list
+ * update subscribers names
+ * refactor gps coord convention
+ * add HIL_RC_INPUTS_RAW sender; cog protec msg structure and content
+ * apply correct rc_in translation; redo cog
+ * apply proper rotations and frame transforms
+ * remote throttle
+ * fix typo and msg api
+ * small changes
+ * refactor rcin_raw_cb
+ * new refactor to rcin_raw_cb arrays
+ * update velocity to meters
+ * readjust all the units so to match mavlink msg def
+ * update cog
+ * correct cog conversion
+ * refefine msg definitions to remove overhead
+ * hil: apply frame transform to body frame
+* msgs fix `#625 `_: Rename SetMode.Response.success to mode_sent
+* [WIP] Plugins: setpoint_attitude: add sync between thrust and attitude (`#700 `_)
+ * plugins: setpoint_attitude: add sync between throttle and attitude topics to be sent together
+ * plugins: typo correction: replace throttle with thrust
+ * plugins: msgs: setpoint_attitude: replaces Float32Stamped for Thrust msg
+ * plugins: setpoint_attitude: add sync between twist and thrust (RPY+Thrust)
+ * setpoint_attitude: update the logic of thrust normalization verification
+ * setpoint_attitude: implement sync between tf listener and thrust subscriber
+ * TF sync listener: generalize topic type that can be syncronized with TF2
+ * TF2ListenerMixin: keep class template, use template for tf sync method only
+ * TF2ListenerMixin: fix and improve sync tf2_start method
+ * general update to yaml config files and parameters
+ * setpoint_attitude: add note on Thrust sub name
+ * setpoint_attitude: TF sync: pass subscriber pointer instead of binding it
+* Use GeographicLib tools to guarantee ROS msg def and enhance features (`#693 `_)
+ * first commit
+ * Check for GeographicLib first without having to install it from the beginning each compile time
+ * add necessary cmake files
+ * remove gps_conversions.h and use GeographicLib to obtain the UTM coordinates
+ * move conversion functions to utils.h
+ * geographic conversions: update CMakeLists and package.xml
+ * geographic conversions: force download of the datasets
+ * geographic conversions: remove unneeded cmake module
+ * dependencies: use SHARED libs of geographiclib
+ * dependencies: correct FindGeographicLib.cmake so it can work for common Debian platforms
+ * CMakeList: do not be so restrict about GeographicLib dependency
+ * global position: odometry-use ECEF instead of UTM; update other fields
+ * global position: make travis happy
+ * global position: fix ident
+ * global_position: apply correct frames and frame transforms given each coordinate frame
+ * global_position: convert rcvd global origin to ECEF
+ * global_position: be more explicit about the ecef-enu transform
+ * global position: use home position as origin of map frame
+ * global position: minor refactoring
+ * global position: shield code with exception catch
+ * fix identation
+ * move dataset install to script; update README with new functionalities
+ * update README with warning
+ * global_position: fix identation
+ * update HomePosition to be consistent with the conversions in global_position to ensure the correct transformation of height
+ * home|global_position: fix compile errors, logic and dependencies
+ * home position: add height conversion
+ * travis: update to get datasets
+ * install geo dataset: update to verify alternative dataset folders
+ * travis: remove dataset install to allow clean build
+ * hp and gp: initialize geoid dataset once and make it thread safe
+ * README: update description relative to GeographicLib; fix typos
+ * global position: improve doxygen references
+ * README: update with some tips on rosdep install
+* update ExtendedState with new MAV_LANDED_STATE enum
+* Contributors: Nicklas Stockton, Nuno Marques, Vladimir Ermakov
+
+0.19.0 (2017-05-05)
+-------------------
+* msgs: Add cog script to finish ADSBVehicle.msg
+* extras: Add ADSB plugin
+* plugin `#695 `_: Fix plugin
+* plugin: Add home_position
+* Contributors: Nuno Marques, Vladimir Ermakov
+
+0.18.7 (2017-02-24)
+-------------------
+* trigger interface : rename to cycle_time to be consistent with PX4
+* Contributors: Kabir Mohammed
+
+0.18.6 (2017-02-07)
+-------------------
+* Plugins: system_status change status field to system_status
+ Add comment to State.msg for system_status enum
+* Plugins: add system_status to state message
+* Contributors: Pierre Kancir
+
+0.18.5 (2016-12-12)
+-------------------
+
+0.18.4 (2016-11-11)
+-------------------
+* msgs: Fix `#609 `_
+* add hil_actuator_controls mavlink message
+* Contributors: Beat Kung, Vladimir Ermakov
+
+0.18.3 (2016-07-07)
+-------------------
+
+0.18.2 (2016-06-30)
+-------------------
+
+0.18.1 (2016-06-24)
+-------------------
+
+0.18.0 (2016-06-23)
+-------------------
+* Adding anchor to the HIL_CONTROLS message reference link
+* Utilizing synchronise_stamp and adding reference to MAVLINK msg documentation
+* Added a plugin that publishes HIL_CONTROLS as ROS messages
+* node: Rename plugib base class - API incompatible to old class
+* msgs `#543 `_: Update for MAVLink 2.0
+* Contributors: Pavel, Vladimir Ermakov
+
+0.17.3 (2016-05-20)
+-------------------
+
+0.17.2 (2016-04-29)
+-------------------
+
+0.17.1 (2016-03-28)
+-------------------
+
+0.17.0 (2016-02-09)
+-------------------
+* rebased with master
+* Contributors: francois
+
+0.16.6 (2016-02-04)
+-------------------
+
+0.16.5 (2016-01-11)
+-------------------
+
+0.16.4 (2015-12-14)
+-------------------
+* Update mavlink message documentation links
+* remove "altitude\_" prefix from members
+* implemented altitude plugin
+* Contributors: Andreas Antener, Vladimir Ermakov
+
+0.16.3 (2015-11-19)
+-------------------
+
+0.16.2 (2015-11-17)
+-------------------
+
+0.16.1 (2015-11-13)
+-------------------
+
+0.16.0 (2015-11-09)
+-------------------
+* msgs `#418 `_: add message for attitude setpoints
+* plugin: waypoint fix `#414 `_: remove GOTO service.
+ It is replaced with more standard global setpoint messages.
+* msgs `#415 `_: Add message for raw global setpoint
+* msgs `#402 `_: PositionTarget message type
+* setting constant values and reference docs
+* pass new extended state to ros
+* msgs `#371 `_: add missing message
+* msgs `#371 `_: add HomePosition message
+* Contributors: Andreas Antener, Vladimir Ermakov
+
+0.15.0 (2015-09-17)
+-------------------
+* msgs `#286 `_: fix bug with packet header.
+* msgs `#286 `_: Add valid flag and checksum to Mavlink.msg
+* plugin: manual_control: Use shared pointer message
+ Fix alphabetic order of msgs.
+* removed old commend in .msg file
+* Add MANUAL_CONTROL handling with new plugin
+* Contributors: Vladimir Ermakov, v01d
+
+0.14.2 (2015-08-20)
+-------------------
+
+0.14.1 (2015-08-19)
+-------------------
+
+0.14.0 (2015-08-17)
+-------------------
+* msgs: Add mixer group constants ActuatorControl
+* msgs: Add notes to message headers.
+* msgs: sort msgs in alphabetical order
+* msgs: use std::move for mavlink->ros convert
+* msgs: add note about convert function
+* msgs: change description, make catkin lint happy
+* msgs: move convert functions to msgs package.
+* msgs: fix message generator and runtime depent tags
+* msgs: remove never used Mavlink.fromlcm field.
+* msgs: add package name for all non basic types
+* msgs: fix msgs build
+* msgs `#354 `_: move all messages to mavros_msgs package.
+* Contributors: Vladimir Ermakov
diff --git a/src/external/mavros_msgs/CMakeLists.txt b/src/external/mavros_msgs/CMakeLists.txt
new file mode 100644
index 0000000..a3220b0
--- /dev/null
+++ b/src/external/mavros_msgs/CMakeLists.txt
@@ -0,0 +1,208 @@
+cmake_minimum_required(VERSION 3.5)
+project(mavros_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ # we dont use add_compile_options with pedantic in message packages
+ # because the Python C extensions dont comply with it
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(geographic_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# include_directories(include)
+
+# [[[cog:
+# import mavros_cog
+# ]]]
+# [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e)
+
+set(msg_files
+ # [[[cog:
+ # mavros_cog.outl_glob_files('msg', '*.msg')
+ # ]]]
+ msg/ADSBVehicle.msg
+ msg/ActuatorControl.msg
+ msg/Altitude.msg
+ msg/AttitudeTarget.msg
+ msg/CamIMUStamp.msg
+ msg/CameraImageCaptured.msg
+ msg/CellularStatus.msg
+ msg/CommandCode.msg
+ msg/CompanionProcessStatus.msg
+ msg/DebugValue.msg
+ msg/ESCInfo.msg
+ msg/ESCInfoItem.msg
+ msg/ESCStatus.msg
+ msg/ESCStatusItem.msg
+ msg/ESCTelemetry.msg
+ msg/ESCTelemetryItem.msg
+ msg/EstimatorStatus.msg
+ msg/ExtendedState.msg
+ msg/FileEntry.msg
+ msg/GPSINPUT.msg
+ msg/GPSRAW.msg
+ msg/GPSRTK.msg
+ msg/GimbalDeviceAttitudeStatus.msg
+ msg/GimbalDeviceInformation.msg
+ msg/GimbalDeviceSetAttitude.msg
+ msg/GimbalManagerInformation.msg
+ msg/GimbalManagerSetAttitude.msg
+ msg/GimbalManagerSetPitchyaw.msg
+ msg/GimbalManagerStatus.msg
+ msg/GlobalPositionTarget.msg
+ msg/HilActuatorControls.msg
+ msg/HilControls.msg
+ msg/HilGPS.msg
+ msg/HilSensor.msg
+ msg/HilStateQuaternion.msg
+ msg/HomePosition.msg
+ msg/LandingTarget.msg
+ msg/LogData.msg
+ msg/LogEntry.msg
+ msg/MagnetometerReporter.msg
+ msg/ManualControl.msg
+ msg/Mavlink.msg
+ msg/MountControl.msg
+ msg/NavControllerOutput.msg
+ msg/OnboardComputerStatus.msg
+ msg/OpticalFlow.msg
+ msg/OpticalFlowRad.msg
+ msg/OverrideRCIn.msg
+ msg/Param.msg
+ msg/ParamEvent.msg
+ msg/ParamValue.msg
+ msg/PlayTuneV2.msg
+ msg/PositionTarget.msg
+ msg/RCIn.msg
+ msg/RCOut.msg
+ msg/RTCM.msg
+ msg/RTKBaseline.msg
+ msg/RadioStatus.msg
+ msg/State.msg
+ msg/StatusEvent.msg
+ msg/StatusText.msg
+ msg/SysStatus.msg
+ msg/TerrainReport.msg
+ msg/Thrust.msg
+ msg/TimesyncStatus.msg
+ msg/Trajectory.msg
+ msg/Tunnel.msg
+ msg/VehicleInfo.msg
+ msg/VfrHud.msg
+ msg/Vibration.msg
+ msg/Waypoint.msg
+ msg/WaypointList.msg
+ msg/WaypointReached.msg
+ msg/WheelOdomStamped.msg
+ # [[[end]]] (checksum: a8e24eb0a6da5cea6cc049fdc6b2612e)
+)
+
+set(srv_files
+ # [[[cog:
+ # mavros_cog.outl_glob_files('srv', '*.srv')
+ # ]]]
+ srv/CommandAck.srv
+ srv/CommandBool.srv
+ srv/CommandHome.srv
+ srv/CommandInt.srv
+ srv/CommandLong.srv
+ srv/CommandTOL.srv
+ srv/CommandTOLLocal.srv
+ srv/CommandTriggerControl.srv
+ srv/CommandTriggerInterval.srv
+ srv/CommandVtolTransition.srv
+ srv/EndpointAdd.srv
+ srv/EndpointDel.srv
+ srv/FileChecksum.srv
+ srv/FileClose.srv
+ srv/FileList.srv
+ srv/FileMakeDir.srv
+ srv/FileOpen.srv
+ srv/FileRead.srv
+ srv/FileRemove.srv
+ srv/FileRemoveDir.srv
+ srv/FileRename.srv
+ srv/FileTruncate.srv
+ srv/FileWrite.srv
+ srv/GimbalGetInformation.srv
+ srv/GimbalManagerCameraTrack.srv
+ srv/GimbalManagerConfigure.srv
+ srv/GimbalManagerPitchyaw.srv
+ srv/GimbalManagerSetRoi.srv
+ srv/LogRequestData.srv
+ srv/LogRequestEnd.srv
+ srv/LogRequestList.srv
+ srv/MessageInterval.srv
+ srv/MountConfigure.srv
+ srv/ParamGet.srv
+ srv/ParamPull.srv
+ srv/ParamPush.srv
+ srv/ParamSet.srv
+ srv/ParamSetV2.srv
+ srv/SetMavFrame.srv
+ srv/SetMode.srv
+ srv/StreamRate.srv
+ srv/VehicleInfoGet.srv
+ srv/WaypointClear.srv
+ srv/WaypointPull.srv
+ srv/WaypointPush.srv
+ srv/WaypointSetCurrent.srv
+ # [[[end]]] (checksum: cd7701b28a3176d96ef65cb1f2157917)
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ ${msg_files}
+ ${srv_files}
+ DEPENDENCIES
+ builtin_interfaces
+ rcl_interfaces
+ geographic_msgs
+ geometry_msgs
+ sensor_msgs
+ std_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+install(
+ FILES mavros_msgs_mapping_rule.yaml
+ DESTINATION share/${PROJECT_NAME}
+)
+
+if(rcl_interfaces_VERSION VERSION_LESS "1.2.0")
+ install(
+ DIRECTORY include/
+ DESTINATION include
+ FILES_MATCHING PATTERN "*.hpp"
+ )
+else()
+ # NOTE(vooon): Humble
+ install(
+ DIRECTORY include/
+ DESTINATION include/mavros_msgs
+ FILES_MATCHING PATTERN "*.hpp"
+ )
+endif()
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+
+ # NOTE(vooon): Does not support our custom triple-license, tiered to make it to work.
+ list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright)
+
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+# vim: ts=2 sw=2 et:
diff --git a/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp b/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp
new file mode 100644
index 0000000..6241e8a
--- /dev/null
+++ b/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp
@@ -0,0 +1,145 @@
+//
+// Copyright 2015,2016,2021 Vladimir Ermakov.
+//
+// This file is part of the mavros package and subject to the license terms
+// in the top-level LICENSE file of the mavros repository.
+// https://github.com/mavlink/mavros/tree/master/LICENSE.md
+//
+/**
+ * @brief Mavlink convert utils
+ * @file
+ * @author Vladimir Ermakov
+ */
+
+#pragma once
+#ifndef MAVROS_MSGS__MAVLINK_CONVERT_HPP_
+#define MAVROS_MSGS__MAVLINK_CONVERT_HPP_
+
+#include
+#include
+
+#include
+
+namespace mavros_msgs
+{
+namespace mavlink
+{
+
+using ::mavlink::mavlink_message_t;
+using mavros_msgs::msg::Mavlink;
+
+// [[[cog:
+// FIELD_NAMES = [
+// "magic",
+// "len",
+// "incompat_flags",
+// "compat_flags",
+// "seq",
+// "sysid",
+// "compid",
+// "msgid",
+// "checksum",
+// ]
+// ]]]
+// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e)
+
+// NOTE(vooon): Ignore impossible warning as
+// memcpy() should work with unaligned pointers without any trouble.
+//
+// warning: taking address of packed member of ‘mavlink::__mavlink_message’
+// may result in an unaligned pointer value
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+
+/**
+ * @brief Convert mavros_msgs/Mavlink message to mavlink_message_t
+ *
+ * @note signature vector should be empty for unsigned OR
+ * MAVLINK_SIGNATURE_BLOCK size for signed messages
+ *
+ * @param[in] rmsg mavros_msgs/Mavlink message
+ * @param[out] mmsg mavlink_message_t struct
+ * @return true if success
+ */
+inline bool convert(const Mavlink & rmsg, mavlink_message_t & mmsg)
+{
+ if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) {
+ return false;
+ }
+
+ if (!rmsg.signature.empty() && rmsg.signature.size() != sizeof(mmsg.signature)) {
+ return false;
+ }
+
+ // [[[cog:
+ // for f in FIELD_NAMES:
+ // cog.outl(f"mmsg.{f} = rmsg.{f};")
+ // ]]]
+ mmsg.magic = rmsg.magic;
+ mmsg.len = rmsg.len;
+ mmsg.incompat_flags = rmsg.incompat_flags;
+ mmsg.compat_flags = rmsg.compat_flags;
+ mmsg.seq = rmsg.seq;
+ mmsg.sysid = rmsg.sysid;
+ mmsg.compid = rmsg.compid;
+ mmsg.msgid = rmsg.msgid;
+ mmsg.checksum = rmsg.checksum;
+ // [[[end]]] (checksum: 0b66f0fc1cd46db0f18a2429c56a6b8c)
+ std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64);
+ std::copy(rmsg.signature.begin(), rmsg.signature.end(), mmsg.signature);
+
+ return true;
+}
+
+/**
+ * @brief Convert mavlink_message_t to mavros/Mavlink
+ *
+ * @param[in] mmsg mavlink_message_t struct
+ * @param[out] rmsg mavros_msgs/Mavlink message
+ * @param[in] framing_status framing parse result (OK, BAD_CRC or BAD_SIGNATURE)
+ * @return true, this convertion can't fail
+ */
+inline bool convert(
+ const mavlink_message_t & mmsg, Mavlink & rmsg,
+ uint8_t framing_status = Mavlink::FRAMING_OK)
+{
+ const size_t payload64_len = (mmsg.len + 7) / 8;
+
+ rmsg.framing_status = framing_status;
+
+ // [[[cog:
+ // for f in FIELD_NAMES:
+ // cog.outl(f"rmsg.{f} = mmsg.{f};")
+ // ]]]
+ rmsg.magic = mmsg.magic;
+ rmsg.len = mmsg.len;
+ rmsg.incompat_flags = mmsg.incompat_flags;
+ rmsg.compat_flags = mmsg.compat_flags;
+ rmsg.seq = mmsg.seq;
+ rmsg.sysid = mmsg.sysid;
+ rmsg.compid = mmsg.compid;
+ rmsg.msgid = mmsg.msgid;
+ rmsg.checksum = mmsg.checksum;
+ // [[[end]]] (checksum: 64ef6c1af60c622ed427e005d8ca4f2a)
+ rmsg.payload64.assign(
+ mmsg.payload64,
+ mmsg.payload64 + payload64_len);
+
+ // copy signature block only if message is signed
+ if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED) {
+ rmsg.signature.assign(
+ mmsg.signature,
+ mmsg.signature + sizeof(mmsg.signature));
+ } else {
+ rmsg.signature.clear();
+ }
+
+ return true;
+}
+
+#pragma GCC diagnostic pop
+
+} // namespace mavlink
+} // namespace mavros_msgs
+
+#endif // MAVROS_MSGS__MAVLINK_CONVERT_HPP_
diff --git a/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml b/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml
new file mode 100644
index 0000000..9f69364
--- /dev/null
+++ b/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml
@@ -0,0 +1,8 @@
+# This file defines mappings between ROS 1 and ROS 2 interfaces.
+# It is used with the ros1_bridge to allow for communcation between ROS 1 and ROS 2.
+
+-
+ ros1_package_name: 'mavros_msgs'
+ ros1_message_name: 'VFR_HUD'
+ ros2_package_name: 'mavros_msgs'
+ ros2_message_name: 'VfrHud'
diff --git a/src/external/mavros_msgs/msg/ADSBVehicle.msg b/src/external/mavros_msgs/msg/ADSBVehicle.msg
new file mode 100644
index 0000000..44b9caa
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ADSBVehicle.msg
@@ -0,0 +1,66 @@
+# The location and information of an ADSB vehicle
+#
+# https://mavlink.io/en/messages/common.html#ADSB_VEHICLE
+
+# [[[cog:
+# import mavros_cog
+# mavros_cog.idl_decl_enum('ADSB_ALTITUDE_TYPE', 'ALT_')
+# mavros_cog.idl_decl_enum('ADSB_EMITTER_TYPE', 'EMITTER_')
+# mavros_cog.idl_decl_enum('ADSB_FLAGS', 'FLAG_', 16)
+# ]]]
+# ADSB_ALTITUDE_TYPE
+uint8 ALT_PRESSURE_QNH = 0 # Altitude reported from a Baro source using QNH reference
+uint8 ALT_GEOMETRIC = 1 # Altitude reported from a GNSS source
+# ADSB_EMITTER_TYPE
+uint8 EMITTER_NO_INFO = 0
+uint8 EMITTER_LIGHT = 1
+uint8 EMITTER_SMALL = 2
+uint8 EMITTER_LARGE = 3
+uint8 EMITTER_HIGH_VORTEX_LARGE = 4
+uint8 EMITTER_HEAVY = 5
+uint8 EMITTER_HIGHLY_MANUV = 6
+uint8 EMITTER_ROTOCRAFT = 7
+uint8 EMITTER_UNASSIGNED = 8
+uint8 EMITTER_GLIDER = 9
+uint8 EMITTER_LIGHTER_AIR = 10
+uint8 EMITTER_PARACHUTE = 11
+uint8 EMITTER_ULTRA_LIGHT = 12
+uint8 EMITTER_UNASSIGNED2 = 13
+uint8 EMITTER_UAV = 14
+uint8 EMITTER_SPACE = 15
+uint8 EMITTER_UNASSGINED3 = 16
+uint8 EMITTER_EMERGENCY_SURFACE = 17
+uint8 EMITTER_SERVICE_SURFACE = 18
+uint8 EMITTER_POINT_OBSTACLE = 19
+# ADSB_FLAGS
+uint16 FLAG_VALID_COORDS = 1
+uint16 FLAG_VALID_ALTITUDE = 2
+uint16 FLAG_VALID_HEADING = 4
+uint16 FLAG_VALID_VELOCITY = 8
+uint16 FLAG_VALID_CALLSIGN = 16
+uint16 FLAG_VALID_SQUAWK = 32
+uint16 FLAG_SIMULATED = 64
+uint16 FLAG_VERTICAL_VELOCITY_VALID = 128
+uint16 FLAG_BARO_VALID = 256
+uint16 FLAG_SOURCE_UAT = 32768
+# [[[end]]] (checksum: a34f2a081739921b6e3e443ed0516d8d)
+
+std_msgs/Header header
+
+uint32 icao_address
+string callsign
+
+float64 latitude
+float64 longitude
+float32 altitude # AMSL
+
+float32 heading # deg [0..360)
+float32 hor_velocity # m/s
+float32 ver_velocity # m/s
+
+uint8 altitude_type # Type from ADSB_ALTITUDE_TYPE enum
+uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum
+
+builtin_interfaces/Duration tslc # Duration from last communication, seconds [0..255]
+uint16 flags # ADSB_FLAGS bit field
+uint16 squawk # Squawk code
diff --git a/src/external/mavros_msgs/msg/ActuatorControl.msg b/src/external/mavros_msgs/msg/ActuatorControl.msg
new file mode 100644
index 0000000..5332a08
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ActuatorControl.msg
@@ -0,0 +1,16 @@
+# raw servo values for direct actuator controls
+#
+# about groups, mixing and channels:
+# https://pixhawk.org/dev/mixing
+
+# constant for mixer group
+uint8 PX4_MIX_FLIGHT_CONTROL = 0
+uint8 PX4_MIX_FLIGHT_CONTROL_VTOL_ALT = 1
+uint8 PX4_MIX_PAYLOAD = 2
+uint8 PX4_MIX_MANUAL_PASSTHROUGH = 3
+#uint8 PX4_MIX_FC_MC_VIRT = 4
+#uint8 PX4_MIX_FC_FW_VIRT = 5
+
+std_msgs/Header header
+uint8 group_mix
+float32[8] controls
diff --git a/src/external/mavros_msgs/msg/Altitude.msg b/src/external/mavros_msgs/msg/Altitude.msg
new file mode 100644
index 0000000..81ae0a9
--- /dev/null
+++ b/src/external/mavros_msgs/msg/Altitude.msg
@@ -0,0 +1,12 @@
+# Altitude information
+#
+# https://mavlink.io/en/messages/common.html#ALTITUDE
+
+std_msgs/Header header
+
+float32 monotonic
+float32 amsl
+float32 local
+float32 relative
+float32 terrain
+float32 bottom_clearance
diff --git a/src/external/mavros_msgs/msg/AttitudeTarget.msg b/src/external/mavros_msgs/msg/AttitudeTarget.msg
new file mode 100644
index 0000000..a62712b
--- /dev/null
+++ b/src/external/mavros_msgs/msg/AttitudeTarget.msg
@@ -0,0 +1,17 @@
+# Message for SET_ATTITUDE_TARGET
+#
+# Some complex system requires all feautures that mavlink
+# message provide. See issue #402, #418.
+
+std_msgs/Header header
+
+uint8 type_mask
+uint8 IGNORE_ROLL_RATE = 1 # body_rate.x
+uint8 IGNORE_PITCH_RATE = 2 # body_rate.y
+uint8 IGNORE_YAW_RATE = 4 # body_rate.z
+uint8 IGNORE_THRUST = 64
+uint8 IGNORE_ATTITUDE = 128 # orientation field
+
+geometry_msgs/Quaternion orientation
+geometry_msgs/Vector3 body_rate
+float32 thrust
diff --git a/src/external/mavros_msgs/msg/CamIMUStamp.msg b/src/external/mavros_msgs/msg/CamIMUStamp.msg
new file mode 100644
index 0000000..52b9e8b
--- /dev/null
+++ b/src/external/mavros_msgs/msg/CamIMUStamp.msg
@@ -0,0 +1,4 @@
+# IMU-Camera synchronisation data
+
+builtin_interfaces/Time frame_stamp # Timestamp when the camera was triggered
+int32 frame_seq_id # Sequence number of the image frame
diff --git a/src/external/mavros_msgs/msg/CameraImageCaptured.msg b/src/external/mavros_msgs/msg/CameraImageCaptured.msg
new file mode 100644
index 0000000..44c1a98
--- /dev/null
+++ b/src/external/mavros_msgs/msg/CameraImageCaptured.msg
@@ -0,0 +1,11 @@
+# MAVLink message: CAMERA_IMAGE_CAPTURED
+# https://mavlink.io/en/messages/common.html#CAMERA_IMAGE_CAPTURED
+
+std_msgs/Header header
+
+geometry_msgs/Quaternion orientation # Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)
+geographic_msgs/GeoPoint geo
+float32 relative_alt # mm Altitude above ground
+int32 image_index # Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1)
+int8 capture_result # Boolean indicating success (1) or failure (0) while capturing this image.
+string file_url #URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface.
diff --git a/src/external/mavros_msgs/msg/CellularStatus.msg b/src/external/mavros_msgs/msg/CellularStatus.msg
new file mode 100644
index 0000000..2c9f99f
--- /dev/null
+++ b/src/external/mavros_msgs/msg/CellularStatus.msg
@@ -0,0 +1,9 @@
+#Follows https://mavlink.io/en/messages/common.html#CELLULAR_STATUS specification
+
+uint8 status
+uint8 failure_reason
+uint8 type
+uint8 quality
+uint16 mcc
+uint16 mnc
+uint16 lac
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/CommandCode.msg b/src/external/mavros_msgs/msg/CommandCode.msg
new file mode 100644
index 0000000..00fd7da
--- /dev/null
+++ b/src/external/mavros_msgs/msg/CommandCode.msg
@@ -0,0 +1,200 @@
+# MAV_CMD command codes.
+# Actual meaning and params you may find in MAVLink documentation
+# https://mavlink.io/en/messages/common.html#MAV_CMD
+
+# [[[cog:
+# import mavros_cog
+# mavros_cog.idl_decl_enum_mav_cmd()
+# ]]]
+# MAV_CMD_AIRFRAME
+uint16 AIRFRAME_CONFIGURATION = 2520
+
+# MAV_CMD_ARM
+uint16 ARM_AUTHORIZATION_REQUEST = 3001 # Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON.
+
+# MAV_CMD_CAMERA
+uint16 CAMERA_TRACK_POINT = 2004 # If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking.
+uint16 CAMERA_TRACK_RECTANGLE = 2005 # If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking.
+uint16 CAMERA_STOP_TRACKING = 2010 # Stops ongoing tracking.
+
+# MAV_CMD_CAN
+uint16 CAN_FORWARD = 32000 # Request forwarding of CAN packets from the given CAN bus to this interface. CAN Frames are sent using CAN_FRAME and CANFD_FRAME messages
+
+# MAV_CMD_COMPONENT
+uint16 COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component
+
+# MAV_CMD_CONDITION
+uint16 CONDITION_DELAY = 112 # Delay mission state machine.
+uint16 CONDITION_CHANGE_ALT = 113 # Ascend/descend to target altitude at specified rate. Delay mission state machine until desired altitude reached.
+uint16 CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point.
+uint16 CONDITION_YAW = 115 # Reach a certain target angle.
+uint16 CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration
+
+# MAV_CMD_CONTROL
+uint16 CONTROL_HIGH_LATENCY = 2600 # Request to start/stop transmitting over the high latency telemetry
+
+# MAV_CMD_DO
+uint16 DO_FOLLOW = 32 # Begin following a target
+uint16 DO_FOLLOW_REPOSITION = 33 # Reposition the MAV after a follow target command has been sent
+uint16 DO_SET_MODE = 176 # Set system mode.
+uint16 DO_JUMP = 177 # Jump to the desired command in the mission list. Repeat this action only the specified number of times
+uint16 DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points
+uint16 DO_SET_HOME = 179 # Changes the home location either to the current location or a specified location.
+uint16 DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter.
+uint16 DO_SET_RELAY = 181 # Set a relay to a condition.
+uint16 DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cycles with a desired period.
+uint16 DO_SET_SERVO = 183 # Set a servo to a desired PWM value.
+uint16 DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period.
+uint16 DO_FLIGHTTERMINATION = 185 # Terminate flight immediately
+uint16 DO_CHANGE_ALTITUDE = 186 # Change altitude set point.
+uint16 DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude/Altitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence.
+uint16 DO_RALLY_LAND = 190 # Mission command to perform a landing from a rally point.
+uint16 DO_GO_AROUND = 191 # Mission command to safely abort an autonomous landing.
+uint16 DO_REPOSITION = 192 # Reposition the vehicle to a specific WGS84 global position.
+uint16 DO_PAUSE_CONTINUE = 193 # If in a GPS controlled position mode, hold the current position or continue.
+uint16 DO_SET_REVERSE = 194 # Set moving direction to forward or reverse.
+uint16 DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 DO_SET_ROI_NONE = 197 # Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 DO_SET_ROI_SYSID = 198 # Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means.
+uint16 DO_CONTROL_VIDEO = 200 # Control onboard camera system.
+uint16 DO_SET_ROI = 201 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 DO_DIGICAM_CONFIGURE = 202 # Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ).
+uint16 DO_DIGICAM_CONTROL = 203 # Control digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ).
+uint16 DO_MOUNT_CONFIGURE = 204 # Mission command to configure a camera or antenna mount
+uint16 DO_MOUNT_CONTROL = 205 # Mission command to control a camera or antenna mount
+uint16 DO_SET_CAM_TRIGG_DIST = 206 # Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera.
+uint16 DO_FENCE_ENABLE = 207 # Mission command to enable the geofence
+uint16 DO_PARACHUTE = 208 # Mission item/command to release a parachute or enable/disable auto release.
+uint16 DO_MOTOR_TEST = 209 # Mission command to perform motor test.
+uint16 DO_INVERTED_FLIGHT = 210 # Change to/from inverted flight.
+uint16 DO_GRIPPER = 211 # Mission command to operate a gripper.
+uint16 DO_AUTOTUNE_ENABLE = 212 # Enable/disable autotune.
+uint16 DO_SET_CAM_TRIGG_INTERVAL = 214 # Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera.
+uint16 DO_MOUNT_CONTROL_QUAT = 220 # Mission command to control a camera or antenna mount, using a quaternion as reference.
+uint16 DO_GUIDED_MASTER = 221 # set id of master controller
+uint16 DO_GUIDED_LIMITS = 222 # Set limits for external control
+uint16 DO_ENGINE_CONTROL = 223 # Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines
+uint16 DO_SET_MISSION_CURRENT = 224 # Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between).
+uint16 DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration
+uint16 DO_JUMP_TAG = 601 # Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number.
+uint16 DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Set gimbal manager pitch/yaw setpoints (low rate command). It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: only the gimbal manager will react to this command - it will be ignored by a gimbal device. Use GIMBAL_MANAGER_SET_PITCHYAW if you need to stream pitch/yaw setpoints at higher rate.
+uint16 DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control.
+uint16 DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system.
+uint16 DO_VTOL_TRANSITION = 3000 # Request VTOL transition
+uint16 DO_ADSB_OUT_IDENT = 10001 # Trigger the start of an ADSB-out IDENT. This should only be used when requested to do so by an Air Traffic Controller in controlled airspace. This starts the IDENT which is then typically held for 18 seconds by the hardware per the Mode A, C, and S transponder spec.
+uint16 DO_WINCH = 42600 # Command to operate winch.
+
+# MAV_CMD_FIXED
+uint16 FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
+
+# MAV_CMD_GET
+uint16 GET_HOME_POSITION = 410 # Request the home position from the vehicle.
+uint16 GET_MESSAGE_INTERVAL = 510 # Request the interval between messages for a particular MAVLink message ID. The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message.
+
+# MAV_CMD_IMAGE
+uint16 IMAGE_START_CAPTURE = 2000 # Start image capture sequence. CAMERA_IMAGE_CAPTURED must be emitted after each capture. Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). It is also needed to specify the target camera in missions. When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). If the param1 is 0 the autopilot should do both. When sent in a command the target MAVLink address is set using target_component. If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). If addressed to a MAVLink camera, param 1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels.
+uint16 IMAGE_STOP_CAPTURE = 2001 # Stop image capture sequence. Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). It is also needed to specify the target camera in missions. When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). If the param1 is 0 the autopilot should do both. When sent in a command the target MAVLink address is set using target_component. If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). If addressed to a MAVLink camera, param1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels.
+
+# MAV_CMD_JUMP
+uint16 JUMP_TAG = 600 # Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG.
+
+# MAV_CMD_LOGGING
+uint16 LOGGING_START = 2510 # Request to start streaming logging data over MAVLink (see also LOGGING_DATA message)
+uint16 LOGGING_STOP = 2511 # Request to stop streaming log data over MAVLink
+
+# MAV_CMD_MISSION
+uint16 MISSION_START = 300 # start running a mission
+
+# MAV_CMD_NAV
+uint16 NAV_WAYPOINT = 16 # Navigate to waypoint.
+uint16 NAV_LOITER_UNLIM = 17 # Loiter around this waypoint an unlimited amount of time
+uint16 NAV_LOITER_TURNS = 18 # Loiter around this waypoint for X turns
+uint16 NAV_LOITER_TIME = 19 # Loiter around this waypoint for X seconds
+uint16 NAV_RETURN_TO_LAUNCH = 20 # Return to launch location
+uint16 NAV_LAND = 21 # Land at location.
+uint16 NAV_TAKEOFF = 22 # Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode.
+uint16 NAV_LAND_LOCAL = 23 # Land at local position (local frame only)
+uint16 NAV_TAKEOFF_LOCAL = 24 # Takeoff from local position (local frame only)
+uint16 NAV_FOLLOW = 25 # Vehicle following, i.e. this waypoint represents the position of a moving vehicle
+uint16 NAV_CONTINUE_AND_CHANGE_ALT = 30 # Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached.
+uint16 NAV_LOITER_TO_ALT = 31 # Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint.
+uint16 NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras.
+uint16 NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV.
+uint16 NAV_SPLINE_WAYPOINT = 82 # Navigate to waypoint using a spline path.
+uint16 NAV_VTOL_TAKEOFF = 84 # Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.).
+uint16 NAV_VTOL_LAND = 85 # Land using VTOL mode
+uint16 NAV_GUIDED_ENABLE = 92 # hand control over to an external controller
+uint16 NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time
+uint16 NAV_PAYLOAD_PLACE = 94 # Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload.
+uint16 NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration
+uint16 NAV_SET_YAW_SPEED = 213 # Sets a desired vehicle turn angle and speed change.
+uint16 NAV_FENCE_RETURN_POINT = 5000 # Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead.
+uint16 NAV_FENCE_POLYGON_VERTEX_INCLUSION = 5001 # Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required.
+uint16 NAV_FENCE_POLYGON_VERTEX_EXCLUSION = 5002 # Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required.
+uint16 NAV_FENCE_CIRCLE_INCLUSION = 5003 # Circular fence area. The vehicle must stay inside this area.
+uint16 NAV_FENCE_CIRCLE_EXCLUSION = 5004 # Circular fence area. The vehicle must stay outside this area.
+uint16 NAV_RALLY_POINT = 5100 # Rally point. You can have multiple rally points defined.
+
+# MAV_CMD_OBLIQUE
+uint16 OBLIQUE_SURVEY = 260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera.
+
+# MAV_CMD_OVERRIDE
+uint16 OVERRIDE_GOTO = 252 # Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position.
+
+# MAV_CMD_PANORAMA
+uint16 PANORAMA_CREATE = 2800 # Create a panorama at the current position
+
+# MAV_CMD_PAYLOAD
+uint16 PAYLOAD_PREPARE_DEPLOY = 30001 # Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity.
+uint16 PAYLOAD_CONTROL_DEPLOY = 30002 # Control the payload deployment.
+
+# MAV_CMD_PREFLIGHT
+uint16 PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero.
+uint16 PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode.
+uint16 PREFLIGHT_UAVCAN = 243 # Trigger UAVCAN configuration (actuator ID assignment and direction mapping). Note that this maps to the legacy UAVCAN v0 function UAVCAN_ENUMERATE, which is intended to be executed just once during initial vehicle configuration (it is not a normal pre-flight command and has been poorly named).
+uint16 PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode.
+uint16 PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components.
+
+# MAV_CMD_REQUEST
+uint16 REQUEST_MESSAGE = 512 # Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL).
+uint16 REQUEST_PROTOCOL_VERSION = 519 # Request MAVLink protocol version compatibility. All receivers should ACK the command and then emit their capabilities in an PROTOCOL_VERSION message
+uint16 REQUEST_AUTOPILOT_CAPABILITIES = 520 # Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message
+uint16 REQUEST_CAMERA_INFORMATION = 521 # Request camera information (CAMERA_INFORMATION).
+uint16 REQUEST_CAMERA_SETTINGS = 522 # Request camera settings (CAMERA_SETTINGS).
+uint16 REQUEST_STORAGE_INFORMATION = 525 # Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage.
+uint16 REQUEST_CAMERA_CAPTURE_STATUS = 527 # Request camera capture status (CAMERA_CAPTURE_STATUS)
+uint16 REQUEST_FLIGHT_INFORMATION = 528 # Request flight information (FLIGHT_INFORMATION)
+uint16 REQUEST_VIDEO_STREAM_INFORMATION = 2504 # Request video stream information (VIDEO_STREAM_INFORMATION)
+uint16 REQUEST_VIDEO_STREAM_STATUS = 2505 # Request video stream status (VIDEO_STREAM_STATUS)
+
+# MAV_CMD_RESET
+uint16 RESET_CAMERA_SETTINGS = 529 # Reset all camera settings to Factory Default
+
+# MAV_CMD_RUN
+uint16 RUN_PREARM_CHECKS = 401 # Instructs system to run pre-arm checks. This command should return MAV_RESULT_TEMPORARILY_REJECTED in the case the system is armed, otherwse MAV_RESULT_ACCEPTED. Note that the return value from executing this command does not indicate whether the vehicle is armable or not, just whether the system has successfully run/is currently running the checks. The result of the checks is reflected in the SYS_STATUS message.
+
+# MAV_CMD_SET
+uint16 SET_MESSAGE_INTERVAL = 511 # Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM.
+uint16 SET_CAMERA_MODE = 530 # Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming.
+uint16 SET_CAMERA_ZOOM = 531 # Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success).
+uint16 SET_CAMERA_FOCUS = 532 # Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success).
+uint16 SET_GUIDED_SUBMODE_STANDARD = 4000 # This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes.
+uint16 SET_GUIDED_SUBMODE_CIRCLE = 4001 # This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position.
+
+# MAV_CMD_START
+uint16 START_RX_PAIR = 500 # Starts receiver pairing.
+
+# MAV_CMD_STORAGE
+uint16 STORAGE_FORMAT = 526 # Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage.
+
+# MAV_CMD_UAVCAN
+uint16 UAVCAN_GET_NODE_INFO = 5200 # Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages.
+
+# MAV_CMD_VIDEO
+uint16 VIDEO_START_CAPTURE = 2500 # Starts video capture (recording).
+uint16 VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture (recording).
+uint16 VIDEO_START_STREAMING = 2502 # Start video streaming
+uint16 VIDEO_STOP_STREAMING = 2503 # Stop the given video stream
+
+# [[[end]]] (checksum: 73ee94ac661c9fcb61528a6668f71d94)
diff --git a/src/external/mavros_msgs/msg/CompanionProcessStatus.msg b/src/external/mavros_msgs/msg/CompanionProcessStatus.msg
new file mode 100644
index 0000000..ea8b08c
--- /dev/null
+++ b/src/external/mavros_msgs/msg/CompanionProcessStatus.msg
@@ -0,0 +1,19 @@
+# Mavros message: COMPANIONPROCESSSTATUS
+
+std_msgs/Header header
+
+uint8 state # See enum COMPANION_PROCESS_STATE
+uint8 component # See enum MAV_COMPONENT
+
+uint8 MAV_STATE_UNINIT = 0
+uint8 MAV_STATE_BOOT = 1
+uint8 MAV_STATE_CALIBRATING = 2
+uint8 MAV_STATE_STANDBY = 3
+uint8 MAV_STATE_ACTIVE = 4
+uint8 MAV_STATE_CRITICAL = 5
+uint8 MAV_STATE_EMERGENCY = 6
+uint8 MAV_STATE_POWEROFF = 7
+uint8 MAV_STATE_FLIGHT_TERMINATION = 8
+
+uint8 MAV_COMP_ID_OBSTACLE_AVOIDANCE = 196
+uint8 MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197
diff --git a/src/external/mavros_msgs/msg/DebugValue.msg b/src/external/mavros_msgs/msg/DebugValue.msg
new file mode 100644
index 0000000..286b6c2
--- /dev/null
+++ b/src/external/mavros_msgs/msg/DebugValue.msg
@@ -0,0 +1,26 @@
+# Msg for Debug MAVLink API
+#
+# Supported types:
+# DEBUG https://mavlink.io/en/messages/common.html#DEBUG
+# DEBUG_VECTOR https://mavlink.io/en/messages/common.html#DEBUG_VECT
+# DEBUG_FLOAT_ARRAY https://mavlink.io/en/messages/common.html#DEBUG_FLOAT_ARRAY
+# NAMED_VALUE_FLOAT https://mavlink.io/en/messages/common.html#NAMED_VALUE_FLOAT
+# NAMED_VALUE_INT https://mavlink.io/en/messages/common.html#NAMED_VALUE_INT
+
+std_msgs/Header header
+
+int32 index # index value of DEBUG value (-1 if not indexed)
+int32 array_id # Unique ID used to discriminate between DEBUG_FLOAT_ARRAYS (-1 if not used)
+
+string name # value name/key
+
+float32 value_float # float value for NAMED_VALUE_FLOAT and DEBUG
+int32 value_int # int value for NAMED_VALUE_INT
+float32[] data # DEBUG vector or array
+
+uint8 type
+uint8 TYPE_DEBUG = 0
+uint8 TYPE_DEBUG_VECT = 1
+uint8 TYPE_DEBUG_FLOAT_ARRAY = 2
+uint8 TYPE_NAMED_VALUE_FLOAT = 3
+uint8 TYPE_NAMED_VALUE_INT = 4
diff --git a/src/external/mavros_msgs/msg/ESCInfo.msg b/src/external/mavros_msgs/msg/ESCInfo.msg
new file mode 100644
index 0000000..71bcc41
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ESCInfo.msg
@@ -0,0 +1,14 @@
+# ESCInfo.msg
+#
+#
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#ESC_INFO
+
+std_msgs/Header header
+
+uint16 counter
+uint8 count
+uint8 connection_type
+uint8 info
+mavros_msgs/ESCInfoItem[] esc_info
+
diff --git a/src/external/mavros_msgs/msg/ESCInfoItem.msg b/src/external/mavros_msgs/msg/ESCInfoItem.msg
new file mode 100644
index 0000000..fc63856
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ESCInfoItem.msg
@@ -0,0 +1,12 @@
+# ESCInfoItem.msg
+#
+#
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#ESC_INFO
+
+std_msgs/Header header
+
+uint16 failure_flags
+uint32 error_count
+int32 temperature
+
diff --git a/src/external/mavros_msgs/msg/ESCStatus.msg b/src/external/mavros_msgs/msg/ESCStatus.msg
new file mode 100644
index 0000000..870579b
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ESCStatus.msg
@@ -0,0 +1,9 @@
+# ESCStatus.msg
+#
+#
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#ESC_STATUS
+
+std_msgs/Header header
+
+mavros_msgs/ESCStatusItem[] esc_status
diff --git a/src/external/mavros_msgs/msg/ESCStatusItem.msg b/src/external/mavros_msgs/msg/ESCStatusItem.msg
new file mode 100644
index 0000000..252aa0b
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ESCStatusItem.msg
@@ -0,0 +1,11 @@
+# ESCStatusItem.msg
+#
+#
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#ESC_STATUS
+
+std_msgs/Header header
+
+int32 rpm
+float32 voltage
+float32 current
diff --git a/src/external/mavros_msgs/msg/ESCTelemetry.msg b/src/external/mavros_msgs/msg/ESCTelemetry.msg
new file mode 100644
index 0000000..f0c5916
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ESCTelemetry.msg
@@ -0,0 +1,10 @@
+# APM ESC Telemetry as returned by BLHeli
+#
+# See:
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_1_TO_4
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_5_TO_8
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_9_TO_12
+
+std_msgs/Header header
+
+mavros_msgs/ESCTelemetryItem[] esc_telemetry
diff --git a/src/external/mavros_msgs/msg/ESCTelemetryItem.msg b/src/external/mavros_msgs/msg/ESCTelemetryItem.msg
new file mode 100644
index 0000000..e5c0c17
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ESCTelemetryItem.msg
@@ -0,0 +1,15 @@
+# APM ESC Telemetry as returned by BLHeli
+#
+# See:
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_1_TO_4
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_5_TO_8
+# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_9_TO_12
+
+std_msgs/Header header
+
+float32 temperature # deg C
+float32 voltage # V
+float32 current # A
+float32 totalcurrent # Ah
+int32 rpm # 1/min
+uint16 count # count of telemetry packets
diff --git a/src/external/mavros_msgs/msg/EstimatorStatus.msg b/src/external/mavros_msgs/msg/EstimatorStatus.msg
new file mode 100644
index 0000000..40101b8
--- /dev/null
+++ b/src/external/mavros_msgs/msg/EstimatorStatus.msg
@@ -0,0 +1,23 @@
+# Current autopilot estimator state
+#
+# https://mavlink.io/en/messages/common.html#ESTIMATOR_STATUS_FLAGS
+
+std_msgs/Header header
+bool attitude_status_flag
+
+bool velocity_horiz_status_flag
+bool velocity_vert_status_flag
+
+bool pos_horiz_rel_status_flag
+bool pos_horiz_abs_status_flag
+
+bool pos_vert_abs_status_flag
+bool pos_vert_agl_status_flag
+
+bool const_pos_mode_status_flag
+
+bool pred_pos_horiz_rel_status_flag
+bool pred_pos_horiz_abs_status_flag
+
+bool gps_glitch_status_flag
+bool accel_error_status_flag
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/ExtendedState.msg b/src/external/mavros_msgs/msg/ExtendedState.msg
new file mode 100644
index 0000000..47c5c29
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ExtendedState.msg
@@ -0,0 +1,19 @@
+# Extended autopilot state
+#
+# https://mavlink.io/en/messages/common.html#EXTENDED_SYS_STATE
+
+uint8 VTOL_STATE_UNDEFINED = 0
+uint8 VTOL_STATE_TRANSITION_TO_FW = 1
+uint8 VTOL_STATE_TRANSITION_TO_MC = 2
+uint8 VTOL_STATE_MC = 3
+uint8 VTOL_STATE_FW = 4
+
+uint8 LANDED_STATE_UNDEFINED = 0
+uint8 LANDED_STATE_ON_GROUND = 1
+uint8 LANDED_STATE_IN_AIR = 2
+uint8 LANDED_STATE_TAKEOFF = 3
+uint8 LANDED_STATE_LANDING = 4
+
+std_msgs/Header header
+uint8 vtol_state
+uint8 landed_state
diff --git a/src/external/mavros_msgs/msg/FileEntry.msg b/src/external/mavros_msgs/msg/FileEntry.msg
new file mode 100644
index 0000000..e733326
--- /dev/null
+++ b/src/external/mavros_msgs/msg/FileEntry.msg
@@ -0,0 +1,12 @@
+# File/Dir information
+
+uint8 TYPE_FILE = 0
+uint8 TYPE_DIRECTORY = 1
+
+string name
+uint8 type
+uint64 size
+
+# Not supported by MAVLink FTP
+#builtin_interfaces/Time atime
+#int32 access_flags
diff --git a/src/external/mavros_msgs/msg/GPSINPUT.msg b/src/external/mavros_msgs/msg/GPSINPUT.msg
new file mode 100644
index 0000000..15a038a
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GPSINPUT.msg
@@ -0,0 +1,37 @@
+# FCU GPS INPUT message for the gps_input plugin
+# mavlink GPS_INPUT message.
+
+std_msgs/Header header
+## GPS_FIX_TYPE enum
+uint8 GPS_FIX_TYPE_NO_GPS = 0 # No GPS connected
+uint8 GPS_FIX_TYPE_NO_FIX = 1 # No position information, GPS is connected
+uint8 GPS_FIX_TYPE_2D_FIX = 2 # 2D position
+uint8 GPS_FIX_TYPE_3D_FIX = 3 # 3D position
+uint8 GPS_FIX_TYPE_DGPS = 4 # DGPS/SBAS aided 3D position
+uint8 GPS_FIX_TYPE_RTK_FLOATR = 5 # TK float, 3D position
+uint8 GPS_FIX_TYPE_RTK_FIXEDR = 6 # TK Fixed, 3D position
+uint8 GPS_FIX_TYPE_STATIC = 7 # Static fixed, typically used for base stations
+uint8 GPS_FIX_TYPE_PPP = 8 # PPP, 3D position
+uint8 fix_type # [GPS_FIX_TYPE] GPS fix type
+
+uint8 gps_id # ID of the GPS for multiple GPS inputs
+uint16 ignore_flags # Bitmap indicating which GPS input flags fields to ignore. All other fields must be provided.
+
+uint32 time_week_ms # [ms] GPS time (from start of GPS week)
+uint16 time_week # GPS week number
+int32 lat # [degE7] Latitude (WGS84, EGM96 ellipsoid)
+int32 lon # [degE7] Longitude (WGS84, EGM96 ellipsoid)
+float32 alt # [m] Altitude (MSL). Positive for up.
+
+float32 hdop # [m] GPS HDOP horizontal dilution of position.
+float32 vdop # [m] GPS VDOP vertical dilution of position
+float32 vn # [m/s] GPS velocity in NORTH direction in earth-fixed NED frame
+float32 ve # [m/s] GPS velocity in EAST direction in earth-fixed NED frame
+float32 vd # [m/s] GPS velocity in DOWN direction in earth-fixed NED frame
+
+float32 speed_accuracy # [m/s] GPS speed accuracy
+float32 horiz_accuracy # [m] GPS horizontal accuracy
+float32 vert_accuracy # [m] GPS vertical accuracy
+
+uint8 satellites_visible # Number of satellites visible. If unknown, set to 255
+uint16 yaw # [cdeg] Yaw in earth frame from north.
diff --git a/src/external/mavros_msgs/msg/GPSRAW.msg b/src/external/mavros_msgs/msg/GPSRAW.msg
new file mode 100644
index 0000000..5b754b3
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GPSRAW.msg
@@ -0,0 +1,37 @@
+# FCU GPS RAW message for the gps_status plugin
+# A merge of mavlink GPS_RAW_INT and
+# mavlink GPS2_RAW messages.
+
+std_msgs/Header header
+## GPS_FIX_TYPE enum
+uint8 GPS_FIX_TYPE_NO_GPS = 0 # No GPS connected
+uint8 GPS_FIX_TYPE_NO_FIX = 1 # No position information, GPS is connected
+uint8 GPS_FIX_TYPE_2D_FIX = 2 # 2D position
+uint8 GPS_FIX_TYPE_3D_FIX = 3 # 3D position
+uint8 GPS_FIX_TYPE_DGPS = 4 # DGPS/SBAS aided 3D position
+uint8 GPS_FIX_TYPE_RTK_FLOAT = 5 # RTK float, 3D position
+uint8 GPS_FIX_TYPE_RTK_FIXED = 6 # RTK Fixed, 3D position
+uint8 GPS_FIX_TYPE_STATIC = 7 # Static fixed, typically used for base stations
+uint8 GPS_FIX_TYPE_PPP = 8 # PPP, 3D position
+uint8 fix_type # [GPS_FIX_TYPE] GPS fix type
+
+int32 lat # [degE7] Latitude (WGS84, EGM96 ellipsoid)
+int32 lon # [degE7] Longitude (WGS84, EGM96 ellipsoid)
+int32 alt # [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude.
+uint16 eph # GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX
+uint16 epv # GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX
+uint16 vel # [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX
+uint16 cog # [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
+uint8 satellites_visible # Number of satellites visible. If unknown, set to 255
+
+# -*- only available with MAVLink v2.0 and GPS_RAW_INT messages -*-
+int32 alt_ellipsoid # [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up.
+uint32 h_acc # [mm] Position uncertainty. Positive for up.
+uint32 v_acc # [mm] Altitude uncertainty. Positive for up.
+uint32 vel_acc # [mm] Speed uncertainty. Positive for up.
+int32 hdg_acc # [degE5] Heading / track uncertainty
+uint16 yaw # [cdeg] Yaw in earth frame from north.
+
+# -*- only available with MAVLink v2.0 and GPS2_RAW messages -*-
+uint8 dgps_numch # Number of DGPS satellites
+uint32 dgps_age # [ms] Age of DGPS info
diff --git a/src/external/mavros_msgs/msg/GPSRTK.msg b/src/external/mavros_msgs/msg/GPSRTK.msg
new file mode 100644
index 0000000..2cac818
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GPSRTK.msg
@@ -0,0 +1,18 @@
+# FCU GPS RTK message for the gps_status plugin
+# A copy of mavlink GPS_RTK message
+
+std_msgs/Header header
+
+uint8 rtk_receiver_id # Identification of connected RTK receiver.
+int16 wn # GPS Week Number of last baseline.
+uint32 tow # [ms] GPS Time of Week of last baseline.
+uint8 rtk_health # GPS-specific health report for RTK data.
+uint8 rtk_rate # [Hz] Rate of baseline messages being received by GPS.
+uint8 nsats # Current number of sats used for RTK calculation.
+int32 baseline_a # [mm] Current baseline in ECEF x or NED north component, depends on header.frame_id.
+int32 baseline_b # [mm] Current baseline in ECEF y or NED east component, depends on header.frame_id.
+int32 baseline_c # [mm] Current baseline in ECEF z or NED down component, depends on header.frame_id.
+uint32 accuracy # Current estimate of baseline accuracy.
+int32 iar_num_hypotheses # Current number of integer ambiguity hypotheses.
+
+
diff --git a/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg b/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg
new file mode 100644
index 0000000..612375d
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg
@@ -0,0 +1,32 @@
+# MAVLink message: GIMBAL_DEVICE_ATTITUDE_STATUS
+# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS
+
+std_msgs/Header header
+
+uint8 target_system # System ID
+uint8 target_component # Component ID
+
+uint16 flags # Current gimbal flags set (bitwise) - See GIMBAL_DEVICE_FLAGS
+#GIMBAL_DEVICE_FLAGS
+uint16 FLAGS_RETRACT = 1 # Set to retracted safe position (no stabilization), takes presedence over all other flags.
+uint16 FLAGS_NEUTRAL = 2 # Set to neutral/default position, taking precedence over all other flags except RETRACT. Neutral is commonly forward-facing and horizontal (pitch=yaw=0) but may be any orientation.
+uint16 FLAGS_ROLL_LOCK = 4 # Lock roll angle to absolute angle relative to horizon (not relative to drone). This is generally the default with a stabilizing gimbal.
+uint16 FLAGS_PITCH_LOCK = 8 # Lock pitch angle to absolute angle relative to horizon (not relative to drone). This is generally the default.
+uint16 FLAGS_YAW_LOCK = 16 # Lock yaw angle to absolute angle relative to North (not relative to drone). If this flag is set, the quaternion is in the Earth frame with the x-axis pointing North (yaw absolute). If this flag is not set, the quaternion frame is in the Earth frame rotated so that the x-axis is pointing forward (yaw relative to vehicle).
+
+geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)
+float32 angular_velocity_x # X component of angular velocity (NaN if unknown)
+float32 angular_velocity_y # Y component of angular velocity (NaN if unknown)
+float32 angular_velocity_z # Z component of angular velocity (NaN if unknown)
+
+uint32 failure_flags # Failure flags (0 for no failure) (bitwise) - See GIMBAL_DEVICE_ERROR_FLAGS
+#GIMBAL_DEVICE_ERROR_FLAGS
+uint32 ERROR_FLAGS_AT_ROLL_LIMIT = 1 # Gimbal device is limited by hardware roll limit.
+uint32 ERROR_FLAGS_AT_PITCH_LIMIT = 2 # Gimbal device is limited by hardware pitch limit.
+uint32 ERROR_FLAGS_AT_YAW_LIMIT = 4 # Gimbal device is limited by hardware yaw limit.
+uint32 ERROR_FLAGS_ENCODER_ERROR = 8 # There is an error with the gimbal encoders.
+uint32 ERROR_FLAGS_POWER_ERROR = 16 # There is an error with the gimbal power source.
+uint32 ERROR_FLAGS_MOTOR_ERROR = 32 # There is an error with the gimbal motor's.
+uint32 ERROR_FLAGS_SOFTWARE_ERROR = 64 # There is an error with the gimbal's software.
+uint32 ERROR_FLAGS_COMMS_ERROR = 128 # There is an error with the gimbal's communication.
+uint32 ERROR_FLAGS_CALIBRATION_RUNNING = 256 # Gimbal is currently calibrating.
diff --git a/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg b/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg
new file mode 100644
index 0000000..0d68309
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg
@@ -0,0 +1,34 @@
+# MAVLink message: GIMBAL_DEVICE_INFORMATION
+# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION
+
+std_msgs/Header header
+
+string vendor_name # Name of the gimbal vendor.
+string model_name # Name of the gimbal model.
+string custom_name # Custom name of the gimbal given to it by the user.
+uint32 firmware_version # Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).
+uint32 hardware_version # Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).
+uint64 uid # UID of gimbal hardware (0 if unknown).
+
+uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_DEVICE_CAP_FLAGS
+#GIMBAL_DEVICE_CAP_FLAGS
+uint32 CAP_FLAGS_HAS_RETRACT = 1 # Gimbal device supports a retracted position
+uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Gimbal device supports a horizontal, forward looking position, stabilized
+uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Gimbal device supports rotating around roll axis.
+uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Gimbal device supports to follow a roll angle relative to the vehicle
+uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Gimbal device supports locking to an roll angle (generally that's the default with roll stabilized)
+uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Gimbal device supports rotating around pitch axis.
+uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Gimbal device supports to follow a pitch angle relative to the vehicle
+uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Gimbal device supports locking to an pitch angle (generally that's the default with pitch stabilized)
+uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Gimbal device supports rotating around yaw axis.
+uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default)
+uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Gimbal device supports locking to an absolute heading (often this is an option available)
+uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Gimbal device supports yawing/panning infinetely (e.g. using slip disk).
+
+uint16 custom_cap_flags # Bitmap for use for gimbal-specific capability flags.
+float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)
+float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)
+float32 pitch_min # Minimum pitch angle (positive: up, negative: down)
+float32 pitch_max # Maximum pitch angle (positive: up, negative: down)
+float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left)
+float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left)
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg b/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg
new file mode 100644
index 0000000..fdbc73d
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg
@@ -0,0 +1,18 @@
+# MAVLink message: GIMBAL_DEVICE_SET_ATTITUDE
+# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE
+
+uint8 target_system # System ID
+uint8 target_component # Component ID
+
+uint16 flags # Low level gimbal flags (bitwise) - See GIMBAL_DEVICE_FLAGS
+#GIMBAL_DEVICE_FLAGS
+uint16 FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint16 FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint16 FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint16 FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint16 FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)
+float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored.
+float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored.
+float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored.
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/GimbalManagerInformation.msg b/src/external/mavros_msgs/msg/GimbalManagerInformation.msg
new file mode 100644
index 0000000..38dbc88
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GimbalManagerInformation.msg
@@ -0,0 +1,29 @@
+# MAVLink message: GIMBAL_MANAGER_INFORMATION
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION
+
+std_msgs/Header header
+
+uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_MANAGER_CAP_FLAGS
+#GIMBAL_MANAGER_CAP_FLAGS
+uint32 CAP_FLAGS_HAS_RETRACT = 1 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT.
+uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL.
+uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS.
+uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW.
+uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK.
+uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS.
+uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW.
+uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK.
+uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS.
+uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW.
+uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK.
+uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW.
+uint32 CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 # Gimbal manager supports to point to a local position.
+uint32 CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 # Gimbal manager supports to point to a global latitude, longitude, altitude position.
+
+uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for.
+float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)
+float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)
+float32 pitch_min # Minimum pitch angle (positive: up, negative: down)
+float32 pitch_max # Maximum pitch angle (positive: up, negative: down)
+float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left)
+float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left)
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg b/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg
new file mode 100644
index 0000000..8bbec37
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg
@@ -0,0 +1,24 @@
+# MAVLink message: GIMBAL_MANAGER_SET_ATTITUDE
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_ATTITUDE
+
+uint8 target_system # System ID
+uint8 target_component # Component ID
+
+uint32 flags # High level gimbal manager flags to use (bitwise) - See GIMBAL_MANAGER_FLAGS
+#GIMBAL_MANAGER_FLAGS
+uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+uint8 gimbal_device_id # Component ID of gimbal device to address
+ # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device
+ # components. Send command multiple times for more than
+ # one gimbal (but not all gimbals). Default Mavlink gimbal
+ # device ids: 154, 171-175
+
+geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)
+float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored.
+float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored.
+float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored.
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg b/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg
new file mode 100644
index 0000000..e87874e
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg
@@ -0,0 +1,27 @@
+# MAVLink message: GIMBAL_MANAGER_SET_PITCHYAW
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_PITCHYAW
+# Note that this message structure is identical also to GIMBAL_MANAGER_SET_MANUAL_CONTROL and is
+# reused as such by the plugin
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_MANUAL_CONTROL
+
+uint8 target_system # System ID
+uint8 target_component # Component ID
+
+uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS
+#GIMBAL_MANAGER_FLAGS
+uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+uint8 gimbal_device_id # Component ID of gimbal device to address
+ # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device
+ # components. Send command multiple times for more than
+ # one gimbal (but not all gimbals). Default Mavlink gimbal
+ # device ids: 154, 171-175
+
+float32 pitch # Pitch angle (positive: up, negative: down, NaN to be ignored).
+float32 yaw # Yaw angle (positive: to the right, negative: to the left, NaN to be ignored).
+float32 pitch_rate # Pitch angular rate (positive: up, negative: down, NaN to be ignored).
+float32 yaw_rate # Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored).
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/GimbalManagerStatus.msg b/src/external/mavros_msgs/msg/GimbalManagerStatus.msg
new file mode 100644
index 0000000..8df8b3e
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GimbalManagerStatus.msg
@@ -0,0 +1,19 @@
+# MAVLink message: GIMBAL_MANAGER_STATUS
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_STATUS
+
+std_msgs/Header header
+
+uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS
+#GIMBAL_MANAGER_FLAGS
+uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for.
+
+uint8 sysid_primary # System ID of MAVLink component with primary control, 0 for none.
+uint8 compid_primary # Component ID of MAVLink component with primary control, 0 for none.
+uint8 sysid_secondary # System ID of MAVLink component with secondary control, 0 for none.
+uint8 compid_secondary # Component ID of MAVLink component with secondary control, 0 for none.
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/GlobalPositionTarget.msg b/src/external/mavros_msgs/msg/GlobalPositionTarget.msg
new file mode 100644
index 0000000..445a1a7
--- /dev/null
+++ b/src/external/mavros_msgs/msg/GlobalPositionTarget.msg
@@ -0,0 +1,34 @@
+# Message for SET_POSITION_TARGET_GLOBAL_INT
+#
+# https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT
+# Some complex system requires all feautures that mavlink
+# message provide. See issue #402, #415.
+
+std_msgs/Header header
+
+uint8 coordinate_frame
+uint8 FRAME_GLOBAL_INT = 5
+uint8 FRAME_GLOBAL_REL_ALT = 6
+uint8 FRAME_GLOBAL_TERRAIN_ALT = 11
+
+uint16 type_mask
+uint16 IGNORE_LATITUDE = 1 # Position ignore flags
+uint16 IGNORE_LONGITUDE = 2
+uint16 IGNORE_ALTITUDE = 4
+uint16 IGNORE_VX = 8 # Velocity vector ignore flags
+uint16 IGNORE_VY = 16
+uint16 IGNORE_VZ = 32
+uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags
+uint16 IGNORE_AFY = 128
+uint16 IGNORE_AFZ = 256
+uint16 FORCE = 512 # Force in af vector flag
+uint16 IGNORE_YAW = 1024
+uint16 IGNORE_YAW_RATE = 2048
+
+float64 latitude
+float64 longitude
+float32 altitude # in meters, AMSL or above terrain
+geometry_msgs/Vector3 velocity
+geometry_msgs/Vector3 acceleration_or_force
+float32 yaw
+float32 yaw_rate
diff --git a/src/external/mavros_msgs/msg/HilActuatorControls.msg b/src/external/mavros_msgs/msg/HilActuatorControls.msg
new file mode 100644
index 0000000..755384f
--- /dev/null
+++ b/src/external/mavros_msgs/msg/HilActuatorControls.msg
@@ -0,0 +1,10 @@
+# HilActuatorControls.msg
+#
+# ROS representation of MAVLink HIL_ACTUATOR_CONTROLS
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS
+
+std_msgs/Header header
+float32[16] controls
+uint8 mode
+uint64 flags
diff --git a/src/external/mavros_msgs/msg/HilControls.msg b/src/external/mavros_msgs/msg/HilControls.msg
new file mode 100644
index 0000000..a953aff
--- /dev/null
+++ b/src/external/mavros_msgs/msg/HilControls.msg
@@ -0,0 +1,18 @@
+# HilControls.msg
+#
+# ROS representation of MAVLink HIL_CONTROLS
+# (deprecated, use HIL_ACTUATOR_CONTROLS instead)
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_CONTROLS
+
+std_msgs/Header header
+float32 roll_ailerons
+float32 pitch_elevator
+float32 yaw_rudder
+float32 throttle
+float32 aux1
+float32 aux2
+float32 aux3
+float32 aux4
+uint8 mode
+uint8 nav_mode
diff --git a/src/external/mavros_msgs/msg/HilGPS.msg b/src/external/mavros_msgs/msg/HilGPS.msg
new file mode 100644
index 0000000..d74c204
--- /dev/null
+++ b/src/external/mavros_msgs/msg/HilGPS.msg
@@ -0,0 +1,17 @@
+# HilControls.msg
+#
+# ROS representation of MAVLink HIL_GPS
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_GPS
+
+std_msgs/Header header
+uint8 fix_type
+geographic_msgs/GeoPoint geo
+uint16 eph
+uint16 epv
+uint16 vel
+int16 vn
+int16 ve
+int16 vd
+uint16 cog
+uint8 satellites_visible
diff --git a/src/external/mavros_msgs/msg/HilSensor.msg b/src/external/mavros_msgs/msg/HilSensor.msg
new file mode 100644
index 0000000..c7ceccb
--- /dev/null
+++ b/src/external/mavros_msgs/msg/HilSensor.msg
@@ -0,0 +1,16 @@
+# HilSensor.msg
+#
+# ROS representation of MAVLink HIL_SENSOR
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_SENSOR
+
+std_msgs/Header header
+
+geometry_msgs/Vector3 acc
+geometry_msgs/Vector3 gyro
+geometry_msgs/Vector3 mag
+float32 abs_pressure
+float32 diff_pressure
+float32 pressure_alt
+float32 temperature
+uint32 fields_updated
diff --git a/src/external/mavros_msgs/msg/HilStateQuaternion.msg b/src/external/mavros_msgs/msg/HilStateQuaternion.msg
new file mode 100644
index 0000000..980db1e
--- /dev/null
+++ b/src/external/mavros_msgs/msg/HilStateQuaternion.msg
@@ -0,0 +1,15 @@
+# HilStateQuaternion.msg
+#
+# ROS representation of MAVLink HIL_STATE_QUATERNION
+# See mavlink message documentation here:
+# https://mavlink.io/en/messages/common.html#HIL_STATE_QUATERNION
+
+std_msgs/Header header
+
+geometry_msgs/Quaternion orientation
+geometry_msgs/Vector3 angular_velocity
+geometry_msgs/Vector3 linear_acceleration
+geometry_msgs/Vector3 linear_velocity
+geographic_msgs/GeoPoint geo
+float32 ind_airspeed
+float32 true_airspeed
diff --git a/src/external/mavros_msgs/msg/HomePosition.msg b/src/external/mavros_msgs/msg/HomePosition.msg
new file mode 100644
index 0000000..f73bb51
--- /dev/null
+++ b/src/external/mavros_msgs/msg/HomePosition.msg
@@ -0,0 +1,10 @@
+# MAVLink message: HOME_POSITION
+# https://mavlink.io/en/messages/common.html#HOME_POSITION
+
+std_msgs/Header header
+
+geographic_msgs/GeoPoint geo # geodetic coordinates in WGS-84 datum
+
+geometry_msgs/Point position # local position
+geometry_msgs/Quaternion orientation # XXX: verify field name (q[4])
+geometry_msgs/Vector3 approach # position of the end of approach vector
diff --git a/src/external/mavros_msgs/msg/LandingTarget.msg b/src/external/mavros_msgs/msg/LandingTarget.msg
new file mode 100644
index 0000000..d5e6bd0
--- /dev/null
+++ b/src/external/mavros_msgs/msg/LandingTarget.msg
@@ -0,0 +1,32 @@
+# MAVLink message: LANDING_TARGET
+# https://mavlink.io/en/messages/common.html
+
+std_msgs/Header header
+
+## MAV_FRAME enum
+uint8 GLOBAL = 0 # Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL)
+uint8 LOCAL_NED = 2 # Local coordinate frame, Z-up (x: north, y: east, z: down).
+uint8 MISSION = 3 # NOT a coordinate frame, indicates a mission command.
+uint8 GLOBAL_RELATIVE_ALT = 4 # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location.
+uint8 LOCAL_ENU = 5 # Local coordinate frame, Z-down (x: east, y: north, z: up)
+uint8 GLOBAL_INT = 6 # Global coordinate frame, WGS84 coordinate system. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL)
+uint8 GLOBAL_RELATIVE_ALT_INT = 7 # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location.
+uint8 LOCAL_OFFSET_NED = 8 # Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position.
+uint8 BODY_NED = 9 # Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right.
+uint8 BODY_OFFSET_NED = 10 # Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east.
+uint8 GLOBAL_TERRAIN_ALT = 11 # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model.
+uint8 GLOBAL_TERRAIN_ALT_INT = 12 # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model.
+
+## LANDING_TARGET_TYPE enum
+uint8 LIGHT_BEACON = 0 # Landing target signaled by light beacon (ex: IR-LOCK)
+uint8 RADIO_BEACON = 1 # Landing target signaled by radio beacon (ex: ILS, NDB)
+uint8 VISION_FIDUCIAL = 2 # Landing target represented by a fiducial marker (ex: ARTag)
+uint8 VISION_OTHER = 3 # Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square)
+
+uint8 target_num
+uint8 frame
+float32[2] angle
+float32 distance
+float32[2] size
+geometry_msgs/Pose pose
+uint8 type
diff --git a/src/external/mavros_msgs/msg/LogData.msg b/src/external/mavros_msgs/msg/LogData.msg
new file mode 100644
index 0000000..829a0a3
--- /dev/null
+++ b/src/external/mavros_msgs/msg/LogData.msg
@@ -0,0 +1,11 @@
+# Reply to LogRequestData, - a chunk of a log
+#
+# :id: - log id
+# :offset: - offset into the log
+# :data: - chunk of data (if zero-sized, then there are no more chunks)
+
+std_msgs/Header header
+
+uint16 id
+uint32 offset
+uint8[] data
diff --git a/src/external/mavros_msgs/msg/LogEntry.msg b/src/external/mavros_msgs/msg/LogEntry.msg
new file mode 100644
index 0000000..b212686
--- /dev/null
+++ b/src/external/mavros_msgs/msg/LogEntry.msg
@@ -0,0 +1,15 @@
+# Information about a single log
+#
+# :id: - log id
+# :num_logs: - total number of logs
+# :last_log_num: - id of last log
+# :time_utc: - UTC timestamp of log (ros::Time(0) if not available)
+# :size: - size of log in bytes (may be approximate)
+
+std_msgs/Header header
+
+uint16 id
+uint16 num_logs
+uint16 last_log_num
+builtin_interfaces/Time time_utc
+uint32 size
diff --git a/src/external/mavros_msgs/msg/MagnetometerReporter.msg b/src/external/mavros_msgs/msg/MagnetometerReporter.msg
new file mode 100644
index 0000000..593f5f6
--- /dev/null
+++ b/src/external/mavros_msgs/msg/MagnetometerReporter.msg
@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+uint8 report
+float32 confidence
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/ManualControl.msg b/src/external/mavros_msgs/msg/ManualControl.msg
new file mode 100644
index 0000000..f6db210
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ManualControl.msg
@@ -0,0 +1,7 @@
+# Manual Control state
+std_msgs/Header header
+float32 x
+float32 y
+float32 z
+float32 r
+uint16 buttons
diff --git a/src/external/mavros_msgs/msg/Mavlink.msg b/src/external/mavros_msgs/msg/Mavlink.msg
new file mode 100644
index 0000000..afb80ed
--- /dev/null
+++ b/src/external/mavros_msgs/msg/Mavlink.msg
@@ -0,0 +1,38 @@
+# Mavlink message transport type.
+#
+# Used to transport mavlink_message_t via ROS topic
+#
+# :framing_status:
+# Frame decoding status: OK, CRC error, bad Signature (mavlink v2.0)
+# You may simply drop all non valid messages.
+# Used for GCS Bridge to transport unknown messages.
+#
+# :magic:
+# STX byte, used to determine protocol version v1.0 or v2.0.
+#
+# Please use mavros_msgs::mavlink::convert() from
+# to convert between ROS and MAVLink message type
+
+# mavlink_framing_t enum
+uint8 FRAMING_OK = 1
+uint8 FRAMING_BAD_CRC = 2
+uint8 FRAMING_BAD_SIGNATURE = 3
+
+# stx values
+uint8 MAVLINK_V10 = 254
+uint8 MAVLINK_V20 = 253
+
+std_msgs/Header header
+uint8 framing_status
+
+uint8 magic # STX byte
+uint8 len
+uint8 incompat_flags
+uint8 compat_flags
+uint8 seq
+uint8 sysid
+uint8 compid
+uint32 msgid # 24-bit message id
+uint16 checksum
+uint64[<=33] payload64 # max size: (255+2+7)/8
+uint8[<=13] signature # optional signature, max size: 13
diff --git a/src/external/mavros_msgs/msg/MountControl.msg b/src/external/mavros_msgs/msg/MountControl.msg
new file mode 100644
index 0000000..0c2ee72
--- /dev/null
+++ b/src/external/mavros_msgs/msg/MountControl.msg
@@ -0,0 +1,18 @@
+# MAVLink message: DO_MOUNT_CONTROL
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONTROL
+
+std_msgs/Header header
+
+uint8 mode # See enum MAV_MOUNT_MODE.
+uint8 MAV_MOUNT_MODE_RETRACT = 0
+uint8 MAV_MOUNT_MODE_NEUTRAL = 1
+uint8 MAV_MOUNT_MODE_MAVLINK_TARGETING = 2
+uint8 MAV_MOUNT_MODE_RC_TARGETING = 3
+uint8 MAV_MOUNT_MODE_GPS_POINT = 4
+
+float32 pitch # pitch degrees or degrees/second depending on mount mode.
+float32 roll # roll degrees or degrees/second depending on mount mode.
+float32 yaw # yaw degrees or degrees/second depending on mount mode.
+float32 altitude # altitude depending on mount mode.
+float32 latitude # latitude in degrees * 1E7, set if appropriate mount mode.
+float32 longitude # longitude in degrees * 1E7, set if appropriate mount mode.
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/NavControllerOutput.msg b/src/external/mavros_msgs/msg/NavControllerOutput.msg
new file mode 100644
index 0000000..d5f8f6d
--- /dev/null
+++ b/src/external/mavros_msgs/msg/NavControllerOutput.msg
@@ -0,0 +1,12 @@
+# https://mavlink.io/en/messages/common.html#NAV_CONTROLLER_OUTPUT
+
+std_msgs/Header header
+
+float32 nav_roll # Current desired roll
+float32 nav_pitch # Current desired pitch
+int16 nav_bearing # Current desired heading
+int16 target_bearing # Bearing to current waypoint/target
+uint16 wp_dist # Distance to active waypoint
+float32 alt_error # Current altitude error
+float32 aspd_error # Current airspeed error
+float32 xtrack_error # Current crosstrack error on x-y plane
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/OnboardComputerStatus.msg b/src/external/mavros_msgs/msg/OnboardComputerStatus.msg
new file mode 100644
index 0000000..d6d06b0
--- /dev/null
+++ b/src/external/mavros_msgs/msg/OnboardComputerStatus.msg
@@ -0,0 +1,25 @@
+# Mavros message: ONBOARDCOMPUTERSTATUS
+
+std_msgs/Header header
+
+uint8 component # See enum MAV_COMPONENT
+
+uint32 uptime # [ms] Time since system boot
+uint8 type # Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers.
+uint8[8] cpu_cores # CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused.
+uint8[10] cpu_combined # Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused
+uint8[4] gpu_cores # GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused
+uint8[10] gpu_combined # Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused.
+int8 temperature_board # [degC] Temperature of the board. A value of INT8_MAX implies the field is unused.
+int8[8] temperature_core # [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused.
+int16[4] fan_speed # [rpm] Fan speeds. A value of INT16_MAX implies the field is unused.
+uint32 ram_usage # [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused.
+uint32 ram_total # [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused.
+uint32[4] storage_type # Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused.
+uint32[4] storage_usage # [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused.
+uint32[4] storage_total # [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused.
+uint32[6] link_type # Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary.
+uint32[6] link_tx_rate # [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused.
+uint32[6] link_rx_rate # [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused.
+uint32[6] link_tx_max # [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused.
+uint32[6] link_rx_max # [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused.
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/OpticalFlow.msg b/src/external/mavros_msgs/msg/OpticalFlow.msg
new file mode 100644
index 0000000..db857b8
--- /dev/null
+++ b/src/external/mavros_msgs/msg/OpticalFlow.msg
@@ -0,0 +1,9 @@
+# OPTICAL_FLOW message data
+
+std_msgs/Header header
+
+geometry_msgs/Vector3 flow
+geometry_msgs/Vector3 flow_comp_m
+uint8 quality
+float32 ground_distance
+geometry_msgs/Vector3 flow_rate
diff --git a/src/external/mavros_msgs/msg/OpticalFlowRad.msg b/src/external/mavros_msgs/msg/OpticalFlowRad.msg
new file mode 100644
index 0000000..f773fac
--- /dev/null
+++ b/src/external/mavros_msgs/msg/OpticalFlowRad.msg
@@ -0,0 +1,14 @@
+# OPTICAL_FLOW_RAD message data
+
+std_msgs/Header header
+
+uint32 integration_time_us
+float32 integrated_x
+float32 integrated_y
+float32 integrated_xgyro
+float32 integrated_ygyro
+float32 integrated_zgyro
+int16 temperature
+uint8 quality
+uint32 time_delta_distance_us
+float32 distance
diff --git a/src/external/mavros_msgs/msg/OverrideRCIn.msg b/src/external/mavros_msgs/msg/OverrideRCIn.msg
new file mode 100644
index 0000000..81bc040
--- /dev/null
+++ b/src/external/mavros_msgs/msg/OverrideRCIn.msg
@@ -0,0 +1,9 @@
+# Override RC Input
+# Currently MAVLink defines override for 18 channels
+
+# https://mavlink.io/en/messages/common.html#RC_CHANNELS_OVERRIDE
+
+uint16 CHAN_RELEASE=0
+uint16 CHAN_NOCHANGE=65535
+
+uint16[18] channels
diff --git a/src/external/mavros_msgs/msg/Param.msg b/src/external/mavros_msgs/msg/Param.msg
new file mode 100644
index 0000000..f2ccfaa
--- /dev/null
+++ b/src/external/mavros_msgs/msg/Param.msg
@@ -0,0 +1,11 @@
+# Parameter msg.
+
+# XXX DEPRECATED: replaced by ParamEvent
+
+std_msgs/Header header
+
+string param_id
+mavros_msgs/ParamValue value
+
+uint16 param_index
+uint16 param_count
diff --git a/src/external/mavros_msgs/msg/ParamEvent.msg b/src/external/mavros_msgs/msg/ParamEvent.msg
new file mode 100644
index 0000000..493df0a
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ParamEvent.msg
@@ -0,0 +1,14 @@
+# Parameter Event
+#
+# That messages replaces mavros_msgs/Param from mavros v1.
+# Reason for that: ROS2 have native message for parameters
+#
+# ROS2 also have it's own ParameterEvent stream, which could be used
+# to get FCU updates too. But that message is simpler to use.
+
+std_msgs/Header header
+
+string param_id
+rcl_interfaces/ParameterValue value
+uint16 param_index
+uint16 param_count
diff --git a/src/external/mavros_msgs/msg/ParamValue.msg b/src/external/mavros_msgs/msg/ParamValue.msg
new file mode 100644
index 0000000..4a98639
--- /dev/null
+++ b/src/external/mavros_msgs/msg/ParamValue.msg
@@ -0,0 +1,12 @@
+# Parameter value storage type.
+#
+# Integer and float fields:
+#
+# if integer != 0: it is integer value
+# else if real != 0.0: it is float value
+# else: it is zero.
+
+# XXX DEPRECATED: replaced by rmw_interfaces/ParameterValue
+
+int64 integer
+float64 real
diff --git a/src/external/mavros_msgs/msg/PlayTuneV2.msg b/src/external/mavros_msgs/msg/PlayTuneV2.msg
new file mode 100644
index 0000000..d92a976
--- /dev/null
+++ b/src/external/mavros_msgs/msg/PlayTuneV2.msg
@@ -0,0 +1,10 @@
+# Play tune V2
+#
+# https://mavlink.io/en/messages/common.html#PLAY_TUNE_V2
+
+## TUNE_FORMAT enum
+uint8 QBASIC1_1 = 1
+uint8 MML_MODERN = 2
+
+uint8 format
+string tune
diff --git a/src/external/mavros_msgs/msg/PositionTarget.msg b/src/external/mavros_msgs/msg/PositionTarget.msg
new file mode 100644
index 0000000..680be87
--- /dev/null
+++ b/src/external/mavros_msgs/msg/PositionTarget.msg
@@ -0,0 +1,32 @@
+# Message for SET_POSITION_TARGET_LOCAL_NED
+#
+# Some complex system requires all feautures that mavlink
+# message provide. See issue #402.
+
+std_msgs/Header header
+
+uint8 coordinate_frame
+uint8 FRAME_LOCAL_NED = 1
+uint8 FRAME_LOCAL_OFFSET_NED = 7
+uint8 FRAME_BODY_NED = 8
+uint8 FRAME_BODY_OFFSET_NED = 9
+
+uint16 type_mask
+uint16 IGNORE_PX = 1 # Position ignore flags
+uint16 IGNORE_PY = 2
+uint16 IGNORE_PZ = 4
+uint16 IGNORE_VX = 8 # Velocity vector ignore flags
+uint16 IGNORE_VY = 16
+uint16 IGNORE_VZ = 32
+uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags
+uint16 IGNORE_AFY = 128
+uint16 IGNORE_AFZ = 256
+uint16 FORCE = 512 # Force in af vector flag
+uint16 IGNORE_YAW = 1024
+uint16 IGNORE_YAW_RATE = 2048
+
+geometry_msgs/Point position
+geometry_msgs/Vector3 velocity
+geometry_msgs/Vector3 acceleration_or_force
+float32 yaw
+float32 yaw_rate
diff --git a/src/external/mavros_msgs/msg/RCIn.msg b/src/external/mavros_msgs/msg/RCIn.msg
new file mode 100644
index 0000000..2a38d9c
--- /dev/null
+++ b/src/external/mavros_msgs/msg/RCIn.msg
@@ -0,0 +1,5 @@
+# RAW RC input state
+
+std_msgs/Header header
+uint8 rssi
+uint16[] channels
diff --git a/src/external/mavros_msgs/msg/RCOut.msg b/src/external/mavros_msgs/msg/RCOut.msg
new file mode 100644
index 0000000..5ba3d15
--- /dev/null
+++ b/src/external/mavros_msgs/msg/RCOut.msg
@@ -0,0 +1,4 @@
+# RAW Servo out state
+
+std_msgs/Header header
+uint16[] channels
diff --git a/src/external/mavros_msgs/msg/RTCM.msg b/src/external/mavros_msgs/msg/RTCM.msg
new file mode 100644
index 0000000..8c74498
--- /dev/null
+++ b/src/external/mavros_msgs/msg/RTCM.msg
@@ -0,0 +1,6 @@
+# RTCM message for the gps_rtk plugin
+# The gps_rtk plugin will fragment the data if necessary and
+# forward it to the FCU via Mavlink through the available link.
+# data should be <= 4*180, higher will be discarded.
+std_msgs/Header header
+uint8[] data
diff --git a/src/external/mavros_msgs/msg/RTKBaseline.msg b/src/external/mavros_msgs/msg/RTKBaseline.msg
new file mode 100644
index 0000000..cbbd386
--- /dev/null
+++ b/src/external/mavros_msgs/msg/RTKBaseline.msg
@@ -0,0 +1,23 @@
+# RTKBaseline received from the FCU.
+# Full description: https://mavlink.io/en/messages/common.html#GPS_RTK
+# Mavlink Common, #127and #128
+
+std_msgs/Header header
+
+uint32 time_last_baseline_ms
+uint8 rtk_receiver_id
+uint16 wn
+uint32 tow
+uint8 rtk_health
+uint8 rtk_rate
+uint8 nsats
+
+uint8 baseline_coords_type
+uint8 RTK_BASELINE_COORDINATE_SYSTEM_ECEF = 0 # Earth-centered, earth-fixed
+uint8 RTK_BASELINE_COORDINATE_SYSTEM_NED = 1 # RTK basestation centered, north, east, down
+
+int32 baseline_a_mm
+int32 baseline_b_mm
+int32 baseline_c_mm
+uint32 accuracy
+int32 iar_num_hypotheses
diff --git a/src/external/mavros_msgs/msg/RadioStatus.msg b/src/external/mavros_msgs/msg/RadioStatus.msg
new file mode 100644
index 0000000..dc7d3b4
--- /dev/null
+++ b/src/external/mavros_msgs/msg/RadioStatus.msg
@@ -0,0 +1,16 @@
+# RADIO_STATUS message
+
+std_msgs/Header header
+
+# message data
+uint8 rssi
+uint8 remrssi
+uint8 txbuf
+uint8 noise
+uint8 remnoise
+uint16 rxerrors
+uint16 fixed
+
+# calculated
+float32 rssi_dbm
+float32 remrssi_dbm
diff --git a/src/external/mavros_msgs/msg/State.msg b/src/external/mavros_msgs/msg/State.msg
new file mode 100644
index 0000000..db00886
--- /dev/null
+++ b/src/external/mavros_msgs/msg/State.msg
@@ -0,0 +1,82 @@
+# Current autopilot state
+#
+# Known modes listed here:
+# http://wiki.ros.org/mavros/CustomModes
+#
+# For system_status values
+# see https://mavlink.io/en/messages/common.html#MAV_STATE
+#
+
+std_msgs/Header header
+bool connected
+bool armed
+bool guided
+bool manual_input
+string mode
+uint8 system_status
+
+string MODE_APM_PLANE_MANUAL = MANUAL
+string MODE_APM_PLANE_CIRCLE = CIRCLE
+string MODE_APM_PLANE_STABILIZE = STABILIZE
+string MODE_APM_PLANE_TRAINING = TRAINING
+string MODE_APM_PLANE_ACRO = ACRO
+string MODE_APM_PLANE_FBWA = FBWA
+string MODE_APM_PLANE_FBWB = FBWB
+string MODE_APM_PLANE_CRUISE = CRUISE
+string MODE_APM_PLANE_AUTOTUNE = AUTOTUNE
+string MODE_APM_PLANE_AUTO = AUTO
+string MODE_APM_PLANE_RTL = RTL
+string MODE_APM_PLANE_LOITER = LOITER
+string MODE_APM_PLANE_LAND = LAND
+string MODE_APM_PLANE_GUIDED = GUIDED
+string MODE_APM_PLANE_INITIALISING = INITIALISING
+string MODE_APM_PLANE_QSTABILIZE = QSTABILIZE
+string MODE_APM_PLANE_QHOVER = QHOVER
+string MODE_APM_PLANE_QLOITER = QLOITER
+string MODE_APM_PLANE_QLAND = QLAND
+string MODE_APM_PLANE_QRTL = QRTL
+
+string MODE_APM_COPTER_STABILIZE = STABILIZE
+string MODE_APM_COPTER_ACRO = ACRO
+string MODE_APM_COPTER_ALT_HOLD = ALT_HOLD
+string MODE_APM_COPTER_AUTO = AUTO
+string MODE_APM_COPTER_GUIDED = GUIDED
+string MODE_APM_COPTER_LOITER = LOITER
+string MODE_APM_COPTER_RTL = RTL
+string MODE_APM_COPTER_CIRCLE = CIRCLE
+string MODE_APM_COPTER_POSITION = POSITION
+string MODE_APM_COPTER_LAND = LAND
+string MODE_APM_COPTER_OF_LOITER = OF_LOITER
+string MODE_APM_COPTER_DRIFT = DRIFT
+string MODE_APM_COPTER_SPORT = SPORT
+string MODE_APM_COPTER_FLIP = FLIP
+string MODE_APM_COPTER_AUTOTUNE = AUTOTUNE
+string MODE_APM_COPTER_POSHOLD = POSHOLD
+string MODE_APM_COPTER_BRAKE = BRAKE
+string MODE_APM_COPTER_THROW = THROW
+string MODE_APM_COPTER_AVOID_ADSB = AVOID_ADSB
+string MODE_APM_COPTER_GUIDED_NOGPS = GUIDED_NOGPS
+
+string MODE_APM_ROVER_MANUAL = MANUAL
+string MODE_APM_ROVER_LEARNING = LEARNING
+string MODE_APM_ROVER_STEERING = STEERING
+string MODE_APM_ROVER_HOLD = HOLD
+string MODE_APM_ROVER_AUTO = AUTO
+string MODE_APM_ROVER_RTL = RTL
+string MODE_APM_ROVER_GUIDED = GUIDED
+string MODE_APM_ROVER_INITIALISING = INITIALISING
+
+string MODE_PX4_MANUAL = MANUAL
+string MODE_PX4_ACRO = ACRO
+string MODE_PX4_ALTITUDE = ALTCTL
+string MODE_PX4_POSITION = POSCTL
+string MODE_PX4_OFFBOARD = OFFBOARD
+string MODE_PX4_STABILIZED = STABILIZED
+string MODE_PX4_RATTITUDE = RATTITUDE
+string MODE_PX4_MISSION = AUTO.MISSION
+string MODE_PX4_LOITER = AUTO.LOITER
+string MODE_PX4_RTL = AUTO.RTL
+string MODE_PX4_LAND = AUTO.LAND
+string MODE_PX4_RTGS = AUTO.RTGS
+string MODE_PX4_READY = AUTO.READY
+string MODE_PX4_TAKEOFF = AUTO.TAKEOFF
diff --git a/src/external/mavros_msgs/msg/StatusEvent.msg b/src/external/mavros_msgs/msg/StatusEvent.msg
new file mode 100644
index 0000000..3f90aea
--- /dev/null
+++ b/src/external/mavros_msgs/msg/StatusEvent.msg
@@ -0,0 +1,19 @@
+# EVENT message representation
+# https://mavlink.io/en/messages/common.html#EVENT
+
+# Severity levels
+uint8 EMERGENCY = 0
+uint8 ALERT = 1
+uint8 CRITICAL = 2
+uint8 ERROR = 3
+uint8 WARNING = 4
+uint8 NOTICE = 5
+uint8 INFO = 6
+uint8 DEBUG = 7
+
+# Fields
+std_msgs/Header header
+uint8 severity
+uint32 px4_id
+uint8[40] arguments
+uint16 sequence
diff --git a/src/external/mavros_msgs/msg/StatusText.msg b/src/external/mavros_msgs/msg/StatusText.msg
new file mode 100644
index 0000000..901b869
--- /dev/null
+++ b/src/external/mavros_msgs/msg/StatusText.msg
@@ -0,0 +1,17 @@
+# STATUSTEXT message representation
+# https://mavlink.io/en/messages/common.html#STATUSTEXT
+
+# Severity levels
+uint8 EMERGENCY = 0
+uint8 ALERT = 1
+uint8 CRITICAL = 2
+uint8 ERROR = 3
+uint8 WARNING = 4
+uint8 NOTICE = 5
+uint8 INFO = 6
+uint8 DEBUG = 7
+
+# Fields
+std_msgs/Header header
+uint8 severity
+string text
diff --git a/src/external/mavros_msgs/msg/SysStatus.msg b/src/external/mavros_msgs/msg/SysStatus.msg
new file mode 100644
index 0000000..551940a
--- /dev/null
+++ b/src/external/mavros_msgs/msg/SysStatus.msg
@@ -0,0 +1,15 @@
+std_msgs/Header header
+
+uint32 sensors_present
+uint32 sensors_enabled
+uint32 sensors_health
+uint16 load
+uint16 voltage_battery
+int16 current_battery
+int8 battery_remaining
+uint16 drop_rate_comm
+uint16 errors_comm
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/TerrainReport.msg b/src/external/mavros_msgs/msg/TerrainReport.msg
new file mode 100644
index 0000000..5c9efce
--- /dev/null
+++ b/src/external/mavros_msgs/msg/TerrainReport.msg
@@ -0,0 +1,12 @@
+# Message for TERRAIN_REPORT
+# https://mavlink.io/en/messages/common.html#TERRAIN_REPORT
+
+std_msgs/Header header
+
+float64 latitude
+float64 longitude
+uint16 spacing
+float32 terrain_height # in meters, terrain height
+float32 current_height # in meters, vehicle height above terrain
+uint16 pending
+uint16 loaded
diff --git a/src/external/mavros_msgs/msg/Thrust.msg b/src/external/mavros_msgs/msg/Thrust.msg
new file mode 100644
index 0000000..fa2fcd6
--- /dev/null
+++ b/src/external/mavros_msgs/msg/Thrust.msg
@@ -0,0 +1,5 @@
+# Thrust to send to the FCU
+
+std_msgs/Header header
+
+float32 thrust
diff --git a/src/external/mavros_msgs/msg/TimesyncStatus.msg b/src/external/mavros_msgs/msg/TimesyncStatus.msg
new file mode 100644
index 0000000..dbfbf30
--- /dev/null
+++ b/src/external/mavros_msgs/msg/TimesyncStatus.msg
@@ -0,0 +1,7 @@
+# Status of the MAVLink time synchroniser
+
+std_msgs/Header header
+uint64 remote_timestamp_ns # remote system timestamp (nanoseconds)
+int64 observed_offset_ns # raw time offset directly observed from this timesync packet (nanoseconds)
+int64 estimated_offset_ns # smoothed time offset between companion system and Mavros (nanoseconds)
+float32 round_trip_time_ms # round trip time of this timesync packet (milliseconds)
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/Trajectory.msg b/src/external/mavros_msgs/msg/Trajectory.msg
new file mode 100644
index 0000000..c43f63a
--- /dev/null
+++ b/src/external/mavros_msgs/msg/Trajectory.msg
@@ -0,0 +1,19 @@
+# MAVLink message: TRAJECTORY
+# https://mavlink.io/en/messages/common.html#TRAJECTORY
+
+std_msgs/Header header
+
+uint8 type # See enum MAV_TRAJECTORY_REPRESENTATION.
+uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0
+uint8 MAV_TRAJECTORY_REPRESENTATION_BEZIER = 1
+
+mavros_msgs/PositionTarget point_1
+mavros_msgs/PositionTarget point_2
+mavros_msgs/PositionTarget point_3
+mavros_msgs/PositionTarget point_4
+mavros_msgs/PositionTarget point_5
+
+uint8[5] point_valid # States if respective point is valid.
+uint16[5] command # MAV_CMD associated with each point. UINT16_MAX if unused.
+
+float32[5] time_horizon # if type MAV_TRAJECTORY_REPRESENTATION_BEZIER, it represents the time horizon for each point, otherwise set to NaN
diff --git a/src/external/mavros_msgs/msg/Tunnel.msg b/src/external/mavros_msgs/msg/Tunnel.msg
new file mode 100644
index 0000000..a04dcd1
--- /dev/null
+++ b/src/external/mavros_msgs/msg/Tunnel.msg
@@ -0,0 +1,27 @@
+# Tunnel
+#
+# https://mavlink.io/en/messages/common.html#TUNNEL
+
+uint8 target_system
+uint8 target_component
+uint16 payload_type
+uint8 payload_length
+uint8[128] payload
+
+# [[[cog:
+# import mavros_cog
+# mavros_cog.idl_decl_enum('MAV_TUNNEL_PAYLOAD_TYPE', 'PAYLOAD_TYPE_', 16)
+# ]]]
+# MAV_TUNNEL_PAYLOAD_TYPE
+uint16 PAYLOAD_TYPE_UNKNOWN = 0 # Encoding of payload unknown.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED0 = 200 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED1 = 201 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED2 = 202 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED3 = 203 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED4 = 204 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED5 = 205 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED6 = 206 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED7 = 207 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED8 = 208 # Registered for STorM32 gimbal controller.
+uint16 PAYLOAD_TYPE_STORM32_RESERVED9 = 209 # Registered for STorM32 gimbal controller.
+# [[[end]]] (checksum: 3327b212af02c2d47d940cd6de049624)
diff --git a/src/external/mavros_msgs/msg/VehicleInfo.msg b/src/external/mavros_msgs/msg/VehicleInfo.msg
new file mode 100644
index 0000000..ea80b16
--- /dev/null
+++ b/src/external/mavros_msgs/msg/VehicleInfo.msg
@@ -0,0 +1,31 @@
+# Vehicle Info msg
+
+std_msgs/Header header
+
+uint8 HAVE_INFO_HEARTBEAT = 1
+uint8 HAVE_INFO_AUTOPILOT_VERSION = 2
+uint8 available_info # Bitmap shows what info is available
+
+# Vehicle address
+uint8 sysid # SYSTEM ID
+uint8 compid # COMPONENT ID
+
+# -*- Heartbeat info -*-
+uint8 autopilot # MAV_AUTOPILOT
+uint8 type # MAV_TYPE
+uint8 system_status # MAV_STATE
+uint8 base_mode
+uint32 custom_mode
+string mode # MAV_MODE string
+uint32 mode_id # MAV_MODE number
+
+# -*- Autopilot version -*-
+uint64 capabilities # MAV_PROTOCOL_CAPABILITY
+uint32 flight_sw_version # Firmware version number
+uint32 middleware_sw_version # Middleware version number
+uint32 os_sw_version # Operating system version number
+uint32 board_version # HW / board version (last 8 bytes should be silicon ID, if any)
+string flight_custom_version # Custom version field, commonly from the first 8 bytes of the git hash
+uint16 vendor_id # ID of the board vendor
+uint16 product_id # ID of the product
+uint64 uid # UID if provided by hardware
diff --git a/src/external/mavros_msgs/msg/VfrHud.msg b/src/external/mavros_msgs/msg/VfrHud.msg
new file mode 100644
index 0000000..81d0ba2
--- /dev/null
+++ b/src/external/mavros_msgs/msg/VfrHud.msg
@@ -0,0 +1,11 @@
+# Metrics typically displayed on a HUD for fixed wing aircraft
+#
+# VFR_HUD message
+
+std_msgs/Header header
+float32 airspeed # m/s
+float32 groundspeed # m/s
+int16 heading # degrees 0..360
+float32 throttle # normalized to 0.0..1.0
+float32 altitude # MSL
+float32 climb # current climb rate m/s
diff --git a/src/external/mavros_msgs/msg/Vibration.msg b/src/external/mavros_msgs/msg/Vibration.msg
new file mode 100644
index 0000000..d9170d0
--- /dev/null
+++ b/src/external/mavros_msgs/msg/Vibration.msg
@@ -0,0 +1,7 @@
+# VIBRATION message data
+# @description: Vibration levels and accelerometer clipping
+
+std_msgs/Header header
+
+geometry_msgs/Vector3 vibration # 3-axis vibration levels
+float32[3] clipping # Accelerometers clipping
\ No newline at end of file
diff --git a/src/external/mavros_msgs/msg/Waypoint.msg b/src/external/mavros_msgs/msg/Waypoint.msg
new file mode 100644
index 0000000..7afa95e
--- /dev/null
+++ b/src/external/mavros_msgs/msg/Waypoint.msg
@@ -0,0 +1,45 @@
+# Waypoint.msg
+#
+# ROS representation of MAVLink MISSION_ITEM
+# See mavlink documentation
+
+
+
+# see enum MAV_FRAME
+uint8 frame
+uint8 FRAME_GLOBAL = 0
+uint8 FRAME_LOCAL_NED = 1
+uint8 FRAME_MISSION = 2
+uint8 FRAME_GLOBAL_REL_ALT = 3
+uint8 FRAME_LOCAL_ENU = 4
+uint8 FRAME_GLOBAL_INT = 5
+uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6
+uint8 FRAME_LOCAL_OFFSET_NED = 7
+uint8 FRAME_BODY_NED = 8
+uint8 FRAME_BODY_OFFSET_NED = 9
+uint8 FRAME_GLOBAL_TERRAIN_ALT = 10
+uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11
+uint8 FRAME_BODY_FRD = 12
+uint8 FRAME_RESERVED_13 = 13
+uint8 FRAME_RESERVED_14 = 14
+uint8 FRAME_RESERVED_15 = 15
+uint8 FRAME_RESERVED_16 = 16
+uint8 FRAME_RESERVED_17 = 17
+uint8 FRAME_RESERVED_18 = 18
+uint8 FRAME_RESERVED_19 = 19
+uint8 FRAME_LOCAL_FRD = 20
+uint8 FRAME_LOCAL_FLU = 21
+
+# see enum MAV_CMD and CommandCode.msg
+uint16 command
+
+bool is_current
+bool autocontinue
+# meaning of this params described in enum MAV_CMD
+float32 param1
+float32 param2
+float32 param3
+float32 param4
+float64 x_lat
+float64 y_long
+float64 z_alt
diff --git a/src/external/mavros_msgs/msg/WaypointList.msg b/src/external/mavros_msgs/msg/WaypointList.msg
new file mode 100644
index 0000000..de4111e
--- /dev/null
+++ b/src/external/mavros_msgs/msg/WaypointList.msg
@@ -0,0 +1,9 @@
+# WaypointList.msg
+#
+# :current_seq: seq nr of currently active waypoint
+# waypoints[current_seq].is_current == True
+#
+# :waypoints: list of waypoints
+
+uint16 current_seq
+mavros_msgs/Waypoint[] waypoints
diff --git a/src/external/mavros_msgs/msg/WaypointReached.msg b/src/external/mavros_msgs/msg/WaypointReached.msg
new file mode 100644
index 0000000..6fc36a9
--- /dev/null
+++ b/src/external/mavros_msgs/msg/WaypointReached.msg
@@ -0,0 +1,7 @@
+# That message represent MISSION_ITEM_REACHED
+#
+# :wp_seq: index number of reached waypoint
+
+std_msgs/Header header
+
+uint16 wp_seq
diff --git a/src/external/mavros_msgs/msg/WheelOdomStamped.msg b/src/external/mavros_msgs/msg/WheelOdomStamped.msg
new file mode 100644
index 0000000..782c3ab
--- /dev/null
+++ b/src/external/mavros_msgs/msg/WheelOdomStamped.msg
@@ -0,0 +1,6 @@
+# Stamped wheel odometry message
+#
+# For streaming timestamped data from FCU wheel encoders (RPM or WHEEL_DISTANCE).
+
+std_msgs/Header header
+float64[] data
diff --git a/src/external/mavros_msgs/package.xml b/src/external/mavros_msgs/package.xml
new file mode 100644
index 0000000..9c5d8bc
--- /dev/null
+++ b/src/external/mavros_msgs/package.xml
@@ -0,0 +1,46 @@
+
+
+
+ mavros_msgs
+ 2.9.0
+
+ mavros_msgs defines messages for MAVROS.
+
+
+ Vladimir Ermakov
+
+ GPLv3
+ LGPLv3
+ BSD
+
+ http://wiki.ros.org/mavros_msgs
+ https://github.com/mavlink/mavros.git
+ https://github.com/mavlink/mavros/issues
+
+ Vladimir Ermakov
+
+ ament_cmake
+
+ rosidl_default_generators
+ rosidl_default_runtime
+
+
+ rcl_interfaces
+ geographic_msgs
+ geometry_msgs
+ sensor_msgs
+
+
+
+ ament_lint_auto
+ ament_lint_common
+
+ rosidl_interface_packages
+
+
+ ament_cmake
+
+
+
diff --git a/src/external/mavros_msgs/srv/CommandAck.srv b/src/external/mavros_msgs/srv/CommandAck.srv
new file mode 100644
index 0000000..f1d708e
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandAck.srv
@@ -0,0 +1,11 @@
+# Generic COMMAND_ACK
+
+uint16 command
+uint8 result
+uint8 progress
+uint32 result_param2
+
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result
diff --git a/src/external/mavros_msgs/srv/CommandBool.srv b/src/external/mavros_msgs/srv/CommandBool.srv
new file mode 100644
index 0000000..54eab1f
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandBool.srv
@@ -0,0 +1,6 @@
+# Common type for switch commands
+
+bool value
+---
+bool success
+uint8 result
diff --git a/src/external/mavros_msgs/srv/CommandHome.srv b/src/external/mavros_msgs/srv/CommandHome.srv
new file mode 100644
index 0000000..89882c7
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandHome.srv
@@ -0,0 +1,10 @@
+# request set new home position
+
+bool current_gps
+float32 yaw
+float32 latitude
+float32 longitude
+float32 altitude
+---
+bool success
+uint8 result
diff --git a/src/external/mavros_msgs/srv/CommandInt.srv b/src/external/mavros_msgs/srv/CommandInt.srv
new file mode 100644
index 0000000..f2242c7
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandInt.srv
@@ -0,0 +1,19 @@
+# Generic COMMAND_INT
+
+bool broadcast # send this command in broadcast mode
+
+uint8 frame
+uint16 command
+uint8 current
+uint8 autocontinue
+float32 param1
+float32 param2
+float32 param3
+float32 param4
+int32 x # latitude in deg * 1E7 or local x * 1E4 m
+int32 y # longitude in deg * 1E7 or local y * 1E4 m
+float32 z # altitude
+---
+bool success
+# seems that this message don't produce andy COMMAND_ACK messages
+# so no result field
diff --git a/src/external/mavros_msgs/srv/CommandLong.srv b/src/external/mavros_msgs/srv/CommandLong.srv
new file mode 100644
index 0000000..ca7f2bb
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandLong.srv
@@ -0,0 +1,17 @@
+# Generic COMMAND_LONG
+
+bool broadcast # send this command in broadcast mode
+
+uint16 command
+uint8 confirmation
+float32 param1
+float32 param2
+float32 param3
+float32 param4
+float32 param5 # x_lat
+float32 param6 # y_lon
+float32 param7 # z_alt
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result
diff --git a/src/external/mavros_msgs/srv/CommandTOL.srv b/src/external/mavros_msgs/srv/CommandTOL.srv
new file mode 100644
index 0000000..b76c85e
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandTOL.srv
@@ -0,0 +1,10 @@
+# Common type for Take Off and Landing
+
+float32 min_pitch # used by takeoff
+float32 yaw
+float32 latitude
+float32 longitude
+float32 altitude
+---
+bool success
+uint8 result
diff --git a/src/external/mavros_msgs/srv/CommandTOLLocal.srv b/src/external/mavros_msgs/srv/CommandTOLLocal.srv
new file mode 100644
index 0000000..33d522f
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandTOLLocal.srv
@@ -0,0 +1,10 @@
+#Common type for LOCAL Take Off and Landing
+
+float32 min_pitch # used by takeoff
+float32 offset # used by land (landing position accuracy)
+float32 rate # speed of takeoff/land in m/s
+float32 yaw # in radians
+geometry_msgs/Vector3 position #(x,y,z) in meters
+---
+bool success
+uint8 result
diff --git a/src/external/mavros_msgs/srv/CommandTriggerControl.srv b/src/external/mavros_msgs/srv/CommandTriggerControl.srv
new file mode 100644
index 0000000..fa3977b
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandTriggerControl.srv
@@ -0,0 +1,8 @@
+# Type for controlling onboard camera triggering system
+
+bool trigger_enable # Trigger enable/disable
+bool sequence_reset # Reset the trigger sequence
+bool trigger_pause # Pause triggering, but without switching the camera off or retracting it.
+---
+bool success
+uint8 result
diff --git a/src/external/mavros_msgs/srv/CommandTriggerInterval.srv b/src/external/mavros_msgs/srv/CommandTriggerInterval.srv
new file mode 100644
index 0000000..85af0f5
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandTriggerInterval.srv
@@ -0,0 +1,7 @@
+# Type for controlling camera trigger interval and integration time
+
+float32 cycle_time # Trigger cycle_time (interval between to triggers) - set to 0 to ignore command
+float32 integration_time # Camera shutter integration_time - set to 0 to ignore command
+---
+bool success
+uint8 result
\ No newline at end of file
diff --git a/src/external/mavros_msgs/srv/CommandVtolTransition.srv b/src/external/mavros_msgs/srv/CommandVtolTransition.srv
new file mode 100644
index 0000000..fce9a72
--- /dev/null
+++ b/src/external/mavros_msgs/srv/CommandVtolTransition.srv
@@ -0,0 +1,16 @@
+
+# MAVLink command: DO_VTOL_TRANSITION
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_VTOL_TRANSITION
+
+std_msgs/Header header
+
+# MAV_VTOL_STATE
+uint8 STATE_MC = 3
+uint8 STATE_FW = 4
+
+uint8 state # See enum MAV_VTOL_STATE.
+
+---
+bool success
+uint8 result # Raw result returned by COMMAND_ACK
+
\ No newline at end of file
diff --git a/src/external/mavros_msgs/srv/EndpointAdd.srv b/src/external/mavros_msgs/srv/EndpointAdd.srv
new file mode 100644
index 0000000..c102adb
--- /dev/null
+++ b/src/external/mavros_msgs/srv/EndpointAdd.srv
@@ -0,0 +1,14 @@
+#
+# Adds endpoint to router
+#
+
+uint8 TYPE_FCU = 0
+uint8 TYPE_GCS = 1
+uint8 TYPE_UAS = 2
+
+string url # mavconn URL or topic prefix for TYPE_UAS
+uint8 type # should be set to one of the TYPE_xxx
+---
+bool successful # true if endpoint added and opened
+string reason # returns error description if open fails
+uint32 id # ID of new endpoint, should be > 0 if endpoint created
diff --git a/src/external/mavros_msgs/srv/EndpointDel.srv b/src/external/mavros_msgs/srv/EndpointDel.srv
new file mode 100644
index 0000000..04d1ff3
--- /dev/null
+++ b/src/external/mavros_msgs/srv/EndpointDel.srv
@@ -0,0 +1,17 @@
+#
+# Removes endpoint from router
+#
+
+uint8 TYPE_FCU = 0
+uint8 TYPE_GCS = 1
+uint8 TYPE_UAS = 2
+
+# delete by ID, leave 0 for second option
+uint32 id
+
+# delete by url+type pair
+string url
+uint8 type
+
+---
+bool successful
diff --git a/src/external/mavros_msgs/srv/FileChecksum.srv b/src/external/mavros_msgs/srv/FileChecksum.srv
new file mode 100644
index 0000000..75cb4be
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileChecksum.srv
@@ -0,0 +1,12 @@
+# FTP::Checksum
+#
+# :file_path: file to calculate checksum
+# :crc32: file checksum
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string file_path
+---
+uint32 crc32
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileClose.srv b/src/external/mavros_msgs/srv/FileClose.srv
new file mode 100644
index 0000000..b99419e
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileClose.srv
@@ -0,0 +1,10 @@
+# FTP::Close
+#
+# Call FTP::Open first.
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string file_path
+---
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileList.srv b/src/external/mavros_msgs/srv/FileList.srv
new file mode 100644
index 0000000..d589a9e
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileList.srv
@@ -0,0 +1,10 @@
+# FTP::List
+#
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string dir_path
+---
+mavros_msgs/FileEntry[] list
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileMakeDir.srv b/src/external/mavros_msgs/srv/FileMakeDir.srv
new file mode 100644
index 0000000..c2067d9
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileMakeDir.srv
@@ -0,0 +1,9 @@
+# FTP::MakeDir
+#
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string dir_path
+---
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileOpen.srv b/src/external/mavros_msgs/srv/FileOpen.srv
new file mode 100644
index 0000000..9ad26c3
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileOpen.srv
@@ -0,0 +1,17 @@
+# FTP::Open
+#
+# :file_path: used as session id in read/write/close services
+# :size: file size returned for MODE_READ
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+uint8 MODE_READ = 0 # open for read
+uint8 MODE_WRITE = 1 # open for write
+uint8 MODE_CREATE = 2 # do creat()
+
+string file_path
+uint8 mode
+---
+uint32 size
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileRead.srv b/src/external/mavros_msgs/srv/FileRead.srv
new file mode 100644
index 0000000..d8aa592
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileRead.srv
@@ -0,0 +1,13 @@
+# FTP::Read
+#
+# Call FTP::Open first.
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string file_path
+uint64 offset
+uint64 size
+---
+uint8[] data
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileRemove.srv b/src/external/mavros_msgs/srv/FileRemove.srv
new file mode 100644
index 0000000..af0328f
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileRemove.srv
@@ -0,0 +1,9 @@
+# FTP::Remove
+#
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string file_path
+---
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileRemoveDir.srv b/src/external/mavros_msgs/srv/FileRemoveDir.srv
new file mode 100644
index 0000000..c95d52f
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileRemoveDir.srv
@@ -0,0 +1,9 @@
+# FTP::RemoveDir
+#
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string dir_path
+---
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileRename.srv b/src/external/mavros_msgs/srv/FileRename.srv
new file mode 100644
index 0000000..0e367a5
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileRename.srv
@@ -0,0 +1,10 @@
+# FTP::Rename
+#
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string old_path
+string new_path
+---
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileTruncate.srv b/src/external/mavros_msgs/srv/FileTruncate.srv
new file mode 100644
index 0000000..4c78596
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileTruncate.srv
@@ -0,0 +1,10 @@
+# FTP::Truncate
+#
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string file_path
+uint64 length
+---
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/FileWrite.srv b/src/external/mavros_msgs/srv/FileWrite.srv
new file mode 100644
index 0000000..d94815e
--- /dev/null
+++ b/src/external/mavros_msgs/srv/FileWrite.srv
@@ -0,0 +1,12 @@
+# FTP::Write
+#
+# Call FTP::Open first.
+# :success: indicates success end of request
+# :r_errno: remote errno if applicapable
+
+string file_path
+uint64 offset
+uint8[] data
+---
+bool success
+int32 r_errno
diff --git a/src/external/mavros_msgs/srv/GimbalGetInformation.srv b/src/external/mavros_msgs/srv/GimbalGetInformation.srv
new file mode 100644
index 0000000..c91bda2
--- /dev/null
+++ b/src/external/mavros_msgs/srv/GimbalGetInformation.srv
@@ -0,0 +1,10 @@
+# MAVLink command: MAV_CMD_REQUEST_MESSAGE
+# https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_MESSAGE
+# Specifically used to request Information messages from Gimbal Device and Gimbal Manager
+# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION
+# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION
+
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result
\ No newline at end of file
diff --git a/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv b/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv
new file mode 100644
index 0000000..ef46dc9
--- /dev/null
+++ b/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv
@@ -0,0 +1,28 @@
+# MAVLink commands: CAMERA_TRACK_POINT, CAMERA_TRACK_RECTANGLE, CAMERA_STOP_TRACKING
+# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT
+# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_RECTANGLE
+# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_STOP_TRACKING
+
+uint8 mode # enumerator to indicate camera track mode setting - see CAMERA_TRACK_MODE
+#CAMERA_TRACK_MODE
+uint8 CAMERA_TRACK_MODE_POINT = 0 # If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. [CAMERA_TRACK_POINT]
+uint8 CAMERA_TRACK_MODE_RECTANGLE = 1 # If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. [CAMERA_TRACK_RECTANGLE]
+uint8 CAMERA_TRACK_MODE_STOP_TRACKING = 2 # Stops ongoing tracking. [CAMERA_STOP_TRACKING]
+
+#For CAMERA_TRACK_POINT
+float32 x # Point to track x value (normalized 0..1, 0 is left, 1 is right).
+float32 y # Point to track y value (normalized 0..1, 0 is top, 1 is bottom).
+float32 radius # Point radius (normalized 0..1, 0 is image left, 1 is image right).
+
+#For CAMERA_TRACK_RECTANGLE
+float32 top_left_x # Top left corner of rectangle x value (normalized 0..1, 0 is left, 1 is right).
+float32 top_left_y # Top left corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom).
+float32 bottom_right_x # Bottom right corner of rectangle x value (normalized 0..1, 0 is left, 1 is right).
+float32 bottom_right_y # Bottom right corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom).
+
+#CAMERA_STOP_TRACKING doesn't take extra parameters
+
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result
\ No newline at end of file
diff --git a/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv b/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv
new file mode 100644
index 0000000..c0a2176
--- /dev/null
+++ b/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv
@@ -0,0 +1,32 @@
+# MAVLink command: DO_GIMBAL_MANAGER_CONFIGURE
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE
+# Note: default MAV_COMP_ID_ONBOARD_COMPUTER = 191, see MAV_COMPONENT documentation
+# https://mavlink.io/en/messages/common.html#MAV_COMPONENT
+
+int16 sysid_primary # Sysid for primary control (0: no one in control,
+ # -1: leave unchanged, -2: set itself in control
+ # (for missions where the own sysid is still unknown),
+ # -3: remove control if currently in control).
+int16 compid_primary # Compid for primary control (0: no one in control,
+ # -1: leave unchanged, -2: set itself in control
+ # (for missions where the own sysid is still unknown),
+ # -3: remove control if currently in control).
+int16 sysid_secondary # Sysid for secondary control (0: no one in control,
+ # -1: leave unchanged, -2: set itself in control
+ # (for missions where the own sysid is still unknown),
+ # -3: remove control if currently in control).
+int16 compid_secondary # Compid for secondary control (0: no one in control,
+ # -1: leave unchanged, -2: set itself in control
+ # (for missions where the own sysid is still unknown),
+ # -3: remove control if currently in control).
+
+uint8 gimbal_device_id # Component ID of gimbal device to address
+ # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device
+ # components. Send command multiple times for more than
+ # one gimbal (but not all gimbals).
+ # Note: Default Mavlink gimbal device ids: 154, 171-175
+
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result
\ No newline at end of file
diff --git a/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv b/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv
new file mode 100644
index 0000000..6e3c14e
--- /dev/null
+++ b/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv
@@ -0,0 +1,27 @@
+# MAVLink commands: DO_GIMBAL_MANAGER_PITCHYAW
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW
+
+
+float32 pitch # Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode). (-180 to 180 deg)
+float32 yaw # Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode). (-180 to 180 deg)
+float32 pitch_rate # Pitch rate (positive to pitch up). (deg/s)
+float32 yaw_rate # Yaw rate (positive to yaw to the right). (deg/s)
+
+uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS
+#GIMBAL_MANAGER_FLAGS
+uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT
+uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL
+uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK
+uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK
+
+uint8 gimbal_device_id # Component ID of gimbal device to address
+ # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device
+ # components. Send command multiple times for more than
+ # one gimbal (but not all gimbals). Default Mavlink gimbal
+ # device ids: 154, 171-175
+
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result
\ No newline at end of file
diff --git a/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv b/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv
new file mode 100644
index 0000000..02904d8
--- /dev/null
+++ b/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv
@@ -0,0 +1,38 @@
+# MAVLink commands: DO_SET_ROI_LOCATION, DO_SET_ROI_WPNEXT_OFFSET, DO_SET_ROI_SYSID, DO_SET_ROI_NONE
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_LOCATION
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_SYSID
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_NONE
+
+uint8 mode # enumerator to indicate ROI mode setting - see ROI_MODE
+#ROI_MODE
+uint8 ROI_MODE_LOCATION = 0 # Sets the region of interest (ROI) to a location. [DO_SET_ROI_LOCATION]
+uint8 ROI_MODE_WP_NEXT_OFFSET = 1 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. [DO_SET_ROI_WPNEXT_OFFSET]
+uint8 ROI_MODE_SYSID = 2 # Mount tracks system with specified system ID [DO_SET_ROI_SYSID]
+uint8 ROI_MODE_NONE = 3 # Cancels any previous ROI setting and returns vehicle to defaults [DO_SET_ROI_NONE]
+
+uint8 gimbal_device_id # Component ID of gimbal device to address
+ # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device
+ # components. Send command multiple times for more than
+ # one gimbal (but not all gimbals). Default Mavlink gimbal
+ # device ids: 154, 171-175
+
+#For ROI_MODE_LOCATION
+float32 latitude
+float32 longitude
+float32 altitude # Meters
+
+#For ROI_MODE_WP_NEXT_OFFSET
+float32 pitch_offset # Pitch offset from next waypoint, positive pitching up
+float32 roll_offset # Roll offset from next waypoint, positive rolling to the right
+float32 yaw_offset # Yaw offset from next waypoint, positive yawing to the right
+
+#For ROI_MODE_SYSID
+uint8 sysid # System ID to track (min: 1, max: 255)
+
+#ROI_MODE_NONE doesn't take extra parameters
+
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result
\ No newline at end of file
diff --git a/src/external/mavros_msgs/srv/LogRequestData.srv b/src/external/mavros_msgs/srv/LogRequestData.srv
new file mode 100644
index 0000000..b306df6
--- /dev/null
+++ b/src/external/mavros_msgs/srv/LogRequestData.srv
@@ -0,0 +1,11 @@
+# Request a chunk of a log
+#
+# :id: - log id from LogEntry message
+# :offset: - offset into the log
+# :count: - number of bytes to get
+
+uint16 id
+uint32 offset
+uint32 count
+---
+bool success
diff --git a/src/external/mavros_msgs/srv/LogRequestEnd.srv b/src/external/mavros_msgs/srv/LogRequestEnd.srv
new file mode 100644
index 0000000..956e89f
--- /dev/null
+++ b/src/external/mavros_msgs/srv/LogRequestEnd.srv
@@ -0,0 +1,4 @@
+# Stop log transfer and resume normal logging
+
+---
+bool success
diff --git a/src/external/mavros_msgs/srv/LogRequestList.srv b/src/external/mavros_msgs/srv/LogRequestList.srv
new file mode 100644
index 0000000..14eb46c
--- /dev/null
+++ b/src/external/mavros_msgs/srv/LogRequestList.srv
@@ -0,0 +1,9 @@
+# Request a list of available logs
+#
+# :start: - first log id (0 for first available)
+# :end: - last log id (0xffff for last available)
+
+uint16 start
+uint16 end
+---
+bool success
diff --git a/src/external/mavros_msgs/srv/MessageInterval.srv b/src/external/mavros_msgs/srv/MessageInterval.srv
new file mode 100644
index 0000000..5db5436
--- /dev/null
+++ b/src/external/mavros_msgs/srv/MessageInterval.srv
@@ -0,0 +1,7 @@
+# sets message interval
+# See MAV_CMD_SET_MESSAGE_INTERVAL
+
+uint32 message_id
+float32 message_rate
+---
+bool success
diff --git a/src/external/mavros_msgs/srv/MountConfigure.srv b/src/external/mavros_msgs/srv/MountConfigure.srv
new file mode 100644
index 0000000..2b9afaf
--- /dev/null
+++ b/src/external/mavros_msgs/srv/MountConfigure.srv
@@ -0,0 +1,28 @@
+# MAVLink message: DO_MOUNT_CONTROL
+# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONFIGURE
+
+std_msgs/Header header
+
+uint8 mode # See enum MAV_MOUNT_MODE.
+#MAV_MOUNT_MODE
+uint8 MODE_RETRACT = 0
+uint8 MODE_NEUTRAL = 1
+uint8 MODE_MAVLINK_TARGETING = 2
+uint8 MODE_RC_TARGETING = 3
+uint8 MODE_GPS_POINT = 4
+
+bool stabilize_roll # stabilize roll? (1 = yes, 0 = no)
+bool stabilize_pitch # stabilize pitch? (1 = yes, 0 = no)
+bool stabilize_yaw # stabilize yaw? (1 = yes, 0 = no)
+uint8 roll_input # roll input (See enum MOUNT_INPUT)
+uint8 pitch_input # pitch input (See enum MOUNT_INPUT)
+uint8 yaw_input # yaw input (See enum MOUNT_INPUT)
+
+#MOUNT_INPUT
+uint8 INPUT_ANGLE_BODY_FRAME = 0
+uint8 INPUT_ANGULAR_RATE = 1
+uint8 INPUT_ANGLE_ABSOLUTE_FRAME = 2
+---
+bool success
+# raw result returned by COMMAND_ACK
+uint8 result
\ No newline at end of file
diff --git a/src/external/mavros_msgs/srv/ParamGet.srv b/src/external/mavros_msgs/srv/ParamGet.srv
new file mode 100644
index 0000000..cf092f0
--- /dev/null
+++ b/src/external/mavros_msgs/srv/ParamGet.srv
@@ -0,0 +1,8 @@
+# Request parameter from attached device
+
+# XXX DEPRECATED: use ros2 parameters api instead
+
+string param_id
+---
+bool success
+ParamValue value
diff --git a/src/external/mavros_msgs/srv/ParamPull.srv b/src/external/mavros_msgs/srv/ParamPull.srv
new file mode 100644
index 0000000..db11796
--- /dev/null
+++ b/src/external/mavros_msgs/srv/ParamPull.srv
@@ -0,0 +1,8 @@
+# Request parameters from device
+#
+# Returns success status and param_recived count
+
+bool force_pull
+---
+bool success
+uint32 param_received
diff --git a/src/external/mavros_msgs/srv/ParamPush.srv b/src/external/mavros_msgs/srv/ParamPush.srv
new file mode 100644
index 0000000..fe0d4f8
--- /dev/null
+++ b/src/external/mavros_msgs/srv/ParamPush.srv
@@ -0,0 +1,9 @@
+# Send current send
+#
+# Returns success status and param_transfered count
+
+# XXX DEPRECATED: not used. param plugin listen to parameter changes
+
+---
+bool success
+uint32 param_transfered
diff --git a/src/external/mavros_msgs/srv/ParamSet.srv b/src/external/mavros_msgs/srv/ParamSet.srv
new file mode 100644
index 0000000..f5b64d9
--- /dev/null
+++ b/src/external/mavros_msgs/srv/ParamSet.srv
@@ -0,0 +1,9 @@
+# Request set parameter value
+
+# XXX DEPRECATED: replaced by ParamSetV2
+
+string param_id
+mavros_msgs/ParamValue value
+---
+bool success
+mavros_msgs/ParamValue value
diff --git a/src/external/mavros_msgs/srv/ParamSetV2.srv b/src/external/mavros_msgs/srv/ParamSetV2.srv
new file mode 100644
index 0000000..e06b85a
--- /dev/null
+++ b/src/external/mavros_msgs/srv/ParamSetV2.srv
@@ -0,0 +1,14 @@
+# Request set parameter value
+#
+# That interface allow to bypass some checks
+# and send value as is to the FCU if force_set is true.
+#
+# Use that api only if ROS2 Parameter API is not sufficient
+# for your application.
+
+bool force_set
+string param_id
+rcl_interfaces/ParameterValue value
+---
+bool success
+rcl_interfaces/ParameterValue value
diff --git a/src/external/mavros_msgs/srv/SetMavFrame.srv b/src/external/mavros_msgs/srv/SetMavFrame.srv
new file mode 100644
index 0000000..b12f76a
--- /dev/null
+++ b/src/external/mavros_msgs/srv/SetMavFrame.srv
@@ -0,0 +1,36 @@
+# Set MAV_FRAME for setpoints
+
+# XXX DEPRECATED
+
+# [[[cog:
+# import mavros_cog
+# mavros_cog.idl_decl_enum('MAV_FRAME', 'FRAME_')
+# ]]]
+# MAV_FRAME
+uint8 FRAME_GLOBAL = 0 # Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL).
+uint8 FRAME_LOCAL_NED = 1 # NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
+uint8 FRAME_MISSION = 2 # NOT a coordinate frame, indicates a mission command.
+uint8 FRAME_GLOBAL_RELATIVE_ALT = 3 # Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location.
+uint8 FRAME_LOCAL_ENU = 4 # ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
+uint8 FRAME_GLOBAL_INT = 5 # Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude over mean sea level (MSL).
+uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude with 0 being at the altitude of the home location.
+uint8 FRAME_LOCAL_OFFSET_NED = 7 # NED local tangent frame (x: North, y: East, z: Down) with origin that travels with the vehicle.
+uint8 FRAME_BODY_NED = 8 # Same as MAV_FRAME_LOCAL_NED when used to represent position values. Same as MAV_FRAME_BODY_FRD when used with velocity/accelaration values.
+uint8 FRAME_BODY_OFFSET_NED = 9 # This is the same as MAV_FRAME_BODY_FRD.
+uint8 FRAME_GLOBAL_TERRAIN_ALT = 10 # Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model.
+uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11 # Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude in meters with 0 being at ground level in terrain model.
+uint8 FRAME_BODY_FRD = 12 # FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle. The forward axis is aligned to the front of the vehicle in the horizontal plane.
+uint8 FRAME_RESERVED_13 = 13 # MAV_FRAME_BODY_FLU - Body fixed frame of reference, Z-up (x: Forward, y: Left, z: Up).
+uint8 FRAME_RESERVED_14 = 14 # MAV_FRAME_MOCAP_NED - Odometry local coordinate frame of data given by a motion capture system, Z-down (x: North, y: East, z: Down).
+uint8 FRAME_RESERVED_15 = 15 # MAV_FRAME_MOCAP_ENU - Odometry local coordinate frame of data given by a motion capture system, Z-up (x: East, y: North, z: Up).
+uint8 FRAME_RESERVED_16 = 16 # MAV_FRAME_VISION_NED - Odometry local coordinate frame of data given by a vision estimation system, Z-down (x: North, y: East, z: Down).
+uint8 FRAME_RESERVED_17 = 17 # MAV_FRAME_VISION_ENU - Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: East, y: North, z: Up).
+uint8 FRAME_RESERVED_18 = 18 # MAV_FRAME_ESTIM_NED - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: North, y: East, z: Down).
+uint8 FRAME_RESERVED_19 = 19 # MAV_FRAME_ESTIM_ENU - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: East, y: North, z: Up).
+uint8 FRAME_LOCAL_FRD = 20 # FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane.
+uint8 FRAME_LOCAL_FLU = 21 # FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane.
+# [[[end]]] (checksum: c5ddb537c91e87c4efba8b24c9cde50e)
+
+uint8 mav_frame
+---
+bool success
diff --git a/src/external/mavros_msgs/srv/SetMode.srv b/src/external/mavros_msgs/srv/SetMode.srv
new file mode 100644
index 0000000..2637c0e
--- /dev/null
+++ b/src/external/mavros_msgs/srv/SetMode.srv
@@ -0,0 +1,22 @@
+# set FCU mode
+#
+# Known custom modes listed here:
+# http://wiki.ros.org/mavros/CustomModes
+
+# basic modes from MAV_MODE
+uint8 MAV_MODE_PREFLIGHT = 0
+uint8 MAV_MODE_STABILIZE_DISARMED = 80
+uint8 MAV_MODE_STABILIZE_ARMED = 208
+uint8 MAV_MODE_MANUAL_DISARMED = 64
+uint8 MAV_MODE_MANUAL_ARMED = 192
+uint8 MAV_MODE_GUIDED_DISARMED = 88
+uint8 MAV_MODE_GUIDED_ARMED = 216
+uint8 MAV_MODE_AUTO_DISARMED = 92
+uint8 MAV_MODE_AUTO_ARMED = 220
+uint8 MAV_MODE_TEST_DISARMED = 66
+uint8 MAV_MODE_TEST_ARMED = 194
+
+uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != ''
+string custom_mode # string mode representation or integer
+---
+bool mode_sent # Mode known/parsed correctly and SET_MODE are sent
diff --git a/src/external/mavros_msgs/srv/StreamRate.srv b/src/external/mavros_msgs/srv/StreamRate.srv
new file mode 100644
index 0000000..a66ca54
--- /dev/null
+++ b/src/external/mavros_msgs/srv/StreamRate.srv
@@ -0,0 +1,17 @@
+# sets stream rate
+# See REQUEST_DATA_STREAM message
+
+uint8 STREAM_ALL = 0
+uint8 STREAM_RAW_SENSORS = 1
+uint8 STREAM_EXTENDED_STATUS = 2
+uint8 STREAM_RC_CHANNELS = 3
+uint8 STREAM_RAW_CONTROLLER = 4
+uint8 STREAM_POSITION = 6
+uint8 STREAM_EXTRA1 = 10
+uint8 STREAM_EXTRA2 = 11
+uint8 STREAM_EXTRA3 = 12
+
+uint8 stream_id
+uint16 message_rate
+bool on_off
+---
diff --git a/src/external/mavros_msgs/srv/VehicleInfoGet.srv b/src/external/mavros_msgs/srv/VehicleInfoGet.srv
new file mode 100644
index 0000000..9e32236
--- /dev/null
+++ b/src/external/mavros_msgs/srv/VehicleInfoGet.srv
@@ -0,0 +1,14 @@
+# Request the Vehicle Info
+# use this to request the current target sysid / compid defined in mavros
+# set get_all = True to request all available vehicles
+
+uint8 GET_MY_SYSID = 0
+uint8 GET_MY_COMPID = 0
+
+uint8 sysid
+uint8 compid
+bool get_all
+---
+bool success
+mavros_msgs/VehicleInfo[] vehicles
+
diff --git a/src/external/mavros_msgs/srv/WaypointClear.srv b/src/external/mavros_msgs/srv/WaypointClear.srv
new file mode 100644
index 0000000..ac3d34c
--- /dev/null
+++ b/src/external/mavros_msgs/srv/WaypointClear.srv
@@ -0,0 +1,4 @@
+# Request clear waypoint
+
+---
+bool success
diff --git a/src/external/mavros_msgs/srv/WaypointPull.srv b/src/external/mavros_msgs/srv/WaypointPull.srv
new file mode 100644
index 0000000..2d7ac2c
--- /dev/null
+++ b/src/external/mavros_msgs/srv/WaypointPull.srv
@@ -0,0 +1,7 @@
+# Requests waypoints from device
+#
+# Returns success status and received count
+
+---
+bool success
+uint32 wp_received
diff --git a/src/external/mavros_msgs/srv/WaypointPush.srv b/src/external/mavros_msgs/srv/WaypointPush.srv
new file mode 100644
index 0000000..e29727b
--- /dev/null
+++ b/src/external/mavros_msgs/srv/WaypointPush.srv
@@ -0,0 +1,11 @@
+# Send waypoints to device
+#
+# :start_index: will define a partial waypoint update. Set to 0 for full update
+#
+# Returns success status and transfered count
+
+uint16 start_index
+mavros_msgs/Waypoint[] waypoints
+---
+bool success
+uint32 wp_transfered
diff --git a/src/external/mavros_msgs/srv/WaypointSetCurrent.srv b/src/external/mavros_msgs/srv/WaypointSetCurrent.srv
new file mode 100644
index 0000000..b99612f
--- /dev/null
+++ b/src/external/mavros_msgs/srv/WaypointSetCurrent.srv
@@ -0,0 +1,7 @@
+# Request set current waypoint
+#
+# wp_seq - index in waypoint array
+
+uint16 wp_seq
+---
+bool success
diff --git a/src/fc_interfaces/CMakeLists.txt b/src/fc_interfaces/CMakeLists.txt
new file mode 100644
index 0000000..c5c4a41
--- /dev/null
+++ b/src/fc_interfaces/CMakeLists.txt
@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.8)
+project(fc_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "srv/MavPing.srv"
+ "srv/MavCommandLong.srv"
+ "srv/MavPositionTargetGlobalInt.srv"
+ )
+
+ament_package()
diff --git a/src/fc_interfaces/package.xml b/src/fc_interfaces/package.xml
new file mode 100644
index 0000000..292f81b
--- /dev/null
+++ b/src/fc_interfaces/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ fc_interfaces
+ 0.0.0
+ TODO: Package description
+ picars
+ TODO: License declaration
+
+ ament_cmake
+ rosidl_default_generators
+ rosidl_interface_packages
+ rosidl_default_runtime
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/src/fc_interfaces/srv/MavCommandLong.srv b/src/fc_interfaces/srv/MavCommandLong.srv
new file mode 100644
index 0000000..3ff2e6e
--- /dev/null
+++ b/src/fc_interfaces/srv/MavCommandLong.srv
@@ -0,0 +1,18 @@
+# Request
+uint8 target_sysid
+uint8 target_compid
+uint16 command
+uint8 confirmation
+float32 param1
+float32 param2
+float32 param3
+float32 param4
+float32 param5
+float32 param6
+float32 param7
+float32 timeout_sec
+---
+# Response
+bool success
+string message
+uint8 ack_result
\ No newline at end of file
diff --git a/src/fc_interfaces/srv/MavPing.srv b/src/fc_interfaces/srv/MavPing.srv
new file mode 100644
index 0000000..400baf0
--- /dev/null
+++ b/src/fc_interfaces/srv/MavPing.srv
@@ -0,0 +1,9 @@
+# Request
+uint8 target_sysid
+uint8 target_compid
+uint8 ping_seq
+---
+# Response
+bool success
+string message
+float32 rtt_ms
\ No newline at end of file
diff --git a/src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv b/src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv
new file mode 100644
index 0000000..bd58ca0
--- /dev/null
+++ b/src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv
@@ -0,0 +1,36 @@
+# Request
+uint8 target_sysid
+uint8 target_compid
+
+uint32 time_boot_ms # ms since system boot, 一般用 0 讓飛控自己填
+uint8 coordinate_frame # MAV_FRAME_*
+uint16 type_mask # POSITION_TARGET_TYPEMASK bitmask
+int32 lat_int # 1e7 * latitude (deg)
+int32 lon_int # 1e7 * longitude (deg)
+float32 alt # altitude (m)
+float32 vx # X velocity (m/s)
+float32 vy # Y velocity (m/s)
+float32 vz # Z velocity (m/s)
+float32 afx # X acceleration or force (m/s^2)
+float32 afy # Y acceleration or force (m/s^2)
+float32 afz # Z acceleration or force (m/s^2)
+float32 yaw # yaw (rad)
+float32 yaw_rate # yaw rate (rad/s)
+float32 timeout_sec # 等待 POSITION_TARGET_GLOBAL_INT 回應的時間上限
+---
+# Response
+string message
+uint8 ack_result
+uint32 r_time_boot_ms
+uint16 r_type_mask
+int32 r_lat_int
+int32 r_lon_int
+float32 r_alt
+float32 r_vx
+float32 r_vy
+float32 r_vz
+float32 r_afx
+float32 r_afy
+float32 r_afz
+float32 r_yaw
+float32 r_yaw_rate
\ No newline at end of file
diff --git a/src/fc_network_adapter/fc_network_adapter/__init__.py b/src/fc_network_adapter/fc_network_adapter/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/fc_network_adapter/fc_network_adapter/ackEnum.py b/src/fc_network_adapter/fc_network_adapter/ackEnum.py
new file mode 100644
index 0000000..aefd253
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/ackEnum.py
@@ -0,0 +1,31 @@
+
+'''
+單獨出來定義回傳碼的意義
+若跟飛控韌體有正常的通訊 (無論是否被拒絕) 則以 mavlink 定義的結果回傳
+若跟飛控韌體無法通訊 才使用這邊的錯誤代碼
+所以要避開重疊的號碼
+
+這邊備註 Mavlink 的 MAV_RESULT 參數意義
+0 : 封包正常接受並執行
+1 : 封包正常接受 但是正在忙不執行 需要等待後重試
+4 : 封包正常接受 但是載具的狀態異常 不執行
+
+2 : 封包不正常 參數可能給錯了
+3 : 封包不正常 根本認不出是啥
+
+'''
+
+
+from enum import IntEnum
+
+class serviceAckResult(IntEnum):
+ MAVLINK_TIMEOUT = 51 # mavlink communication timeout
+
+ MAVLINK_SEND_BUT_NO_EXP_STEAM = 30 # some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair
+
+ MAVLINK_BUSY = 53 # there already has a command being execution for this sysid, try later
+ MAVLINK_DEV_NOTFOUND = 54 # this sysid not fount
+
+ UNKNOWN = 90 # i did not capture this error, try read the logs
+
+
diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
new file mode 100644
index 0000000..dc87053
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
@@ -0,0 +1,231 @@
+
+這個檔案整理 此專案下 程式代碼的流程與思路
+只會挑出重要的變數與方法描述
+以利後續開發使用
+
+# 開發此專案的注意事項
+- 預設 autopilot 的 component id = 1
+- 不允許 system id 重複
+- 預設同一載具僅會使用相同的 socket
+- 增加一個固定數值監控然後要到 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 物件的狀態
+- *self.MAVLink* pymavlink 的一個裝置物件 模擬自己是某一個裝置 用以對於該 mavlink bus 負責封包發送
+---
+- *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 的功能 再做利用
+
+## mavlinkROS2Nodes.py
+ 這裡支持了所有 ROS2 框架的對應功能 包涵
+ 1. 將載具的串流更新的狀態更新到對應的 topic
+ 2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
+
+### **[Class]** fc_ros_manager
+ MAVLink ROS2 節點管理器 管理兩個獨立的 Node
+ 會開一個執行緒 讓兩個 Node 都跑在裡面
+ 然後用 spin_once 保持 Node 的活性
+- *self.status_publisher* 物件實例
+- *self.command_service* 物件實例
+- *self.spin_thread* 執行緒
+---
+- *start()* 啟動自己
+- *stop()* 停下自己
+- *shutdown()* 關閉自己並清理
+
+### **[Class]** VehicleStatusPublisher
+ 這整個組件都是自動的 目前沒有什麼需要 runtime 設定的地方
+- *self.fc_publishers* 儲存所有 publisher 的資料結構
+- *self.rate_controller* 儲存頻率參數的地方 也可以關掉某個 topic 的發佈
+---
+- *timer_callback()* 自動的抓出現有的 vehicle 中有的資訊 然後固定的將訊息丟到負責發佈的方法中
+- *_publish_vehicle_status()* 接 timer_callback 後去分配到各項發佈中
+- *_get_or_create_publisher()* 實際創建 topic 的地方
+- *_publish_XXX()* 各項發佈的方法
+
+### **[Class]** MavlinkCommandService
+ 提供 ros2 service 讓 serial_object 去丟出 mavlink 封包
+ 然後會從回應封包中挑出期待的回應 再傳給 ros2 request
+ 其中 PendingEntry 是很關鍵的東西
+ 它的每個物件 代表需要等待的封包種類與必需包含的內容
+- *self._pending_by_sysid* 儲存 PendingEntry 的地方
+---
+- *return_router()* 檢查並消耗 return_packet_ring 然後將封包分配到 pending 去
+- *__init__()* 這邊登入要創建的 service 到 ros2 系統
+- *handle_XXX()* 這邊是實現 service 的具體方法
+
+
+# 開發記錄
+
+## 已實現功能
+1. mavlink 分流解析
+2. mavlink socket 建立
+3. mavlink socket 轉拋 proxy
+4. 建立 Serial 轉 UDP 連結 並管理
+5. 建立 serial 連線
+6. 各單元模組化
+7. 終端機介面控制
+8. 基礎載具流量觀測
+9. 載具狀態收集與彙整
+10. a. ros2 topic 應用開發介面 (基礎框架)
+ b. ros2 service 應用開發介面 (基礎框架)
+
+### 待開發功能
+5-1. 建立 serial 連線 並可以對接收器下達AT指令
+5-2. 模組化 serial 連線機制 以利後期擴容其他模組
\ No newline at end of file
diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
new file mode 100644
index 0000000..84e89a9
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
@@ -0,0 +1,1620 @@
+
+'''
+
+主要調配流程的程式
+
+這個檔案包含 Terminal Utility Layer (TUL) 作為人機互動介面，並調用 mavlinkDevice 和 mavlinkObject 來處理 MAVLink 通訊和物件管理。
+
+'''
+
+import os
+import time
+import sys
+
+import curses
+import threading
+import queue
+import signal
+
+from pymavlink import mavutil
+
+# 自定義的 import
+from . import mavlinkObject as mo
+from . import serialManager as sm
+from . import mavlinkVehicleView as mvv
+from . import mavlinkROS2Nodes as mros
+
+from .utils import RingBuffer, setup_logger
+from .utils import acquireSerial, acquirePort
+from .utils.acquirePort import find_available_port
+
+logger = setup_logger(os.path.basename(__file__))
+PROJECT_VER = "v0.61"
+
+class PanelState:
+ def __init__(self):
+ self.panel_status = "Idle"
+ termination_start_time = None
+ self.mavlink_bridge_state = "Stopped"
+ self.object_manager_state = "Stopped"
+ self.serial_manager_state = "Stopped"
+ self.ros2_manager_state = "Stopped"
+ self.socket_object_list = [] # 已有的 mavlink object
+ self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string
+ self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息
+
+ # 關於創建通道時的暫存資訊
+ self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊
+ self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": False} # 暫存 Serial 設定資訊
+
+ # 關於顯示通道資訊
+ self.socket_info_single = {
+ "socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "",
+ "target_sockets": "", "primary_socket_id": "", "socket_connection_string": "",
+ "InfoReady": False} # 暫存單一 socket 的資訊
+ self.serial_info_single = {
+ "serial_port": "", "baudrate": "", "receiver_type": "", "target_port": "",
+ "InfoReady": False} # 暫存單一 serial 連結的資訊
+
+ # 關於顯示載具資訊
+ self.connected_vehicles_dict = {} # {(sysid, compid): {...基本資訊...}}
+ self.vehicle_info_single = {
+ "sysid": 0,
+ "compid": 0,
+ "vehicle_type": "",
+ "component_type": "",
+ "mav_autopilot": "",
+ "socket_id": None,
+ "connection_type": "",
+ "packet_stats": {},
+ "msg_type_counts": {},
+ "prev_stats": {}, # 用於計算變化率
+ "InfoReady": False
+ }
+
+ def intoSTART(self):
+ self.panel_status = "Running"
+
+ def intoTERMINATION(self):
+ self.termination_start_time = time.time()
+ self.panel_status = "Terminating"
+
+ def intoENGINEER(self):
+ self.panel_status = "Engineer"
+
+ def intoSTOPPED(self):
+ self.panel_status = "Stopped"
+
+ # def set_user_input(self, text):
+ # self.user_input = text
+
+class MenuNode:
+ def __init__(self, name, desc="", action=None, children=None):
+ self.name = name
+ self.desc = desc
+ self.action = action # 可以是函式或特殊字串
+ self.children = children or [] # 子選單列表
+
+class ControlPanel:
+ def __init__(self):
+ pass
+
+ def input_dialog(stdscr, prompt="請輸入文字: "):
+ """顯示輸入對話框"""
+ height, width = stdscr.getmaxyx()
+
+ # 建立輸入視窗
+ dialog_height = 5
+ dialog_width = min(60, width - 4)
+ start_y = (height - dialog_height) // 2
+ start_x = (width - dialog_width) // 2
+
+ # 建立視窗邊框
+ dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
+ dialog_win.border()
+ dialog_win.addstr(1, 2, prompt)
+ dialog_win.addstr(3, 2, "按 Enter 確認, ESC 取消")
+ dialog_win.refresh()
+
+ # 輸入區域
+ input_win = curses.newwin(1, dialog_width - 6, start_y + 2, start_x + 2)
+ input_win.keypad(True)
+
+ curses.echo()
+ curses.curs_set(1)
+
+ user_input = ""
+
+ while True:
+ input_win.clear()
+ input_win.addstr(0, 0, user_input[-dialog_width+8:]) # 顯示輸入內容（滾動）
+ input_win.refresh()
+
+ ch = input_win.getch()
+
+ if ch == 27: # ESC
+ user_input = None
+ break
+ elif ch in (curses.KEY_ENTER, 10, 13): # Enter
+ break
+ elif ch in (curses.KEY_BACKSPACE, 127, 8): # Backspace
+ user_input = user_input[:-1]
+ elif 32 <= ch <= 126: # 可打印字符
+ user_input += chr(ch)
+
+ curses.noecho()
+ curses.curs_set(0)
+
+ # 清理視窗
+ del input_win
+ del dialog_win
+ stdscr.clear()
+ stdscr.refresh()
+
+ return user_input
+
+ # ================ 關於 主要選單 的部份 ===================
+
+ def menu_tree(self):
+ """建立多層選單結構"""
+ return MenuNode("Main Menu", children=[
+ MenuNode("MavLink Object", "UDP MavLink 通道選項", children=[
+ MenuNode("New+", children=[
+ MenuNode("UDP InBound", children=[
+ MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"),
+ MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"),
+ MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"),
+ ]),
+ MenuNode("UDP OutBound", children=[
+ MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"),
+ MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"),
+ MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"),
+ ]),
+ ]),
+ MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"),
+ ]),
+ MenuNode("Serial Manager", "Serial 連接埠選項", children=[
+ MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"),
+ MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"),
+ ]),
+ MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
+ MenuNode("Engineer Mode", "工程模式", children=[
+ MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"),
+ MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"),
+ MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"),
+ ]),
+ MenuNode("Shutdown", "關閉整個系統", children=[
+ MenuNode("Return", "繼續運行", "BACK"),
+ MenuNode("Confirm", "關閉系統", "QUIT"),
+ ]),
+ ])
+
+ def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event):
+ stdscr = None
+
+ def cleanup():
+ """清理 curses 狀態"""
+ if stdscr:
+ stdscr.keypad(False)
+ curses.nocbreak()
+ curses.echo()
+ curses.endwin()
+
+ def pre_panel_shutdown():
+ # 先關閉所有模組 再關閉面板
+ cmd_q.put("SHUTDOWN_BRIDGE")
+ cmd_q.put("SHUTDOWN_MANAGER")
+ cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
+
+ def draw_menu(screen):
+ nonlocal stdscr
+ stdscr = screen
+
+ curses.curs_set(0)
+ stdscr.nodelay(False) # 阻塞讀鍵
+ stdscr.keypad(True)
+
+ # 選單導航狀態
+ menu_stack = [self.menu_tree()] # 選單堆疊
+ idx_stack = [0] # 索引堆疊
+
+ state.intoSTART() # 設定狀態為運行中
+
+ while not stop_evt.is_set():
+
+ current_menu = menu_stack[-1]
+ current_idx = idx_stack[-1]
+
+ # 獲取終端機尺寸
+ height, width = stdscr.getmaxyx()
+ # 簡單暴力的限制視窗的大小
+ MIN_HEIGHT = (
+ 2 + # 邊界
+ 6 + # 狀態列 操作說明列 一個空白
+ 11+ # 最大選單 與 空白區
+ 5 # 訊息區域
+ )
+ if height < MIN_HEIGHT or width < 60:
+ logger.error("Terminal size too small for Control Panel.")
+ break
+
+ stdscr.clear()
+ stdscr.border()
+
+ # 更新模組狀態顯示
+ stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD)
+ stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}")
+ stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}")
+ stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}")
+ stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}")
+ stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}")
+ stdscr.addstr(3, 36, f"ROS2 Manager State : {state.ros2_manager_state}")
+
+ # 顯示當前選單項目
+ start_line = 6
+ for i, child in enumerate(current_menu.children):
+ marker = "➤ " if i == current_idx else " "
+ # 動態顯示已輸入的值
+ desc = child.desc
+ if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]:
+ desc = f"{child.desc} [{state.udp_info_temp['IP']}]"
+ elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]:
+ desc = f"{child.desc} [{state.udp_info_temp['Port']}]"
+ elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]:
+ desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]"
+ elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]:
+ desc = f"{child.desc} [{state.serial_info_temp['Baud']}]"
+ elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP":
+ link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No"
+ desc = f"{child.desc} [{link_status}]"
+
+ line = f"{marker}{child.name:15s} – {desc}"
+ attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL
+ stdscr.addstr(start_line + i, 4, line, attr)
+
+ # 顯示訊息區域
+ # info_start_line = start_line + len(current_menu.children) + 1
+ info_start_line = height - 8
+ max_msg_lines = 5 # 最多顯示 5 行訊息
+ current_time = time.time()
+
+ # 清理過時的訊息
+ state.panel_info_msg_list = [
+ (msg, timestamp) for msg, timestamp in state.panel_info_msg_list
+ if current_time - timestamp < 2.0 #秒數
+ ]
+
+ # 只顯示最新的 max_msg_lines 條訊息
+ display_msgs = state.panel_info_msg_list[-max_msg_lines:]
+
+ for i, msg_data in enumerate(display_msgs):
+ if info_start_line + i >= help_line - 1: # 避免超出邊界
+ break
+ msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data
+ # 截斷過長的訊息
+ max_msg_width = width - 6
+ if len(msg) > max_msg_width:
+ msg = msg[:max_msg_width-3] + "..."
+
+ stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD)
+
+
+
+ # 操作說明
+ # help_line = start_line + len(current_menu.children) + 2
+ help_line = height - 2
+ stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM)
+ stdscr.addstr(height-1 , width-12, f" {PROJECT_VER} ", curses.A_DIM)
+
+ stdscr.refresh()
+
+ # 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作
+ # 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈
+ # 超過幾秒沒有反應就強制關閉
+ if state.panel_status == "Terminating":
+ if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點
+ logger.warning("Control Panel forced shutdown after timeout.")
+ state.intoSTOPPED()
+ # stop_evt.set()
+ # continue
+ break
+ time.sleep(0.1)
+ if (state.mavlink_bridge_state == "Stopped" and
+ state.object_manager_state == "Stopped" and
+ state.serial_manager_state == "Stopped"):
+ state.intoSTOPPED()
+ # stop_evt.set()
+ break
+ continue
+
+ # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt
+ stdscr.timeout(100)
+ ch = stdscr.getch()
+
+ if ch == -1: # 沒有操作
+ continue
+
+ # 處理按鍵
+ if ch in (curses.KEY_UP, ord('k')):
+ idx_stack[-1] = (current_idx - 1) % len(current_menu.children)
+
+ elif ch in (curses.KEY_DOWN, ord('j')):
+ idx_stack[-1] = (current_idx + 1) % len(current_menu.children)
+
+ elif ch == (ord('O')):
+ # 進入工程模式
+ state.intoENGINEER()
+
+ elif ch == (ord('o')):
+ # 離開工程模式
+ state.intoSTART()
+
+ elif ch == curses.KEY_LEFT:
+ # 返回上層
+ if len(menu_stack) > 1:
+ menu_stack.pop()
+ idx_stack.pop()
+
+ elif ch == curses.KEY_RIGHT:
+ # 進入下層 (但不執行動作)
+ selected = current_menu.children[current_idx]
+ if selected.children: # 有子選單
+ menu_stack.append(selected)
+ idx_stack.append(0)
+
+ elif ch in (ord('q'), 27):
+ if state.panel_status == "Engineer":
+ state.intoTERMINATION()
+ pre_panel_shutdown()
+
+ elif ch in (curses.KEY_ENTER, 10, 13):
+ selected = current_menu.children[current_idx]
+
+ # 處理不同類型的動作
+ if selected.children: # 有子選單
+ menu_stack.append(selected)
+ idx_stack.append(0)
+
+ elif selected.action == "BACK":
+ if len(menu_stack) > 1:
+ menu_stack.pop()
+ idx_stack.pop()
+
+ elif selected.action == "QUIT":
+ state.intoTERMINATION()
+ pre_panel_shutdown()
+
+ elif selected.action == "TEXT_UDP_IP":
+ result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
+ if result is not None:
+ state.udp_info_temp["IP"] = result
+
+ elif selected.action == "TEXT_UDP_PORT":
+ result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ")
+ if result is not None:
+ state.udp_info_temp["Port"] = result
+
+ elif selected.action == "CREATE_UDP_INBOUND":
+ cmd_q.put("CREATE_UDP_INBOUND")
+ # 確認後回到上兩層
+ if len(menu_stack) > 1:
+ menu_stack.pop()
+ idx_stack.pop()
+ # menu_stack.pop()
+ # idx_stack.pop()
+
+ elif selected.action == "CREATE_UDP_OUTBOUND":
+ cmd_q.put("CREATE_UDP_OUTBOUND")
+ # 確認後回到上兩層
+ if len(menu_stack) > 1:
+ menu_stack.pop()
+ idx_stack.pop()
+ # menu_stack.pop()
+ # idx_stack.pop()
+
+ elif selected.action == "TEXT_BAUD_SERIAL":
+ result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ")
+ if result is not None:
+ try:
+ baud_rate = int(result)
+ except ValueError:
+ state.panel_info_msg_list.append(("Invalid Baud Rate input.", time.time()))
+ state.serial_info_temp["Baud"] = baud_rate
+
+ elif selected.action == "SET_SERIAL_COMM_XBEE":
+ state.serial_info_temp["CommunicationType"] = "XBee(API-AT)"
+ menu_stack.pop()
+ idx_stack.pop()
+ elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
+ state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)"
+ menu_stack.pop()
+ idx_stack.pop()
+
+ elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES":
+ state.serial_info_temp["Go2Middleware"] = True
+ menu_stack.pop()
+ idx_stack.pop()
+ elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO":
+ state.serial_info_temp["Go2Middleware"] = False
+ menu_stack.pop()
+ idx_stack.pop()
+
+ elif selected.action == "CREATE_SERIAL_PORT":
+ state.serial_info_temp["Port"] = menu_stack[-1].name # 從選單取得 Port 名稱
+ cmd_q.put("CREATE_SERIAL_PORT")
+ # 確認後回到上兩層
+ if len(menu_stack) > 1:
+ menu_stack.pop()
+ idx_stack.pop()
+ menu_stack.pop()
+ idx_stack.pop()
+
+ elif selected.action == "LIST_SERIAL_RES":
+ created_list_menu = self.create_serial_port_menu(state, page=0)
+ menu_stack.append(created_list_menu)
+ idx_stack.append(0)
+
+ elif selected.action == "LIST_SERIAL_LINKS":
+ created_list_menu = self.create_linked_serial_menu(state, page=0)
+ menu_stack.append(created_list_menu)
+ idx_stack.append(0)
+
+ elif selected.action == "INSPECT_LINKED_SERIAL":
+ # 顯示 Serial 連結詳細資訊
+ if hasattr(selected, 'serial_id'):
+ cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id))
+ self.show_linked_serial_info(stdscr, selected.serial_id, state)
+
+ elif selected.action == "REMOVE_LINKED_SERIAL":
+ # 移除 Serial 連結
+ if hasattr(selected, 'serial_id'):
+ cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id))
+ # 返回上層（回到列表）
+ if len(menu_stack) > 1:
+ menu_stack.pop()
+ idx_stack.pop()
+ # 一樣退兩層
+ menu_stack.pop()
+ idx_stack.pop()
+
+ elif selected.action == "LIST_MAV_OBJECT":
+ # 動態生成 mavlink_object 列表選單
+ created_list_menu = self.create_object_list_menu(state, page=0)
+ menu_stack.append(created_list_menu)
+ idx_stack.append(0)
+
+ elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
+ if hasattr(selected, 'page'):
+ current_list_menu = menu_stack[-1]
+ menu_stack.pop()
+ idx_stack.pop()
+
+ # 依據選單種類 重新建立分頁
+ if current_list_menu.name == "Serial Port List":
+ created_list_menu = self.create_serial_port_menu(state, page=selected.page)
+ elif current_list_menu.name == "Object List":
+ created_list_menu = self.create_object_list_menu(state, page=selected.page)
+ elif current_list_menu.name == "Linked Serial List":
+ created_list_menu = self.create_linked_serial_menu(state, page=selected.page)
+ elif current_list_menu.name == "Connected Vehicles":
+ created_list_menu = self.create_vehicles_list_menu(state, page=selected.page)
+ else:
+ # 不支援的選單類型，回到原本的選單
+ menu_stack.append(current_list_menu)
+ idx_stack.append(0)
+ continue
+
+ menu_stack.append(created_list_menu)
+ idx_stack.append(0)
+
+ elif selected.action == "INSPECT_MAV_OBJECT":
+ # 顯示物件詳細資訊
+ if hasattr(selected, 'socket_id'):
+ cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id))
+ self.show_object_info(stdscr, selected.socket_id, state)
+
+ elif selected.action == "REMOVE_MAV_OBJECT":
+ # 移除物件
+ if hasattr(selected, 'socket_id'):
+ cmd_q.put(("REMOVE_OBJECT", selected.socket_id))
+ # 返回上層（回到列表）
+ if len(menu_stack) > 1:
+ menu_stack.pop()
+ idx_stack.pop()
+ # 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好
+ menu_stack.pop()
+ idx_stack.pop()
+
+ elif selected.action == "MAVOBJ_MAKE_LINK":
+ # 建立轉發連結
+ if hasattr(selected, 'socket_id'):
+ target_id = self.select_target_socket(stdscr, selected.socket_id, state)
+ if target_id is not None:
+ cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
+ cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結
+
+ elif selected.action == "MAVOBJ_CANCEL_LINK":
+ # 取消轉發連結
+ if hasattr(selected, 'socket_id'):
+ target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True)
+ if target_id is not None:
+ cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id))
+ cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id)) # 雙向取消連結
+
+ elif selected.action == "MAVOBJ_ADD_TARGET":
+ # 添加目標端口
+ if state.panel_status != "Engineer":
+ state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+ continue # 只有在工程模式下才能操作
+ if hasattr(selected, 'socket_id'):
+ target_id = self.select_target_socket(stdscr, selected.socket_id, state)
+ if target_id is not None:
+ cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
+
+ elif selected.action == "STOP_MANAGER":
+ if state.panel_status != "Engineer":
+ state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+ continue # 只有在工程模式下才能操作
+ cmd_q.put("SHUTDOWN_MANAGER")
+
+ elif selected.action == "STOP_BRIDGE":
+ if state.panel_status != "Engineer":
+ state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+ continue # 只有在工程模式下才能操作
+ cmd_q.put("SHUTDOWN_BRIDGE")
+
+ elif selected.action == "STOP_SERIAL_MANAGER":
+ if state.panel_status != "Engineer":
+ state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
+ continue # 只有在工程模式下才能操作
+ cmd_q.put("SHUTDOWN_SERIAL_MANAGER")
+
+ elif selected.action == "INSPECT_VEHICLES":
+ # 進入載具檢視選單
+ cmd_q.put("UPDATE_VEHICLES_LIST")
+ created_list_menu = self.create_vehicles_list_menu(state, page=0)
+ menu_stack.append(created_list_menu)
+ idx_stack.append(0)
+
+ elif selected.action == "INSPECT_VEHICLE":
+ # 顯示載具詳細資訊
+ if hasattr(selected, 'sysid') and hasattr(selected, 'compid'):
+ cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid))
+ self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state)
+
+ elif callable(selected.action):
+ # 執行函式
+ cmd_q.put(selected.action)
+
+ try:
+ curses.wrapper(draw_menu)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ cleanup()
+
+ # ================ 關於 mavlink object 的部份 ===================
+
+ def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8):
+ """動態創建 mavlink_object 列表選單（支持分頁）"""
+ children = []
+
+ if not state.socket_object_list:
+ children.append(MenuNode("(Empty)", "目前沒有連結口", None))
+ else:
+ total_items = len(state.socket_object_list)
+ total_pages = (total_items + items_per_page - 1) // items_per_page
+ start_idx = page * items_per_page
+ end_idx = min(start_idx + items_per_page, total_items)
+
+ # 顯示當前頁的物件
+ for socket_id in state.socket_object_list[start_idx:end_idx]:
+ # 為每個 socket 創建子選單
+ obj_menu = MenuNode(f"Socket #{socket_id}", f"連結口 {socket_id}", None, children=[
+ MenuNode("Info", "查看詳細資訊", "INSPECT_MAV_OBJECT"),
+ MenuNode("Make Link", "建立轉發連結", "MAVOBJ_MAKE_LINK"),
+ MenuNode("Cancel Link", "取消轉發連結", "MAVOBJ_CANCEL_LINK"),
+ MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"),
+ MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"),
+ MenuNode("GoUp", "回到列表", "BACK"),
+ ])
+ # 將 socket_id 附加到每個子選單項目上
+ for child in obj_menu.children:
+ child.socket_id = socket_id
+ children.append(obj_menu)
+
+ # 添加分頁控制
+ if total_pages > 1:
+ children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
+ if page > 0:
+ prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE")
+ prev_node.page = page - 1
+ children.append(prev_node)
+ if page < total_pages - 1:
+ next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
+ next_node.page = page + 1
+ children.append(next_node)
+
+ children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
+ menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children)
+ menu.current_page = page
+ return menu
+
+ def show_object_info(self, stdscr, socket_id, state: PanelState):
+ """顯示物件詳細資訊的對話框"""
+
+ start = time.time()
+ while not state.socket_info_single.get('InfoReady', False):
+ # 太久沒有回應
+ if time.time() - start > 2:
+ state.panel_info_msg_list.append(("Fail! Socket Info NOT Aquire!", time.time()))
+ return
+ time.sleep(0.05) # 等待資訊準備好
+
+ height, width = stdscr.getmaxyx()
+ dialog_height = 15
+ dialog_width = min(70, width - 4)
+ start_y = (height - dialog_height) // 2
+ start_x = (width - dialog_width) // 2
+
+ dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
+ dialog_win.border()
+ dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD)
+
+ # 這裡顯示基本資訊
+ dialog_win.addstr(2, 2, f"Socket ID : {socket_id}")
+ dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}")
+ # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', '')))
+ dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}")
+ dialog_win.addstr(4, 30, f"{state.socket_info_single.get('socket_connection_string', '')}")
+ show_str = ",".join(map(str, state.socket_info_single.get('bridge_msg_types', '')))
+ dialog_win.addstr(5, 2, f"Bridge Pack : {show_str if show_str else 'N/A'}")
+ show_str = ",".join(map(str, state.socket_info_single.get('return_msg_types', '')))
+ dialog_win.addstr(6, 2, f"Return Pack : {show_str if show_str else 'N/A'}")
+ dialog_win.addstr(7, 2, f"Primary Socket ID: {state.socket_info_single.get('primary_socket_id', 'It Self')}")
+ show_str = ",".join(map(str, state.socket_info_single.get('target_sockets', '')))
+ dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}")
+
+ state.socket_info_single['InfoReady'] = False # 重置狀態以便下次使用
+
+ dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...")
+ dialog_win.refresh()
+
+ dialog_win.getch()
+ del dialog_win
+ stdscr.clear()
+ stdscr.refresh()
+
+ def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False):
+ """選擇目標 socket 的對話框"""
+ height, width = stdscr.getmaxyx()
+ dialog_height = min(15, len(state.socket_object_list) + 5)
+ dialog_width = min(50, width - 4)
+ start_y = (height - dialog_height) // 2
+ start_x = (width - dialog_width) // 2
+
+ dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
+ dialog_win.keypad(True)
+
+ title = "選擇要移除的目標" if remove_mode else "選擇轉發目標"
+ available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id]
+
+ if not available_sockets:
+ dialog_win.border()
+ dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD)
+ dialog_win.addstr(2, 2, "沒有可用的目標")
+ dialog_win.addstr(4, 2, "按任意鍵返回...")
+ dialog_win.refresh()
+ dialog_win.getch()
+ del dialog_win
+ stdscr.clear()
+ stdscr.refresh()
+ return None
+
+ selected_idx = 0
+
+ while True:
+ dialog_win.clear()
+ dialog_win.border()
+ dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD)
+
+ for i, socket_id in enumerate(available_sockets):
+ marker = "➤" if i == selected_idx else " "
+ attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL
+ dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr)
+
+ dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消")
+ dialog_win.refresh()
+
+ ch = dialog_win.getch()
+
+ if ch in (curses.KEY_UP, ord('k')):
+ selected_idx = (selected_idx - 1) % len(available_sockets)
+ elif ch in (curses.KEY_DOWN, ord('j')):
+ selected_idx = (selected_idx + 1) % len(available_sockets)
+ elif ch in (curses.KEY_ENTER, 10, 13):
+ result = available_sockets[selected_idx]
+ del dialog_win
+ stdscr.clear()
+ stdscr.refresh()
+ return result
+ elif ch == 27: # ESC
+ del dialog_win
+ stdscr.clear()
+ stdscr.refresh()
+ return None
+
+ # ================ 關於 serial link 的部份 ===================
+
+ def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8):
+ """動態創建 serial port 列表選單（支持分頁）"""
+ children = []
+
+ # 獲取可用的 Serial 連接埠列表
+ # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓
+ serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*'])
+
+ if not serial_ports:
+ children.append(MenuNode("(Empty)", "目前沒有串口設備", None))
+ else:
+ total_items = len(serial_ports)
+ total_pages = (total_items + items_per_page - 1) // items_per_page
+ start_idx = page * items_per_page
+ end_idx = min(start_idx + items_per_page, total_items)
+
+ # 顯示當前頁的串口
+ for port in serial_ports[start_idx:end_idx]:
+ port_menu = MenuNode(f"{port}", children=[
+ MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[
+ MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"),
+ # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
+ ]),
+ MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"),
+ MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[
+ MenuNode("Yes", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES"),
+ MenuNode("No", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO"),
+ ]),
+ MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"),
+ MenuNode("GoUp", "回到列表", "BACK"),
+ ])
+ # 將 port 附加到每個子選單項目上
+ for child in port_menu.children:
+ child.port = port
+ children.append(port_menu)
+
+ # 添加分頁控制
+ if total_pages > 1:
+ children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
+ if page > 0:
+ prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE")
+ prev_node.page = page - 1
+ children.append(prev_node)
+ if page < total_pages - 1:
+ next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
+ next_node.page = page + 1
+ children.append(next_node)
+
+ children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
+ menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children)
+ menu.current_page = page
+ return menu
+
+ def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8):
+ """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能"""
+ children = []
+
+ if not state.linked_serial_dict:
+ children.append(MenuNode("(Empty)", "目前沒有連結口", None))
+ else:
+ total_items = len(state.linked_serial_dict)
+ total_pages = (total_items + items_per_page - 1) // items_per_page
+ start_idx = page * items_per_page
+ end_idx = min(start_idx + items_per_page, total_items)
+
+ # 顯示當前頁的物件
+ linked_serial_id_list = list(state.linked_serial_dict.keys())
+ for serial_id in linked_serial_id_list[start_idx:end_idx]:
+ # 為每個 socket 創建子選單
+ obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[
+ MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"),
+ MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"),
+ # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"),
+ MenuNode("GoUp", "回到列表", "BACK"),
+ ])
+ # 將 serial_id 附加到每個子選單項目上
+ for child in obj_menu.children:
+ child.serial_id = serial_id
+ children.append(obj_menu)
+
+ # 添加分頁控制
+ if total_pages > 1:
+ children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
+ if page > 0:
+ prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE")
+ prev_node.page = page - 1
+ children.append(prev_node)
+ if page < total_pages - 1:
+ next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
+ next_node.page = page + 1
+ children.append(next_node)
+
+ children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
+ menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children)
+ menu.current_page = page
+ return menu
+
+ def show_linked_serial_info(self, stdscr, serial_id, state: PanelState):
+ """顯示 Serial 連結詳細資訊的對話框"""
+
+ start = time.time()
+ while not state.serial_info_single.get('InfoReady', False):
+ # 太久沒有回應
+ if time.time() - start > 2:
+ state.panel_info_msg_list.append(("Fail! Serial Info NOT Aquire!", time.time()))
+ return
+ time.sleep(0.05) # 等待資訊準備好
+
+ height, width = stdscr.getmaxyx()
+ dialog_height = 15
+ dialog_width = min(70, width - 4)
+ start_y = (height - dialog_height) // 2
+ start_x = (width - dialog_width) // 2
+
+ dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
+ dialog_win.border()
+ dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD)
+
+ # 從 linked_serial_dict 獲取資訊
+ serial_info = state.linked_serial_dict.get(serial_id, {})
+
+ if not serial_info:
+ dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊")
+ else:
+ # 顯示基本資訊
+ dialog_win.addstr(2, 2, f"Serial ID : {serial_id}")
+ dialog_win.addstr(3, 2, f"Serial Port : {state.serial_info_single.get('serial_port', 'N/A')}")
+ dialog_win.addstr(4, 2, f"Baudrate : {state.serial_info_single.get('baudrate', 'N/A')}")
+ dialog_win.addstr(5, 2, f"Communication : {state.serial_info_single.get('receiver_type', 'N/A')}")
+ dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}")
+ dialog_win.addstr(7, 2, f"Status : {state.serial_info_single.get('status', 'Running')}")
+
+ # 如果有額外資訊可以繼續添加
+ if 'thread_alive' in serial_info:
+ thread_status = "Alive" if serial_info['thread_alive'] else "Stopped"
+ dialog_win.addstr(8, 2, f"Thread Status : {thread_status}")
+
+ state.serial_info_single['InfoReady'] = False # 重置狀態以便下次使用
+
+ dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...")
+ dialog_win.refresh()
+
+ dialog_win.getch()
+ del dialog_win
+ stdscr.clear()
+ stdscr.refresh()
+
+ # ================ 關於載具檢視的部份 ===================
+
+ def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8):
+ """動態創建 已連線載具 列表選單（支持分頁）
+ 每個 vehicle-component 組合都是獨立的選單項目
+ """
+ children = []
+
+ if not state.connected_vehicles_dict:
+ children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None))
+ else:
+ total_items = len(state.connected_vehicles_dict)
+ total_pages = (total_items + items_per_page - 1) // items_per_page
+ start_idx = page * items_per_page
+ end_idx = min(start_idx + items_per_page, total_items)
+
+ # vehicle_id_list 現在是 (sysid, compid) 的元組列表
+ vehicle_comp_list = list(state.connected_vehicles_dict.keys())
+
+ # 顯示當前頁的物件
+ for sysid, compid in vehicle_comp_list[start_idx:end_idx]:
+ # 建立顯示名稱
+ display_name = f"Vehicle #{sysid} - Comp #{compid}"
+ desc = f"載具 {sysid} 組件 {compid}"
+
+ vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE")
+ # 將 sysid 和 compid 附加到選單項目上
+ vehicle_menu.sysid = sysid
+ vehicle_menu.compid = compid
+ children.append(vehicle_menu)
+
+ # 添加分頁控制
+ if total_pages > 1:
+ children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
+ if page > 0:
+ prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE")
+ prev_node.page = page - 1
+ children.append(prev_node)
+ if page < total_pages - 1:
+ next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
+ next_node.page = page + 1
+ children.append(next_node)
+
+ children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
+ menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children)
+ menu.current_page = page
+ return menu
+
+ def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState):
+ """顯示載具組件詳細資訊（動態更新，顯示變化率）"""
+
+ # 等待資訊準備
+ start = time.time()
+ while not state.vehicle_info_single.get('InfoReady', False):
+ if time.time() - start > 2:
+ state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time()))
+ return
+ time.sleep(0.05)
+
+ info = state.vehicle_info_single
+ height, width = stdscr.getmaxyx()
+ dialog_height = min(22, height - 4)
+ dialog_width = min(70, width - 4)
+ start_y = (height - dialog_height) // 2
+ start_x = (width - dialog_width) // 2
+
+ dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
+ dialog_win.nodelay(True) # 非阻塞模式，允許動態更新
+ dialog_win.keypad(True)
+
+ # MAV_TYPE 名稱對應
+ MAV_TYPE_NAMES = {
+ 0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter",
+ 4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover",
+ 12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera"
+ }
+
+ # 動態更新迴圈
+ last_update = time.time()
+ while True:
+ current_time = time.time()
+
+ # 每 1 秒重新請求資料
+ if current_time - last_update >= 1.0:
+ # 觸發資料更新（透過 Orchestrator）
+ cmd_q.put(("INSPECT_VEHICLE", sysid, compid))
+ # 等待新資料準備好
+ wait_start = time.time()
+ state.vehicle_info_single['InfoReady'] = False
+ while not state.vehicle_info_single.get('InfoReady', False):
+ if time.time() - wait_start > 0.5: # 最多等 0.5 秒
+ break
+ time.sleep(0.01)
+ # 更新 info 參照
+ info = state.vehicle_info_single
+ last_update = current_time
+
+ dialog_win.clear()
+ dialog_win.border()
+ dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD)
+
+ # === 基礎資訊 ===
+ dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE)
+ dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE)
+
+ # # MAV Type # 這個用不到
+ # mav_type = info.get('vehicle_type', 'N/A')
+ # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type)
+ # dialog_win.addstr(3, 2, f"MAV Type : {mav_type_str}")
+
+ # Component Type
+ dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}")
+
+ # Autopilot Type
+ if info.get('mav_autopilot') is not None:
+ dialog_win.addstr(4, 2, f"Autopilot : {info.get('mav_autopilot', 'N/A')}")
+
+ # Connection Info
+ dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}")
+ dialog_win.addstr(4, 32, f"Socket ID : #{info.get('socket_id', 'N/A')}")
+
+ # === 封包統計 ===
+ stats = info.get('packet_stats', {})
+ dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE)
+
+ received = stats.get('received', 0)
+ lost = stats.get('lost', 0)
+ loss_rate = stats.get('loss_rate', 0.0)
+ last_seq = stats.get('last_seq', 'N/A')
+
+ # 計算變化
+ received_delta = stats.get('received_delta', 0)
+ lost_delta = stats.get('lost_delta', 0)
+
+ # 顯示變化率
+ recv_str = f"{received:6d}"
+ if received_delta > 0:
+ recv_str += f" (+{received_delta}↑)"
+
+ lost_str = f"{lost:4d}"
+ if lost_delta > 0:
+ lost_str += f" (+{lost_delta}↑)"
+
+ dialog_win.addstr(8, 2, f"Received : {recv_str}")
+ dialog_win.addstr(8, 32, f"Lost : {lost_str}")
+ dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%")
+ dialog_win.addstr(9, 32, f"Last Seq : {last_seq}")
+
+ # 最後接收時間
+ last_msg_time = stats.get('last_msg_time')
+ if last_msg_time:
+ time_str = time.strftime("%H:%M:%S", time.localtime(last_msg_time))
+ elapsed = current_time - last_msg_time
+ dialog_win.addstr(10, 2, f"Last Time : {time_str}")
+ dialog_win.addstr(10, 32, f"Elapsed : {elapsed:.1f}s")
+ else:
+ dialog_win.addstr(10, 2, "Last Time : N/A")
+
+ # === 訊息類型分佈 ===
+ dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE)
+
+ msg_counts = info.get('msg_type_counts', {})
+
+ # MAVLink 訊息名稱對應（縮寫版本）
+ MSG_NAMES = {
+ 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU",
+ 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL",
+ 74: "VFR_HUD", 147: "BATT_ST"
+ }
+
+ # 顯示前 12 個最常見的訊息類型（兩列各 6 個）
+ msg_items = list(msg_counts.items())[:12]
+ line = 13
+ for i, (msg_id, count) in enumerate(msg_items):
+ msg_name = MSG_NAMES.get(msg_id, "???")
+ delta = stats.get(f'msg_delta_{msg_id}', 0)
+
+ # 格式化數據
+ if delta > 0:
+ data_str = f"{count}(+{delta}↑)"
+ else:
+ data_str = f"{count}"
+
+ # 格式化顯示：[ID]NAME DATA (ID固定3字符寬度，右對齊)
+ display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}"
+
+ # 左列(偶數索引)或右列(奇數索引)
+ col = 2 if i % 2 == 0 else 36
+ row = line + (i // 2)
+
+ if row >= dialog_height - 3: # 避免超出邊界
+ break
+
+ dialog_win.addstr(row, col, display_str)
+
+ # 確保跳過顯示區域
+ line = line + 6
+
+ dialog_win.addstr(dialog_height - 2, 2, "Press R: Reset Stats, C: Unregister, other key to return...")
+ dialog_win.refresh()
+
+ # 檢查是否有按鍵（非阻塞）
+ ch = dialog_win.getch()
+ if ch in (ord('R'), ord('r')):
+ cmd_q.put(("RESET_VEHICLE_STATISTICS", sysid, compid))
+ elif ch in (ord('C'), ord('c')):
+ cmd_q.put(("UNREGISTER_VEHICLE", sysid))
+ break
+ elif ch != -1: # 有按鍵則退出
+ break
+
+ # 短暫延遲
+ time.sleep(0.1)
+
+ state.vehicle_info_single['InfoReady'] = False
+ del dialog_win
+ stdscr.clear()
+ stdscr.refresh()
+
+
+
+class Orchestrator:
+ def __init__(self, stop_sig):
+ self.stop_evt = stop_sig # 外部操作去中斷 "面板" 執行緒的訊號 (內部自己停止的話不需要用這個)
+ self.occupied_ip_ports = {} # 紀錄已被佔用的 ip:port 組合 "ip str" : [port int, port int, ...]
+
+ self.vehicle_registry = mvv.vehicle_registry
+
+ # === 1) 面板部分的準備 ===
+ self.cmd_q = queue.Queue()
+ self.panelState = PanelState() # 面板的狀態儲存
+ self.cPanel = ControlPanel() # 面板的功能
+ self.panel_thread = None
+
+ # === 2) async_io_manager & mavlink_bridge 部分的準備 ===
+ mo.stream_bridge_ring.clear()
+ mo.return_packet_ring.clear()
+ self.manager = mo.async_io_manager()
+ self.bridge = mo.mavlink_bridge()
+
+ # === 3) serial_manager 部分的準備 ===
+ self.plumber = sm.serial_manager()
+
+ # === 4) ros 部分的準備 ===
+ self.fc_ros_manager = mros.ros2_manager
+ self.fc_ros_manager.initialize()
+ self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
+ 'position': 1.0,
+ 'position_ned': 1.0,
+ 'attitude': 1.0,
+ 'velocity': 0.0,
+ 'battery': 1.0,
+ 'vfr_hud': 1.0,
+ 'mode': 0.0,
+ 'summary': 1.0,
+ }
+
+ def engageWholeSystem(self):
+ """啟動整個系統"""
+ # === 1) 面板部分的啟動 ===
+ self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt))
+ self.panel_thread.start()
+
+ # === 2) async_io_manager & mavlink_bridge 部分的啟動 ===
+ self.manager.start()
+ self.bridge.start()
+
+ # === 3) serial_manager 部分的啟動 ===
+ self.plumber.start()
+
+ # === 4) ros 部分的啟動 ===
+ self.fc_ros_manager.start()
+
+ def mainLoop(self):
+ logger.info("Main orchestrator started <-")
+ try:
+ # while not self.stop_evt.is_set():
+ while self.panel_thread.is_alive():
+
+ # A. 更新模組狀態
+ if self.manager.running:
+ self.panelState.object_manager_state = 'Running'
+ else:
+ self.panelState.object_manager_state = 'Stopped'
+
+ socketid_list = self.manager.get_managed_objects()
+ self.panelState.socket_object_list = socketid_list
+
+ if self.bridge.running:
+ self.panelState.mavlink_bridge_state = 'Running'
+ else:
+ self.panelState.mavlink_bridge_state = 'Stopped'
+
+ if self.plumber.running:
+ self.panelState.serial_manager_state = 'Running'
+ else:
+ self.panelState.serial_manager_state = 'Stopped'
+
+ linked_serial_dict = self.plumber.get_serial_link()
+ self.panelState.linked_serial_dict = linked_serial_dict
+
+ if self.fc_ros_manager.running:
+ self.panelState.ros2_manager_state = 'Running'
+ else:
+ self.panelState.ros2_manager_state = 'Stopped'
+
+ # B. 更新載具列表（從 vehicle_registry 獲取）
+ self._update_vehicles_list()
+
+ # 取出面板丟過來的「動作」
+ try:
+ cmd = self.cmd_q.get_nowait()
+ if callable(cmd):
+ cmd() # 執行對應動作
+ elif isinstance(cmd, tuple):
+ # 處理多參數命令
+ action = cmd[0]
+ if action == "REMOVE_OBJECT":
+ socket_id = cmd[1]
+ # 先移除所有跟他關聯的 target sockets
+ for s_id in mo.mavlink_object.mavlinkObjects:
+ s_obj = mo.mavlink_object.mavlinkObjects[s_id]
+ if socket_id in s_obj.target_sockets:
+ self.remove_target_from_object(s_id, socket_id)
+ # 再移除該物件
+ self.delete_udp_object(socket_id)
+ elif action == "MAVOBJ_ADD_TARGET":
+ source_id, target_id = cmd[1], cmd[2]
+ self.add_target_to_object(source_id, target_id)
+ elif action == "MAVOBJ_REMOVE_TARGET":
+ source_id, target_id = cmd[1], cmd[2]
+ self.remove_target_from_object(source_id, target_id)
+ elif action == "INSPECT_MAV_OBJECT":
+ socket_id = cmd[1]
+ mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id, None)
+ if mav_obj:
+ self.panelState.socket_info_single["socket_type"] = mav_obj.socket_type
+ self.panelState.socket_info_single["socket_state"] = mav_obj.state.name
+ self.panelState.socket_info_single["bridge_msg_types"] = mav_obj.bridge_msg_types
+ self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types
+ self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id
+ self.panelState.socket_info_single["target_sockets"] = mav_obj.target_sockets
+ self.panelState.socket_info_single["socket_connection_string"] = mav_obj.mavlink_socket.address
+ self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好
+ elif action == "INSPECT_LINKED_SERIAL":
+ serial_id = cmd[1]
+ serial_obj = self.plumber.serial_objects.get(serial_id, None)
+ if serial_obj:
+ self.panelState.serial_info_single["serial_port"] = serial_obj.serial_port
+ self.panelState.serial_info_single["baudrate"] = serial_obj.baudrate
+ self.panelState.serial_info_single["receiver_type"] = serial_obj.receiver_type.name
+ self.panelState.serial_info_single["target_port"] = serial_obj.target_port
+ self.panelState.serial_info_single["InfoReady"] = True # 標記資訊已準備好
+ elif action == "REMOVE_LINKED_SERIAL":
+ serial_id = cmd[1]
+ self.plumber.remove_serial_link(serial_id)
+
+ elif action == "INSPECT_VEHICLE":
+ sysid, compid = cmd[1], cmd[2]
+ self._prepare_vehicle_info(sysid, compid)
+ # elif action == "UPDATE_VEHICLES_LIST": # TODO 這個擺這邊 不知道為何可以有作用 先不動 也許後面有bug?
+ # logger.debug("Orchestrator: Updating vehicles list upon request")
+ # self._update_vehicles_list()
+ elif action == "RESET_VEHICLE_STATISTICS":
+ sysid, compid = cmd[1], cmd[2]
+ vehicle_sys = self.vehicle_registry.get(sysid)
+ vehicle_sys.reset_component_stats(compid)
+ elif action == "UNREGISTER_VEHICLE":
+ sysid = cmd[1]
+ self.vehicle_registry.unregister(sysid)
+
+ elif cmd == "CREATE_UDP_INBOUND":
+ self.panelState.udp_info_temp["direction"] = "inbound"
+ self.create_udp_object()
+ elif cmd == "CREATE_UDP_OUTBOUND":
+ self.panelState.udp_info_temp["direction"] = "outbound"
+ self.create_udp_object()
+ elif cmd == "CREATE_SERIAL_PORT":
+ self.create_serial_port_object()
+ elif cmd == "SHUTDOWN_BRIDGE":
+ self.bridge.stop()
+ elif cmd == "SHUTDOWN_MANAGER":
+ self.manager.shutdown()
+ elif cmd == "SHUTDOWN_SERIAL_MANAGER":
+ self.plumber.shutdown()
+
+ except queue.Empty:
+ pass
+ except Exception as e:
+ logger.error(f"Error processing command: {e}")
+
+ time.sleep(0.1)
+
+ except KeyboardInterrupt:
+ pass
+ except Exception as e:
+ logger.error(f"Unexpected error in main loop: {e}")
+ finally:
+
+ # 驗證並確保所有模組都被下達關閉訊號
+ # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發
+ if self.bridge.thread.is_alive():
+ if self.bridge.running:
+ self.bridge.stop()
+ self.bridge.thread.join(timeout=2)
+
+ if self.manager.thread.is_alive():
+ if self.manager.running:
+ self.manager.shutdown()
+ self.manager.thread.join(timeout=2)
+
+ if self.plumber.thread.is_alive():
+ if self.plumber.running:
+ self.plumber.shutdown()
+ self.plumber.thread.join(timeout=2)
+
+ if self.fc_ros_manager.spin_thread.is_alive():
+ if self.fc_ros_manager.running:
+ self.fc_ros_manager.shutdown()
+ self.fc_ros_manager.spin_thread.join(timeout=2)
+
+ # 關閉面板執行緒
+ if self.panel_thread.is_alive():
+ self.panel_thread.join(timeout=2)
+
+ time.sleep(0.5) # 等待各模組穩定關閉
+
+ logger.info("Main orchestrator END!")
+
+ # =============== 面板動作 - Mavlink Object ===============
+
+ def create_udp_object(self, socket_type:str = ""):
+ # 監聽部分
+ if self.panelState.udp_info_temp["direction"] == "inbound":
+ connection_string = f"udp:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}"
+ # 監聽的 port 要先檢查是否被佔用
+ ip = self.panelState.udp_info_temp['IP']
+ port = int(self.panelState.udp_info_temp['Port'])
+ port_check_list = self.occupied_ip_ports.get(ip, []) + self.occupied_ip_ports.get("0.0.0.0", [])
+ if port in port_check_list:
+ self.panelState.panel_info_msg_list.append((f"Failed! Port {port} on IP {ip} occupied.", time.time()-1))
+ return
+ # 再記錄被佔用的 port
+ if ip in self.occupied_ip_ports:
+ self.occupied_ip_ports[ip].append(port)
+ else:
+ self.occupied_ip_ports[ip] = [port]
+
+ # 外放資訊部分
+ elif self.panelState.udp_info_temp["direction"] == "outbound":
+ connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}"
+
+ try:
+ # 檢測這個 connection_string 是否能成功建立 mavlink 連結
+ mavlink_socket = mavutil.mavlink_connection(connection_string)
+ except Exception as e:
+ self.panelState.panel_info_msg_list.append((f"Failed to create UDP {self.panelState.udp_info_temp['direction']} object: {e}", time.time()-1))
+ return
+
+ # mavlink 連結建立成功 把他丟到 mavlink_object # 重點句
+ mavlink_object = mo.mavlink_object(mavlink_socket)
+ # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 # 重點句
+ self.manager.add_mavlink_object(mavlink_object)
+
+ # 設定一下 mavlink_object 的類型描述
+ if socket_type == "":
+ mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize()
+ else:
+ mavlink_object.socket_type = socket_type
+
+ self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time()))
+
+ def delete_udp_object(self, socket_id):
+ """移除指定的 mavlink_object"""
+
+ mavlink_obj = mo.mavlink_object.mavlinkObjects[socket_id]
+ connection_string = mavlink_obj.mavlink_socket.address
+ strings = connection_string.split(':')
+ ip = strings[0]
+ port = int(strings[1])
+ self.occupied_ip_ports[ip].remove(port)
+
+ self.manager.remove_mavlink_object(socket_id)
+
+ def add_target_to_object(self, source_id, target_id):
+ """為 mavlink_object 添加轉發目標"""
+ if source_id in mo.mavlink_object.mavlinkObjects:
+ source_obj = mo.mavlink_object.mavlinkObjects[source_id]
+ else:
+ self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time()))
+ return
+
+ if source_obj.add_target_socket(target_id):
+ self.panelState.panel_info_msg_list.append((f"Added target {target_id} to socket {source_id}", time.time()))
+ else:
+ self.panelState.panel_info_msg_list.append((f"Fail Adding target {target_id} to socket {source_id}", time.time()))
+
+ def remove_target_from_object(self, source_id, target_id):
+ """從 mavlink_object 移除轉發目標"""
+ if source_id in mo.mavlink_object.mavlinkObjects:
+ source_obj = mo.mavlink_object.mavlinkObjects[source_id]
+ else:
+ self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time()))
+ return
+
+ if source_obj.remove_target_socket(target_id):
+ self.panelState.panel_info_msg_list.append((f"Removed target {target_id} from socket {source_id}", time.time()))
+ else:
+ self.panelState.panel_info_msg_list.append((f"Fail Removing target {target_id} from socket {source_id}", time.time()))
+
+ # =============== 面板動作 - Vehicle Inspector ===============
+
+ def _update_vehicles_list(self):
+ """更新已連線載具列表（從 vehicle_registry 獲取）"""
+ vehicles_dict = {}
+
+ # 從 vehicle_registry 獲取所有載具
+ all_vehicles = self.vehicle_registry.get_all()
+
+ for sysid, vehicle in all_vehicles.items():
+ # 遍歷每個載具的所有組件
+ for compid, component in vehicle.components.items():
+ # 使用 (sysid, compid) 作為 key
+ vehicles_dict[(sysid, compid)] = {
+ 'sysid': sysid,
+ 'compid': compid,
+ 'vehicle_type': vehicle.vehicle_type,
+ 'component_type': component.type.value,
+ 'connection_via': vehicle.connected_via.value,
+ 'socket_id': vehicle.custom_meta.get('socket_id', 'N/A')
+ }
+
+ self.panelState.connected_vehicles_dict = vehicles_dict
+
+ def _prepare_vehicle_info(self, sysid, compid):
+ """準備載具組件的詳細資訊（包含變化率計算）"""
+ vehicle = self.vehicle_registry.get(sysid)
+ if not vehicle:
+ logger.warning(f"Vehicle {sysid} not found")
+ return
+
+ socket_id = vehicle.custom_meta.get('socket_id', 'N/A')
+
+ component = vehicle.get_component(compid)
+ if not component:
+ logger.warning(f"Component {compid} not found in vehicle {sysid}")
+ return
+
+ stats = component.packet_stats
+
+ # 取得之前的統計資料（用於計算變化）
+ prev_stats = self.panelState.vehicle_info_single.get('prev_stats', {})
+ prev_received = prev_stats.get('received', 0)
+ prev_lost = prev_stats.get('lost', 0)
+ prev_msg_counts = prev_stats.get('msg_counts', {})
+
+ # 計算變化率
+ received_delta = stats.received_count - prev_received
+ lost_delta = stats.lost_count - prev_lost
+
+ # 準備訊息類型計數（排序後取前幾個）
+ sorted_msg_counts = dict(sorted(
+ stats.msg_type_count.items(),
+ key=lambda x: x[1],
+ reverse=True
+ )[:12]) # 取前 12 個最常見的
+
+ # 計算每種訊息類型的變化
+ msg_deltas = {}
+ for msg_id, count in sorted_msg_counts.items():
+ prev_count = prev_msg_counts.get(msg_id, 0)
+ msg_deltas[f'msg_delta_{msg_id}'] = count - prev_count
+
+ # 更新 vehicle_info_single
+ socket_type = "N/A"
+ socket_obj =mo.mavlink_object.mavlinkObjects.get(socket_id, None)
+ if socket_obj:
+ socket_type = socket_obj.socket_type
+
+ self.panelState.vehicle_info_single = {
+ "sysid": sysid,
+ "compid": compid,
+ # "vehicle_type": vehicle.vehicle_type, # 這個用不到
+ "component_type": component.type.value,
+ "mav_autopilot": component.mav_autopilot,
+ "socket_id": socket_id,
+ "connection_type": socket_type,
+ "packet_stats": {
+ "received": stats.received_count,
+ "lost": stats.lost_count,
+ "loss_rate": (stats.lost_count / stats.received_count * 100
+ if stats.received_count > 0 else 0),
+ "last_seq": stats.last_seq,
+ "last_msg_time": stats.last_msg_time,
+ "received_delta": received_delta,
+ "lost_delta": lost_delta,
+ **msg_deltas # 展開訊息類型的變化
+ },
+ "msg_type_counts": sorted_msg_counts,
+ "prev_stats": { # 保存當前數據用於下次計算變化
+ "received": stats.received_count,
+ "lost": stats.lost_count,
+ "msg_counts": dict(stats.msg_type_count)
+ },
+ "InfoReady": True
+ }
+
+ # =============== 面板動作 - Serial Manager ===============
+
+ def create_serial_port_object(self):
+ # 先檢查是否已有相同的 Serial Port 被建立
+ serial_port_strs = self.panelState.linked_serial_dict.values() # linked_serial_dict 會在上面的 mainLoop 被不斷更新
+ if self.panelState.serial_info_temp['Port'] in serial_port_strs:
+ self.panelState.panel_info_msg_list.append(
+ (f"Fail! Serial Port {self.panelState.serial_info_temp['Port']} already linked.", time.time())
+ )
+ return
+
+ # 獲取可用的 udp port
+ udp_port_tmp = find_available_port(19000, 20000)
+
+ # 定義通訊類型映射表
+ COMM_TYPE_MAP = {
+ "XBee(API-AT)": sm.SerialReceiverType.XBEEAPI2AT,
+ # "XBee(AT-AT)": sm.SerialReceiverType.TELEMETRY, # TODO: 之後再弄
+ # 新增區
+ }
+
+ # 驗證輸入
+ comm_type = self.panelState.serial_info_temp['CommunicationType']
+ if not comm_type:
+ self.panelState.panel_info_msg_list.append(
+ ("Please select Communication Type first.", time.time())
+ )
+ return
+
+ # 查找對應的通訊類型
+ comm_type_tmp = COMM_TYPE_MAP.get(comm_type)
+ if comm_type_tmp is None:
+ self.panelState.panel_info_msg_list.append(
+ (f"Communication type '{comm_type}' not supported yet.", time.time())
+ )
+ return
+
+ ret = self.plumber.create_serial_link(
+ serial_port=self.panelState.serial_info_temp['Port'],
+ baudrate=self.panelState.serial_info_temp['Baud'],
+ target_port=udp_port_tmp,
+ receiver_type=comm_type_tmp,
+ )
+
+ if not ret:
+ self.panelState.panel_info_msg_list.append((f"Failed to create Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time()))
+ return
+
+ self.panelState.panel_info_msg_list.append((f"Created Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time()))
+
+ # 自動建立對應的 UDP 監聽端口
+ if self.panelState.serial_info_temp['Go2Middleware']:
+ self.panelState.udp_info_temp['IP'] = "127.0.0.1"
+ self.panelState.udp_info_temp['Port'] = str(udp_port_tmp)
+ self.panelState.udp_info_temp['direction'] = "inbound"
+ self.create_udp_object("SERIAL_XBEE")
+
+
+def main():
+
+ # =========== 各項模組的版本先驗 ===========
+ # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
+ version_check = True
+ if mo.MODULE_VER != "1.50":
+ print("Module Version Error! : mavlinkObkect")
+ version_check = False
+ if mros.MODULE_VER != "1.50":
+ print("Module Version Error! : mavlinkROS2Nodes")
+ version_check = False
+ if mvv.MODULE_VER != "1.00":
+ print("Module Version Error! : mavlinkVehicleView")
+ version_check = False
+ if sm.MODULE_VER != "0.60":
+ print("Module Version Error! : serialManager")
+ version_check = False
+ if version_check == False:
+ print("Environment Obstacle! Check YOUR Execution System Path First!!")
+ return
+ # ========================================
+
+ stop_evt = threading.Event()
+ def signal_handler(signum, frame):
+ """處理 Ctrl+C 信號 藉此訊號 會結束下面的 while 迴圈 並逐步關閉各模組"""
+ stop_evt.set()
+
+ # 註冊信號處理器
+ signal.signal(signal.SIGINT, signal_handler)
+ signal.signal(signal.SIGTERM, signal_handler)
+
+ orchestrator = Orchestrator(stop_evt)
+ orchestrator.engageWholeSystem()
+ orchestrator.mainLoop() # 程式會 block 在這邊 直到收到中斷信號
+
+if __name__ == "__main__":
+ main()
+
+
+'''
+================= 改版記錄 ============================
+
+2025.12.23
+1. 新增 serial 通道的資訊顯示完整化
+2. 新增 serial 通道刪除功能
+3. 新增 serial 直接順便開 ip object
+4. 修改 避免 serial 與 ip port 重複建立相同的通道
+5. 修改 show_object_info 與 show_linked_serial_info 改變檢核 Ready 方式 避免卡死
+
+2025.01.16
+1. 新增 "移除載具" 與 "重置載具統計" 功能
+2. 修正 udp port 在移除後仍被記錄為佔用的問題
+3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式
+4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊
+5. 系統納入 mavlink ROS2 Manager 模組
+
+2025.04.13
+1. 加入各項模組的版本先驗 檢驗失敗就直接離開
+
+'''
diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
new file mode 100644
index 0000000..dd9515b
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
@@ -0,0 +1,826 @@
+'''
+這個檔案是對 udp 進來的 mavlink 訊息做 "分流" "轉拋" 的地方 (並不會做 "分析")
+概念上 把每個 udp 接口 視為一個獨立的 mavlink bus 針對 bus 來統籌管理
+
+主要的重點部分:
+1. stream_bridge_ring & return_packet_ring
+2. mavlink_object & async_io_manager
+3. mavlink_bridge
+
+
+stream_bridge_ring & return_packet_ring:
+ 這兩個 ring buffer 是用來做 mavlink 訊息的分流
+ stream_bridge_ring 這邊的資訊比較是給會固定更新的資訊 (例如 HEARTBEAT, ATTITUDE 之類的)
+ return_packet_ring 比較是處理指令後的回應封包 (例如 PARAM_VALUE, MISSION_ITEM 等等)
+
+mavlink_object:
+ 每個 mavlink bus 都會有一個 mavlink_object
+ 使用 asyncio 處理資料流 用 RingBuffer 來分配訊息
+ 內容中沒有獨立的執行緒 只有一個個 asyncio function 會被放到 async_io_manager 裡面執行
+
+ 關於分流與轉拋的具體實現是在 process_data 這個 asyncio function 裡面
+
+async_io_manager:
+ 首先它紀律並管理所有 mavlink_object 實例
+ 有自己一個獨立的執行緒 執行 asyncio loop (mavlink_object 裡面的 asyncio function 都會被放到這個 loop 裡面執行)
+
+mavlink_bridge:
+ 專門處理 stream_bridge_ring 裡面的訊息流
+ 會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖
+
+'''
+
+# 基礎功能的 import
+import os
+import signal
+import time
+import threading
+import asyncio
+from enum import Enum, auto
+from collections import deque
+# from typing import Dict, List, Optional, Set, Any, Tuple
+
+# mavlink 的 import
+from pymavlink import mavutil
+from pymavlink.dialects.v20 import common as mav_common
+from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
+
+# 自定義的 import
+from .mavlinkVehicleView import (
+ vehicle_registry,
+ VehicleView,
+ VehicleComponent,
+ ComponentType,
+ ConnectionType
+)
+from .utils import RingBuffer, setup_logger
+
+# ====================== 分割線 =====================
+
+logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "1.50"
+
+
+stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255)
+return_packet_ring = RingBuffer(capacity=256, buffer_id=254)
+
+# ====================== 分割線 =====================
+
+# 使用 vehicle_registry 來管理所有的載具視圖
+# vehicle_registry 是從 mavlinkVehicleView 導入的全域實例
+
+class mavlink_bridge:
+ '''
+ 這個 class 就是 固定串流橋接器
+ 是用來接收 mavlink 訊息 並進行橋接處理
+ 這個地方是針對 stream_bridge_ring 的資料做處理的
+ 記錄有 mavlink bus 上有那些 system id 和 component id
+ 為了每個 system id 都有一個對應的 device object
+
+ 此類別負責：
+ 1. 從 stream_bridge_ring 接收訊息
+ 2. 管理 mavlink_systems（device 和 component objects）
+ 3. 處理接收到的訊息並更新對應的 component 狀態
+ 4. 提供發送訊息的介面，將訊息路由到正確的 mavlink_object
+
+ ps. 此 class 為 singleton，只能有一個 instance
+ '''
+ _instance = None
+ _lock = threading.Lock() # 確保多線程安全
+
+ def __new__(cls, *args, **kwargs):
+ with cls._lock: # 確保多線程環境下只有一個實例被創建
+ if cls._instance is None:
+ cls._instance = super(mavlink_bridge, cls).__new__(cls)
+ return cls._instance
+
+ def __init__(self):
+ if not hasattr(self, "initialized"): # 防止重複初始化
+ self.initialized = True
+ self.thread = None
+ self.running = False
+
+ # 初始化訊息處理器字典 (msg_id -> handler_function)
+ self._init_message_handlers()
+ else:
+ logger.error('mavlink_bridge instance already exists. Do not create another one.')
+
+ def _init_message_handlers(self):
+ """初始化訊息處理器映射表，提高處理效率"""
+ self.message_handlers = {
+ 0: self._handle_heartbeat, # HEARTBEAT
+ 30: self._handle_attitude, # ATTITUDE
+ 32: self._handle_local_position, # LOCAL_POSITION_NED
+ 33: self._handle_global_position, # GLOBAL_POSITION_INT
+ 74: self._handle_vfr_hud, # VFR_HUD
+ 147: self._handle_battery_status, # BATTERY_STATUS
+ }
+
+ def start(self):
+ """啟動 mavlink_bridge 的運作"""
+ if not self.running:
+ self.running = True
+ self.thread = threading.Thread(target=self._run_thread, name="MavlinkBridge")
+ self.thread.start()
+ else:
+ logger.warning("mavlink_bridge is already running")
+
+ def stop(self):
+ """停止 mavlink_bridge 的運作"""
+ self.running = False
+ if self.thread and self.thread.is_alive():
+ self.thread.join(timeout=5.0)
+
+ # === Thread 區塊 ===
+ def _run_thread(self):
+ """主執行緒：從 stream_bridge_ring 接收並處理 mavlink 訊息"""
+ logger.info('mavlink_bridge started <-')
+
+ while self.running:
+ # 檢查是否有訊息
+ if stream_bridge_ring.is_empty():
+ time.sleep(0.001) # 避免忙碌等待
+ continue
+
+ # 取出訊息包：(socket_id, timestamp, mavlink_msg)
+ msg_pack = stream_bridge_ring.get()
+ socket_id, timestamp, msg = msg_pack[0], msg_pack[1], msg_pack[2]
+
+ # 解析訊息基本資訊
+ sysid = msg.get_srcSystem()
+ compid = msg.get_srcComponent()
+ msg_id = msg.get_msgId()
+
+ # 確保 VehicleView 存在
+ vehicle = vehicle_registry.get(sysid)
+ if vehicle is None:
+ vehicle = vehicle_registry.register(sysid)
+ # 存儲 socket_id 到自定義 meta 中
+ vehicle.custom_meta['socket_id'] = socket_id
+
+ # 確保 VehicleComponent 存在
+ component = vehicle.get_component(compid)
+ if component is None:
+ if msg_id == 0: # 只有透過 HEARTBEAT 才能創建新的 component
+ # 根據 mav_type 判斷 component 類型
+ comp_type = self._determine_component_type(msg.type)
+ component = vehicle.add_component(compid, comp_type)
+ component.mav_type = msg.type
+ component.mav_autopilot = msg.autopilot
+ else:
+ # component 不存在且非 HEARTBEAT，忽略此訊息
+ continue
+
+ # 使用處理器字典分發訊息處理
+ if msg_id in self.message_handlers:
+ try:
+ self.message_handlers[msg_id](vehicle, component, msg, timestamp)
+ except Exception as e:
+ logger.error(f'Error handling message type {msg_id}: {e}')
+ else:
+ logger.debug(f'Unhandled message type: {msg_id} / {msg.get_type()}')
+
+ logger.info('mavlink_bridge END!')
+
+ def _determine_component_type(self, mav_type: int) -> ComponentType:
+ """根據 MAV_TYPE 判斷組件類型"""
+ # MAV_TYPE 定義：
+ # 0=通用, 1=固定翼, 2=四旋翼, 3=直升機, 4=天線追蹤器, 5=GCS, 6=飛船,
+ # 26=雲台, 27=ADSB, 28=降落傘等
+ if mav_type == 6: # MAV_TYPE_GCS
+ return ComponentType.GCS
+ elif mav_type == 26: # MAV_TYPE_GIMBAL
+ return ComponentType.GIMBAL
+ elif mav_type == 30: # MAV_TYPE_CAMERA
+ return ComponentType.CAMERA
+ elif mav_type in [1, 2, 3, 4, 13, 14, 15, 19, 20, 21, 22]: # 各種飛行器類型
+ return ComponentType.AUTOPILOT
+ else:
+ return ComponentType.OTHER
+
+ # === 訊息處理器 ===
+ def _handle_heartbeat(self, vehicle, component, msg, timestamp):
+ """處理 HEARTBEAT 訊息 (msg_id: 0)"""
+ component.status.mode.base_mode = msg.base_mode
+ component.status.mode.custom_mode = msg.custom_mode
+ component.status.mode.mode_name = mavutil.mode_string_v10(msg)
+ component.status.mode.timestamp = timestamp
+ component.status.system_status = msg.system_status
+ component.status.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
+ # print("get mode:", mavutil.mode_string_v10(msg)) # debug
+
+ def _handle_attitude(self, vehicle, component, msg, timestamp):
+ """處理 ATTITUDE 訊息 (msg_id: 30)"""
+ component.status.attitude.roll = msg.roll
+ component.status.attitude.pitch = msg.pitch
+ component.status.attitude.yaw = msg.yaw
+ component.status.attitude.rollspeed = msg.rollspeed
+ component.status.attitude.pitchspeed = msg.pitchspeed
+ component.status.attitude.yawspeed = msg.yawspeed
+ component.status.attitude.timestamp = timestamp
+
+ def _handle_local_position(self, vehicle, component, msg, timestamp):
+ """處理 LOCAL_POSITION_NED 訊息 (msg_id: 32)"""
+ # LOCAL_POSITION_NED 提供相對位置資訊
+ component.status.position.relative_altitude = -msg.z # NED 座標系，z 為負表示高度
+ component.status.position.timestamp = timestamp
+ # 也可以存儲到 custom_status 中保留原始資料
+ component.status.custom_status['local_position'] = {
+ 'x': msg.x, 'y': msg.y, 'z': msg.z,
+ 'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz
+ }
+
+ def _handle_global_position(self, vehicle, component, msg, timestamp):
+ """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)"""
+ component.status.position.latitude = msg.lat / 1e7 # 轉換為度
+ component.status.position.longitude = msg.lon / 1e7 # 轉換為度
+ component.status.position.altitude = msg.alt / 1000.0 # 轉換為公尺
+ component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺
+ component.status.position.timestamp = timestamp
+
+ def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
+ """處理 VFR_HUD 訊息 (msg_id: 74)"""
+ component.status.vfr.airspeed = msg.airspeed
+ component.status.vfr.groundspeed = msg.groundspeed
+ component.status.vfr.heading = msg.heading
+ component.status.vfr.throttle = msg.throttle
+ component.status.vfr.climb = msg.climb
+ component.status.vfr.timestamp = timestamp
+
+ def _handle_battery_status(self, vehicle, component, msg, timestamp):
+ """處理 BATTERY_STATUS 訊息 (msg_id: 147)"""
+ # 計算電池總電壓（mV 轉 V）
+ if hasattr(msg, 'voltages') and msg.voltages[0] != 65535:
+ # voltages 是一個陣列，包含各個電池單元的電壓
+ total_voltage = sum(v for v in msg.voltages if v != 65535) / 1000.0
+ component.status.battery.voltage = total_voltage
+
+ component.status.battery.current = msg.current_battery / 100.0 if msg.current_battery != -1 else None # cA 轉 A
+ component.status.battery.remaining = msg.battery_remaining if msg.battery_remaining != -1 else None # 百分比
+ component.status.battery.temperature = msg.temperature / 100.0 if hasattr(msg, 'temperature') and msg.temperature != 32767 else None # 轉換為攝氏
+ component.status.battery.timestamp = timestamp
+
+ # === 訊息發送功能 ===
+ def send_message(self, message_bytes, target_sysid=None, target_socket_id=None, broadcast=False):
+ """
+ 發送訊息到指定的 mavlink_object
+
+ Args:
+ message_bytes: 準備好的 mavlink 封包 (bytes)
+ target_sysid: 目標系統 ID (可選，用於自動查找對應的 socket)
+ target_socket_id: 目標 socket ID (可選，直接指定)
+
+ Returns:
+ bool: 是否成功發送
+
+ 使用方式：
+ 1. broadcast: 廣播到所有活動的 mavlink_object
+ 2. 指定 target_socket_id：直接發送到該 socket
+ 3. 指定 target_sysid：自動查找該系統對應的 socket 並發送
+ """
+ if not isinstance(message_bytes, (bytes, bytearray)):
+ logger.error(f"Invalid message type: {type(message_bytes)}")
+ return False
+
+ # 情況 1: 廣播到所有活動的 mavlink_object
+ if broadcast:
+ success_count = 0
+ for socket_id, mav_obj in mavlink_object.mavlinkObjects.items():
+ if self._send_to_socket(message_bytes, socket_id):
+ success_count += 1
+
+ return success_count > 0
+
+ # 情況 2: 直接指定 socket_id
+ if target_socket_id is not None:
+ return self._send_to_socket(message_bytes, target_socket_id)
+
+ # 情況 3: 透過 sysid 查找對應的 socket
+ if target_sysid is not None:
+ vehicle = vehicle_registry.get(target_sysid)
+ if vehicle and 'socket_id' in vehicle.custom_meta:
+ socket_id = vehicle.custom_meta['socket_id']
+ return self._send_to_socket(message_bytes, socket_id)
+ else:
+ logger.warning(f"System ID {target_sysid} not found or no socket_id")
+ return False
+
+ logger.warning("No target specified for sending message. WTF ARE YOU DOING?")
+ return False # 若無指定任何目標，則返回失敗
+
+ def _send_to_socket(self, message_bytes, socket_id):
+ """
+ 將訊息發送到指定的 mavlink_object
+
+ Args:
+ message_bytes: mavlink 封包
+ socket_id: 目標 socket ID
+
+ Returns:
+ bool: 是否成功
+ """
+ if socket_id not in mavlink_object.mavlinkObjects:
+ logger.warning(f"mavlink_object {socket_id} not found")
+ return False
+
+ mav_obj = mavlink_object.mavlinkObjects[socket_id]
+ return mav_obj.message_put_queue(message_bytes)
+
+# 定義 mavlink_object 的狀態
+class MavlinkObjectState(Enum):
+ INIT = auto() # 初始化狀態
+ RUNNING = auto() # 運行中狀態
+ SHUTTINGDOWN = auto() # 關閉中狀態
+ ERROR = auto() # 錯誤狀態
+ CLOSED = auto() # 已關閉狀態
+
+class mavlink_object:
+ '''
+ 每個 mavlink bus 都會有一個 mavlink_object
+ 使用 asyncio 處理資料流 用 RingBuffer 來分配訊息
+ 直接透過 socket 寫出
+ '''
+ mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) }
+ socket_num = 0 # 用來記錄目前的 socket 數量
+
+ # 用來銜接 pymavlink 的 MavLink 實例的管道
+ class CapturingPipeline:
+ def __init__(self, target_deque):
+ self.target_deque = target_deque
+ self.last_data = b''
+ def write(self, data):
+ self.last_data = bytes(data)
+ # logger.debug(f'CapturingPipeline write data: {data.hex()}') # developer debug
+ # 把收到的資料 bytes 放到 target_deque
+ self.target_deque.append(data)
+ def seek(self, pos): pass
+ def tell(self): return 0
+
+ def __new__(cls, *args, **kwargs):
+ # 每創建一個實例 就將其添加到 mavlinkObjects 列表中
+ # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號
+ # 若序號無中斷 則 socket_id 往後加一 若序號有中斷 則填補最小的序號
+ # socket_id 從 0 開始
+ instance = super().__new__(cls)
+ socket_id = 0
+ for k in cls.mavlinkObjects.keys():
+ if k == socket_id:
+ socket_id += 1
+ else:
+ break
+ instance.socket_id = socket_id
+ cls.socket_num += 1
+ cls.mavlinkObjects[socket_id] = instance
+ return instance
+
+ def __init__(self, socket, socket_dialect = 'common'):
+ # 登入所需的 socket
+ self.mavlink_socket = socket
+
+ # 用於主線程發送消息的緩衝區
+ self.outgoing_msgs = deque()
+ self.mavlinkPipeline = self.CapturingPipeline(self.outgoing_msgs)
+
+ if socket_dialect == 'common':
+ self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
+
+ # 記錄訊息過濾類型 (可選)
+ self.bridge_msg_types = set([0, 30, 32, 33, 74, 147]) # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED
+ self.return_msg_types = set([])
+
+ # 轉發到別的 mavlink object 作為目標端口 的列表
+ self.target_sockets = set()
+
+ # 物件變數
+ self.task = None # Task reference
+ self.dirtyDataCount = 0 # 髒資料計數器
+ self.dirtyDataMax = 10 # 髒資料容許閾值
+
+ self.state = MavlinkObjectState.INIT
+ # logger.info(f'mavlink_object instance {self.socket_id} created') # 先註解掉避免太多 log 但是為了 debug 保留
+
+ # 附加參數 (並非 mavlink_object 運行本體必要 但是要給上層結構運用的)
+ # 若這個 socket 是 另一個"主要 socket"的備用連接 則設定為"主要 socket id"
+ self.primary_socket_id = None # None 表示不是備用連接
+ # socket type
+ self.socket_type = 'UNDEFINED' # 可選 'UDP_INBOUND', 'SERIAL_XBEE'...etc
+
+ def __del__(self):
+ # # 先移除其他 socket 指向這個 socket 的目標 # 還是不要在這邊做了 畢竟本來就有判斷 object 不活躍就不轉拋
+ # for mavlink_obj in self.mavlinkObjects.values():
+ # if self.socket_id in mavlink_obj.target_sockets:
+ # mavlink_obj.remove_target_socket(self.socket_id)
+
+ # 關閉 socket
+ if hasattr(self, 'mavlink_socket') and self.mavlink_socket:
+ try:
+ self.mavlink_socket.close()
+ except:
+ pass
+
+ # 處理 class 的 instance 記錄
+ if hasattr(self.__class__, 'socket_num'):
+ self.__class__.socket_num -= 1
+
+ if hasattr(self.__class__, 'mavlinkObjects') and hasattr(self, 'socket_id'):
+ if self.socket_id in self.__class__.mavlinkObjects:
+ self.__class__.mavlinkObjects.pop(self.socket_id)
+
+ # 這段不知道怎麼了 反正會一直讓 logger ERROR 我先關掉
+ # try:
+ # logger.info(f'mavlink_object instance {self.socket_id} deleted')
+ # except Exception as e:
+ # print(f"Error logging in __del__: {e}")
+
+ async def process_data(self):
+ """處理 mavlink 數據的主要 asyncio 協程"""
+ # logger.info(f'Start of mavlink_object id: {self.socket_id}') # 先註解掉避免太多 log 但是為了 debug 保留
+
+ while self.state == MavlinkObjectState.RUNNING:
+ timestamp = time.time()
+
+ # 處理接收到的封包
+ mavlinkMsg = self.mavlink_socket.recv_msg()
+
+ if mavlinkMsg:
+ # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間
+ sysid = mavlinkMsg.get_srcSystem()
+ compid = mavlinkMsg.get_srcComponent()
+
+ # 更新封包統計資訊
+ vehicle = vehicle_registry.get(sysid)
+ if vehicle: # 只有當這個 system id 已經被註冊過才會記錄統計
+ component = vehicle.get_component(compid)
+ if component:
+ component.update_packet_stats(
+ mavlinkMsg.get_seq(),
+ mavlinkMsg.get_msgId(),
+ timestamp
+ )
+
+ # 分發訊息到 RingBuffer
+ msg_id = mavlinkMsg.get_msgId()
+
+ if (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types):
+ stream_bridge_ring.put((self.socket_id, timestamp, mavlinkMsg))
+
+ if (msg_id in self.return_msg_types or -1 in self.return_msg_types):
+ return_packet_ring.put((self.socket_id, timestamp, mavlinkMsg))
+
+ # 將全部接收到的訊息轉發給目標列表中的 mavlink_object
+ for target_socket in self.target_sockets:
+ if target_socket in self.mavlinkObjects:
+ target_obj = self.mavlinkObjects[target_socket]
+ if target_obj.state == MavlinkObjectState.RUNNING:
+ target_obj.mavlink_socket.write(mavlinkMsg.get_msgbuf())
+
+ if self.outgoing_msgs:
+ send_msg = self.outgoing_msgs.popleft()
+ self.mavlink_socket.write(send_msg)
+
+
+ # 這邊的重點不是延遲 而是透過 await 讓出 event loop 的控制權
+ await asyncio.sleep(0.001)
+
+ logger.info(f'End of mavlink_object id: {self.socket_id}')
+ self.state = MavlinkObjectState.CLOSED
+
+ def add_target_socket(self, target_socket_id):
+ """添加一個目標端口用於轉發"""
+ if (target_socket_id != self.socket_id) and (target_socket_id != self.primary_socket_id):
+ if target_socket_id not in self.target_sockets:
+ self.target_sockets.add(target_socket_id)
+ logger.info(f"mavlink_object Added target port {target_socket_id} to mavlink_object {self.socket_id}")
+ return True
+ return False # 已存在
+ return False # 不能添加自己 也不能添加主要 socket
+
+ def remove_target_socket(self, target_socket_id):
+ """移除目標端口"""
+ if target_socket_id in self.target_sockets:
+ self.target_sockets.remove(target_socket_id)
+ logger.info(f"mavlink_object Removed target port {target_socket_id} from mavlink_object {self.socket_id}")
+ return True
+ return False # 不存在
+
+ def set_bridge_message_types(self, msg_types):
+ """設置需要分流到橋接器的訊息類型"""
+ if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types):
+ self.bridge_msg_types = set(msg_types)
+ return True
+ logger.error(f"Invalid bridge message types: {msg_types}")
+ return False
+
+ def set_return_message_types(self, msg_types):
+ """設置需要分流到回傳處理器的訊息類型"""
+ if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types):
+ self.return_msg_types = set(msg_types)
+ return True
+ logger.error(f"Invalid return message types: {msg_types}")
+ return False
+
+ def message_put_queue(self, message_bytes):
+ """
+ 從主線程向此 mavlink_object 的 socket 發送數據
+ 將數據添加到簡單的列表中，由 asyncio 任務處理
+
+ Args:
+ message_bytes: 要發送的 mavlink 消息的字節數據
+
+ Returns:
+ bool: 是否成功添加消息到列表
+ """
+ # 狀態檢查
+ if self.state != MavlinkObjectState.RUNNING:
+ logger.warning(f"Cannot send message: mavlink_object {self.socket_id} is not running")
+ return False
+
+ # 基本數據類型檢查（輕量級）
+ if not isinstance(message_bytes, (bytes, bytearray)):
+ logger.error(f"Invalid message type: expected bytes/bytearray, got {type(message_bytes)}")
+ return False
+
+ # 基本長度檢查（MAVLink v1.0 最小8字節，v2.0最小12字節）
+ if len(message_bytes) < 8:
+ logger.error(f"Message too short: {len(message_bytes)} bytes (minimum 8)")
+ return False
+
+ # MAVLink 起始標記檢查（輕量級）
+ if len(message_bytes) > 0 and message_bytes[0] not in (0xFE, 0xFD): # v1.0: 0xFE, v2.0: 0xFD
+ logger.error(f"Invalid MAVLink start marker: 0x{message_bytes[0]:02X}")
+ return False
+
+ # 緩衝區大小保護（防止記憶體耗盡）
+ if len(self.outgoing_msgs) > 1000: # 可調整的閾值
+ logger.warning(f"Outgoing message buffer full for mavlink_object {self.socket_id}")
+ return False
+
+ self.outgoing_msgs.append(message_bytes)
+ return True
+
+class async_io_manager:
+ """
+ 管理所有 mavlink_object 實例的 asyncio 任務
+ 提供單一線程來處理所有 mavlink 通道的數據
+
+ 首先 async_io_manager 是 singleton 的 所以只能有一個實例
+
+ 這個 async_io_manager 是藉由 start 方法來啟動的
+
+ start 方法 會先做一個新的執行緒 然後讓新的執行緒 透過 _run_event_loop 方法來建立一個空的事件循環 self.loop
+ 然後在 _run_event_loop 方法中 會建立一個異步任務 _main_task 來監控和管理所有的 mavlink_object 任務
+ """
+
+ _instance = None
+ _lock = threading.Lock()
+
+ def __new__(cls, *args, **kwargs):
+ with cls._lock:
+ if cls._instance is None:
+ cls._instance = super(async_io_manager, cls).__new__(cls)
+ return cls._instance
+
+ def __init__(self):
+ if not hasattr(self, 'initialized'):
+ self.initialized = True
+ self.loop = None
+ self.running = False
+ # self.main_task = None
+ self.thread = None
+
+ def __del__(self):
+ self.loop = None
+ self.thread = None
+
+ def start(self):
+ """
+ 啟動 async_io_manager
+
+ """
+ if self.running:
+ logger.warning("async_io_manager already running")
+ return
+
+ self.running = True
+
+ # 啟動獨立線程 命名為 AsyncIOManager
+ self.thread = threading.Thread(
+ target=self._run_event_loop,
+ name="AsyncIOManager"
+ )
+ self.thread.daemon = False # 不設為 daemon，確保正確關閉
+ self.thread.start()
+
+ # 等待 _run_event_loop 建立事件循環的物件 self.loop
+ start_timeout = 2.0
+ start_time = time.time()
+ while not self.loop and time.time() - start_time < start_timeout:
+ time.sleep(0.1)
+
+ # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop
+ if self.loop:
+ logger.info("async_io_manager thread started <-")
+ return True
+ else:
+ logger.error("async_io_manager failed to start")
+ return False
+
+ def shutdown(self):
+ """停止 async_io_manager 和其管理的所有 mavlink_object"""
+
+ # 自己在 running 狀態下才執行停止程序
+ if not self.running:
+ return
+
+ # 停止所有被管理的 mavlink_object 所屬的 task
+ for socket_id in list(mavlink_object.mavlinkObjects.keys()):
+ self.remove_mavlink_object(socket_id)
+
+ # 停止自己的 task
+ self.running = False
+
+ # 解開事件循環的阻塞
+ self.loop.call_soon_threadsafe(self.loop.stop)
+
+ # print("mark A", len(asyncio.all_tasks(self.loop))) # debug
+
+ # 等待線程結束
+ if self.thread and self.thread.is_alive():
+ self.thread.join(timeout=5.0)
+ if self.thread.is_alive():
+ logger.warning("async_io_manager thread did not stop gracefully")
+ os.kill(os.getpid(), signal.SIGTERM) # 強制終止程序
+
+ logger.info("async_io_manager thread END!")
+
+ def _run_event_loop(self):
+ """在獨立線程中運行事件循環"""
+ try:
+ self.loop = asyncio.new_event_loop()
+ asyncio.set_event_loop(self.loop)
+
+ logger.info("async_io_manager event loop started <-")
+
+ # 創建主任務
+ # main_task = self.loop.create_task(self._main_task())
+
+ # 運行事件循環
+ self.loop.run_forever()
+
+ except Exception as e:
+ logger.error(f"Error in async_io_manager event loop: {e}")
+ finally:
+ # 清理
+ if self.loop:
+ self.loop.close()
+ self.loop = None
+ self.running = False
+ logger.info("async_io_manager event loop END!")
+
+ def add_mavlink_object(self, mavlink_obj: mavlink_object):
+ """添加 mavlink_object"""
+ # 一個防呆 確保有 event loop 與 _main_task 正在運作
+ if not self.running or not self.loop:
+ logger.error("Cannot add mavlink_object: async_io_manager is not running")
+ return False
+
+ socket_id = mavlink_obj.socket_id
+
+ # 檢查該對象是否已經在運行中
+ if socket_id in mavlink_object.mavlinkObjects:
+ existing_obj = mavlink_object.mavlinkObjects[socket_id]
+ if existing_obj.state == MavlinkObjectState.RUNNING:
+ logger.warning(f"mavlink_object {socket_id} already managed")
+ return False
+
+ # 使用 run_coroutine_threadsafe 執行協程並獲取結果
+ future = asyncio.run_coroutine_threadsafe(
+ self._async_add_mavlink_object(mavlink_obj),
+ self.loop
+ )
+
+ try:
+ # 等待結果，設定合理的超時時間
+ result = future.result(timeout=3.0)
+ return result
+ except asyncio.TimeoutError:
+ logger.error(f"Timeout adding mavlink_object {socket_id}")
+ return False
+ except Exception as e:
+ logger.error(f"Failed to add mavlink_object {socket_id}: {e}")
+ return False
+
+ async def _async_add_mavlink_object(self, mavlink_obj):
+ """在事件循環線程中同步執行"""
+ socket_id = mavlink_obj.socket_id
+
+ try:
+ task = asyncio.create_task(mavlink_obj.process_data())
+ mavlink_obj.task = task
+ mavlink_obj.state = MavlinkObjectState.RUNNING
+ mavlink_obj.outgoing_msgs.clear()
+ logger.info(f"Added mavlink_object {socket_id} into manager.")
+ return True
+ except Exception as e:
+ logger.error(f"Failed to create task for mavlink_object {socket_id}: {e}")
+ return False
+
+ def remove_mavlink_object(self, socket_id: int):
+ """移除 mavlink_object"""
+
+ # 一個防呆 確保有 event loop 正在運作
+ if not self.loop:
+ return False
+
+ # 同樣使用 run_coroutine_threadsafe
+ future = asyncio.run_coroutine_threadsafe(
+ self._async_remove_mavlink_object(socket_id),
+ self.loop
+ )
+
+ try:
+ result = future.result(timeout=3.0)
+ return result
+ except asyncio.TimeoutError:
+ logger.error(f"Timeout removing mavlink_object {socket_id}")
+ return False
+ except Exception as e:
+ logger.error(f"Failed to remove mavlink_object {socket_id}: {e}")
+ return False
+
+ async def _async_remove_mavlink_object(self, socket_id):
+ """在事件循環線程中同步執行"""
+ if socket_id not in mavlink_object.mavlinkObjects:
+ logger.warning(f"mavlink_object {socket_id} not found")
+ return False
+
+ mavlink_obj = mavlink_object.mavlinkObjects[socket_id]
+ mavlink_obj.state = MavlinkObjectState.SHUTTINGDOWN
+
+ if not mavlink_obj.task.done():
+ mavlink_obj.task.cancel()
+
+ # 等待一秒或者 task完全結束
+ timeout = 1.0
+ start_time = asyncio.get_event_loop().time()
+ while not mavlink_obj.task.done():
+ if asyncio.get_event_loop().time() - start_time > timeout:
+ break
+ await asyncio.sleep(0.1)
+
+ # 如果正常結束 則設置為關閉狀態（物件的清理由 __del__ 處理）
+ if mavlink_obj.task.done():
+ mavlink_obj.state = MavlinkObjectState.CLOSED
+ logger.info(f"Removed mavlink_object {socket_id} from manager.")
+ return True
+ else:
+ mavlink_obj.state = MavlinkObjectState.ERROR
+ logger.warning(f"mavlink_object {socket_id} task did not terminate in time")
+ return False
+
+ def get_managed_objects(self):
+ """獲取所有被管理的對象列表（狀態為 RUNNING 的對象）"""
+ return [socket_id for socket_id, obj in mavlink_object.mavlinkObjects.items()
+ if obj.state == MavlinkObjectState.RUNNING]
+
+# ====================== 分割線 =====================
+
+if __name__ == '__main__':
+ pass
+
+
+'''
+================= 改版記錄 ============================
+
+2025年 6月 20日
+1. mavlink_object 中 由於 Queue 的效能太差 會完全移除
+ 其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現
+ 而 multiplexingToSwap 會完全被移除代替方式下一條描述
+2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流
+ (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object)
+ 因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移
+ 並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出
+3. mavlink_object 需要加上 state 去管理其狀態
+4. mavlink_object 需要加上 target port 去管理寫出的目標
+5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道
+6. 基於第1,2項 updateMultiplexingList 會被完全移除
+7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object
+8. 基於第1項全域變數 fixed_stream_bridge_queue return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除
+
+
+2025年 11月 15日
+1. mavlink_bridge 類別新增為 singleton 模式 確保全系統只有一個實例在運行
+2. mavlink_bridge 處理封包改為映射表 (需要在 _init_message_handlers 中新增處理器函式)
+3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包
+4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚
+5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期
+6. mavlink_object 優化 message_put_queue 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作
+7. 移除迴圈內的 try except 堆疊 增加效能
+8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具
+
+2026年 01月 15日
+1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects
+2. async_io_manager 的 _stop_event 無效變數移除
+
+'''
+
diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
new file mode 100644
index 0000000..89ab179
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
@@ -0,0 +1,1144 @@
+"""
+MAVLink ROS2 Nodes
+主要包含兩個獨立的 ROS2 Node :
+1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
+ 從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
+2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
+ 以最基礎的 mavlink microservice 會實現目標
+ 並不會包含額外的功能
+
+與一個節點管理器
+- fc_ros_manager
+
+"""
+
+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 nav_msgs.msg
+import mavros_msgs.msg
+
+# ROS2 Service imports
+import fc_interfaces.srv as fcsrv
+from .ackEnum import serviceAckResult
+
+# 自定義 imports
+from . import mavlinkVehicleView as mvv
+from . import mavlinkObject as mo
+from .utils import setup_logger
+
+logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "1.50"
+
+# ============================================================================
+# VehicleStatusPublisher Node
+# ============================================================================
+
+# 頻率控制器
+class PublishRateController:
+ """發布頻率控制器 - 按時間間隔控制發布頻率"""
+
+ def __init__(self):
+ # ═══════════════════════════════════════════════════════════════
+ # 【新增 Topic 位置 1/4】
+ # 若要新增 topic 種類，請在此字典中加入新的 topic 名稱和發布間隔
+ # 例如：'ekf_status': 1.0, # EKF 狀態
+ # ═══════════════════════════════════════════════════════════════
+ # 各 topic 的發布間隔（秒）
+ # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
+ # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
+ self.topic_intervals = {
+ 'position': 0.0, # GNSS位置
+ 'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度)
+ 'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態)
+ 'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除)
+ 'battery': 0.0, # 電池
+ 'vfr_hud': 0.0, # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
+ 'mode': 0.0, # 飛行模式 (已經在 summary 裡 未來移除)
+ 'summary': 0.0, # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
+ # 在這裡新增更多 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()
+
+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
+
+ # 高頻快路徑：讀取 registry 的 immutable 快照
+ for sysid, vehicle in mvv.vehicle_registry.read_all():
+ 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_position_ned(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_position_ned(self, sysid: int, status: mvv.ComponentStatus):
+ """發布 LOCAL_POSITION_NED（NED 座標，含位置與速度）"""
+ if not self.rate_controller.should_publish(sysid, 'position_ned'):
+ return
+
+ local_pos = status.custom_status.get('local_position')
+ if not local_pos:
+ return
+
+ required_keys = ('x', 'y', 'z', 'vx', 'vy', 'vz')
+ if any(local_pos.get(k) is None for k in required_keys):
+ return
+
+ publisher = self._get_or_create_publisher(
+ sysid, 'position_ned', nav_msgs.msg.Odometry
+ )
+
+ if publisher.get_subscription_count() == 0:
+ return
+
+ msg = nav_msgs.msg.Odometry()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.frame_id = 'local_ned'
+ msg.child_frame_id = 'base_link_ned'
+
+ # 保持 MAVLink LOCAL_POSITION_NED 座標定義，不進行 ENU 轉換
+ msg.pose.pose.position.x = float(local_pos['x'])
+ msg.pose.pose.position.y = float(local_pos['y'])
+ msg.pose.pose.position.z = float(local_pos['z'])
+
+ msg.twist.twist.linear.x = float(local_pos['vx'])
+ msg.twist.twist.linear.y = float(local_pos['vy'])
+ msg.twist.twist.linear.z = float(local_pos['vz'])
+
+ publisher.publish(msg)
+
+ def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
+ """發布姿態（IMU）"""
+ if not self.rate_controller.should_publish(sysid, 'attitude'):
+ return
+
+ att = status.attitude
+ if att.roll is None:
+ return
+ publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
+
+ if publisher.get_subscription_count() == 0:
+ return
+
+ msg = sensor_msgs.msg.Imu()
+
+ # 歐拉角轉四元數
+ qx, qy, qz, qw = self._euler_to_quaternion(
+ att.roll, att.pitch, att.yaw
+ )
+ msg.orientation.x = qx
+ msg.orientation.y = qy
+ msg.orientation.z = qz
+ msg.orientation.w = qw
+
+ # 角速度
+ if att.rollspeed is not None:
+ msg.angular_velocity.x = att.rollspeed
+ msg.angular_velocity.y = att.pitchspeed
+ msg.angular_velocity.z = att.yawspeed
+
+ publisher.publish(msg)
+
+ def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus):
+ """發布速度"""
+ if not self.rate_controller.should_publish(sysid, 'velocity'):
+ return
+
+ vfr = status.vfr
+ if vfr.groundspeed is None:
+ return
+
+ publisher = self._get_or_create_publisher(sysid, 'velocity', geometry_msgs.msg.TwistStamped)
+
+ if publisher.get_subscription_count() == 0:
+ return
+
+ msg = geometry_msgs.msg.TwistStamped()
+ msg.header.stamp = self.get_clock().now().to_msg()
+
+ # 使用 VFR HUD 的地速和航向計算速度分量
+ if vfr.heading is not None:
+ heading_rad = math.radians(vfr.heading)
+ msg.twist.linear.x = vfr.groundspeed * math.cos(heading_rad)
+ msg.twist.linear.y = vfr.groundspeed * math.sin(heading_rad)
+
+ # 爬升率作為 z 軸速度
+ if vfr.climb is not None:
+ msg.twist.linear.z = vfr.climb
+
+ publisher.publish(msg)
+
+ def _publish_battery(self, sysid: int, status: mvv.ComponentStatus):
+ """發布電池狀態"""
+ if not self.rate_controller.should_publish(sysid, 'battery'):
+ return
+
+ bat = status.battery
+ if bat.voltage is None:
+ return
+
+ publisher = self._get_or_create_publisher(sysid, 'battery', sensor_msgs.msg.BatteryState)
+
+ if publisher.get_subscription_count() == 0:
+ return
+
+ msg = sensor_msgs.msg.BatteryState()
+ msg.voltage = bat.voltage
+
+ if bat.current is not None:
+ msg.current = bat.current
+
+ if bat.remaining is not None:
+ msg.percentage = bat.remaining / 100.0
+
+ if bat.temperature is not None:
+ msg.temperature = bat.temperature
+
+ publisher.publish(msg)
+
+ def _publish_vfr_hud(self, sysid: int, status: mvv.ComponentStatus):
+ """發布 VFR HUD"""
+ if not self.rate_controller.should_publish(sysid, 'vfr_hud'):
+ return
+
+ vfr = status.vfr
+ if vfr.airspeed is None:
+ return
+
+ publisher = self._get_or_create_publisher(sysid, 'vfr_hud', mavros_msgs.msg.VfrHud)
+
+ if publisher.get_subscription_count() == 0:
+ return
+
+ msg = mavros_msgs.msg.VfrHud()
+ msg.airspeed = vfr.airspeed if vfr.airspeed is not None else 0.0
+ msg.groundspeed = vfr.groundspeed if vfr.groundspeed is not None else 0.0
+ msg.heading = vfr.heading if vfr.heading is not None else 0
+ msg.throttle = float(vfr.throttle) if vfr.throttle is not None else 0.0
+ msg.climb = vfr.climb if vfr.climb is not None else 0.0
+
+ # altitude 需要從 position 獲取
+ if status.position.altitude is not None:
+ msg.altitude = status.position.altitude
+
+ publisher.publish(msg)
+
+ def _publish_mode(self, sysid: int, status: mvv.ComponentStatus):
+ """發布飛行模式"""
+ if not self.rate_controller.should_publish(sysid, 'mode'):
+ return
+
+ mode = status.mode
+ if mode.mode_name is None:
+ return
+
+ publisher = self._get_or_create_publisher(sysid, 'mode', std_msgs.msg.String)
+
+ if publisher.get_subscription_count() == 0:
+ return
+
+ msg = std_msgs.msg.String()
+ msg.data = mode.mode_name
+ publisher.publish(msg)
+
+ def _publish_summary(self, vehicle: mvv.VehicleView):
+ """
+ 發布載具摘要資訊（自定義格式，使用 String 暫時代替）
+ TODO: 未來可以定義專門的 VehicleSummary.msg
+ """
+ sysid = vehicle.sysid
+
+ if not self.rate_controller.should_publish(sysid, 'summary'):
+ return
+
+ publisher = self._get_or_create_publisher(sysid, 'summary', std_msgs.msg.String)
+
+ if publisher.get_subscription_count() == 0:
+ return
+
+ # 獲取 autopilot component
+ component = vehicle.get_component(1)
+ if not component:
+ return
+
+ status = component.status
+
+ # 構建摘要資訊（JSON 格式字串）
+ import json
+ summary = {
+ 'sysid': sysid,
+ 'vehicle_type': vehicle.vehicle_type if vehicle.vehicle_type else 0,
+ 'autopilot': component.mav_autopilot if component.mav_autopilot else 0,
+ 'socket_id': vehicle.custom_meta.get('socket_id', -1), # 重要！
+ 'armed': status.armed if status.armed is not None else False,
+ # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0,
+ 'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN",
+ # 'latitude': status.position.latitude if status.position.latitude else 0.0,
+ # 'longitude': status.position.longitude if status.position.longitude else 0.0,
+ # 'altitude': status.position.altitude if status.position.altitude else 0.0,
+ # 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
+ 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
+ 'connection_type': vehicle.connected_via.value,
+ 'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
+ }
+
+ msg = std_msgs.msg.String()
+ msg.data = json.dumps(summary)
+ publisher.publish(msg)
+
+ # ═══════════════════════════════════════════════════════════════
+ # 【新增 Topic 位置 3/4】
+ # 若要新增 topic，請在此處實作對應的發布方法
+ # 方法命名規則：def _publish_(self, sysid: int, status: mvv.ComponentStatus):
+ # 例如：
+ # def _publish_ekf_status(self, sysid: int, status: mvv.ComponentStatus):
+ # """發布 EKF 狀態"""
+ # if not self.rate_controller.should_publish(sysid, 'ekf_status'):
+ # return
+ #
+ # ekf = status.ekf
+ # if ekf.flags is None:
+ # return
+ #
+ # publisher = self._get_or_create_publisher(sysid, 'ekf_status', ...
+ # # ... 實作發布邏輯
+ # ═══════════════════════════════════════════════════════════════
+
+ @staticmethod
+ def _euler_to_quaternion(roll, pitch, yaw):
+ """
+ 歐拉角轉四元數
+
+ Args:
+ roll: 橫滾角 (弧度)
+ pitch: 俯仰角 (弧度)
+ yaw: 偏航角 (弧度)
+
+ Returns:
+ tuple: (qx, qy, qz, qw)
+ """
+ qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \
+ math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
+ qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \
+ math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
+ qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \
+ math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
+ qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \
+ math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
+ return (qx, qy, qz, qw)
+
+ def stop(self):
+ """停止發布"""
+ self.running = False
+ # logger.info("VehicleStatusPublisher stopped")
+
+
+# ============================================================================
+# MavlinkCommandService Node
+# ============================================================================
+
+class PendingEntry():
+ def __init__(self, event, match_fn):
+ # self.match_msgid = match_msgid # 只會在初始時設定
+ self.match_fn = match_fn
+
+ self.event = event # 只會在外迴圈 set()
+ self.result_mav_msg = None #只會在外迴圈 被寫入
+ self.error = ""
+
+class MavlinkCommandService(Node):
+ """
+ MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令
+
+ 職責:
+ - 作為 service server 等待 client 請求
+ - 接收請求，組裝 MAVLink 封包
+ - 調用 mavlinkObject 發送封包
+ - 處理 ACK 等待和超時
+
+ 設計理念：回歸 MAVLink 純粹結構
+ - 只負責將 ROS2 請求轉換為 MAVLink 封包
+ - 不預設功能（如 ARM/DISARM) 保持模組化
+ - 高層應用可透過此 service 實現各種功能
+
+ 講白話一點就是
+ 每次接到一個 service 請求 要整個系統丟某種指令給載具時
+ 會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
+
+ """
+
+ serviceString_prefix = '/fc_network/vehicle'
+
+ def __init__(self):
+ super().__init__('mavlink_command_service')
+
+ # 狀態標記
+ self.running = True
+
+ # # mavlinkObject 的引用（將由外部設置） 用不到
+ # self.mavlink_analyzer = None
+
+ # pending 旗標物件的儲存庫
+ self._pending_by_sysid = {} # sysid(int) : PendingEntry
+
+ # ═══════════════════════════════════════════════════════════════════
+ # Service : ADD_TWO 測試用，未來刪除
+ # ═══════════════════════════════════════════════════════════════════
+ from example_interfaces.srv import AddTwoInts
+ self.srv_test = self.create_service(
+ AddTwoInts,
+ 'mavlink/add_two_ints',
+ self.handle_add_two_ints
+ )
+
+ # ═══════════════════════════════════════════════════════════════════
+ # Service : TIMESYNC 可以作為模板
+ # ═══════════════════════════════════════════════════════════════════
+ self.srv_mav_ping = self.create_service(
+ fcsrv.MavPing,
+ self.serviceString_prefix + '/mav_ping',
+ self.handle_mav_timesync_ping
+ )
+
+ # ═══════════════════════════════════════════════════════════════════
+ # Service : 發送 COMMAND_LONG
+ # ═══════════════════════════════════════════════════════════════════
+ self.srv_command_long = self.create_service(
+ fcsrv.MavCommandLong,
+ self.serviceString_prefix + '/send_command_long',
+ self.handle_command_long
+ )
+
+ # ═══════════════════════════════════════════════════════════════════
+ # Service : 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
+ # ═══════════════════════════════════════════════════════════════════
+ self.srv_pos_global_int = self.create_service(
+ fcsrv.MavPositionTargetGlobalInt,
+ self.serviceString_prefix + '/pos_global_int',
+ self.handle_set_position_target_global_int
+ )
+
+ logger.info("MavlinkCommandService initialized")
+
+ def _index(self, target_sysid, target_compid):
+
+ # 找到對應的 vehicle
+ vehicle = mvv.vehicle_registry.get(target_sysid)
+ if not vehicle:
+ return None
+
+ socket_id = vehicle.custom_meta.get("socket_id")
+ if socket_id is None:
+ return None
+
+ # 提取主要的 socket_id
+ mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
+ if mav_obj is None:
+ return None
+ return mav_obj
+
+ def return_router(self):
+ '''
+ 在節點管理器哪邊被呼叫
+ 消耗 return_packet_ring 裡接收到的 mavlink 封包
+ 分送到各自的 pending 中
+ 藉由 event.set() 解開 service 中的 block
+ '''
+ return_tuple = mo.return_packet_ring.get()
+ # 確認 return_packet_ring 有資料
+ if return_tuple == None: return
+
+ socketid, timestamp, msg = return_tuple
+ sysid = msg.get_srcSystem()
+
+ _pending = self._pending_by_sysid.get(sysid)
+ # 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略
+ if _pending == None:
+ return
+
+ if _pending.match_fn(msg):
+ _pending.result_mav_msg = msg
+ _pending.event.set()
+
+ # ═══════════════════════════════════════════════════════════════════════
+ # Service Handler: 發送 TIMESYNC 可以作為模板
+ # ═══════════════════════════════════════════════════════════════════════
+ def handle_mav_timesync_ping(self, request, response):
+ '''
+ 用 timesync 封包驗證來回的時間
+ 隸屬於 MavLink PING Protocol
+ '''
+ # 設定失效回應
+ response.success = False
+ response.message = "Unknown error"
+ response.rtt_ms = 0.0
+ timeout_sec = 2.0
+
+ # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
+ if request.target_sysid in self._pending_by_sysid:
+ response.message = f"sysid {request.target_sysid} already has pending request"
+ return response
+
+ # 2) 找到 socket 標地
+ socketObject = self._index(request.target_sysid, request.target_compid)
+ if socketObject is None:
+ response.message = "This system id not found."
+ return response
+
+ # 3) 接收封包系統 的設定
+ # 在 socket 那邊先把要的封包種類導流進來
+ expect_recieve_msg_id = 111 # TIMESYNC
+ socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
+ evt = threading.Event()
+ # 設定封包檢驗
+ # 這邊是設定 tc1 不為 0
+ def match_fn(compare_msg):
+ if compare_msg.get_msgId() != expect_recieve_msg_id :
+ return False
+ # logger.debug(f"mark A : {compare_msg.get_srcSystem()} ,{compare_msg.get_msgId()}, {compare_msg.tc1}, {compare_msg.ts1}")
+ if compare_msg.tc1 == 0 :
+ return False
+ return True
+
+ _pending = PendingEntry(event = evt, match_fn = match_fn)
+
+ self._pending_by_sysid[request.target_sysid] = _pending
+
+ # 4) 送出封包
+ now_us = int(time.monotonic() * 1e6)
+ socketObject.MAVLink.timesync_send(0, now_us)
+
+ fail_skip = False
+ # 5) 等待回應封包
+ if not evt.wait(timeout_sec):
+ response.message = "mavlink timeout - TIMESYNC"
+ fail_skip = True
+
+ # 6) 處理回應封包
+ if not fail_skip:
+ ack_msg = _pending.result_mav_msg
+
+ current_time = int(time.monotonic() * 1e6)
+ response.rtt_ms = (current_time - now_us) / 1e3
+ response.message = ""
+
+ # 7) 接收封包系統 的重置
+ msg_types = list(socketObject.return_msg_types)
+ if expect_recieve_msg_id in msg_types:
+ msg_types.remove(expect_recieve_msg_id)
+ socketObject.set_return_message_types(msg_types)
+ del evt
+ self._pending_by_sysid.pop(request.target_sysid, None)
+
+ return response
+
+ # ═══════════════════════════════════════════════════════════════════════
+ # Service Handler: 發送 COMMAND_LONG (76)
+ # ═══════════════════════════════════════════════════════════════════════
+ def handle_command_long(self, request, response):
+ '''
+ 用 command long 丟出 MAV_CMD
+ 隸屬於 MavLink Command Protocol
+ '''
+ # 設定失效回應
+ response.success = False
+ response.message = "Unknown error"
+ response.ack_result = serviceAckResult.UNKNOWN
+ timeout_sec = request.timeout_sec
+
+ # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
+ if request.target_sysid in self._pending_by_sysid:
+ response.message = f"sysid {request.target_sysid} already has pending request"
+ response.ack_result = serviceAckResult.MAVLINK_BUSY
+ return response
+
+ # 2) 找到 socket 標地
+ socketObject = self._index(request.target_sysid, request.target_compid)
+ if socketObject is None:
+ response.message = "This system id not found."
+ response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
+ return response
+
+ # 3) 接收封包系統 的設定
+ expect_recieve_msg_id = 77 # mavutil.mavlink.MAVLINK_MSG_ID_COMMAND_ACK
+ socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
+ evt = threading.Event()
+
+ def match_fn(compare_msg):
+ if compare_msg.get_msgId() != expect_recieve_msg_id :
+ return False
+ return True
+
+ _pending = PendingEntry(event = evt, match_fn = match_fn)
+
+ self._pending_by_sysid[request.target_sysid] = _pending
+
+ # 4) 送出封包
+ socketObject.MAVLink.command_long_send(
+ request.target_sysid,
+ request.target_compid,
+ request.command,
+ request.confirmation,
+ request.param1, request.param2, request.param3, request.param4,
+ request.param5, request.param6, request.param7
+ )
+
+ fail_skip = False
+ # 5) 等待回應封包
+ if not evt.wait(timeout_sec):
+ response.message = "mavlink timeout - CommLONG"
+ response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
+ fail_skip = True
+
+ # 6) 處理回應封包
+ if not fail_skip:
+ ack_msg = _pending.result_mav_msg
+ response.success = (ack_msg.result == 0) # mavutil.mavlink.MAV_RESULT_ACCEPTED
+ response.message = "" # 沒有消息就是好消息
+ response.ack_result = ack_msg.result
+
+ # 7) 接收封包系統 的重置
+ msg_types = list(socketObject.return_msg_types)
+ if expect_recieve_msg_id in msg_types:
+ msg_types.remove(expect_recieve_msg_id)
+ socketObject.set_return_message_types(msg_types)
+ del evt
+ self._pending_by_sysid.pop(request.target_sysid, None)
+
+ return response
+
+ # ═══════════════════════════════════════════════════════════════════════
+ # Service Handler: 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
+ # ═══════════════════════════════════════════════════════════════════════
+ def handle_set_position_target_global_int(self, request, response):
+ '''
+ 隸屬於 MavLink Offboard Control Protocol
+ '''
+ # 設定失效回應
+ response.message = "Unknown error"
+ response.ack_result = serviceAckResult.UNKNOWN
+ timeout_sec = request.timeout_sec
+
+ # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
+ if request.target_sysid in self._pending_by_sysid:
+ response.message = f"sysid {request.target_sysid} already has pending request"
+ response.ack_result = serviceAckResult.MAVLINK_BUSY
+ return response
+
+ # 2) 找到 socket 標地
+ socketObject = self._index(request.target_sysid, request.target_compid)
+ if socketObject is None:
+ response.message = "This system id not found."
+ response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
+ return response
+
+ # 3) 接收封包系統 的設定
+ # 在 socket 那邊先把要的封包種類導流進來
+ expect_recieve_msg_id = 87 # POSITION_TARGET_GLOBAL_INT
+ socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
+ evt = threading.Event()
+ # 設定封包檢驗
+ def match_fn(compare_msg):
+ if compare_msg.get_msgId() != expect_recieve_msg_id :
+ return False
+ return True
+
+ # 4) 送出封包
+ socketObject.MAVLink.set_position_target_global_int_send(
+ request.time_boot_ms,
+ request.target_sysid,
+ request.target_compid,
+ request.coordinate_frame,
+ request.type_mask,
+ request.lat_int, request.lon_int, request.alt,
+ request.vx, request.vy, request.vz,
+ request.afx, request.afy, request.afz,
+ request.yaw, request.yaw_rate
+ )
+
+ time.sleep(0.01) # 因應 mavlink 的廣播式回應 後置回應監聽 並且故意延遲10ms 讓系統代謝掉舊的87封包
+ _pending = PendingEntry(event = evt, match_fn = match_fn)
+ self._pending_by_sysid[request.target_sysid] = _pending
+
+ fail_skip = False
+ # 5) 等待回應封包
+ if not evt.wait(timeout_sec):
+ response.message = "mavlink timeout - SET POSITION 86"
+ response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
+ fail_skip = True
+
+ # 6) 處理回應封包
+ if not fail_skip:
+ ack_msg = _pending.result_mav_msg
+
+ response.message = ""
+ response.ack_result = 0
+ response.r_time_boot_ms = ack_msg.time_boot_ms
+ response.r_type_mask = ack_msg.type_mask
+ response.r_lat_int = ack_msg.lat_int
+ response.r_lon_int = ack_msg.lon_int
+ response.r_alt = ack_msg.alt
+ response.r_vx = ack_msg.vx
+ response.r_vy = ack_msg.vy
+ response.r_vz = ack_msg.vz
+ response.r_afx = ack_msg.afx
+ response.r_afy = ack_msg.afy
+ response.r_afz = ack_msg.afz
+ response.r_yaw = ack_msg.yaw
+ response.r_yaw_rate = ack_msg.yaw_rate
+
+ # 7) 接收封包系統 的重置
+ msg_types = list(socketObject.return_msg_types)
+ if expect_recieve_msg_id in msg_types:
+ msg_types.remove(expect_recieve_msg_id)
+ socketObject.set_return_message_types(msg_types)
+ del evt
+ self._pending_by_sysid.pop(request.target_sysid, None)
+
+ 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
+
+
+ def handle_add_two_ints(self, request, response):
+ """測試用 Service Handler 未來刪除"""
+ logger.info(f"Received add_two_ints request: {request.a} + {request.b}")
+ response.sum = request.a + request.b
+ logger.info(
+ f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
+ )
+ time.sleep(1)
+
+ return response
+
+
+
+ def stop(self):
+ """停止服務"""
+ self.running = False
+ logger.info("MavlinkCommandService stopped")
+
+
+# ============================================================================
+# ROS2 節點管理器
+# ============================================================================
+
+class fc_ros_manager:
+ """
+ MAVLink ROS2 節點管理器
+
+ 因為物件創建在 mavlinkROS2Nodes.py 中，所以會分為 __init__ 跟 initialize 兩個階段。
+
+ stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
+ shutdown 是完全關閉 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
+ if not rclpy.ok():
+ rclpy.init(args=None)
+
+ # 創建節點 node
+ self.status_publisher = VehicleStatusPublisher()
+ self.command_service = MavlinkCommandService()
+
+ # 創建執行者 MultiThreadedExecutor 並把 node 加入其中
+ 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)
+ self.command_service.return_router()
+ 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 結構（稍後實現）
+
+2026.03.27
+1. 完成 ros2 service 結構與 timesync 與 command_long 協議
+
+2026.04.07
+1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
+2. 優化 response.ack_result 回傳值的資訊
+
+
+TODO
+1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
+2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
+'''
+
diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py
new file mode 100644
index 0000000..81dbbf9
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py
@@ -0,0 +1,476 @@
+"""
+VehicleView - Pure State Container
+純粹的狀態容器，不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量
+只提供狀態存取介面，由外部手動餵資料（push state）
+"""
+
+import os
+from typing import Dict, Optional, Any, Tuple
+from dataclasses import dataclass, field
+from enum import Enum
+
+from .utils import setup_logger
+
+logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "1.00"
+
+# ====================== 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] = {}
+ # 高頻讀取用 snapshot（弱一致性）
+ self._vehicles_snapshot_items: Tuple[Tuple[int, VehicleView], ...] = tuple()
+
+ def _refresh_snapshot(self) -> None:
+ """重建快照（僅在 key 集合變動時觸發）"""
+ self._vehicles_snapshot_items = tuple(self._vehicles.items())
+
+ def register(self, sysid: int) -> VehicleView:
+ """註冊新的載具視圖"""
+ if sysid not in self._vehicles:
+ self._vehicles[sysid] = VehicleView(sysid)
+ self._refresh_snapshot()
+ 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]
+ self._refresh_snapshot()
+ logger.info(f"Unregistered VehicleView for system {sysid}")
+ return True
+ return False
+
+ def get_all(self) -> Dict[int, VehicleView]:
+ """取得所有載具視圖"""
+ return self._vehicles.copy()
+
+ def read_all(self) -> Tuple[Tuple[int, VehicleView], ...]:
+ """
+ 高效讀取所有載具視圖快照（弱一致性）
+
+ 注意:
+ - 回傳 immutable tuple 快照，適合高頻迭代
+ - 僅在 register/unregister/clear 後更新，可能不是最新資料
+ - 適合高頻讀取且可接受弱一致性的情境
+ """
+ return self._vehicles_snapshot_items
+
+ def clear(self) -> None:
+ """清空所有載具視圖"""
+ self._vehicles.clear()
+ self._refresh_snapshot()
+ 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. 新增 重置指定組件的封包統計 功能
+
+2026.04.13
+1. 新增 VehicleViewRegistry.read_all 方法 讓 ROS2 Node 組件更有效率的讀取其中的資料
+
+'''
+
diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py
new file mode 100644
index 0000000..8638432
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py
@@ -0,0 +1,617 @@
+'''
+
+
+'''
+
+# 基礎功能的 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
+from abc import ABC, abstractmethod
+
+# # XBee 模組
+# from xbee.frame import APIFrame
+
+# 自定義的 import
+from .utils import setup_logger
+
+# ====================== 分割線 =====================
+
+logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "0.60"
+
+# ====================== 分割線 =====================
+
+# 定義 serial 連線的模式
+class SerialMode(Enum):
+ """連接類型"""
+ STRAIGHT = auto() # 原始數據直通
+ XBEEAPI2AT = auto() # XBee API 模式
+ NOT_USE = auto() # 不使用
+
+
+# ====================== Frame Processor 基類與實現 =====================
+
+class FrameProcessor(ABC):
+ """協議處理器基類"""
+
+ def __init__(self):
+ self.buffer = bytearray()
+
+ @abstractmethod
+ def process_incoming(self, data: bytes):
+ """
+ 處理接收到的數據
+ 返回：已完整解析的 payload 列表
+ """
+ pass
+
+ @abstractmethod
+ def process_outgoing(self, data: bytes) -> bytes:
+ """
+ 封裝要發送的數據
+ 返回：封裝後的完整幀
+ """
+ pass
+
+
+class RawFrameProcessor(FrameProcessor):
+ """原始數據直通處理器"""
+
+ def process_incoming(self, data: bytes):
+ """直接返回原始數據，不進行緩衝"""
+ return [data] if data else []
+
+ def process_outgoing(self, data: bytes) -> bytes:
+ """直接返回原始數據，不進行封裝"""
+ return data
+
+
+class XBeeFrameProcessor(FrameProcessor):
+ """XBee API 協議處理器"""
+
+ def process_incoming(self, data: bytes):
+ """處理 XBee API 幀並提取 payload"""
+ self.buffer.extend(data)
+ payloads = []
+
+ while len(self.buffer) >= 3:
+ # 尋找幀頭
+ if self.buffer[0] != 0x7E:
+ self.buffer.pop(0)
+ continue
+
+ # 讀取 payload 長度
+ length = (self.buffer[1] << 8) | self.buffer[2]
+ full_length = 3 + length + 1 # 起始符(1) + 長度(2) + payload + 校驗和(1)
+
+ # 等待完整幀
+ if len(self.buffer) < full_length:
+ break
+
+ # 提取完整 frame 並從緩衝區移除
+ frame = bytes(self.buffer[:full_length])
+ del self.buffer[:full_length]
+
+ # 判斷 frame 類型並處理
+ frame_type = frame[3]
+
+ if frame_type == 0x90: # RX Packet
+ payload = XBeeFrameHandler.decapsulate_data(frame)
+ if payload:
+ payloads.append(payload)
+ elif frame_type == 0x88: # AT Response
+ # 可以在這裡處理 AT 指令回應
+ # response = XBeeFrameHandler.parse_at_command_response(frame)
+ # 目前忽略
+ pass
+ elif frame_type == 0x8B: # Transmit Status
+ # 傳輸狀態，目前忽略
+ pass
+ else:
+ logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
+
+ return payloads
+
+ def process_outgoing(self, data: bytes) -> bytes:
+ """將數據封裝為 XBee API 傳輸幀"""
+ return XBeeFrameProcessor.encapsulate_data(data)
+
+
+# ====================== XBee Frame Handler =====================
+
+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):
+
+ # 獲取數據長度 (不包括校驗和)
+ length = (data[1] << 8) | data[2]
+
+ rf_data_start = 3 + 12
+ return data[rf_data_start:3 + length]
+
+
+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:
+ logger.warning(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:
+ logger.debug(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
+
+ def _handle_rssi(self, data: bytes):
+ """處理 DB (RSSI) 回應"""
+ # 未來可實現 RSSI 處理邏輯
+ pass
+
+ def _handle_serial_high(self, data: bytes):
+ """處理 SH (Serial Number High)"""
+ pass
+
+ def _handle_serial_low(self, data: bytes):
+ """處理 SL (Serial Number Low)"""
+ pass
+
+
+# ====================== Serial Handler =====================
+
+class SerialHandler(asyncio.Protocol):
+ """asyncio.Protocol 用於處理 Serial 收發"""
+
+ def __init__(self, udp_handler, serial_port_str, serial_mode: SerialMode):
+ self.udp_handler = udp_handler # UDP 的傳輸物件
+ self.serial_port_str = serial_port_str
+ self.serial_mode = serial_mode
+ self.transport = None # Serial 自己的傳輸物件
+
+ # 根據模式創建對應的 processor
+ self.processor = self._create_processor(serial_mode)
+
+ # AT 指令處理器（僅 XBee 模式使用）
+ if serial_mode == SerialMode.XBEEAPI2AT:
+ self.at_handler = ATCommandHandler(serial_port_str)
+ else:
+ self.at_handler = None
+
+ def _create_processor(self, serial_mode: SerialMode) -> FrameProcessor:
+ """工廠方法：根據模式創建處理器"""
+ if serial_mode == SerialMode.STRAIGHT:
+ return RawFrameProcessor()
+ elif serial_mode == SerialMode.XBEEAPI2AT:
+ return XBeeFrameProcessor()
+ else:
+ logger.warning(f"Unknown serial mode: {serial_mode}, using Raw")
+ return RawFrameProcessor()
+
+ def connection_made(self, transport):
+ """連接建立時的回調"""
+ self.transport = transport
+ if hasattr(self.udp_handler, 'set_serial_handler'):
+ self.udp_handler.set_serial_handler(self)
+ logger.debug(f"Serial port {self.serial_port_str} connected")
+
+ def data_received(self, data):
+ """Serial 收到資料的處理過程"""
+ # 使用 processor 處理接收到的數據
+ payloads = self.processor.process_incoming(data)
+
+ # 將所有解析完成的 payload 轉發到 UDP
+ for payload in payloads:
+ self.udp_handler.transport.sendto(
+ payload,
+ (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port)
+ )
+
+
+# ====================== UDP Handler =====================
+
+class UDPHandler(asyncio.DatagramProtocol):
+ """asyncio.DatagramProtocol 用於處理 UDP 收發"""
+
+ LOCAL_HOST_IP = '127.0.0.1' # 只送給本地端 IP
+
+ def __init__(self, target_port, serial_mode: SerialMode):
+ self.target_port = target_port # 目標 UDP 端口
+ self.serial_mode = serial_mode
+ self.serial_handler = None # Serial 的傳輸物件
+ self.transport = None # UDP 自己的傳輸物件
+
+ def connection_made(self, transport):
+ """連接建立時的回調"""
+ self.transport = transport
+ logger.debug(f"UDP transport ready for port {self.target_port}")
+
+ def set_serial_handler(self, serial_handler):
+ """設置對應的 Serial Handler"""
+ self.serial_handler = serial_handler
+
+ def datagram_received(self, data, addr):
+ """UDP 收到資料的處理過程"""
+ if not self.serial_handler:
+ logger.warning("Serial handler not set, dropping UDP packet")
+ return
+
+ # 使用 processor 封裝數據
+ processed_data = self.serial_handler.processor.process_outgoing(data)
+
+ # 發送到串口
+ self.serial_handler.transport.write(processed_data)
+
+
+# ====================== Serial Manager =====================
+
+class serial_manager:
+ """串口管理器"""
+
+ class serial_object:
+ """串口物件"""
+ def __init__(self, serial_port, baudrate, target_port, serial_mode: SerialMode):
+ self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc
+ self.baudrate = baudrate
+ self.serial_mode = serial_mode
+ self.target_port = target_port # 指向的 UDP 端口
+
+ self.transport = None
+ self.protocol = None
+ 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):
+ """啟動 serial_manager"""
+ if self.running:
+ logger.warning("serial_manager already running")
+ return False
+
+ 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"""
+ 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)
+
+ try:
+ self.loop.run_forever()
+ finally:
+ self.loop.close()
+
+ def create_serial_link(self, serial_port, baudrate, target_port, serial_mode: SerialMode):
+ """創建串口連接"""
+ 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, serial_mode),
+ self.loop
+ )
+
+ try:
+ # 等待結果，設定合理的超時時間
+ result = future.result(timeout=5.0)
+ if result:
+ logger.info(f"Create Serial Link: {serial_port} ({serial_mode.name}) -> UDP {target_port}")
+ return result
+ 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, serial_mode: SerialMode):
+ """在事件循環線程中執行實際的連接創建"""
+ try:
+ # 創建 serial_object 實例
+ serial_obj = self.serial_object(serial_port, baudrate, target_port, serial_mode)
+
+ # 建立 UDP 處理器並指定目標端口位置
+ serial_obj.udp_handler = UDPHandler(target_port, serial_mode)
+
+ # 建立 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.debug(f"UDP endpoint created for {serial_port}")
+
+ # 建立 Serial 處理器，將 UDP 處理器和模式傳給它
+ serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port, serial_mode)
+
+ # 建立 Serial 連接
+ serial_transport, _ = await serial_asyncio.create_serial_connection(
+ self.loop,
+ lambda: serial_obj.serial_handler,
+ serial_port,
+ baudrate=baudrate
+ )
+
+ logger.debug(f"Serial connection created for {serial_port}")
+
+ # 將 serial_object 加入管理列表
+ serial_id = self.serial_count + 1
+ self.serial_objects[serial_id] = serial_obj
+ self.serial_count += 1
+
+ logger.debug(f"Serial object {serial_id} added to manager")
+ 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
+
+ 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.debug(f"Serial object {serial_id} removed from manager")
+ 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, SerialMode.XBEEAPI2AT)
+
+ # SERIAL_PORT = '/dev/ttyACM0' # 手動指定
+ # SERIAL_BAUDRATE = 115200
+ # UDP_REMOTE_PORT = 14571
+ # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
+
+ linked_serial = sm.get_serial_link()
+ print(linked_serial)
+ time.sleep(60)
+
+ sm.remove_serial_link(1)
+ time.sleep(3)
+ sm.shutdown()
diff --git a/src/fc_network_adapter/fc_network_adapter/utils/__init__.py b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py
new file mode 100644
index 0000000..921faf4
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py
@@ -0,0 +1,7 @@
+"""
+共用工具模組
+"""
+from .ringBuffer import RingBuffer
+from .theLogger import setup_logger
+
+__all__ = ['RingBuffer', 'setup_logger']
\ No newline at end of file
diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py b/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py
new file mode 100644
index 0000000..0c6a873
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py
@@ -0,0 +1,129 @@
+import socket
+import random
+import os
+
+
+def get_used_ports():
+ """
+ 從 /proc/net/tcp 和 /proc/net/udp 讀取系統已占用的 port
+ 直接讀取 Linux 系統資訊，避免暴力嘗試
+
+ Returns:
+ set: 已被占用的 port 號集合
+ """
+ used_ports = set()
+
+ # 讀取 TCP 占用的 port (包含 IPv4 和 IPv6)
+ for filepath in ['/proc/net/tcp', '/proc/net/tcp6']:
+ if os.path.exists(filepath):
+ try:
+ with open(filepath, 'r') as f:
+ lines = f.readlines()[1:] # 跳過標題行
+ for line in lines:
+ parts = line.split()
+ if len(parts) > 1:
+ # local_address 格式: "0100007F:1F90" (hex)
+ local_addr = parts[1]
+ port_hex = local_addr.split(':')[1]
+ port = int(port_hex, 16)
+ used_ports.add(port)
+ except (IOError, PermissionError):
+ pass
+
+ # 讀取 UDP 占用的 port (包含 IPv4 和 IPv6)
+ for filepath in ['/proc/net/udp', '/proc/net/udp6']:
+ if os.path.exists(filepath):
+ try:
+ with open(filepath, 'r') as f:
+ lines = f.readlines()[1:] # 跳過標題行
+ for line in lines:
+ parts = line.split()
+ if len(parts) > 1:
+ local_addr = parts[1]
+ port_hex = local_addr.split(':')[1]
+ port = int(port_hex, 16)
+ used_ports.add(port)
+ except (IOError, PermissionError):
+ pass
+
+ return used_ports
+
+
+def is_port_available(port):
+ """
+ 測試指定 port 是否可用 (TCP 和 UDP 都測試)
+
+ Args:
+ port (int): 要測試的 port 號
+
+ Returns:
+ bool: True 表示可用，False 表示被占用
+ """
+ # 測試 TCP
+ try:
+ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
+ sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ sock.bind(('', port))
+ except OSError:
+ return False
+
+ # 測試 UDP
+ try:
+ with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock:
+ sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ sock.bind(('', port))
+ except OSError:
+ return False
+
+ return True
+
+
+def find_available_port(start_port=1024, end_port=65535):
+ """
+ 在指定的 port 區間內隨機找出一個未被占用的 port
+ 使用 Linux /proc/net 系統資訊來過濾已占用的 port，避免暴力嘗試
+ 確保 TCP 和 UDP 都可用
+
+ Args:
+ start_port (int): 起始 port 號 (預設 1024)
+ end_port (int): 結束 port 號 (預設 65535)
+
+ Returns:
+ int: 可用的 port 號，如果找不到則返回 None
+ """
+ if start_port < 1 or end_port > 65535 or start_port >= end_port:
+ raise ValueError("Port 範圍必須在 1-65535 之間，且起始 port 必須小於結束 port")
+
+ # 從系統讀取已占用的 port
+ used_ports = get_used_ports()
+
+ # 計算可用的 port 列表 (排除已占用的)
+ available_ports = [p for p in range(start_port, end_port + 1) if p not in used_ports]
+
+ if not available_ports:
+ return None
+
+ # 隨機打亂順序
+ random.shuffle(available_ports)
+
+ # 從可用列表中挑選，再用 socket 雙重確認 TCP 和 UDP 都可用
+ for port in available_ports:
+ if is_port_available(port):
+ return port
+
+ # 如果都不可用
+ return None
+
+
+if __name__ == "__main__":
+ # 使用範例
+ port = find_available_port(8000, 9000)
+ if port:
+ print(f"找到可用的 port: {port}")
+ else:
+ print("找不到可用的 port")
+
+ # 自訂範圍範例
+ port = find_available_port(10000, 20000)
+ if port:
+ print(f"在 10000-20000 範圍找到可用的 port: {port}")
diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py
new file mode 100644
index 0000000..1e1a73f
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py
@@ -0,0 +1,111 @@
+"""
+Serial Port Discovery Utility
+
+This module provides functions to discover available serial ports on the system.
+It uses glob pattern matching to find serial device files in /dev/.
+"""
+
+import glob
+from typing import List, Union
+
+
+def get_serial_ports() -> List[str]:
+ """
+ 獲取系統中所有可用的串口設備列表
+
+ 在 Linux 系統中，會搜尋以下模式的設備：
+ - /dev/ttyUSB* (USB 串口設備)
+ - /dev/ttyACM* (USB CDC ACM 設備)
+ - /dev/ttyS* (標準串口)
+
+ Returns:
+ List[str]: 包含所有找到的串口設備路徑的列表
+
+ Example:
+ >>> ports = get_serial_ports()
+ >>> print(ports)
+ ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyS0']
+ """
+ serial_ports = []
+
+ # 搜尋不同類型的串口設備
+ patterns = [
+ '/dev/ttyUSB*', # USB 串口轉換器
+ '/dev/ttyACM*', # USB CDC ACM 設備（如 Arduino）
+ '/dev/ttyS*', # 標準串口
+ ]
+
+ for pattern in patterns:
+ serial_ports.extend(glob.glob(pattern))
+
+ # 排序以便於閱讀
+ serial_ports.sort()
+
+ return serial_ports
+
+
+def get_serial_ports_with_filter(filter_patterns: Union[str, List[str]] = None) -> List[str]:
+ """
+ 獲取串口設備列表，可選擇性地使用自訂篩選模式
+
+ Args:
+ filter_patterns (Union[str, List[str]], optional):
+ 單一或多個 glob 模式
+ - 字串: '/dev/ttyUSB*'
+ - 列表: ['/dev/ttyUSB*', '/dev/ttyACM*']
+ 如果為 None，則使用預設模式搜尋所有串口
+
+ Returns:
+ List[str]: 符合條件的串口設備路徑列表
+
+ Example:
+ >>> # 單一 pattern
+ >>> ports = get_serial_ports_with_filter('/dev/ttyUSB*')
+ >>> print(ports)
+ ['/dev/ttyUSB0', '/dev/ttyUSB1']
+
+ >>> # 多個 patterns
+ >>> ports = get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*'])
+ >>> print(ports)
+ ['/dev/ttyACM0', '/dev/ttyUSB0', '/dev/ttyUSB1']
+ """
+ if filter_patterns is None:
+ return get_serial_ports()
+
+ # 統一轉成 list 處理
+ if isinstance(filter_patterns, str):
+ filter_patterns = [filter_patterns]
+
+ serial_ports = []
+ for pattern in filter_patterns:
+ serial_ports.extend(glob.glob(pattern))
+
+ serial_ports.sort()
+ return serial_ports
+
+
+if __name__ == "__main__":
+ # 使用範例
+ print("=== Serial Port Discovery ===\n")
+
+ # 1. 獲取所有串口設備
+ all_ports = get_serial_ports()
+ print(f"找到 {len(all_ports)} 個串口設備:")
+ for port in all_ports:
+ print(f" - {port}")
+
+ print("\n" + "="*30 + "\n")
+
+ # 2. 只搜尋 USB 串口
+ usb_ports = get_serial_ports_with_filter('/dev/ttyUSB*')
+ print(f"找到 {len(usb_ports)} 個 USB 串口設備:")
+ for port in usb_ports:
+ print(f" - {port}")
+
+ print("\n" + "="*30 + "\n")
+
+ # 3. 只搜尋 ACM 設備
+ acm_ports = get_serial_ports_with_filter('/dev/ttyACM*')
+ print(f"找到 {len(acm_ports)} 個 ACM 設備:")
+ for port in acm_ports:
+ print(f" - {port}")
diff --git a/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py b/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py
new file mode 100644
index 0000000..6728426
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py
@@ -0,0 +1,231 @@
+# import array
+import threading
+import ctypes
+from typing import Any, Optional, Tuple
+
+class RingBuffer:
+ """
+ 高效能無鎖環形緩衝區，使用原子操作避免鎖定
+ 用於在不同執行緒間高效傳遞資料
+ """
+ # 緩衝區計數器，用於自動分配 buffer_id
+ _buffer_counter = 0
+ _counter_lock = threading.Lock()
+
+ def __init__(self, capacity: int = 256, buffer_id: int = None):
+ """
+ 初始化環形緩衝區
+
+ Args:
+ capacity: 緩衝區容量 (必須是 2 的次方)
+ buffer_id: 緩衝區 ID，如果為 None 則自動分配
+ """
+ # 確保容量是 2 的次方，便於使用位運算取模
+ if capacity & (capacity - 1) != 0:
+ # 找到大於等於 capacity 的最小 2 的次方
+ capacity = 1 << (capacity - 1).bit_length()
+
+ # 分配緩衝區 ID
+ if buffer_id is None:
+ with RingBuffer._counter_lock:
+ buffer_id = RingBuffer._buffer_counter
+ RingBuffer._buffer_counter += 1
+
+ self.buffer_id = buffer_id
+ self.capacity = capacity
+ self.mask = capacity - 1 # 用於快速取模
+ self.buffer = [None] * capacity
+
+ # 原子計數器作為讀寫指標
+ self.write_index = ctypes.c_uint64(0)
+ self.read_index = ctypes.c_uint64(0)
+
+ # 用於檢測上次操作的執行緒 ID
+ self._last_read_thread = None
+ self._last_write_thread = None
+
+ # 添加同時讀寫統計
+ self.concurrent_write_count = ctypes.c_uint64(0) # 同時寫入計數
+ self.concurrent_read_count = ctypes.c_uint64(0) # 同時讀取計數
+ self.total_write_count = ctypes.c_uint64(0) # 總寫入操作計數
+ self.total_read_count = ctypes.c_uint64(0) # 總讀取操作計數
+ self.overflow_count = ctypes.c_uint64(0) # 緩衝區溢出次數
+
+ # 記錄各執行緒的操作次數
+ self.thread_write_counts = {} # {thread_id: count}
+ self.thread_read_counts = {} # {thread_id: count}
+
+ # 用於保護統計數據的鎖（僅用於統計，不影響主要讀寫操作）
+ self._stats_lock = threading.Lock()
+
+ def put(self, item: Any) -> bool:
+ """
+ 將項目存入緩衝區
+
+ Args:
+ item: 要存入的項目
+
+ Returns:
+ bool: 成功寫入返回 True，緩衝區已滿返回 False
+ """
+ # 更新寫入統計
+ self.total_write_count.value += 1
+
+ # 檢測上次寫入的執行緒
+ current_thread = threading.get_ident()
+
+ # 記錄當前執行緒寫入次數
+ with self._stats_lock:
+ self.thread_write_counts[current_thread] = self.thread_write_counts.get(current_thread, 0) + 1
+
+ # 檢測是否為不同執行緒同時寫入
+ if self._last_write_thread is not None and current_thread != self._last_write_thread:
+ self.concurrent_write_count.value += 1
+
+ self._last_write_thread = current_thread
+
+ # 原子獲取當前寫入位置
+ current = self.write_index.value
+ next_pos = (current + 1) & self.mask
+
+ # 檢查緩衝區是否已滿
+ if next_pos == self.read_index.value:
+ self.overflow_count.value += 1
+ return False
+
+ # 寫入資料並原子更新寫入位置
+ self.buffer[current] = item
+ self.write_index.value = next_pos
+ return True
+
+ def get(self) -> Optional[Any]:
+ """
+ 從緩衝區讀取項目
+
+ Returns:
+ 項目或 None（如果緩衝區為空）
+ """
+ # 更新讀取統計
+ self.total_read_count.value += 1
+
+ # 檢測上次讀取的執行緒
+ current_thread = threading.get_ident()
+
+ # 記錄當前執行緒讀取次數
+ with self._stats_lock:
+ self.thread_read_counts[current_thread] = self.thread_read_counts.get(current_thread, 0) + 1
+
+ # 檢測是否為不同執行緒同時讀取
+ if self._last_read_thread is not None and current_thread != self._last_read_thread:
+ self.concurrent_read_count.value += 1
+
+ self._last_read_thread = current_thread
+
+ # 檢查緩衝區是否為空
+ if self.read_index.value == self.write_index.value:
+ return None
+
+ # 原子獲取當前讀取位置並讀取資料
+ current = self.read_index.value
+ item = self.buffer[current]
+
+ # 原子更新讀取位置
+ self.read_index.value = (current + 1) & self.mask
+ return item
+
+ def get_all(self) -> list:
+ """
+ 獲取緩衝區中的所有項目
+
+ Returns:
+ list: 所有可用項目的列表
+ """
+ items = []
+ while True:
+ item = self.get()
+ if item is None:
+ break
+ items.append(item)
+ return items
+
+ def size(self) -> int:
+ # 返回目前緩衝區內項目數量
+ return (self.write_index.value - self.read_index.value) & self.mask
+
+ def is_empty(self) -> bool:
+ # 檢查緩衝區是否為空
+ return self.read_index.value == self.write_index.value
+
+ def is_full(self) -> bool:
+ # 檢查緩衝區是否已滿
+ return ((self.write_index.value + 1) & self.mask) == self.read_index.value
+
+ def clear(self) -> None:
+ """清空緩衝區"""
+ self.read_index.value = self.write_index.value
+
+ def get_stats(self) -> dict:
+ """
+ 獲取緩衝區統計資訊
+
+ Returns:
+ dict: 包含各種統計數據的字典
+ """
+ with self._stats_lock:
+ stats = {
+ "buffer_id": self.buffer_id,
+ "capacity": self.capacity,
+ "current_size": self.size(),
+ "is_full": self.is_full(),
+ "is_empty": self.is_empty(),
+ "total_writes": self.total_write_count.value,
+ "total_reads": self.total_read_count.value,
+ "concurrent_writes": self.concurrent_write_count.value,
+ "concurrent_reads": self.concurrent_read_count.value,
+ "overflow_count": self.overflow_count.value,
+ "write_threads": len(self.thread_write_counts),
+ "read_threads": len(self.thread_read_counts),
+ "concurrent_write_ratio": self.concurrent_write_count.value / max(1, self.total_write_count.value),
+ "concurrent_read_ratio": self.concurrent_read_count.value / max(1, self.total_read_count.value),
+ "top_writer_threads": sorted(self.thread_write_counts.items(), key=lambda x: x[1], reverse=True)[:3],
+ "top_reader_threads": sorted(self.thread_read_counts.items(), key=lambda x: x[1], reverse=True)[:3],
+ }
+ return stats
+
+ def print_stats(self) -> None:
+ """輸出當前緩衝區統計資訊"""
+ stats = self.get_stats()
+
+ print(f"\n=== RingBuffer #{stats['buffer_id']} Statistics ===")
+ print(f"Capacity: {stats['capacity']}, Current Size: {stats['current_size']}")
+ print(f"Total Write Operations: {stats['total_writes']}")
+ print(f"Total Read Operations: {stats['total_reads']}")
+ print(f"Concurrent Write Events: {stats['concurrent_writes']} ({stats['concurrent_write_ratio']:.2%})")
+ print(f"Concurrent Read Events: {stats['concurrent_reads']} ({stats['concurrent_read_ratio']:.2%})")
+ print(f"Buffer Overflow Count: {stats['overflow_count']}")
+ print(f"Writing Threads: {stats['write_threads']}")
+ print(f"Reading Threads: {stats['read_threads']}")
+
+ print("Top Writer Threads:")
+ for thread_id, count in stats['top_writer_threads']:
+ print(f" Thread {thread_id}: {count} writes")
+
+ print("Top Reader Threads:")
+ for thread_id, count in stats['top_reader_threads']:
+ print(f" Thread {thread_id}: {count} reads")
+ print("=============================\n")
+
+ def reset_stats(self) -> None:
+ """重置所有統計數據"""
+ with self._stats_lock:
+ self.concurrent_write_count.value = 0
+ self.concurrent_read_count.value = 0
+ self.total_write_count.value = 0
+ self.total_read_count.value = 0
+ self.overflow_count.value = 0
+ self.thread_write_counts.clear()
+ self.thread_read_counts.clear()
+
+ def __str__(self) -> str:
+ """返回緩衝區的字符串表示"""
+ return f"RingBuffer(id={self.buffer_id}, capacity={self.capacity}, size={self.size()})"
\ No newline at end of file
diff --git a/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py
new file mode 100644
index 0000000..dce2ce7
--- /dev/null
+++ b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py
@@ -0,0 +1,43 @@
+import logging
+import os
+from logging.handlers import TimedRotatingFileHandler
+
+# 全域 Logger 實例
+_global_logger = None
+
+def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger:
+ global _global_logger
+
+ if _global_logger is None:
+ # 確保 logs 資料夾存在
+ os.makedirs(log_dir, exist_ok=True)
+
+ # 建立全域 Logger
+ _global_logger = logging.getLogger("global_logger")
+ _global_logger.setLevel(level)
+ _global_logger.propagate = False # 防止重複輸出
+
+ formatter = logging.Formatter(
+ fmt="%(asctime)s | %(levelname)s | %(name)s | %(message)s",
+ datefmt="%m-%d %H:%M:%S"
+ )
+
+ # 檔案輸出（每天輪替）
+ file_handler = TimedRotatingFileHandler(
+ filename=os.path.join(log_dir, "application.log"),
+ when="midnight", # 每天 0 點輪替
+ backupCount=7, # 保留 7 天
+ encoding="utf-8"
+ )
+ file_handler.setFormatter(formatter)
+ _global_logger.addHandler(file_handler)
+
+ # 終端機輸出
+ console_handler = logging.StreamHandler()
+ console_handler.setFormatter(formatter)
+ _global_logger.addHandler(console_handler)
+
+ # 為每個模組建立子 Logger，並設定名稱
+ module_logger = _global_logger.getChild(name)
+ module_logger.name = name # 修改子 Logger 的名稱，僅保留子 Logger 名稱
+ return module_logger
diff --git a/src/fc_network_adapter/package.xml b/src/fc_network_adapter/package.xml
new file mode 100644
index 0000000..7f65623
--- /dev/null
+++ b/src/fc_network_adapter/package.xml
@@ -0,0 +1,18 @@
+
+
+
+ fc_network_adapter
+ 0.0.0
+ TODO: Package description
+ picars
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/src/fc_network_adapter/resource/fc_network_adapter b/src/fc_network_adapter/resource/fc_network_adapter
new file mode 100644
index 0000000..e69de29
diff --git a/src/fc_network_adapter/setup.cfg b/src/fc_network_adapter/setup.cfg
new file mode 100644
index 0000000..d4fbae2
--- /dev/null
+++ b/src/fc_network_adapter/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/fc_network_adapter
+[install]
+install_scripts=$base/lib/fc_network_adapter
diff --git a/src/fc_network_adapter/setup.py b/src/fc_network_adapter/setup.py
new file mode 100644
index 0000000..b28ac96
--- /dev/null
+++ b/src/fc_network_adapter/setup.py
@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'fc_network_adapter'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='picars',
+ maintainer_email='chiyu1468@hotmail.com',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'mavlink_orchestrator = fc_network_adapter.mainOrchestrator:main',
+ ],
+ },
+)
diff --git a/src/fc_network_adapter/tests/__init__.py b/src/fc_network_adapter/tests/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/fc_network_adapter/tests/demo_integration.py b/src/fc_network_adapter/tests/demo_integration.py
new file mode 100644
index 0000000..11ac1d7
--- /dev/null
+++ b/src/fc_network_adapter/tests/demo_integration.py
@@ -0,0 +1,277 @@
+import os
+import sys
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+
+# 基礎功能的 import
+import queue
+import time
+
+# ROS2 的 import
+import rclpy
+
+# mavlink 的 import
+from pymavlink import mavutil
+
+# 自定義的 import
+from ..fc_network_adapter import mavlinkObject as mo
+from ..fc_network_adapter import mavlinkVehicleView as mvv
+# from ..fc_network_adapter import mavlinkDevice as md
+
+# ====================== 分割線 =====================
+
+test_item = 1
+running_time = 3
+
+
+print('test_item : ', test_item)
+
+'''
+測試項 個位數 表示分離的功能
+
+測試項 1X 表示 mavlink_object 的功能 測試連線的能力
+'''
+
+if test_item == 1:
+ print('===> Start of Program .Test ', test_item)
+
+ connection_string="udp:127.0.0.1:14591"
+ mavlink_socket1 = mavutil.mavlink_connection(connection_string)
+ # mavlink_object1 = mo.mavlink_object(mavlink_socket1)
+
+ time.sleep(1)
+
+ print("mark A")
+
+ # print("Socket IP:", mavlink_socket1.target_system)
+ print("Socket port:", mavlink_socket1.port.getsockname())
+
+ # print("=== ", dir(mavlink_socket1.port))
+
+
+elif test_item == 10:
+ # 需要開啟一個 ardupilot 的模擬器
+ # 測試 mavlink_object 放入 ring buffer 的應用
+ print('===> Start of Program .Test ', test_item)
+
+ # 清空 ring buffer
+ mo.stream_bridge_ring.clear()
+ mo.return_packet_ring.clear()
+
+ manager = mo.async_io_manager()
+ manager.start()
+ time.sleep(0.5) # 等待事件循環啟動
+
+ # 初始化輸入通道
+ connection_string="udp:127.0..1:14571"
+ mavlink_socket1 = mavutil.mavlink_connection(connection_string)
+ mavlink_object1 = mo.mavlink_object(mavlink_socket1)
+
+ sock = mavlink_socket1.port
+ print("Socket port:", sock)
+
+ manager.add_mavlink_object(mavlink_object1)
+
+ start_time = time.time()
+ while (time.time() - start_time) < running_time:
+ items_a = mo.stream_bridge_ring.get_all()
+ items_b = mo.return_packet_ring.get_all()
+ try:
+ print(f"data num {len(items_a)} in return_packet_ring, first data : {items_a[0][2]}")
+ except IndexError:
+ print("stream_bridge_ring is empty")
+
+ try:
+ print(f"data num {len(items_b)} in return_packet_ring, first data : {items_b[0][2]}")
+ except IndexError:
+ print("return_packet_ring is empty")
+ time.sleep(1)
+
+ manager.shutdown()
+
+ print('<=== End of Program')
+
+elif test_item == 11:
+ # 需要開啟一個 ardupilot 的模擬器
+ # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況
+ print('===> Start of Program .Test ', test_item)
+
+ # 清空 ring buffer
+ mo.stream_bridge_ring.clear()
+ mo.return_packet_ring.clear()
+
+ manager = mo.async_io_manager()
+ manager.start()
+ time.sleep(0.5) # 等待事件循環啟動
+
+ # 初始化輸入通道
+ connection_string="udp:127.0.0.1:14571"
+ mavlink_socket1 = mavutil.mavlink_connection(connection_string)
+ mavlink_object1 = mo.mavlink_object(mavlink_socket1)
+ manager.add_mavlink_object(mavlink_object1)
+
+ # 啟動 mavlink_bridge
+ bridge = mo.mavlink_bridge()
+ bridge.start()
+
+ time.sleep(3)
+
+ # 印出目前所有 mavlink_systems 的內容
+ print('目前所有的系統 : ')
+ all_vehicles = mvv.vehicle_registry.get_all()
+ for sysid, vehicle in all_vehicles.items():
+ print(f" System {sysid}: {vehicle}")
+
+ start_time = time.time()
+ show_time = time.time()
+ while time.time() - start_time < running_time:
+ if (time.time() - show_time) >= 2:
+ # print("mark B")
+
+ show_time = time.time()
+ for sysid, vehicle in all_vehicles.items():
+ for compid in vehicle.components:
+ comp = vehicle.get_component(compid)
+ print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, comp.packet_stats.received_count))
+ comp.reset_packet_stats()
+ print("===================")
+
+ manager.shutdown()
+ bridge.stop()
+
+ print('<=== End of Program')
+
+elif test_item == 12:
+ # 需要開啟一個 ardupilot 的模擬器 與 GCS
+ # 這邊是測試 mavlink object 作為交換器功能的代碼
+ print('===> Start of Program .Test ', test_item)
+
+ # 清空 ring buffer
+ mo.stream_bridge_ring.clear()
+ mo.return_packet_ring.clear()
+
+ manager = mo.async_io_manager()
+ manager.start()
+ time.sleep(0.5) # 等待事件循環啟動
+
+ # 初始化輸入通道
+ connection_string="udp:127.0.0.1:14571"
+ mavlink_socket_in1 = mavutil.mavlink_connection(connection_string)
+ mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1)
+
+ connection_string="udp:127.0.0.1:14571"
+ mavlink_socket_in2 = mavutil.mavlink_connection(connection_string)
+ mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2)
+
+ # 初始化輸出通道
+ connection_string="udpout:127.0.0.1:14551"
+ mavlink_socket_out = mavutil.mavlink_connection(connection_string)
+ mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
+
+ manager.add_mavlink_object(mavlink_object_out)
+ manager.add_mavlink_object(mavlink_object_in1)
+ manager.add_mavlink_object(mavlink_object_in2)
+
+ time.sleep(1) # 等待通道啟動
+
+ mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id)
+ mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id)
+
+ mavlink_object_in2.add_target_socket(mavlink_object_out.socket_id)
+ mavlink_object_out.add_target_socket(mavlink_object_in2.socket_id)
+
+ start_time = time.time()
+ while (time.time() - start_time) < running_time:
+
+ time.sleep(1)
+
+ manager.shutdown()
+
+ print('<=== End of Program')
+
+
+
+elif test_item == 21:
+ # 需要開啟一個 ardupilot 的模擬器
+ # 這邊是測試代碼 引入 rclpy 來測試 node 的運行
+
+ print('===> Start of Program .Test ', test_item)
+ # 初始化 rclpy 才能使用 node
+ rclpy.init()
+
+ # 清空 ring buffer
+ mo.stream_bridge_ring.clear()
+ mo.return_packet_ring.clear()
+
+ manager = mo.async_io_manager()
+ manager.start()
+
+ # 啟動 mavlink_bridge
+ analyzer = mo.mavlink_bridge()
+ # 關於 Node 的初始化
+ show_time = time.time()
+ analyzer._init_node() # 初始化 node
+ print('初始化 node 完成 耗時 : ',time.time() - show_time)
+
+ time.sleep(0.5) # 系統 Setup 完成
+
+
+ # 創建通道
+ connection_string="udp:127.0.0.1:14560"
+ mavlink_socket = mavutil.mavlink_connection(connection_string)
+ mavlink_object3 = mo.mavlink_object(mavlink_socket)
+ manager.add_mavlink_object(mavlink_object3)
+
+ print('=== waiting for mavlink data ...')
+ time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
+
+ print('目前所有的系統 : ')
+ for sysid in analyzer.mavlink_systems:
+ print(analyzer.mavlink_systems[sysid])
+
+ compid = 1
+ sysid = 1
+ start_time = time.time()
+ analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
+ end_time = time.time()
+ print(f"Execution time for create_flightMode: {end_time - start_time} seconds")
+
+ print("start emit info")
+
+ start_time = time.time()
+ show_time = time.time()
+ while time.time() - start_time < running_time:
+ try:
+ # print(analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode'])
+ analyzer.emit_info() # 這邊是測試 node 的運行
+ time.sleep(1)
+ except KeyboardInterrupt:
+ break
+
+
+ # 程式結束
+ analyzer.destroy_node()
+ rclpy.shutdown()
+
+ # 結束程式 退出所有 thread
+ manager.stop()
+ analyzer.stop()
+ analyzer.thread.join()
+
+ mavlink_socket.close()
+ print('<=== End of Program')
+
+
+elif test_item == 52:
+ print('===> Start of Program .Test ', test_item)
+
+ manager = mo.async_io_manager()
+ manager.start()
+
+ # print(manager.thread.is_alive())
+
+ manager.shutdown()
+
+ time.sleep(1)
+
+ print('manager stopped')
+
diff --git a/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py b/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py
new file mode 100644
index 0000000..d6954d3
--- /dev/null
+++ b/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py
@@ -0,0 +1,331 @@
+"""
+VehicleView 使用範例
+展示如何使用純狀態容器來管理 MAVLink 載具資訊
+"""
+
+import time
+from ..fc_network_adapter.mavlinkVehicleView import (
+ VehicleView,
+ VehicleComponent,
+ RFModule,
+ vehicle_registry,
+ ConnectionType,
+ ComponentType,
+ RFModuleType
+)
+
+
+def example_basic_usage():
+ """基本使用範例"""
+ print("=== 基本使用範例 ===\n")
+
+ # 1. 建立載具視圖
+ vehicle = VehicleView(sysid=1)
+ vehicle.kind = "Copter"
+ vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR
+ vehicle.connected_via = ConnectionType.UDP
+
+ print(f"建立載具: {vehicle}\n")
+
+ # 2. 新增 autopilot 組件
+ autopilot = vehicle.add_component(
+ component_id=1,
+ comp_type=ComponentType.AUTOPILOT
+ )
+ autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR
+ autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA
+
+ print(f"新增組件: {autopilot}\n")
+
+ # 3. 手動餵入位置資訊
+ autopilot.status.position.latitude = 25.0330
+ autopilot.status.position.longitude = 121.5654
+ autopilot.status.position.altitude = 100.5
+ autopilot.status.position.timestamp = time.time()
+
+ print(f"位置: 緯度={autopilot.status.position.latitude}, "
+ f"經度={autopilot.status.position.longitude}, "
+ f"高度={autopilot.status.position.altitude}m\n")
+
+ # 4. 手動餵入姿態資訊
+ autopilot.status.attitude.roll = 0.05 # 弧度
+ autopilot.status.attitude.pitch = -0.02
+ autopilot.status.attitude.yaw = 1.57
+ autopilot.status.attitude.timestamp = time.time()
+
+ print(f"姿態: Roll={autopilot.status.attitude.roll:.3f}, "
+ f"Pitch={autopilot.status.attitude.pitch:.3f}, "
+ f"Yaw={autopilot.status.attitude.yaw:.3f} rad\n")
+
+ # 5. 手動餵入飛行模式
+ autopilot.status.mode.base_mode = 89
+ autopilot.status.mode.custom_mode = 4
+ autopilot.status.mode.mode_name = "GUIDED"
+ autopilot.status.mode.timestamp = time.time()
+
+ print(f"飛行模式: {autopilot.status.mode.mode_name}\n")
+
+ # 6. 手動餵入電池資訊
+ autopilot.status.battery.voltage = 12.6
+ autopilot.status.battery.current = 15.2
+ autopilot.status.battery.remaining = 75
+ autopilot.status.battery.timestamp = time.time()
+
+ print(f"電池: 電壓={autopilot.status.battery.voltage}V, "
+ f"電流={autopilot.status.battery.current}A, "
+ f"剩餘={autopilot.status.battery.remaining}%\n")
+
+
+def example_packet_tracking():
+ """封包追蹤範例"""
+ print("\n=== 封包追蹤範例 ===\n")
+
+ vehicle = VehicleView(sysid=2)
+ autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+
+ # 模擬接收封包
+ timestamp = time.time()
+
+ # 接收 HEARTBEAT (msg_type=0)
+ autopilot.update_packet_stats(seq=0, msg_type=0, timestamp=timestamp)
+
+ # 接收 ATTITUDE (msg_type=30)
+ autopilot.update_packet_stats(seq=1, msg_type=30, timestamp=timestamp+0.1)
+
+ # 接收 GLOBAL_POSITION_INT (msg_type=33)
+ autopilot.update_packet_stats(seq=2, msg_type=33, timestamp=timestamp+0.2)
+
+ # 模擬封包遺失 (seq 跳過 3, 4, 5)
+ autopilot.update_packet_stats(seq=6, msg_type=0, timestamp=timestamp+0.3)
+
+ stats = autopilot.packet_stats
+ print(f"封包統計:")
+ print(f" 接收: {stats.received_count}")
+ print(f" 遺失: {stats.lost_count}")
+ print(f" 最後序號: {stats.last_seq}")
+ print(f" 訊息類型計數: {stats.msg_type_count}\n")
+
+
+def example_parameters():
+ """參數管理範例"""
+ print("\n=== 參數管理範例 ===\n")
+
+ vehicle = VehicleView(sysid=3)
+ autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+
+ # 手動設定參數 (不會主動下載)
+ autopilot.set_parameter("ARMING_CHECK", 1)
+ autopilot.set_parameter("ANGLE_MAX", 4500)
+ autopilot.set_parameter("WPNAV_SPEED", 500)
+
+ print(f"參數數量: {len(autopilot.parameters)}")
+ print(f"ARMING_CHECK = {autopilot.get_parameter('ARMING_CHECK')}")
+ print(f"ANGLE_MAX = {autopilot.get_parameter('ANGLE_MAX')}")
+ print(f"WPNAV_SPEED = {autopilot.get_parameter('WPNAV_SPEED')}\n")
+
+
+def example_rf_module():
+ """RF模組範例"""
+ print("\n=== RF模組範例 ===\n")
+
+ vehicle = VehicleView(sysid=4)
+ vehicle.connected_via = ConnectionType.SERIAL
+
+ # 設定 XBee RF 模組
+ rf = vehicle.set_rf_module(RFModuleType.XBEE)
+
+ # 更新 Socket 資訊
+ rf.update_socket_info(
+ ip="192.168.1.100",
+ port=14550,
+ local_ip="192.168.1.1",
+ local_port=14551,
+ connected=True
+ )
+
+ # 更新 RSSI
+ rf.update_rssi(rssi=-65, timestamp=time.time())
+
+ # 更新 AT 命令回應
+ rf.update_at_response("OK", timestamp=time.time())
+
+ # 自定義狀態
+ rf.status.custom_status['signal_quality'] = 'excellent'
+ rf.status.custom_status['packet_error_rate'] = 0.001
+
+ print(f"RF模組: {rf}")
+ print(f"Socket: {rf.socket_info.ip}:{rf.socket_info.port}")
+ print(f"RSSI: {rf.status.rssi} dBm")
+ print(f"AT回應: {rf.status.at_response}")
+ print(f"自定義狀態: {rf.status.custom_status}\n")
+
+
+def example_multiple_components():
+ """多組件範例"""
+ print("\n=== 多組件範例 ===\n")
+
+ vehicle = VehicleView(sysid=5)
+ vehicle.kind = "Copter with Gimbal"
+
+ # Autopilot 組件
+ autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+ autopilot.mav_type = 2
+ autopilot.status.mode.mode_name = "AUTO"
+
+ # Gimbal 組件
+ gimbal = vehicle.add_component(154, ComponentType.GIMBAL)
+ gimbal.mav_type = 26 # MAV_TYPE_GIMBAL
+ gimbal.status.attitude.pitch = -0.785 # 向下45度
+ gimbal.status.attitude.yaw = 0.0
+
+ # Camera 組件
+ camera = vehicle.add_component(100, ComponentType.CAMERA)
+ camera.mav_type = 30 # MAV_TYPE_CAMERA
+ camera.status.custom_status['recording'] = True
+ camera.status.custom_status['photo_interval'] = 2.0
+
+ print(f"載具: {vehicle}")
+ print(f"組件數量: {len(vehicle.components)}")
+ for cid, comp in vehicle.components.items():
+ print(f" 組件 {cid}: {comp.type.value}, MAV_TYPE={comp.mav_type}")
+ print()
+
+
+def example_registry():
+ """註冊表使用範例"""
+ print("\n=== 註冊表使用範例 ===\n")
+
+ # 註冊多個載具
+ v1 = vehicle_registry.register(sysid=1)
+ v1.kind = "Copter-1"
+ v1.add_component(1, ComponentType.AUTOPILOT)
+
+ v2 = vehicle_registry.register(sysid=2)
+ v2.kind = "Plane-1"
+ v2.add_component(1, ComponentType.AUTOPILOT)
+
+ v3 = vehicle_registry.register(sysid=3)
+ v3.kind = "Rover-1"
+ v3.add_component(1, ComponentType.AUTOPILOT)
+
+ print(f"註冊表中的載具數量: {len(vehicle_registry)}")
+
+ # 取得所有載具
+ all_vehicles = vehicle_registry.get_all()
+ for sysid, vehicle in all_vehicles.items():
+ print(f" System {sysid}: {vehicle.kind}")
+
+ # 檢查載具是否存在
+ print(f"\nSystem 2 存在? {2 in vehicle_registry}")
+ print(f"System 99 存在? {99 in vehicle_registry}")
+
+ # 取得特定載具
+ vehicle = vehicle_registry.get(2)
+ if vehicle:
+ print(f"\n取得載具: {vehicle}")
+
+ # 註銷載具
+ vehicle_registry.unregister(3)
+ print(f"\n註銷 System 3 後，剩餘載具: {len(vehicle_registry)}\n")
+
+
+def example_serialization():
+ """序列化範例 (除錯/日誌用)"""
+ print("\n=== 序列化範例 ===\n")
+
+ vehicle = VehicleView(sysid=10)
+ vehicle.kind = "Test Copter"
+ vehicle.connected_via = ConnectionType.UDP
+ vehicle.custom_meta['firmware'] = 'ArduCopter 4.3.0'
+ vehicle.custom_meta['frame_type'] = 'X'
+
+ autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+ autopilot.mav_type = 2
+ autopilot.status.position.altitude = 50.0
+ autopilot.status.battery.voltage = 12.4
+ autopilot.update_packet_stats(0, 0, time.time())
+ autopilot.update_packet_stats(1, 30, time.time())
+
+ rf = vehicle.set_rf_module(RFModuleType.UDP)
+ rf.update_rssi(-70)
+ rf.update_socket_info(ip="192.168.1.200", port=14550, connected=True)
+
+ # 轉換為字典
+ data = vehicle.to_dict()
+
+ print("載具資料 (字典格式):")
+ import json
+ print(json.dumps(data, indent=2, ensure_ascii=False))
+
+
+def example_gps_ekf():
+ """GPS 與 EKF 範例"""
+ print("\n\n=== GPS 與 EKF 範例 ===\n")
+
+ vehicle = VehicleView(sysid=11)
+ autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+
+ # GPS 資訊
+ autopilot.status.gps.fix_type = 3 # 3D Fix
+ autopilot.status.gps.satellites_visible = 12
+ autopilot.status.gps.eph = 120 # HDOP = 1.2
+ autopilot.status.gps.epv = 180 # VDOP = 1.8
+ autopilot.status.gps.timestamp = time.time()
+
+ print(f"GPS:")
+ print(f" Fix Type: {autopilot.status.gps.fix_type}")
+ print(f" 衛星數: {autopilot.status.gps.satellites_visible}")
+ print(f" HDOP: {autopilot.status.gps.eph/100}")
+
+ # EKF 資訊
+ autopilot.status.ekf.flags = 0x1FF # 所有 flags 都 OK
+ autopilot.status.ekf.velocity_variance = 0.5
+ autopilot.status.ekf.pos_horiz_variance = 1.2
+ autopilot.status.ekf.pos_vert_variance = 2.0
+ autopilot.status.ekf.timestamp = time.time()
+
+ print(f"\nEKF:")
+ print(f" Flags: 0x{autopilot.status.ekf.flags:X}")
+ print(f" 速度變異: {autopilot.status.ekf.velocity_variance}")
+ print(f" 水平位置變異: {autopilot.status.ekf.pos_horiz_variance}")
+ print(f" 垂直位置變異: {autopilot.status.ekf.pos_vert_variance}\n")
+
+
+def example_vfr_hud():
+ """VFR HUD 範例"""
+ print("\n=== VFR HUD 範例 ===\n")
+
+ vehicle = VehicleView(sysid=12)
+ autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+
+ # VFR HUD 資訊
+ autopilot.status.vfr.airspeed = 15.5 # m/s
+ autopilot.status.vfr.groundspeed = 14.8 # m/s
+ autopilot.status.vfr.heading = 90 # 東方
+ autopilot.status.vfr.throttle = 65 # %
+ autopilot.status.vfr.climb = 2.5 # m/s
+ autopilot.status.vfr.timestamp = time.time()
+
+ print(f"VFR HUD:")
+ print(f" 空速: {autopilot.status.vfr.airspeed} m/s")
+ print(f" 地速: {autopilot.status.vfr.groundspeed} m/s")
+ print(f" 航向: {autopilot.status.vfr.heading}°")
+ print(f" 油門: {autopilot.status.vfr.throttle}%")
+ print(f" 爬升率: {autopilot.status.vfr.climb} m/s\n")
+
+
+if __name__ == "__main__":
+ # 執行所有範例
+ # example_basic_usage()
+ # example_packet_tracking()
+ # example_parameters()
+ # example_rf_module()
+ # example_multiple_components()
+ # example_registry()
+ # example_serialization()
+ # example_gps_ekf()
+ example_vfr_hud()
+
+ print("\n" + "="*50)
+ print("所有範例執行完成！")
+ print("="*50)
diff --git a/src/fc_network_adapter/tests/demo_ringBuffer.py b/src/fc_network_adapter/tests/demo_ringBuffer.py
new file mode 100644
index 0000000..a01ed73
--- /dev/null
+++ b/src/fc_network_adapter/tests/demo_ringBuffer.py
@@ -0,0 +1,152 @@
+import os
+import sys
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+
+import time
+import threading
+
+from ..fc_network_adapter.utils import RingBuffer
+
+
+def producer(buffer, count, interval=0.01):
+ """生產者：向緩衝區添加資料"""
+ print(f"Producer started (thread {threading.get_ident()})")
+ for i in range(count):
+ # 嘗試寫入數據，直到成功
+ while not buffer.put(f"Item-{i}"):
+ print(f"Buffer full, producer waiting... (thread {threading.get_ident()})")
+ time.sleep(0.1)
+
+ print(f"Produced: Item-{i}, buffer size: {buffer.size()}")
+ time.sleep(interval) # 模擬生產過程
+
+ print(f"Producer finished (thread {threading.get_ident()})")
+
+def consumer(buffer, max_items, interval=0.05):
+ """消費者：從緩衝區讀取資料"""
+ print(f"Consumer started (thread {threading.get_ident()})")
+ items_consumed = 0
+
+ while items_consumed < max_items:
+ # 嘗試讀取數據
+ item = buffer.get()
+ if item:
+ print(f"Consumed: {item}, buffer size: {buffer.size()}")
+ items_consumed += 1
+ else:
+ print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})")
+
+ time.sleep(interval) # 模擬消費過程
+
+ print(f"Consumer finished (thread {threading.get_ident()})")
+
+def batch_consumer(buffer, interval=0.2):
+ """批量消費者：一次性讀取緩衝區中的所有資料"""
+ print(f"Batch consumer started (thread {threading.get_ident()})")
+
+ for _ in range(5): # 執行5次批量讀取
+ time.sleep(interval) # 等待緩衝區積累數據
+ items = buffer.get_all()
+ if items:
+ print(f"Batch consumed {len(items)} items: {items}")
+ else:
+ print("Buffer empty for batch consumer")
+
+ print(f"Batch consumer finished (thread {threading.get_ident()})")
+
+def demonstrate_multi_writer():
+ """示範多個寫入執行緒同時操作緩衝區"""
+ print("\n=== Demonstrating Multiple Writers ===")
+ buffer = RingBuffer(capacity=80)
+
+ # 創建多個生產者執行緒
+ threads = []
+ for i in range(3):
+ thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1)))
+ threads.append(thread)
+ thread.start()
+
+ # 等待所有執行緒完成
+ for thread in threads:
+ thread.join()
+
+ buffer.print_stats() # 印出統計資訊
+
+ # 讀出所有剩餘資料
+ remaining = buffer.get_all()
+ print(f"Remaining items in buffer after multiple writers: {remaining}")
+
+def demonstrate_basic_usage():
+ """示範基本使用方式"""
+ print("\n=== Demonstrating Basic Usage ===")
+ # 創建緩衝區
+ buffer = RingBuffer(capacity=20, buffer_id=7)
+
+ # 檢查初始狀態
+ print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
+
+ # 添加幾個項目
+ for i in range(5):
+ buffer.put(f"Test-{i}")
+
+ # 檢查狀態
+ print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
+
+ # 讀取一個項目
+ item = buffer.get()
+ print(f"Read item: {item}")
+ print(f"After reading 1 item - Content Size: {buffer.size()}")
+
+ # 添加更多項目直到滿
+ items_added = 0
+ while not buffer.is_full():
+ buffer.put(f"Fill-{items_added}")
+ items_added += 1
+
+ print(f"Added {items_added} more items until full")
+ print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}")
+
+ # 嘗試添加到已滿的緩衝區
+ result = buffer.put("Overflow")
+ print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}")
+
+ # 獲取所有項目
+ all_items = buffer.get_all()
+ print(f"All items in buffer: {all_items}")
+ print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}")
+
+ # 印出統計資訊
+ buffer.print_stats()
+
+def demonstrate_producer_consumer():
+ """示範生產者-消費者模式"""
+ print("\n=== Demonstrating Producer-Consumer Pattern ===")
+ buffer = RingBuffer(capacity=16)
+
+ # 創建生產者和消費者執行緒
+ producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1))
+ consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2))
+ batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5))
+
+ # 啟動執行緒
+ producer_thread.start()
+ consumer_thread.start()
+ batch_thread.start()
+
+ # 等待執行緒完成
+ producer_thread.join()
+ consumer_thread.join()
+ batch_thread.join()
+
+ # 檢查最終狀態
+ print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}")
+
+ buffer.print_stats()
+
+if __name__ == "__main__":
+ # 展示各種使用場景
+ # demonstrate_basic_usage()
+ # demonstrate_producer_consumer()
+ demonstrate_multi_writer()
+
+ print("\nAll demonstrations completed!")
\ No newline at end of file
diff --git a/src/fc_network_adapter/tests/example_mavlink_client.py b/src/fc_network_adapter/tests/example_mavlink_client.py
new file mode 100644
index 0000000..9f59c33
--- /dev/null
+++ b/src/fc_network_adapter/tests/example_mavlink_client.py
@@ -0,0 +1,180 @@
+"""
+MavlinkCommandService 使用範例
+
+展示如何從其他 ROS2 節點調用 MAVLink 指令服務
+"""
+
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import Trigger
+import time
+
+
+class MavlinkClientExample(Node):
+ """
+ 範例：MAVLink Service Client
+
+ 這個節點展示如何調用 MavlinkCommandService 提供的服務
+ """
+
+ def __init__(self):
+ super().__init__('mavlink_client_example')
+
+ # 創建 service client
+ self.client = self.create_client(
+ Trigger,
+ '/mavlink/send_command_long'
+ )
+
+ # 等待服務可用
+ self.get_logger().info('等待 service 可用...')
+ self.client.wait_for_service()
+ self.get_logger().info('Service 已連接！')
+
+ def send_arm_command(self):
+ """
+ 範例：發送 ARM 指令
+
+ 實際使用時應該發送：
+ - message_id = 76 (COMMAND_LONG)
+ - command = 400 (MAV_CMD_COMPONENT_ARM_DISARM)
+ - param1 = 1 (ARM)
+ """
+ self.get_logger().info('發送 ARM 指令...')
+
+ request = Trigger.Request()
+ # TODO: 實際使用時應該是自定義的 SendCommandLong.Request
+ # request.target_sysid = 1
+ # request.target_compid = 1
+ # request.command = 400 # MAV_CMD_COMPONENT_ARM_DISARM
+ # request.param1 = 1.0 # 1 = ARM, 0 = DISARM
+ # request.param2 = 0.0
+ # ... param3-7
+ # request.timeout = 3.0
+
+ # 異步調用
+ future = self.client.call_async(request)
+
+ # 等待結果
+ rclpy.spin_until_future_complete(self, future)
+
+ if future.done():
+ response = future.result()
+ if response.success:
+ self.get_logger().info('✓ ARM 指令發送成功')
+ else:
+ self.get_logger().error(f'✗ ARM 指令失敗: {response.message}')
+ else:
+ self.get_logger().error('✗ Service 調用超時')
+
+
+def main():
+ """主函數"""
+ rclpy.init()
+
+ # 創建客戶端節點
+ client = MavlinkClientExample()
+
+ # 發送指令
+ client.send_arm_command()
+
+ # 清理
+ client.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
+
+
+"""
+═══════════════════════════════════════════════════════════════════════════
+進階使用範例：高層應用程式
+
+假設我們要創建一個 "任務控制器"，通過 MavlinkCommandService 來控制載具：
+
+```python
+class MissionController(Node):
+ def __init__(self):
+ super().__init__('mission_controller')
+
+ # 創建各種 service clients
+ self.client_arm = self.create_client(
+ SendCommandLong, '/mavlink/send_command_long')
+ self.client_mode = self.create_client(
+ SendCommandLong, '/mavlink/send_command_long')
+ self.client_takeoff = self.create_client(
+ SendCommandLong, '/mavlink/send_command_long')
+ self.client_goto = self.create_client(
+ SendCommandInt, '/mavlink/send_command_int')
+
+ def arm_vehicle(self, sysid=1):
+ \"\"\"解鎖載具\"\"\"
+ request = SendCommandLong.Request()
+ request.target_sysid = sysid
+ request.target_compid = 1
+ request.command = 400 # MAV_CMD_COMPONENT_ARM_DISARM
+ request.param1 = 1.0 # ARM
+
+ future = self.client_arm.call_async(request)
+ # 處理回應...
+
+ def set_mode(self, sysid=1, mode='GUIDED'):
+ \"\"\"設置飛行模式\"\"\"
+ # 實現模式切換邏輯...
+ pass
+
+ def takeoff(self, sysid=1, altitude=10.0):
+ \"\"\"起飛\"\"\"
+ request = SendCommandLong.Request()
+ request.target_sysid = sysid
+ request.target_compid = 1
+ request.command = 22 # MAV_CMD_NAV_TAKEOFF
+ request.param7 = altitude
+
+ future = self.client_takeoff.call_async(request)
+ # 處理回應...
+
+ def goto_position(self, sysid=1, lat=0, lon=0, alt=10):
+ \"\"\"前往指定位置\"\"\"
+ request = SendCommandInt.Request()
+ request.target_sysid = sysid
+ request.target_compid = 1
+ request.command = 192 # MAV_CMD_DO_REPOSITION
+ request.x = int(lat * 1e7)
+ request.y = int(lon * 1e7)
+ request.z = alt
+
+ future = self.client_goto.call_async(request)
+ # 處理回應...
+
+ def execute_mission(self):
+ \"\"\"執行完整任務\"\"\"
+ # 1. 解鎖
+ self.arm_vehicle()
+ time.sleep(1)
+
+ # 2. 切換到 GUIDED 模式
+ self.set_mode(mode='GUIDED')
+ time.sleep(1)
+
+ # 3. 起飛到 10 公尺
+ self.takeoff(altitude=10.0)
+ time.sleep(10)
+
+ # 4. 前往目標點
+ self.goto_position(lat=25.033, lon=121.565, alt=10)
+ time.sleep(30)
+
+ # 5. 返回並降落
+ # ...
+```
+
+這樣的設計讓高層應用可以：
+1. 完全不需要知道 MAVLink 協議細節
+2. 通過 ROS2 service 與載具互動
+3. 模組化開發不同功能
+4. 易於測試和維護
+
+═══════════════════════════════════════════════════════════════════════════
+"""
diff --git a/src/fc_network_adapter/tests/test_mavlinkObject.py b/src/fc_network_adapter/tests/test_mavlinkObject.py
new file mode 100644
index 0000000..9c3ca78
--- /dev/null
+++ b/src/fc_network_adapter/tests/test_mavlinkObject.py
@@ -0,0 +1,468 @@
+#!/usr/bin/env python
+"""
+測試腳本，用於測試 mavlinkObject.py 中的 mavlink_object 和 async_io_manager 類別
+"""
+
+import os
+import sys
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+
+
+import unittest
+import time
+import threading
+import socket
+import asyncio
+
+# 導入要測試的模組
+from ..fc_network_adapter.mavlinkObject import (
+ mavlink_object,
+ async_io_manager,
+ MavlinkObjectState,
+ stream_bridge_ring,
+ return_packet_ring
+)
+
+# 預先定義好的真實 MAVLink heartbeat 封包 (MAVLink 1.0 格式)
+# Format: STX(0xFE) + LEN + SEQ + SYS + COMP + MSG_ID + PAYLOAD(9 bytes for heartbeat) + CRC(2 bytes)
+HEARTBEAT_PACKET_1 = bytes([
+ 0xFE, # STX (MAVLink 1.0)
+ 0x09, # payload length (9 bytes)
+ 0x00, # sequence
+ 0x01, # system ID = 1
+ 0x01, # component ID = 1
+ 0x00, # message ID (HEARTBEAT = 0)
+ # Payload (9 bytes): custom_mode(4), type(1), autopilot(1), base_mode(1), system_status(1), mavlink_version(1)
+ 0x00, 0x00, 0x00, 0x00, # custom_mode = 0
+ 0x02, # type = MAV_TYPE_QUADROTOR (2)
+ 0x03, # autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA (3)
+ 0x40, # base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (64)
+ 0x03, # system_status = MAV_STATE_STANDBY (3)
+ 0x03, # mavlink_version = 3
+ 0x62, 0x8E # CRC (simplified placeholder)
+])
+
+HEARTBEAT_PACKET_2 = bytes([
+ 0xFE, # STX
+ 0x09, # payload length
+ 0x01, # sequence (增加)
+ 0x01, # system ID = 1
+ 0x01, # component ID = 1
+ 0x00, # message ID (HEARTBEAT = 0)
+ 0x00, 0x00, 0x00, 0x00,
+ 0x02, 0x03, 0x41, 0x03, 0x03,
+ 0x33, 0xEC
+])
+
+HEARTBEAT_PACKET_3 = bytes([
+ 0xFE, # STX
+ 0x09, # payload length
+ 0x02, # sequence
+ 0x02, # system ID = 2
+ 0x01, # component ID = 1
+ 0x00, # message ID (HEARTBEAT = 0)
+ 0x00, 0x00, 0x00, 0x00,
+ 0x02, 0x03, 0x42, 0x03, 0x03,
+ 0x37, 0x44
+])
+
+
+class MockMavlinkSocket:
+ """模擬 Mavlink Socket 的類別，用於測試
+ 使用真實的 MAVLink 封包，而不是模擬的訊息對象
+ """
+
+ def __init__(self, test_packets=None):
+ """
+ Args:
+ test_packets: list of bytes，每個元素都是完整的 MAVLink 封包
+ """
+ self.closed = False
+ self.test_packets = test_packets or []
+ self.packet_index = 0
+ self.written_data = []
+
+ # 使用 pymavlink 來解析封包
+ from pymavlink import mavutil
+ self.mav_parser = mavutil.mavlink.MAVLink(self)
+
+ def recv_msg(self):
+ """返回解析後的 MAVLink 訊息對象"""
+ if not self.test_packets or self.packet_index >= len(self.test_packets):
+ return None
+
+ packet = self.test_packets[self.packet_index]
+ self.packet_index += 1
+
+ # 使用 pymavlink 解析封包
+ try:
+ for byte in packet:
+ msg = self.mav_parser.parse_char(bytes([byte]))
+ if msg:
+ return msg
+ except Exception as e:
+ print(f"Error parsing packet: {e}")
+ return None
+
+ return None
+
+ def write(self, data):
+ """寫入數據（用於檢查轉發）"""
+ self.written_data.append(data)
+
+ def close(self):
+ """關閉 socket"""
+ self.closed = True
+
+
+class TestMavlinkObject(unittest.TestCase):
+ """測試 mavlink_object 類別的獨立功能"""
+
+ def setUp(self):
+ """在每個測試方法執行前準備環境"""
+ # 清空全局變數
+ mavlink_object.mavlinkObjects = {}
+ mavlink_object.socket_num = 0
+
+ # 清空 ring buffer
+ stream_bridge_ring.clear()
+ return_packet_ring.clear()
+
+ # 創建模擬的 socket，使用真實封包
+ self.mock_socket = MockMavlinkSocket([HEARTBEAT_PACKET_1])
+
+ # 創建測試對象
+ self.mavlink_obj = mavlink_object(self.mock_socket)
+
+ def test_initialization(self):
+ """測試 mavlink_object 初始化是否正確"""
+ self.assertEqual(self.mavlink_obj.socket_id, 0)
+ self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT)
+ self.assertEqual(len(self.mavlink_obj.target_sockets), 0)
+ self.assertEqual(self.mavlink_obj.bridge_msg_types, [0])
+ self.assertEqual(self.mavlink_obj.return_msg_types, [])
+
+ def test_add_remove_target_socket(self):
+ """測試添加和移除目標端口功能"""
+ # 添加目標端口
+ self.assertTrue(self.mavlink_obj.add_target_socket(1))
+ self.assertEqual(len(self.mavlink_obj.target_sockets), 1)
+ self.assertEqual(self.mavlink_obj.target_sockets[0], 1)
+
+ self.assertTrue(self.mavlink_obj.add_target_socket(2))
+ self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
+ self.assertIn(2, self.mavlink_obj.target_sockets)
+
+ # 嘗試添加已存在的端口
+ self.assertFalse(self.mavlink_obj.add_target_socket(1))
+ self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
+
+ # 嘗試添加自己的端口
+ self.assertFalse(self.mavlink_obj.add_target_socket(0))
+ self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
+
+ # 移除端口
+ self.assertTrue(self.mavlink_obj.remove_target_socket(2))
+ self.assertEqual(len(self.mavlink_obj.target_sockets), 1)
+
+ # 嘗試移除不存在的端口
+ self.assertFalse(self.mavlink_obj.remove_target_socket(2))
+
+ def test_set_message_types(self):
+ """測試設置訊息類型功能"""
+ # 設置橋接器訊息類型
+ self.assertTrue(self.mavlink_obj.set_bridge_message_types([0, 30]))
+ self.assertEqual(self.mavlink_obj.bridge_msg_types, [0, 30])
+
+ # 設置回傳處理器訊息類型
+ self.assertTrue(self.mavlink_obj.set_return_message_types([32]))
+ self.assertEqual(self.mavlink_obj.return_msg_types, [32])
+
+ # 測試無效的訊息類型
+ self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid"))
+ self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"]))
+
+ def test_send_message_validation(self):
+ """測試 send_message 的數據驗證功能（不需要啟動 manager）"""
+ # 測試非運行狀態下發送消息
+ self.assertFalse(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+
+ # 測試無效的數據類型
+ self.mavlink_obj.state = MavlinkObjectState.RUNNING # 臨時設置狀態
+ self.assertFalse(self.mavlink_obj.send_message("invalid"))
+ self.assertFalse(self.mavlink_obj.send_message(123))
+
+ # 測試太短的封包
+ self.assertFalse(self.mavlink_obj.send_message(bytes([0xFE, 0x00])))
+
+ # 測試無效的起始標記
+ invalid_packet = bytes([0xFF, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00])
+ self.assertFalse(self.mavlink_obj.send_message(invalid_packet))
+
+ # 測試有效的封包可以加入佇列
+ self.assertTrue(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+ self.assertEqual(len(self.mavlink_obj.outgoing_msgs), 1)
+
+ self.mavlink_obj.state = MavlinkObjectState.INIT # 恢復狀態
+
+class TestAsyncIOManager(unittest.TestCase):
+ """測試 async_io_manager 類別的獨立功能"""
+
+ def setUp(self):
+ """在每個測試方法執行前準備環境"""
+ # 清空全局變數
+ mavlink_object.mavlinkObjects = {}
+ mavlink_object.socket_num = 0
+
+ # 清空 ring buffer
+ stream_bridge_ring.clear()
+ return_packet_ring.clear()
+
+ # 創建 async_io_manager 實例
+ self.manager = async_io_manager()
+
+ # 創建模擬 mavlink 對象，使用真實封包
+ self.mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2])
+ self.mock_socket2 = MockMavlinkSocket([HEARTBEAT_PACKET_3])
+
+ self.mavlink_obj1 = mavlink_object(self.mock_socket1)
+ self.mavlink_obj2 = mavlink_object(self.mock_socket2)
+
+ def tearDown(self):
+ """在每個測試方法執行後清理環境"""
+ if self.manager.running:
+ self.manager.shutdown()
+
+ def test_singleton_pattern(self):
+ """測試 async_io_manager 的單例模式"""
+ manager1 = async_io_manager()
+ manager2 = async_io_manager()
+ self.assertIs(manager1, manager2)
+
+ def test_start_stop(self):
+ """測試 async_io_manager 的啟動和停止功能"""
+ # 啟動管理器
+ self.manager.start()
+ self.assertTrue(self.manager.running)
+ self.assertIsNotNone(self.manager.thread)
+
+ # 再次啟動應該沒有效果
+ old_thread = self.manager.thread
+ self.manager.start()
+ self.assertIs(self.manager.thread, old_thread)
+
+ # 停止管理器
+ self.manager.shutdown()
+ self.assertFalse(self.manager.running)
+
+ # 最多等待 5 秒讓線程結束
+ start_time = time.time()
+ while self.manager.thread.is_alive() and time.time() - start_time < 5:
+ time.sleep(0.1)
+ self.assertFalse(self.manager.thread.is_alive())
+
+ def test_add_remove_objects(self):
+ """測試添加和移除 mavlink_object"""
+ # 啟動管理器
+ self.manager.start()
+ time.sleep(0.5) # 等待事件循環啟動
+
+ # 添加對象
+ self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj1))
+ self.assertEqual(len(self.manager.managed_objects), 1)
+ self.assertEqual(self.mavlink_obj1.state, MavlinkObjectState.RUNNING)
+
+ # 添加另一個對象
+ self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj2))
+ self.assertEqual(len(self.manager.managed_objects), 2)
+
+ # 檢查受管理對象列表
+ managed_objects = self.manager.get_managed_objects()
+ self.assertEqual(len(managed_objects), 2)
+ self.assertIn(0, managed_objects)
+ self.assertIn(1, managed_objects)
+
+ # 移除對象
+ self.assertTrue(self.manager.remove_mavlink_object(0))
+ self.assertEqual(len(self.manager.managed_objects), 1)
+
+ # 嘗試移除不存在的對象
+ self.assertFalse(self.manager.remove_mavlink_object(999))
+
+ # 停止管理器
+ self.manager.shutdown()
+
+class TestIntegration(unittest.TestCase):
+ """整合測試，測試多個 mavlink_object 之間的互動與資料流"""
+
+ def setUp(self):
+ """在每個測試方法執行前準備環境"""
+ # 清空全局變數
+ mavlink_object.mavlinkObjects = {}
+ mavlink_object.socket_num = 0
+
+ # 清空 ring buffer
+ stream_bridge_ring.clear()
+ return_packet_ring.clear()
+
+ # 創建 async_io_manager 實例
+ self.manager = async_io_manager()
+
+ def tearDown(self):
+ """在每個測試方法執行後清理環境"""
+ if self.manager.running:
+ self.manager.shutdown()
+
+ def test_send_message_with_manager(self):
+ """測試透過 async_io_manager 發送訊息的完整流程"""
+ # 創建一個新的 mavlink_object 實例
+ mock_socket = MockMavlinkSocket()
+ mavlink_obj = mavlink_object(mock_socket)
+
+ # 測試初始狀態
+ self.assertEqual(len(mock_socket.written_data), 0)
+
+ # 測試非運行狀態下發送消息
+ self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+ self.assertEqual(len(mock_socket.written_data), 0)
+
+ # 啟動 manager
+ self.manager.start()
+ time.sleep(0.5) # 等待事件循環啟動
+
+ # 添加對象到 manager
+ self.manager.add_mavlink_object(mavlink_obj)
+ time.sleep(0.1) # 等待對象啟動
+
+ # 確認對象狀態
+ self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING)
+
+ # 測試發送消息
+ self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+ time.sleep(0.2) # 等待消息處理
+
+ # 確認消息已發送
+ self.assertEqual(len(mock_socket.written_data), 1)
+ self.assertEqual(mock_socket.written_data[0], HEARTBEAT_PACKET_1)
+
+ # 測試連續發送多條消息
+ self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_2))
+ time.sleep(0.2) # 等待消息處理
+
+ # 確認兩條消息都已發送
+ self.assertEqual(len(mock_socket.written_data), 2)
+ self.assertEqual(mock_socket.written_data[1], HEARTBEAT_PACKET_2)
+
+ # 停止 manager
+ self.manager.shutdown()
+ time.sleep(0.5) # 等待 manager 停止
+
+ # 測試對象已關閉後發送消息
+ self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
+ self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加
+
+ def test_data_processing_and_forwarding(self):
+ """測試數據處理與轉發流程"""
+ # 創建用於轉發的 mavlink_objects
+ mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2,])
+ mock_socket3 = MockMavlinkSocket()
+
+ mavlink_obj1 = mavlink_object(mock_socket1)
+ mavlink_obj3 = mavlink_object(mock_socket3)
+
+ # 設置訊息類型
+ mavlink_obj1.set_bridge_message_types([0]) # 只處理 HEARTBEAT
+
+ # 設置轉發: obj1 -> obj3
+ mavlink_obj1.add_target_socket(mavlink_obj3.socket_id) # socket1 轉發到 socket3 (socket_id=1)
+
+ # 啟動管理器並添加對象
+ self.manager.start()
+ time.sleep(0.5) # 等待事件循環啟動
+
+ """
+ 這邊出現很奇怪的狀況 應該說 設計時沒有考量 但是實測會發現
+ mavlink_obj3 是接收端 必需要被優先加入 manager 才能正確接收來自 mavlink_obj1 的轉發封包
+ 若先把 mavlink_ojb1 加入 manger 則可能會導致前面幾個封包丟失
+ """
+ self.manager.add_mavlink_object(mavlink_obj3)
+ self.manager.add_mavlink_object(mavlink_obj1)
+
+ # 等待處理完成
+ time.sleep(0.5)
+
+ # 檢查 Ring buffer 是否有正確的數據
+ self.assertGreaterEqual(stream_bridge_ring.size(), 2) # 至少 2 個 HEARTBEAT
+
+ # 檢查是否正確轉發
+ self.assertGreaterEqual(len(mock_socket3.written_data), 2) # 至少 2 個 HEARTBEAT
+
+ # 停止管理器
+ self.manager.shutdown()
+
+ def test_bidirectional_forwarding(self):
+ """測試雙向轉發"""
+ # 清空全局變數和 ring buffer
+ mavlink_object.mavlinkObjects = {}
+ mavlink_object.socket_num = 0
+ stream_bridge_ring.clear()
+ return_packet_ring.clear()
+
+ # 創建三個 mavlink 對象，模擬三個通道
+ socket1 = MockMavlinkSocket()
+ socket2 = MockMavlinkSocket()
+ socket3 = MockMavlinkSocket()
+
+ obj1 = mavlink_object(socket1)
+ obj2 = mavlink_object(socket2)
+ obj3 = mavlink_object(socket3)
+
+ # 設置雙向轉發
+ # obj1 <-> obj2 <-> obj3
+ obj1.add_target_socket(1) # obj1 -> obj2
+ obj2.add_target_socket(0) # obj2 -> obj1
+ obj2.add_target_socket(2) # obj2 -> obj3
+ obj3.add_target_socket(1) # obj3 -> obj2
+
+ # 啟動 async_io_manager
+ self.manager.start()
+ time.sleep(0.5) # 等待事件循環啟動
+
+ # 添加所有 mavlink_object
+ self.manager.add_mavlink_object(obj1)
+ self.manager.add_mavlink_object(obj2)
+ self.manager.add_mavlink_object(obj3)
+
+ # 對三個對象添加數據
+ socket1.test_packets.append(HEARTBEAT_PACKET_1)
+ socket2.test_packets.append(HEARTBEAT_PACKET_2)
+ socket3.test_packets.append(HEARTBEAT_PACKET_3)
+
+ # 等待處理所有訊息
+ time.sleep(1.0)
+
+ # 檢查轉發結果
+ # socket1 應該收到 socket2 的訊息
+ self.assertGreaterEqual(len(socket1.written_data), 1)
+
+ # socket2 應該收到 socket1 和 socket3 的訊息
+ self.assertGreaterEqual(len(socket2.written_data), 2)
+
+ # socket3 應該收到 socket2 的訊息
+ self.assertGreaterEqual(len(socket3.written_data), 1)
+
+ # 檢查 ring buffer 的數據
+ # 所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0]
+ self.assertGreaterEqual(stream_bridge_ring.size(), 3) # 至少 3 個 HEARTBEAT
+
+ # 停止管理器
+ self.manager.shutdown()
+
+
+if __name__ == "__main__":
+ # 可以指定要運行的測試
+ # unittest.main(defaultTest="TestMavlinkObject.test_send_message_validation")
+ # unittest.main(defaultTest="TestAsyncIOManager.test_add_remove_objects")
+ unittest.main(defaultTest="TestIntegration.test_bidirectional_forwarding")
+ unittest.main()
+
diff --git a/src/fc_network_adapter/tests/test_ringBuffer.py b/src/fc_network_adapter/tests/test_ringBuffer.py
new file mode 100644
index 0000000..287a057
--- /dev/null
+++ b/src/fc_network_adapter/tests/test_ringBuffer.py
@@ -0,0 +1,296 @@
+#!/usr/bin/env python
+"""
+測試 RingBuffer 類別的功能
+"""
+
+import unittest
+import threading
+import time
+from concurrent.futures import ThreadPoolExecutor
+
+from ..fc_network_adapter.utils import RingBuffer
+
+class TestRingBuffer(unittest.TestCase):
+ """測試 RingBuffer 基本功能"""
+
+ def setUp(self):
+ """每個測試前的準備"""
+ self.buffer = RingBuffer(capacity=8)
+
+ def test_initialization(self):
+ """測試初始化"""
+ self.assertEqual(self.buffer.capacity, 8)
+ self.assertTrue(self.buffer.is_empty())
+ self.assertFalse(self.buffer.is_full())
+ self.assertEqual(self.buffer.size(), 0)
+
+ def test_put_get_basic(self):
+ """測試基本的放入和取出"""
+ # 測試放入
+ self.assertTrue(self.buffer.put("item1"))
+ self.assertTrue(self.buffer.put("item2"))
+ self.assertEqual(self.buffer.size(), 2)
+ self.assertFalse(self.buffer.is_empty())
+
+ # 測試取出
+ item1 = self.buffer.get()
+ self.assertEqual(item1, "item1")
+ self.assertEqual(self.buffer.size(), 1)
+
+ item2 = self.buffer.get()
+ self.assertEqual(item2, "item2")
+ self.assertTrue(self.buffer.is_empty())
+
+ # 空緩衝區取出應返回 None
+ self.assertIsNone(self.buffer.get())
+
+ def test_capacity_overflow(self):
+ """測試緩衝區容量限制"""
+ # 填滿緩衝區 (容量-1，因為需要預留一個位置)
+ for i in range(7): # 8-1=7
+ self.assertTrue(self.buffer.put(f"item{i}"))
+
+ self.assertTrue(self.buffer.is_full())
+
+ # 嘗試再放入一個應該失敗
+ self.assertFalse(self.buffer.put("overflow"))
+ self.assertEqual(self.buffer.overflow_count.value, 1)
+
+ def test_get_all(self):
+ """測試取出所有項目"""
+ items = ["a", "b", "c", "d"]
+ for item in items:
+ self.buffer.put(item)
+
+ all_items = self.buffer.get_all()
+ self.assertEqual(all_items, items)
+ self.assertTrue(self.buffer.is_empty())
+
+ def test_clear(self):
+ """測試清空緩衝區"""
+ for i in range(5):
+ self.buffer.put(f"item{i}")
+
+ self.buffer.clear()
+ self.assertTrue(self.buffer.is_empty())
+ self.assertEqual(self.buffer.size(), 0)
+
+
+class TestRingBufferThreadSafety(unittest.TestCase):
+ """測試 RingBuffer 的線程安全性"""
+
+ def setUp(self):
+ """每個測試前的準備"""
+ self.buffer = RingBuffer(capacity=256)
+ self.results = []
+ self.write_count = 1000
+ self.read_count = 1000
+
+ def test_concurrent_producers_consumers(self):
+ """測試多生產者多消費者場景"""
+ self.results = []
+ stats = self.buffer.get_stats()
+ self.assertEqual(stats['total_writes'], 0)
+
+ def producer(producer_id, count):
+ """生產者函數"""
+ for i in range(count):
+ item = f"producer_{producer_id}_item_{i}"
+ while not self.buffer.put(item):
+ time.sleep(0.001) # 緩衝區滿時稍微等待
+
+ def consumer(consumer_id, count):
+ """消費者函數"""
+ items = []
+ for _ in range(count):
+ item = None
+ while item is None:
+ item = self.buffer.get()
+ if item is None:
+ time.sleep(0.001) # 緩衝區空時稍微等待
+ items.append(item)
+ self.results.extend(items)
+
+ # 創建多個生產者和消費者
+ with ThreadPoolExecutor(max_workers=8) as executor:
+ # 2 個生產者，每個寫入 500 個項目
+ producer_futures = [
+ executor.submit(producer, 0, 500),
+ executor.submit(producer, 1, 500)
+ ]
+
+ # 2 個消費者，每個讀取 500 個項目
+ consumer_futures = [
+ executor.submit(consumer, 0, 500),
+ executor.submit(consumer, 1, 500)
+ ]
+
+ # 等待所有任務完成
+ for future in producer_futures + consumer_futures:
+ future.result()
+
+ # 驗證結果
+ self.assertEqual(len(self.results), 1000)
+ self.assertTrue(self.buffer.is_empty())
+
+ # 檢查統計數據
+ stats = self.buffer.get_stats()
+ self.assertEqual(stats['total_writes'], 1000)
+ self.assertGreater(stats['total_reads'], 1000) # 包含失敗的讀取嘗試
+ self.assertGreater(stats['write_threads'], 1)
+ self.assertGreater(stats['read_threads'], 1)
+
+ def test_high_throughput(self):
+ """測試高吞吐量場景"""
+ items_per_thread = 10000
+ num_threads = 4
+
+ def writer():
+ for i in range(items_per_thread):
+ while not self.buffer.put(i):
+ pass # 忙等待
+
+ def reader():
+ items = []
+ for _ in range(items_per_thread):
+ item = None
+ while item is None:
+ item = self.buffer.get()
+ items.append(item)
+ self.results.extend(items)
+
+ start_time = time.time()
+
+ with ThreadPoolExecutor(max_workers=num_threads * 2) as executor:
+ # 啟動寫入線程
+ writer_futures = [executor.submit(writer) for _ in range(num_threads)]
+
+ # 啟動讀取線程
+ reader_futures = [executor.submit(reader) for _ in range(num_threads)]
+
+ # 等待完成
+ for future in writer_futures + reader_futures:
+ future.result()
+
+ end_time = time.time()
+
+ # 驗證結果
+ total_items = items_per_thread * num_threads
+ self.assertEqual(len(self.results), total_items)
+
+ # 性能統計
+ duration = end_time - start_time
+ throughput = total_items / duration
+
+ print(f"\nHigh Throughput Test Results:")
+ print(f"Total items: {total_items}")
+ print(f"Duration: {duration:.2f}s")
+ print(f"Throughput: {throughput:.0f} items/sec")
+
+ # 顯示詳細統計
+ self.buffer.print_stats()
+
+
+class TestRingBufferStatistics(unittest.TestCase):
+ """測試 RingBuffer 的統計功能"""
+
+ def test_statistics_tracking(self):
+ """測試統計數據追蹤"""
+ buffer = RingBuffer(capacity=16)
+
+ # 寫入一些數據
+ for i in range(10):
+ buffer.put(f"item{i}")
+
+ # 讀取一些數據
+ for _ in range(5):
+ buffer.get()
+
+ stats = buffer.get_stats()
+
+ # 驗證基本統計
+ self.assertEqual(stats['total_writes'], 10)
+ self.assertEqual(stats['total_reads'], 5)
+ self.assertEqual(stats['current_size'], 5)
+ self.assertEqual(stats['write_threads'], 1)
+ self.assertEqual(stats['read_threads'], 1)
+
+ def test_reset_statistics(self):
+ """測試重置統計數據"""
+ buffer = RingBuffer(capacity=16)
+
+ # 產生一些活動
+ for i in range(5):
+ buffer.put(f"item{i}")
+ for _ in range(3):
+ buffer.get()
+
+ # 重置統計
+ buffer.reset_stats()
+
+ stats = buffer.get_stats()
+ self.assertEqual(stats['total_writes'], 0)
+ self.assertEqual(stats['total_reads'], 0)
+ self.assertEqual(stats['concurrent_writes'], 0)
+ self.assertEqual(stats['concurrent_reads'], 0)
+ self.assertEqual(stats['overflow_count'], 0)
+
+
+def benchmark_ringbuffer():
+ """RingBuffer 性能基準測試"""
+ print("\n=== RingBuffer Performance Benchmark ===")
+
+ buffer = RingBuffer(capacity=1024)
+ num_operations = 100000
+
+ # 單線程性能測試
+ start_time = time.time()
+ for i in range(num_operations):
+ buffer.put(i)
+ for _ in range(num_operations):
+ buffer.get()
+ end_time = time.time()
+
+ single_thread_time = end_time - start_time
+ throughput = (num_operations * 2) / single_thread_time
+
+ print(f"Single Thread: {throughput:.0f} ops/sec")
+
+ # 多線程性能測試
+ buffer = RingBuffer(capacity=1024)
+
+ def producer():
+ for i in range(num_operations // 2):
+ while not buffer.put(i):
+ pass
+
+ def consumer():
+ for _ in range(num_operations // 2):
+ while buffer.get() is None:
+ pass
+
+ start_time = time.time()
+
+ with ThreadPoolExecutor(max_workers=2) as executor:
+ future1 = executor.submit(producer)
+ future2 = executor.submit(consumer)
+ future1.result()
+ future2.result()
+
+ end_time = time.time()
+
+ multi_thread_time = end_time - start_time
+ throughput = num_operations / multi_thread_time
+
+ print(f"Multi Thread: {throughput:.0f} ops/sec")
+ print(f"Speedup: {single_thread_time/multi_thread_time:.2f}x")
+
+ buffer.print_stats()
+
+
+if __name__ == "__main__":
+ # 運行單元測試
+ unittest.main(argv=[''], exit=False, verbosity=2)
+
+ # 運行性能基準測試
+ benchmark_ringbuffer()
\ No newline at end of file
diff --git a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
new file mode 100644
index 0000000..6fd1914
--- /dev/null
+++ b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
@@ -0,0 +1,507 @@
+"""
+VehicleStatusPublisher 測試程式
+
+測試從 vehicle_registry 讀取資料並發布到 ROS2 topics
+"""
+
+import time
+import json
+import threading
+
+# ROS2 imports
+import rclpy
+from rclpy.node import Node
+
+# 標準 ROS2 消息類型
+import std_msgs.msg
+import sensor_msgs.msg
+import geometry_msgs.msg
+import mavros_msgs.msg
+
+# 專案 imports
+from ..fc_network_adapter.mavlinkROS2Nodes import (
+ VehicleStatusPublisher,
+ fc_ros_manager,
+ ros2_manager
+)
+from ..fc_network_adapter.mavlinkVehicleView import (
+ vehicle_registry,
+ ConnectionType,
+ ComponentType,
+)
+
+
+class TestSubscriber(Node):
+ """測試用的訂閱者節點 - 接收並記錄收到的消息"""
+
+ def __init__(self, sysid: int = 1):
+ super().__init__(f'test_subscriber_sys{sysid}')
+
+ self.sysid = sysid
+ self.received_messages = {
+ 'position': [],
+ 'attitude': [],
+ 'velocity': [],
+ 'battery': [],
+ 'vfr_hud': [],
+ 'mode': [],
+ 'summary': [],
+ }
+
+ # 建立所有訂閱者
+ self._create_subscriptions()
+
+ print(f"[TestSubscriber] 已建立，監聽 sys{sysid} 的所有 topics")
+
+ def _create_subscriptions(self):
+ """建立所有 topic 的訂閱者"""
+
+ base_topic = f'{VehicleStatusPublisher.topicString_prefix}/sys{self.sysid}'
+
+ # Position
+ self.create_subscription(
+ sensor_msgs.msg.NavSatFix,
+ f'{base_topic}/position',
+ lambda msg: self._on_message('position', msg),
+ 10
+ )
+
+ # Attitude
+ self.create_subscription(
+ sensor_msgs.msg.Imu,
+ f'{base_topic}/attitude',
+ lambda msg: self._on_message('attitude', msg),
+ 10
+ )
+
+ # Velocity
+ self.create_subscription(
+ geometry_msgs.msg.TwistStamped,
+ f'{base_topic}/velocity',
+ lambda msg: self._on_message('velocity', msg),
+ 10
+ )
+
+ # Battery
+ self.create_subscription(
+ sensor_msgs.msg.BatteryState,
+ f'{base_topic}/battery',
+ lambda msg: self._on_message('battery', msg),
+ 10
+ )
+
+ # VFR HUD
+ self.create_subscription(
+ mavros_msgs.msg.VfrHud,
+ f'{base_topic}/vfr_hud',
+ lambda msg: self._on_message('vfr_hud', msg),
+ 10
+ )
+
+ # Mode
+ self.create_subscription(
+ std_msgs.msg.String,
+ f'{base_topic}/mode',
+ lambda msg: self._on_message('mode', msg),
+ 10
+ )
+
+ # Summary
+ self.create_subscription(
+ std_msgs.msg.String,
+ f'{base_topic}/summary',
+ lambda msg: self._on_message('summary', msg),
+ 10
+ )
+
+ def _on_message(self, topic_name: str, msg):
+ """通用消息接收回調"""
+ self.received_messages[topic_name].append(msg)
+ print(f"[TestSubscriber] 收到 {topic_name}: {self._format_msg(topic_name, msg)}")
+
+ def _format_msg(self, topic_name: str, msg) -> str:
+ """格式化消息以便顯示"""
+ if topic_name == 'position':
+ return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
+ elif topic_name == 'attitude':
+ return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
+ elif topic_name == 'velocity':
+ return f"linear=({msg.twist.linear.x:.2f}, {msg.twist.linear.y:.2f}, {msg.twist.linear.z:.2f})"
+ elif topic_name == 'battery':
+ return f"voltage={msg.voltage:.2f}V, percent={msg.percentage*100:.1f}%"
+ elif topic_name == 'vfr_hud':
+ return f"airspeed={msg.airspeed:.2f}, groundspeed={msg.groundspeed:.2f}, heading={msg.heading}"
+ elif topic_name == 'mode':
+ return f"mode={msg.data}"
+ elif topic_name == 'summary':
+ try:
+ data = json.loads(msg.data)
+ return f"sysid={data['sysid']}, socket_id={data['socket_id']}, mode={data['mode_name']}"
+ except:
+ return msg.data
+ return str(msg)
+
+ def get_message_count(self, topic_name: str) -> int:
+ """獲取收到的消息數量"""
+ return len(self.received_messages[topic_name])
+
+ def clear_messages(self):
+ """清空已收到的消息"""
+ for key in self.received_messages:
+ self.received_messages[key].clear()
+
+
+def setup_test_vehicle(sysid: int = 1, socket_id: int = 10):
+ """
+ 建立測試用的載具數據
+
+ Args:
+ sysid: 系統 ID
+ socket_id: Socket ID
+ """
+ print(f"\n=== 建立測試載具 (sysid={sysid}, socket_id={socket_id}) ===")
+
+ # 註冊載具
+ vehicle = vehicle_registry.register(sysid)
+ vehicle.kind = "Copter"
+ vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR
+ vehicle.connected_via = ConnectionType.UDP
+ vehicle.custom_meta['socket_id'] = socket_id
+
+ # 新增 autopilot 組件 (component_id=1)
+ autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
+ autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR
+ autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA
+
+ # 填充狀態資料
+ status = autopilot.status
+
+ # 位置
+ status.position.latitude = 25.0330
+ status.position.longitude = 121.5654
+ status.position.altitude = 100.5
+ status.position.relative_altitude = 50.0
+ status.position.timestamp = time.time()
+
+ # 姿態
+ status.attitude.roll = 0.1
+ status.attitude.pitch = -0.05
+ status.attitude.yaw = 1.57
+ status.attitude.rollspeed = 0.01
+ status.attitude.pitchspeed = 0.02
+ status.attitude.yawspeed = 0.03
+ status.attitude.timestamp = time.time()
+
+ # 飛行模式
+ status.mode.base_mode = 89
+ status.mode.custom_mode = 4
+ status.mode.mode_name = "GUIDED"
+ status.mode.timestamp = time.time()
+
+ # 電池
+ status.battery.voltage = 12.6
+ status.battery.current = 15.3
+ status.battery.remaining = 75
+ status.battery.temperature = 35.2
+ status.battery.timestamp = time.time()
+
+ # GPS
+ status.gps.fix_type = 3 # 3D fix
+ status.gps.satellites_visible = 12
+ status.gps.eph = 100
+ status.gps.epv = 150
+ status.gps.timestamp = time.time()
+
+ # VFR
+ status.vfr.airspeed = 5.5
+ status.vfr.groundspeed = 6.0
+ status.vfr.heading = 90
+ status.vfr.throttle = 65
+ status.vfr.climb = 1.2
+ status.vfr.timestamp = time.time()
+
+ # 系統狀態
+ status.armed = True
+ status.system_status = 4 # MAV_STATE_ACTIVE
+
+ # 更新封包統計
+ autopilot.update_packet_stats(seq=10, msg_type=33, timestamp=time.time())
+
+ print(f"✓ 載具 {sysid} 已建立並填充測試數據")
+ return vehicle
+
+
+def test_basic_publishing():
+ """測試基本的發布功能"""
+ print("\n" + "="*60)
+ print("測試 1: 基本發布功能")
+ print("="*60)
+
+ # 清空 registry
+ vehicle_registry.clear()
+
+ # 建立測試載具
+ vehicle = setup_test_vehicle(sysid=1, socket_id=10)
+
+ # 初始化 ROS2 管理器
+ if not ros2_manager.initialized:
+ ros2_manager.initialize()
+
+ # 建立測試訂閱者
+ test_sub = TestSubscriber(sysid=1)
+
+ # 啟動 publisher
+ ros2_manager.start()
+
+ print("\n--- 開始發布，等待 5 秒 ---")
+
+ # 運行 5 秒，持續 spin
+ start_time = time.time()
+ while time.time() - start_time < 5.0:
+ rclpy.spin_once(test_sub, timeout_sec=0.1)
+ time.sleep(0.1)
+
+ # 檢查收到的消息
+ print("\n--- 消息統計 ---")
+ total_messages = 0
+ for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
+ count = test_sub.get_message_count(topic)
+ total_messages += count
+ print(f" {topic:15s}: {count:3d} 條消息")
+
+ print(f"\n總計收到: {total_messages} 條消息")
+
+ # 驗證
+ if total_messages > 0:
+ print("\n✓ 測試通過：成功接收到消息")
+ else:
+ print("\n✗ 測試失敗：沒有接收到任何消息")
+
+ # 停止
+ ros2_manager.stop()
+ test_sub.destroy_node()
+
+
+def test_frequency_control():
+ """測試頻率控制功能"""
+ print("\n" + "="*60)
+ print("測試 2: 頻率控制")
+ print("="*60)
+
+ # 清空 registry
+ vehicle_registry.clear()
+
+ # 建立測試載具
+ vehicle = setup_test_vehicle(sysid=1, socket_id=10)
+
+ # 初始化（如果還沒初始化）
+ if not ros2_manager.initialized:
+ ros2_manager.initialize()
+
+ # 建立測試訂閱者
+ test_sub = TestSubscriber(sysid=1)
+
+ # 修改頻率設定
+ publisher_node = ros2_manager.status_publisher
+ publisher_node.rate_controller.topic_intervals = {
+ 'position': 1.5,
+ 'attitude': 1.0,
+ 'velocity': 1.0,
+ 'battery': 1.0,
+ 'vfr_hud': 0.5,
+ 'mode': 0.0,
+ 'summary': 0.0,
+ }
+
+ # 啟動 publisher
+ ros2_manager.start()
+
+ print("\n--- 測試頻率控制，運行 3 秒 ---")
+
+ # 運行 3 秒
+ start_time = time.time()
+ while time.time() - start_time < 3.0:
+ rclpy.spin_once(test_sub, timeout_sec=0.1)
+ time.sleep(0.1)
+
+ # 檢查頻率
+ print("\n--- 頻率分析 ---")
+ print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
+
+ print("2Hz Topics (預期 ~6 條):")
+ for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
+ count = test_sub.get_message_count(topic)
+ print(f" {topic:15s}: {count:3d} 條")
+ for topic in ['battery', 'mode', 'summary']:
+ count = test_sub.get_message_count(topic)
+ print(f" {topic:15s}: {count:3d} 條")
+
+ print("\n✓ 頻率控制測試完成")
+
+ # 停止
+ ros2_manager.stop()
+ test_sub.destroy_node()
+
+
+def test_multi_vehicle():
+ """測試多載具發布"""
+ print("\n" + "="*60)
+ print("測試 3: 多載具發布")
+ print("="*60)
+
+ # 清空 registry
+ vehicle_registry.clear()
+
+ # 建立 3 個測試載具
+ v1 = setup_test_vehicle(sysid=1, socket_id=10)
+ v2 = setup_test_vehicle(sysid=2, socket_id=11)
+ v3 = setup_test_vehicle(sysid=3, socket_id=12)
+
+ # 修改各載具的位置以便區分
+ v2.components[1].status.position.latitude = 26.0
+ v3.components[1].status.position.latitude = 27.0
+
+ # 初始化
+ if not ros2_manager.initialized:
+ ros2_manager.initialize()
+
+ # 建立 3 個測試訂閱者
+ test_sub1 = TestSubscriber(sysid=1)
+ test_sub2 = TestSubscriber(sysid=2)
+ test_sub3 = TestSubscriber(sysid=3)
+
+ # 啟動 publisher
+ ros2_manager.start()
+
+ print("\n--- 測試多載具，運行 3 秒 ---")
+
+ # 運行 3 秒
+ start_time = time.time()
+ while time.time() - start_time < 3.0:
+ rclpy.spin_once(test_sub1, timeout_sec=0.05)
+ rclpy.spin_once(test_sub2, timeout_sec=0.05)
+ rclpy.spin_once(test_sub3, timeout_sec=0.05)
+ time.sleep(0.1)
+
+ # 檢查每個載具的消息
+ print("\n--- 各載具消息統計 ---")
+ for sysid, test_sub in [(1, test_sub1), (2, test_sub2), (3, test_sub3)]:
+ total = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
+ print(f"載具 {sysid}: {total:3d} 條消息")
+
+ # 檢查 summary 中的 socket_id
+ if test_sub.get_message_count('summary') > 0:
+ last_summary = test_sub.received_messages['summary'][-1]
+ data = json.loads(last_summary.data)
+ print(f" └─ socket_id={data['socket_id']}, lat={data['latitude']:.1f}")
+
+ print("\n✓ 多載具測試完成")
+
+ # 停止
+ ros2_manager.stop()
+ test_sub1.destroy_node()
+ test_sub2.destroy_node()
+ test_sub3.destroy_node()
+
+
+def test_dynamic_vehicle():
+ """測試動態新增/移除載具"""
+ print("\n" + "="*60)
+ print("測試 4: 動態載具管理")
+ print("="*60)
+
+ # 清空 registry
+ vehicle_registry.clear()
+
+ # 初始化
+ if not ros2_manager.initialized:
+ ros2_manager.initialize()
+
+ # 建立測試訂閱者
+ test_sub = TestSubscriber(sysid=1)
+
+ # 啟動 publisher
+ ros2_manager.start()
+
+ print("\n--- 階段 1: 無載具，運行 1 秒 ---")
+ start_time = time.time()
+ while time.time() - start_time < 1.0:
+ rclpy.spin_once(test_sub, timeout_sec=0.1)
+ time.sleep(0.1)
+
+ count_before = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
+ print(f"收到消息: {count_before} 條")
+
+ # 新增載具
+ print("\n--- 階段 2: 新增載具，運行 2 秒 ---")
+ vehicle = setup_test_vehicle(sysid=1, socket_id=10)
+
+ start_time = time.time()
+ while time.time() - start_time < 2.0:
+ rclpy.spin_once(test_sub, timeout_sec=0.1)
+ time.sleep(0.1)
+
+ count_after = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
+ print(f"收到消息: {count_after - count_before} 條")
+
+ # 移除載具
+ print("\n--- 階段 3: 移除載具，運行 1 秒 ---")
+ vehicle_registry.unregister(1)
+
+ start_time = time.time()
+ while time.time() - start_time < 1.0:
+ rclpy.spin_once(test_sub, timeout_sec=0.1)
+ time.sleep(0.1)
+
+ count_final = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys())
+ print(f"收到消息: {count_final - count_after} 條（應該為 0）")
+
+ if count_final - count_after == 0:
+ print("\n✓ 動態載具管理測試通過")
+ else:
+ print("\n✗ 移除載具後仍收到消息")
+
+ # 停止
+ ros2_manager.stop()
+ test_sub.destroy_node()
+
+
+def main():
+ """主測試函數"""
+ print("\n" + "="*60)
+ print("VehicleStatusPublisher 測試程式")
+ print("="*60)
+
+ try:
+ # 執行各項測試
+ test_basic_publishing()
+ # time.sleep(1)
+
+ # test_frequency_control()
+ # time.sleep(1)
+
+ # test_multi_vehicle()
+ # time.sleep(1)
+
+ # test_dynamic_vehicle()
+
+ print("\n" + "="*60)
+ print("所有測試完成！")
+ print("="*60)
+
+ except KeyboardInterrupt:
+ print("\n\n測試被中斷")
+ except Exception as e:
+ print(f"\n\n測試出錯: {e}")
+ import traceback
+ traceback.print_exc()
+ finally:
+ # 清理
+ if ros2_manager.initialized:
+ ros2_manager.shutdown()
+ vehicle_registry.clear()
+ print("\n清理完成")
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/fc_network_apps/__init__.py b/src/fc_network_apps/__init__.py
new file mode 100644
index 0000000..09a5885
--- /dev/null
+++ b/src/fc_network_apps/__init__.py
@@ -0,0 +1,10 @@
+from .longCommand import CommandLongClient, CommandLongResult
+from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
+
+
+__all__ = [
+ "CommandLongClient",
+ "PositionTargetGlobalIntClient",
+ "CommandLongResult",
+ "PositionTargetGlobalIntResult",
+]
diff --git a/src/fc_network_apps/longCommand.py b/src/fc_network_apps/longCommand.py
new file mode 100644
index 0000000..7ebd4bb
--- /dev/null
+++ b/src/fc_network_apps/longCommand.py
@@ -0,0 +1,381 @@
+"""ROS2 client for fc_network MavCommandLong service (COMMAND_LONG 系列指令)."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Optional
+import asyncio
+
+import rclpy
+from rclpy.node import Node
+
+from fc_interfaces.srv import MavCommandLong
+
+COMMAND_DO_SET_MODE = 176
+COMMAND_NAV_LAND = 21
+COMMAND_NAV_TAKEOFF = 22
+COMMAND_COMPONENT_ARM_DISARM = 400
+
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+
+@dataclass
+class CommandLongResult:
+ success: bool
+ message: str
+ ack_result: int
+
+class CommandLongClient(Node):
+ """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
+
+ def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None:
+ if not rclpy.ok():
+ rclpy.init(args=None)
+ # 使用提供的 node_name，或使用帶時間戳的預設名稱以避免名稱衝突
+ if node_name is None:
+ import time
+ node_name = f"fc_command_long_client_{int(time.time() * 1000) % 100000}"
+ super().__init__(node_name)
+ self._client = self.create_client(MavCommandLong, service_name)
+
+ def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
+ return self._client.wait_for_service(timeout_sec=timeout_sec)
+
+ def _send_command_long(
+ self,
+ *,
+ target_sysid: int,
+ target_compid: int,
+ command: int,
+ confirmation: int,
+ param1: float,
+ param2: float,
+ param3: float,
+ param4: float,
+ param5: float,
+ param6: float,
+ param7: float,
+ timeout_sec: float,
+ ) -> CommandLongResult:
+ req = MavCommandLong.Request()
+ req.target_sysid = target_sysid
+ req.target_compid = target_compid
+ req.command = command
+ req.confirmation = confirmation
+ req.param1 = param1
+ req.param2 = param2
+ req.param3 = param3
+ req.param4 = param4
+ req.param5 = param5
+ req.param6 = param6
+ req.param7 = param7
+ req.timeout_sec = float(timeout_sec)
+
+ future = self._client.call_async(req)
+ rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0)
+ if not future.done() or future.result() is None:
+ return CommandLongResult(
+ success=False,
+ message="Service timeout.",
+ ack_result=-1,
+ )
+
+ response = future.result()
+ return CommandLongResult(
+ success=response.success,
+ message=response.message,
+ ack_result=response.ack_result,
+ )
+
+ def change_mode(
+ self,
+ *,
+ target_sysid: int,
+ custom_mode: float,
+ target_compid: int = 0,
+ base_mode: float = 1.0,
+ confirmation: int = 0,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> CommandLongResult:
+ return self._send_command_long(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ command=COMMAND_DO_SET_MODE,
+ confirmation=confirmation,
+ param1=float(base_mode),
+ param2=float(custom_mode),
+ param3=0.0,
+ param4=0.0,
+ param5=0.0,
+ param6=0.0,
+ param7=0.0,
+ timeout_sec=timeout_sec,
+ )
+
+ def takeoff(
+ self,
+ *,
+ target_sysid: int,
+ altitude_m: float,
+ target_compid: int = 0,
+ min_pitch_deg: float = 0.0,
+ yaw_deg: float = 0.0,
+ latitude: Optional[float] = None,
+ longitude: Optional[float] = None,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> CommandLongResult:
+ return self._send_command_long(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ command=COMMAND_NAV_TAKEOFF,
+ confirmation=0,
+ param1=float(min_pitch_deg),
+ param2=0.0,
+ param3=0.0,
+ param4=float(yaw_deg),
+ param5=float(latitude) if latitude is not None else 0.0,
+ param6=float(longitude) if longitude is not None else 0.0,
+ param7=float(altitude_m),
+ timeout_sec=timeout_sec,
+ )
+
+ def land(
+ self,
+ *,
+ target_sysid: int,
+ target_compid: int = 0,
+ yaw_deg: float = 0.0,
+ latitude: Optional[float] = None,
+ longitude: Optional[float] = None,
+ altitude_m: float = 0.0,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> CommandLongResult:
+ return self._send_command_long(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ command=COMMAND_NAV_LAND,
+ confirmation=0,
+ param1=0.0,
+ param2=0.0,
+ param3=0.0,
+ param4=float(yaw_deg),
+ param5=float(latitude) if latitude is not None else 0.0,
+ param6=float(longitude) if longitude is not None else 0.0,
+ param7=float(altitude_m),
+ timeout_sec=timeout_sec,
+ )
+
+ def arm_disarm(
+ self,
+ *,
+ target_sysid: int,
+ arm: bool,
+ target_compid: int = 0,
+ confirmation: int = 0,
+ param2: float = 0.0,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> CommandLongResult:
+ return self._send_command_long(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ command=COMMAND_COMPONENT_ARM_DISARM,
+ confirmation=confirmation,
+ param1=1.0 if arm else 0.0,
+ param2=float(param2),
+ param3=0.0,
+ param4=0.0,
+ param5=0.0,
+ param6=0.0,
+ param7=0.0,
+ timeout_sec=timeout_sec,
+ )
+
+ # ============================================================================
+ # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
+ # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
+ # ============================================================================
+
+ # ============================================================================
+ # 【重新實現】正確的非阻塞 async 方法（不使用 ThreadPoolExecutor）
+ # 使用 asyncio 的 polling 機制，避免在線程中調用 spin_until_future_complete
+ # ============================================================================
+
+ async def _send_command_long_async(
+ self,
+ *,
+ target_sysid: int,
+ target_compid: int,
+ command: int,
+ confirmation: int,
+ param1: float,
+ param2: float,
+ param3: float,
+ param4: float,
+ param5: float,
+ param6: float,
+ param7: float,
+ timeout_sec: float,
+ ) -> CommandLongResult:
+ """非阻塞的 async 版本 - 使用 asyncio polling 而非 spin"""
+ req = MavCommandLong.Request()
+ req.target_sysid = target_sysid
+ req.target_compid = target_compid
+ req.command = command
+ req.confirmation = confirmation
+ req.param1 = param1
+ req.param2 = param2
+ req.param3 = param3
+ req.param4 = param4
+ req.param5 = param5
+ req.param6 = param6
+ req.param7 = param7
+ req.timeout_sec = float(timeout_sec)
+
+ # 發送異步調用
+ future = self._client.call_async(req)
+
+ # 使用 asyncio.sleep 進行 polling，而非 spin_until_future_complete
+ # 使用 10ms 輪詢間隔以匹配 GUI 執行器的 spin_once() 頻率
+ timeout = timeout_sec + 1.0
+ elapsed = 0.0
+ poll_interval = 0.01 # 10ms - 與 GUI timer 頻率一致
+
+ while elapsed < timeout:
+ if future.done():
+ try:
+ response = future.result()
+ if response is None:
+ return CommandLongResult(
+ success=False,
+ message="Service returned None.",
+ ack_result=-1,
+ )
+ return CommandLongResult(
+ success=response.success,
+ message=response.message,
+ ack_result=response.ack_result,
+ )
+ except Exception as e:
+ return CommandLongResult(
+ success=False,
+ message=f"Error getting result: {e}",
+ ack_result=-1,
+ )
+
+ # 讓出控制權給事件循環，允許 GUI executor 執行
+ await asyncio.sleep(poll_interval)
+ elapsed += poll_interval
+
+ return CommandLongResult(
+ success=False,
+ message=f"Service timeout after {timeout}s.",
+ ack_result=-1,
+ )
+
+ async def change_mode_async(
+ self,
+ *,
+ target_sysid: int,
+ custom_mode: float,
+ target_compid: int = 0,
+ base_mode: float = 1.0,
+ confirmation: int = 0,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> CommandLongResult:
+ """非阻塞 async 版本的 change_mode"""
+ return await self._send_command_long_async(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ command=COMMAND_DO_SET_MODE,
+ confirmation=confirmation,
+ param1=float(base_mode),
+ param2=float(custom_mode),
+ param3=0.0,
+ param4=0.0,
+ param5=0.0,
+ param6=0.0,
+ param7=0.0,
+ timeout_sec=timeout_sec,
+ )
+
+ async def arm_disarm_async(
+ self,
+ *,
+ target_sysid: int,
+ arm: bool,
+ target_compid: int = 0,
+ confirmation: int = 0,
+ param2: float = 0.0,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> CommandLongResult:
+ """非阻塞 async 版本的 arm_disarm"""
+ return await self._send_command_long_async(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ command=COMMAND_COMPONENT_ARM_DISARM,
+ confirmation=confirmation,
+ param1=1.0 if arm else 0.0,
+ param2=float(param2),
+ param3=0.0,
+ param4=0.0,
+ param5=0.0,
+ param6=0.0,
+ param7=0.0,
+ timeout_sec=timeout_sec,
+ )
+
+ async def takeoff_async(
+ self,
+ *,
+ target_sysid: int,
+ altitude_m: float,
+ target_compid: int = 0,
+ min_pitch_deg: float = 0.0,
+ yaw_deg: float = 0.0,
+ latitude: Optional[float] = None,
+ longitude: Optional[float] = None,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> CommandLongResult:
+ """非阻塞 async 版本的 takeoff"""
+ return await self._send_command_long_async(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ command=COMMAND_NAV_TAKEOFF,
+ confirmation=0,
+ param1=float(min_pitch_deg),
+ param2=0.0,
+ param3=0.0,
+ param4=float(yaw_deg),
+ param5=float(latitude) if latitude is not None else 0.0,
+ param6=float(longitude) if longitude is not None else 0.0,
+ param7=float(altitude_m),
+ timeout_sec=timeout_sec,
+ )
+
+ async def land_async(
+ self,
+ *,
+ target_sysid: int,
+ target_compid: int = 0,
+ yaw_deg: float = 0.0,
+ latitude: Optional[float] = None,
+ longitude: Optional[float] = None,
+ altitude_m: float = 0.0,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> CommandLongResult:
+ """非阻塞 async 版本的 land"""
+ return await self._send_command_long_async(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ command=COMMAND_NAV_LAND,
+ confirmation=0,
+ param1=0.0,
+ param2=0.0,
+ param3=0.0,
+ param4=float(yaw_deg),
+ param5=float(latitude) if latitude is not None else 0.0,
+ param6=float(longitude) if longitude is not None else 0.0,
+ param7=float(altitude_m),
+ timeout_sec=timeout_sec,
+ )
diff --git a/src/fc_network_apps/navigation.py b/src/fc_network_apps/navigation.py
new file mode 100644
index 0000000..a094e2e
--- /dev/null
+++ b/src/fc_network_apps/navigation.py
@@ -0,0 +1,418 @@
+"""ROS2 client for fc_network MavPositionTargetGlobalInt service (SET_POSITION_TARGET_GLOBAL_INT / Offboard)."""
+
+from __future__ import annotations
+
+import asyncio
+import math
+from dataclasses import dataclass
+
+import rclpy
+from rclpy.node import Node
+
+from fc_interfaces.srv import MavPositionTargetGlobalInt
+
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/pos_global_int"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+# MAV_FRAME
+MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # 使用 WGS84 GNSS 座標系 高度為相對於 Home 點高度
+
+# POSITION_TARGET_TYPEMASK
+# 1 1 Ignore position x
+# 2 2 Ignore position y
+# 3 4 Ignore position z
+# 4 8 Ignore velocity x
+# 5 16 Ignore velocity y
+# 6 32 Ignore velocity z
+# 7 64 Ignore acceleration x
+# 8 128 Ignore acceleration y
+# 9 256 Ignore acceleration z
+# 10 512 Use force instead of acceleration
+# 11 1024 Ignore yaw
+# 12 2048 Ignore yaw rate
+# 13+ Not Used
+# 6543210987654321
+POSITION_ONLY = 0b0000110111111000 # 忽略速度、加速度、偏航
+
+
+# 回傳 echo 與送出值比對時，浮點欄位允許的絕對誤差（MAVLink float 轉傳常有小差）
+_FLOAT_ECHO_ABS_TOL = 1e-5
+def _float_echo_equal(a: float, b: float) -> bool:
+ return math.isclose(float(a), float(b), rel_tol=0.0, abs_tol=_FLOAT_ECHO_ABS_TOL)
+
+
+def _echo_position_target_matches(
+ *,
+ type_mask: int,
+ lat_int: int,
+ lon_int: int,
+ alt: float,
+ vx: float,
+ vy: float,
+ vz: float,
+ afx: float,
+ afy: float,
+ afz: float,
+ yaw: float,
+ yaw_rate: float,
+ time_boot_ms: int,
+ resp,
+) -> tuple[bool, str]:
+ """
+ 比對本次送出的軌跡欄位與 server 填回的 r_* 是否一致。
+ 若不符，回傳 (False, 簡短說明)；相符則 (True, "")。
+ """
+
+ _tm_sent = int(type_mask) & 0xFFF
+ _tm_recv = int(resp.r_type_mask) & 0xFFF
+ if _tm_recv != _tm_sent:
+ return False, f"type_mask echo: sent {_tm_sent:012b} != ack {_tm_recv:012b}"
+
+ coordinate_tolerance = 5 * 100
+ if abs(resp.r_lat_int - lat_int) >= coordinate_tolerance:
+ return False, f"lat_int echo: sent {lat_int} != ack {resp.r_lat_int}"
+ if abs(resp.r_lon_int - lon_int) >= coordinate_tolerance:
+ return False, f"lon_int echo: sent {lon_int} != ack {resp.r_lon_int}"
+ # TODO 我給飛控相對高度 它回給我絕對高度 看來這個比較方法 還有很多改良點 但是之後吧
+ # if not _float_echo_equal(resp.r_alt, alt):
+ # return False, f"alt echo: sent {alt} != ack {resp.r_alt}"
+ if not _float_echo_equal(resp.r_vx, vx):
+ return False, f"vx echo: sent {vx} != ack {resp.r_vx}"
+ if not _float_echo_equal(resp.r_vy, vy):
+ return False, f"vy echo: sent {vy} != ack {resp.r_vy}"
+ if not _float_echo_equal(resp.r_vz, vz):
+ return False, f"vz echo: sent {vz} != ack {resp.r_vz}"
+ if not _float_echo_equal(resp.r_afx, afx):
+ return False, f"afx echo: sent {afx} != ack {resp.r_afx}"
+ if not _float_echo_equal(resp.r_afy, afy):
+ return False, f"afy echo: sent {afy} != ack {resp.r_afy}"
+ if not _float_echo_equal(resp.r_afz, afz):
+ return False, f"afz echo: sent {afz} != ack {resp.r_afz}"
+ if not _float_echo_equal(resp.r_yaw, yaw):
+ return False, f"yaw echo: sent {yaw} != ack {resp.r_yaw}"
+ if not _float_echo_equal(resp.r_yaw_rate, yaw_rate):
+ return False, f"yaw_rate echo: sent {yaw_rate} != ack {resp.r_yaw_rate}"
+ # if int(resp.r_time_boot_ms) != int(time_boot_ms):
+ # return False, f"time_boot_ms echo: sent {time_boot_ms} != r {resp.r_time_boot_ms}"
+ return True, ""
+
+
+@dataclass
+class PositionTargetGlobalIntResult:
+ """對應 MavPositionTargetGlobalInt.srv 的 Response；success 依「送出與 r_* echo 是否一致」。"""
+ success: bool
+ message: str
+ ack_result: int
+ r_time_boot_ms: int = 0
+ r_type_mask: int = 0
+ r_lat_int: int = 0
+ r_lon_int: int = 0
+ r_alt: float = 0.0
+ r_vx: float = 0.0
+ r_vy: float = 0.0
+ r_vz: float = 0.0
+ r_afx: float = 0.0
+ r_afy: float = 0.0
+ r_afz: float = 0.0
+ r_yaw: float = 0.0
+ r_yaw_rate: float = 0.0
+
+class PositionTargetGlobalIntClient(Node):
+ """
+ ROS2 client：對 pos_global_int service 發送 SET_POSITION_TARGET_GLOBAL_INT (msg 86)，
+ 並等待 POSITION_TARGET_GLOBAL_INT (msg 87) 回應（由 server 端處理）。
+ """
+
+ def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None:
+ if not rclpy.ok():
+ rclpy.init(args=None)
+ if node_name is None:
+ import time
+ node_name = f"fc_position_target_global_int_client_{int(time.time() * 1000) % 100000}"
+ super().__init__(node_name)
+ self._client = self.create_client(MavPositionTargetGlobalInt, service_name)
+
+ def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
+ return self._client.wait_for_service(timeout_sec=timeout_sec)
+
+ def _send_position_target_global_int(
+ self,
+ *,
+ target_sysid: int,
+ target_compid: int,
+ time_boot_ms: int,
+ coordinate_frame: int,
+ type_mask: int,
+ lat_int: int,
+ lon_int: int,
+ alt: float,
+ vx: float,
+ vy: float,
+ vz: float,
+ afx: float,
+ afy: float,
+ afz: float,
+ yaw: float,
+ yaw_rate: float,
+ timeout_sec: float,
+ ) -> PositionTargetGlobalIntResult:
+ req = MavPositionTargetGlobalInt.Request()
+ req.target_sysid = target_sysid
+ req.target_compid = target_compid
+ req.time_boot_ms = time_boot_ms
+ req.coordinate_frame = coordinate_frame
+ req.type_mask = type_mask
+ req.lat_int = lat_int
+ req.lon_int = lon_int
+ req.alt = float(alt)
+ req.vx = float(vx)
+ req.vy = float(vy)
+ req.vz = float(vz)
+ req.afx = float(afx)
+ req.afy = float(afy)
+ req.afz = float(afz)
+ req.yaw = float(yaw)
+ req.yaw_rate = float(yaw_rate)
+ req.timeout_sec = float(timeout_sec)
+
+ future = self._client.call_async(req)
+ rclpy.spin_until_future_complete(self, future, timeout_sec=float(timeout_sec) + 1.0)
+ if not future.done() or future.result() is None:
+ return PositionTargetGlobalIntResult(
+ success=False,
+ message="Service timeout.",
+ ack_result=-1,
+ )
+
+ resp = future.result()
+
+ if resp.ack_result != 0:
+ return PositionTargetGlobalIntResult(
+ success=False,
+ message=resp.message,
+ ack_result=resp.ack_result,
+ )
+
+ echo_ok, echo_detail = _echo_position_target_matches(
+ type_mask=type_mask,
+ lat_int=lat_int,
+ lon_int=lon_int,
+ alt=alt,
+ vx=vx,
+ vy=vy,
+ vz=vz,
+ afx=afx,
+ afy=afy,
+ afz=afz,
+ yaw=yaw,
+ yaw_rate=yaw_rate,
+ time_boot_ms=time_boot_ms,
+ resp=resp,
+ )
+ # out_message = resp.message
+ # if not echo_ok:
+ # out_message = echo_detail if not out_message else f"{echo_detail} | server: {out_message}"
+
+ return PositionTargetGlobalIntResult(
+ success=echo_ok,
+ message=echo_detail,
+ ack_result=resp.ack_result,
+ r_time_boot_ms=int(resp.r_time_boot_ms),
+ r_type_mask=int(resp.r_type_mask),
+ r_lat_int=int(resp.r_lat_int),
+ r_lon_int=int(resp.r_lon_int),
+ r_alt=float(resp.r_alt),
+ r_vx=float(resp.r_vx),
+ r_vy=float(resp.r_vy),
+ r_vz=float(resp.r_vz),
+ r_afx=float(resp.r_afx),
+ r_afy=float(resp.r_afy),
+ r_afz=float(resp.r_afz),
+ r_yaw=float(resp.r_yaw),
+ r_yaw_rate=float(resp.r_yaw_rate),
+ )
+
+ def goto_position_only(
+ self,
+ *,
+ target_sysid: int,
+ target_compid: int = 0,
+ latitude_deg: float,
+ longitude_deg: float,
+ alt: float = 0.0,
+ timeout_sec: float,
+ ):
+ lat_int = int(round(latitude_deg * 1e7))
+ lon_int = int(round(longitude_deg * 1e7))
+
+ return self._send_position_target_global_int(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ time_boot_ms= 0,
+ coordinate_frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+ type_mask=POSITION_ONLY,
+ lat_int=lat_int,
+ lon_int=lon_int,
+ alt=alt,
+ vx=0,
+ vy=0,
+ vz=0,
+ afx=0,
+ afy=0,
+ afz=0,
+ yaw=0,
+ yaw_rate=0,
+ timeout_sec=timeout_sec,
+ )
+
+ # ============================================================================
+ # 非阻塞 async 版本（asyncio polling，對齊 longCommand.py 的模板）
+ # 避免在 Qt 事件迴圈或主執行緒中呼叫 spin_until_future_complete
+ # ============================================================================
+
+ async def _send_position_target_global_int_async(
+ self,
+ *,
+ target_sysid: int,
+ target_compid: int,
+ time_boot_ms: int,
+ coordinate_frame: int,
+ type_mask: int,
+ lat_int: int,
+ lon_int: int,
+ alt: float,
+ vx: float,
+ vy: float,
+ vz: float,
+ afx: float,
+ afy: float,
+ afz: float,
+ yaw: float,
+ yaw_rate: float,
+ timeout_sec: float,
+ ) -> PositionTargetGlobalIntResult:
+ """非阻塞 async 版本 - 使用 asyncio polling 而非 spin"""
+ req = MavPositionTargetGlobalInt.Request()
+ req.target_sysid = target_sysid
+ req.target_compid = target_compid
+ req.time_boot_ms = time_boot_ms
+ req.coordinate_frame = coordinate_frame
+ req.type_mask = type_mask
+ req.lat_int = lat_int
+ req.lon_int = lon_int
+ req.alt = float(alt)
+ req.vx = float(vx)
+ req.vy = float(vy)
+ req.vz = float(vz)
+ req.afx = float(afx)
+ req.afy = float(afy)
+ req.afz = float(afz)
+ req.yaw = float(yaw)
+ req.yaw_rate = float(yaw_rate)
+ req.timeout_sec = float(timeout_sec)
+
+ future = self._client.call_async(req)
+
+ timeout = float(timeout_sec) + 1.0
+ elapsed = 0.0
+ poll_interval = 0.01 # 10ms - 與 GUI timer 頻率一致
+
+ while elapsed < timeout:
+ if future.done():
+ try:
+ resp = future.result()
+ if resp is None:
+ return PositionTargetGlobalIntResult(
+ success=False,
+ message="Service returned None.",
+ ack_result=-1,
+ )
+ if resp.ack_result != 0:
+ return PositionTargetGlobalIntResult(
+ success=False,
+ message=resp.message,
+ ack_result=resp.ack_result,
+ )
+ echo_ok, echo_detail = _echo_position_target_matches(
+ type_mask=type_mask,
+ lat_int=lat_int,
+ lon_int=lon_int,
+ alt=alt,
+ vx=vx,
+ vy=vy,
+ vz=vz,
+ afx=afx,
+ afy=afy,
+ afz=afz,
+ yaw=yaw,
+ yaw_rate=yaw_rate,
+ time_boot_ms=time_boot_ms,
+ resp=resp,
+ )
+ return PositionTargetGlobalIntResult(
+ success=echo_ok,
+ message=echo_detail,
+ ack_result=resp.ack_result,
+ r_time_boot_ms=int(resp.r_time_boot_ms),
+ r_type_mask=int(resp.r_type_mask),
+ r_lat_int=int(resp.r_lat_int),
+ r_lon_int=int(resp.r_lon_int),
+ r_alt=float(resp.r_alt),
+ r_vx=float(resp.r_vx),
+ r_vy=float(resp.r_vy),
+ r_vz=float(resp.r_vz),
+ r_afx=float(resp.r_afx),
+ r_afy=float(resp.r_afy),
+ r_afz=float(resp.r_afz),
+ r_yaw=float(resp.r_yaw),
+ r_yaw_rate=float(resp.r_yaw_rate),
+ )
+ except Exception as e:
+ return PositionTargetGlobalIntResult(
+ success=False,
+ message=f"Error getting result: {e}",
+ ack_result=-1,
+ )
+
+ await asyncio.sleep(poll_interval)
+ elapsed += poll_interval
+
+ return PositionTargetGlobalIntResult(
+ success=False,
+ message=f"Service timeout after {timeout}s.",
+ ack_result=-1,
+ )
+
+ async def goto_position_only_async(
+ self,
+ *,
+ target_sysid: int,
+ target_compid: int = 0,
+ latitude_deg: float,
+ longitude_deg: float,
+ alt: float = 0.0,
+ timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+ ) -> PositionTargetGlobalIntResult:
+ """非阻塞 async 版本的 goto_position_only"""
+ lat_int = int(round(latitude_deg * 1e7))
+ lon_int = int(round(longitude_deg * 1e7))
+
+ return await self._send_position_target_global_int_async(
+ target_sysid=target_sysid,
+ target_compid=target_compid,
+ time_boot_ms=0,
+ coordinate_frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+ type_mask=POSITION_ONLY,
+ lat_int=lat_int,
+ lon_int=lon_int,
+ alt=alt,
+ vx=0,
+ vy=0,
+ vz=0,
+ afx=0,
+ afy=0,
+ afz=0,
+ yaw=0,
+ yaw_rate=0,
+ timeout_sec=timeout_sec,
+ )
diff --git a/src/someotherpkg/src/example_wholeMoving.py b/src/someotherpkg/src/example_wholeMoving.py
new file mode 100644
index 0000000..4f3dd0d
--- /dev/null
+++ b/src/someotherpkg/src/example_wholeMoving.py
@@ -0,0 +1,111 @@
+
+
+import fc_network_apps as fap
+import time
+
+def main():
+ commandAPI = fap.CommandLongClient()
+ navAPI = fap.PositionTargetGlobalIntClient()
+
+ target_sysid = 3
+
+
+ print("=== change mode ===")
+ result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+ while not result.success:
+ result.ack_result -= 1
+ result = commandAPI.change_mode(
+ target_sysid=target_sysid,
+ base_mode=1.0,
+ custom_mode=4.0,
+ timeout_sec=3.0,
+ )
+ print(f"success : {result.success}")
+ print(f"ack_result: {result.ack_result}")
+ print(f"message : {result.message}")
+ time.sleep(1)
+ if result.ack_result < -3:
+ return
+
+ print("=== arm vehicle ===")
+ result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+ while not result.success:
+ result.ack_result -= 1
+ result = commandAPI.arm_disarm(
+ target_sysid=target_sysid,
+ arm = True,
+ )
+ print(f"success : {result.success}")
+ print(f"ack_result: {result.ack_result}")
+ print(f"message : {result.message}")
+ time.sleep(1)
+ if result.ack_result < -3:
+ return
+
+ print("=== takeoff ===")
+ result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+ while not result.success:
+ result.ack_result -= 1
+ result = commandAPI.takeoff(
+ target_sysid=target_sysid,
+ altitude_m = 100.0,
+ )
+ print(f"success : {result.success}")
+ print(f"ack_result: {result.ack_result}")
+ print(f"message : {result.message}")
+ time.sleep(1)
+ if result.ack_result < -3:
+ return
+
+
+ time.sleep(15)
+
+ print("=== set_position_target_global_int_deg ===")
+ result_deg = navAPI.goto_position_only(
+ target_sysid=target_sysid,
+ latitude_deg=-35.376655,
+ longitude_deg=149.157011,
+ alt=150.0,
+ timeout_sec=3.0,
+ )
+ print(f"success : {result_deg.success}")
+ print(f"ack_result: {result_deg.ack_result}")
+ print(f"message : {result_deg.message}")
+ print(f"r_lat_int : {result_deg.r_lat_int} r_lon_int: {result_deg.r_lon_int}")
+
+ time.sleep(180)
+
+ result_deg = navAPI.goto_position_only(
+ target_sysid=target_sysid,
+ latitude_deg=-35.3632621,
+ longitude_deg=149.1652375,
+ alt=100.0,
+ timeout_sec=3.0,
+ )
+ print("=== set_position_target_global_int_deg ===")
+ print(f"success : {result_deg.success}")
+ print(f"ack_result: {result_deg.ack_result}")
+ print(f"message : {result_deg.message}")
+ print(f"r_lat_int : {result_deg.r_lat_int} r_lon_int: {result_deg.r_lon_int}")
+
+ time.sleep(180)
+
+ print("=== land ===")
+ result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+ while not result.success:
+ result.ack_result -= 1
+ result = commandAPI.land(
+ target_sysid=3,
+ target_compid=0,
+ )
+ print(f"success : {result.success}")
+ print(f"ack_result: {result.ack_result}")
+ print(f"message : {result.message}")
+ time.sleep(1)
+ if result.ack_result < -3:
+ return
+
+if __name__ == "__main__":
+ main()
+
+
diff --git a/src/unitdev01/unitdev01/comm_panel.py b/src/unitdev01/unitdev01/comm_panel.py
new file mode 100644
index 0000000..6259c8d
--- /dev/null
+++ b/src/unitdev01/unitdev01/comm_panel.py
@@ -0,0 +1,398 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
+ QPushButton, QLineEdit)
+from PyQt6.QtCore import pyqtSignal
+
+class CommPanel(QWidget):
+ """通讯设置面板类"""
+
+ # 定义信号
+ udp_connection_added = pyqtSignal(str, int) # ip, port
+ udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
+ udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
+ ws_connection_added = pyqtSignal(str) # url
+ ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label
+ ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel
+ status_message = pyqtSignal(str, int) # message, timeout
+
+ def __init__(self, parent=None):
+ super().__init__(parent)
+ self.udp_connections = []
+ self.ws_connections = []
+ self._init_ui()
+
+ def _init_ui(self):
+ """初始化UI"""
+ layout = QVBoxLayout(self)
+ layout.setContentsMargins(10, 10, 10, 10)
+ layout.setSpacing(10)
+
+ # ========== UDP MAVLink 區域 ==========
+ udp_title = QLabel("UDP")
+ udp_title.setStyleSheet("""
+ color: #DDD;
+ font-size: 14px;
+ font-weight: bold;
+ padding: 5px;
+ background-color: #333;
+ border-radius: 4px;
+ """)
+ layout.addWidget(udp_title)
+
+ # UDP 連接列表容器
+ self.udp_list_widget = QWidget()
+ self.udp_list_layout = QVBoxLayout(self.udp_list_widget)
+ self.udp_list_layout.setContentsMargins(0, 0, 0, 0)
+ self.udp_list_layout.setSpacing(5)
+ layout.addWidget(self.udp_list_widget)
+
+ # UDP 添加新連接區域
+ add_udp_widget = QWidget()
+ add_udp_layout = QHBoxLayout(add_udp_widget)
+ add_udp_layout.setContentsMargins(0, 0, 0, 0)
+
+ self.udp_ip_input = QLineEdit()
+ self.udp_ip_input.setText("127.0.0.1")
+ self.udp_ip_input.setPlaceholderText("IP")
+ self.udp_ip_input.setStyleSheet("""
+ QLineEdit {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 5px;
+ }
+ """)
+
+ self.udp_port_input = QLineEdit()
+ self.udp_port_input.setText("14540")
+ self.udp_port_input.setPlaceholderText("Port")
+ self.udp_port_input.setFixedWidth(80)
+ self.udp_port_input.setStyleSheet("""
+ QLineEdit {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 5px;
+ }
+ """)
+
+ add_udp_btn = QPushButton("添加")
+ add_udp_btn.clicked.connect(self._handle_add_udp)
+ add_udp_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #4CAF50;
+ color: white;
+ border: none;
+ padding: 6px 12px;
+ border-radius: 4px;
+ min-width: 30px;
+ }
+ QPushButton:hover { background-color: #45a049; }
+ """)
+
+ add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;"))
+ add_udp_layout.addWidget(self.udp_ip_input)
+ add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;"))
+ add_udp_layout.addWidget(self.udp_port_input)
+ add_udp_layout.addWidget(add_udp_btn)
+
+ layout.addWidget(add_udp_widget)
+
+ # 分隔線
+ separator = QWidget()
+ separator.setFixedHeight(20)
+ layout.addWidget(separator)
+
+ # ========== WebSocket 區域 ==========
+ ws_title = QLabel("WebSocket")
+ ws_title.setStyleSheet("""
+ color: #DDD;
+ font-size: 14px;
+ font-weight: bold;
+ padding: 5px;
+ background-color: #333;
+ border-radius: 4px;
+ """)
+ layout.addWidget(ws_title)
+
+ # WebSocket 連接列表容器
+ self.ws_list_widget = QWidget()
+ self.ws_list_layout = QVBoxLayout(self.ws_list_widget)
+ self.ws_list_layout.setContentsMargins(0, 0, 0, 0)
+ self.ws_list_layout.setSpacing(5)
+ layout.addWidget(self.ws_list_widget)
+
+ # WebSocket 添加新連接區域
+ add_ws_widget = QWidget()
+ add_ws_layout = QHBoxLayout(add_ws_widget)
+ add_ws_layout.setContentsMargins(0, 0, 0, 0)
+
+ self.ws_url_input = QLineEdit()
+ self.ws_url_input.setPlaceholderText("host")
+ self.ws_url_input.setStyleSheet("""
+ QLineEdit {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 5px;
+ }
+ """)
+
+ add_ws_btn = QPushButton("添加")
+ add_ws_btn.clicked.connect(self._handle_add_ws)
+ add_ws_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #4CAF50;
+ color: white;
+ border: none;
+ padding: 6px 12px;
+ border-radius: 4px;
+ min-width: 30px;
+ }
+ QPushButton:hover { background-color: #45a049; }
+ """)
+
+ add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;"))
+ add_ws_layout.addWidget(self.ws_url_input)
+ add_ws_layout.addWidget(add_ws_btn)
+
+ layout.addWidget(add_ws_widget)
+ layout.addStretch()
+
+ def _handle_add_udp(self):
+ """處理添加 UDP 連接"""
+ ip = self.udp_ip_input.text().strip()
+ port_text = self.udp_port_input.text().strip()
+
+ if not ip or not port_text:
+ self.status_message.emit("請輸入 IP 和 Port", 3000)
+ return
+
+ try:
+ port = int(port_text)
+ if port < 1 or port > 65535:
+ raise ValueError("Port 超出範圍")
+ except ValueError:
+ self.status_message.emit("Port 必須是 1-65535 的數字", 3000)
+ return
+
+ # 檢查是否已存在相同連接
+ for conn in self.udp_connections:
+ if conn['ip'] == ip and conn['port'] == port:
+ self.status_message.emit("連接已存在", 3000)
+ return
+
+ # 發送信號通知主窗口
+ self.udp_connection_added.emit(ip, port)
+
+ # 清空輸入框
+ self.udp_ip_input.clear()
+ self.udp_port_input.clear()
+
+ def _handle_add_ws(self):
+ """處理添加 WebSocket 連接"""
+ input_url = self.ws_url_input.text().strip()
+
+ if not input_url:
+ self.status_message.emit("請輸入 WebSocket URL", 3000)
+ return
+
+ # 自動添加 ws:// 前綴
+ if not input_url.startswith('ws://') and not input_url.startswith('wss://'):
+ url = f'ws://{input_url}'
+ else:
+ url = input_url
+
+ # 基本 URL 格式驗證
+ try:
+ if '://' in url:
+ parts = url.split('://', 1)
+ if len(parts) == 2 and ':' not in parts[1]:
+ self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000)
+ return
+ except:
+ self.status_message.emit("URL 格式錯誤", 3000)
+ return
+
+ # 檢查是否已存在相同連接
+ for conn in self.ws_connections:
+ if conn['url'] == url:
+ self.status_message.emit("連接已存在", 3000)
+ return
+
+ # 發送信號通知主窗口
+ self.ws_connection_added.emit(url)
+
+ # 清空輸入框
+ self.ws_url_input.clear()
+
+ def create_udp_connection_panel(self, conn):
+ """創建 UDP 連接面板"""
+ panel = QWidget()
+ panel.setStyleSheet("""
+ QWidget {
+ background-color: #2A2A2A;
+ border-radius: 6px;
+ padding: 8px;
+ border: 1px solid #444;
+ }
+ """)
+
+ layout = QHBoxLayout(panel)
+ layout.setContentsMargins(8, 8, 8, 8)
+
+ # 連接資訊
+ info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}")
+ info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+
+ # 狀態指示器
+ status_label = QLabel("●")
+ if conn.get('enabled', False):
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ else:
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+
+ # 控制按鈕
+ toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
+ toggle_btn.setFixedWidth(60)
+ toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label))
+ toggle_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+
+ remove_btn = QPushButton("移除")
+ remove_btn.setFixedWidth(60)
+ remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel))
+ remove_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #d32f2f;
+ color: white;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #b71c1c; }
+ """)
+
+ layout.addWidget(status_label)
+ layout.addWidget(info_label)
+ layout.addStretch()
+ layout.addWidget(toggle_btn)
+ layout.addWidget(remove_btn)
+
+ # 儲存引用
+ panel.connection = conn
+ panel.toggle_btn = toggle_btn
+ panel.status_label = status_label
+
+ return panel
+
+ def create_ws_connection_panel(self, conn):
+ """創建 WebSocket 連接面板"""
+ panel = QWidget()
+ panel.setStyleSheet("""
+ QWidget {
+ background-color: #2A2A2A;
+ border-radius: 6px;
+ padding: 8px;
+ border: 1px solid #444;
+ }
+ """)
+
+ layout = QHBoxLayout(panel)
+ layout.setContentsMargins(8, 8, 8, 8)
+
+ # 連接資訊
+ info_label = QLabel(f"{conn['name']} - {conn['url']}")
+ info_label.setStyleSheet("color: #DDD; font-size: 12px;")
+
+ # 狀態指示器
+ status_label = QLabel("●")
+ if conn.get('enabled', False):
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ else:
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+
+ # 控制按鈕
+ toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動")
+ toggle_btn.setFixedWidth(60)
+ toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label))
+ toggle_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+
+ remove_btn = QPushButton("移除")
+ remove_btn.setFixedWidth(60)
+ remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel))
+ remove_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #d32f2f;
+ color: white;
+ border: none;
+ padding: 4px 8px;
+ border-radius: 3px;
+ font-size: 11px;
+ }
+ QPushButton:hover { background-color: #b71c1c; }
+ """)
+
+ layout.addWidget(status_label)
+ layout.addWidget(info_label)
+ layout.addStretch()
+ layout.addWidget(toggle_btn)
+ layout.addWidget(remove_btn)
+
+ # 儲存引用
+ panel.connection = conn
+ panel.toggle_btn = toggle_btn
+ panel.status_label = status_label
+
+ return panel
+
+ def add_udp_panel(self, conn):
+ """添加 UDP 連接面板到列表"""
+ panel = self.create_udp_connection_panel(conn)
+ self.udp_list_layout.addWidget(panel)
+ self.udp_connections.append(conn)
+ return panel
+
+ def add_ws_panel(self, conn):
+ """添加 WebSocket 連接面板到列表"""
+ panel = self.create_ws_connection_panel(conn)
+ self.ws_list_layout.addWidget(panel)
+ self.ws_connections.append(conn)
+ return panel
+
+ def remove_udp_connection_from_list(self, conn):
+ """從列表中移除 UDP 連接"""
+ if conn in self.udp_connections:
+ self.udp_connections.remove(conn)
+
+ def remove_ws_connection_from_list(self, conn):
+ """從列表中移除 WebSocket 連接"""
+ if conn in self.ws_connections:
+ self.ws_connections.remove(conn)
\ No newline at end of file
diff --git a/src/unitdev01/unitdev01/communication.py b/src/unitdev01/unitdev01/communication.py
new file mode 100644
index 0000000..2db56c1
--- /dev/null
+++ b/src/unitdev01/unitdev01/communication.py
@@ -0,0 +1,646 @@
+from rclpy.node import Node
+from PyQt6.QtCore import QObject, pyqtSignal
+import math
+import re
+import threading
+from threading import Lock
+import asyncio
+import websockets
+import json
+import socket
+from pymavlink import mavutil
+from geometry_msgs.msg import Point, Vector3
+from sensor_msgs.msg import BatteryState, NavSatFix, Imu
+from std_msgs.msg import Float64
+from mavros_msgs.msg import State, VfrHud
+from mavros_msgs.srv import CommandBool, CommandTOL
+
+class DroneSignals(QObject):
+ update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
+
+class UDPMavlinkReceiver(threading.Thread):
+ """UDP MAVLink 接收器"""
+ def __init__(self, ip, port, signals, connection_name):
+ super().__init__(daemon=True)
+ self.ip = ip
+ self.port = port
+ self.signals = signals
+ self.connection_name = connection_name
+ self.running = False
+ self.sock = None
+
+ def run(self):
+ """執行 UDP 接收循環"""
+ self.running = True
+ try:
+ print(f"UDP MAVLink receiver started on {self.ip}:{self.port}")
+
+ # 創建 MAVLink 連接
+ mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}')
+ while self.running:
+ try:
+ msg = mav.recv_match(blocking=True, timeout=1.0)
+ if msg is None:
+ continue
+
+ self.process_mavlink_message(msg)
+
+ except socket.timeout:
+ continue
+ except Exception as e:
+ print(f"Error receiving MAVLink message: {e}")
+
+ except Exception as e:
+ print(f"UDP receiver error: {e}")
+ finally:
+ if self.sock:
+ self.sock.close()
+
+ def process_mavlink_message(self, msg):
+ """處理 MAVLink 訊息"""
+ try:
+ msg_type = msg.get_type()
+ system_id = msg.get_srcSystem()
+ drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源
+
+ if msg_type == "HEARTBEAT":
+ mode = mavutil.mode_string_v10(msg)
+ armed = bool(msg.base_mode & 128)
+ self.signals.update_signal.emit('state', drone_id, {
+ 'mode': mode,
+ 'armed': armed
+ })
+
+ elif msg_type == "BATTERY_STATUS":
+ voltage = msg.voltages[0] / 1000
+ self.signals.update_signal.emit('battery', drone_id, {
+ 'voltage': voltage
+ })
+
+ elif msg_type == "GLOBAL_POSITION_INT":
+ latitude = msg.lat / 1e7
+ longitude = msg.lon / 1e7
+ self.signals.update_signal.emit('gps', drone_id, {
+ 'lat': latitude,
+ 'lon': longitude
+ })
+
+ elif msg_type == "GPS_RAW_INT":
+ fix_type = msg.fix_type
+
+ elif msg_type == "LOCAL_POSITION_NED":
+ x = msg.y
+ y = msg.x
+ z = -msg.z
+ self.signals.update_signal.emit('local_pose', drone_id, {
+ 'x': x,
+ 'y': y,
+ 'z': z
+ })
+ self.signals.update_signal.emit('altitude', drone_id, {
+ 'altitude': z
+ })
+ self.signals.update_signal.emit('velocity', drone_id, {
+ 'vx': msg.vx,
+ 'vy': msg.vy,
+ 'vz': msg.vz
+ })
+
+ elif msg_type == "ATTITUDE":
+ pitch = math.degrees(msg.pitch)
+ self.signals.update_signal.emit('attitude', drone_id, {
+ 'pitch': pitch,
+ 'roll': 0,
+ 'yaw': 0,
+ 'rates': (0, 0, 0)
+ })
+
+ elif msg_type == "VFR_HUD":
+ groundspeed = msg.groundspeed
+ heading = msg.heading
+ self.signals.update_signal.emit('hud', drone_id, {
+ 'heading': heading,
+ 'groundspeed': groundspeed
+ })
+
+ except Exception as e:
+ print(f"Error processing MAVLink message: {e}")
+
+ def stop(self):
+ """停止接收器"""
+ self.running = False
+
+class SerialMavlinkReceiver(threading.Thread):
+ """串口 MAVLink 接收器"""
+ def __init__(self, port, baudrate, signals, connection_name):
+ super().__init__(daemon=True)
+ self.port = port
+ self.baudrate = baudrate
+ self.signals = signals
+ self.connection_name = connection_name
+ self.running = False
+ self.mav = None
+
+ def run(self):
+ """執行串口接收循環"""
+ self.running = True
+ try:
+ print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
+
+ # 創建 MAVLink 串口連接
+ self.mav = mavutil.mavlink_connection(
+ self.port,
+ baud=self.baudrate,
+ source_system=255
+ )
+
+ print(f"Waiting for heartbeat from {self.port}...")
+ self.mav.wait_heartbeat()
+ print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
+
+ while self.running:
+ try:
+ msg = self.mav.recv_match(blocking=True, timeout=1.0)
+ if msg is None:
+ continue
+
+ self.process_mavlink_message(msg)
+
+ except Exception as e:
+ if self.running:
+ print(f"Error receiving MAVLink message from serial: {e}")
+
+ except Exception as e:
+ print(f"Serial receiver error: {e}")
+ finally:
+ if self.mav:
+ try:
+ self.mav.close()
+ except:
+ pass
+
+ def process_mavlink_message(self, msg):
+ """處理 MAVLink 訊息"""
+ try:
+ msg_type = msg.get_type()
+ system_id = msg.get_srcSystem()
+ drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源
+
+ if msg_type == "HEARTBEAT":
+ mode = mavutil.mode_string_v10(msg)
+ armed = bool(msg.base_mode & 128)
+ self.signals.update_signal.emit('state', drone_id, {
+ 'mode': mode,
+ 'armed': armed
+ })
+
+ elif msg_type == "BATTERY_STATUS":
+ voltage = msg.voltages[0] / 1000
+ self.signals.update_signal.emit('battery', drone_id, {
+ 'voltage': voltage
+ })
+
+ elif msg_type == "GLOBAL_POSITION_INT":
+ latitude = msg.lat / 1e7
+ longitude = msg.lon / 1e7
+ self.signals.update_signal.emit('gps', drone_id, {
+ 'lat': latitude,
+ 'lon': longitude
+ })
+
+ elif msg_type == "GPS_RAW_INT":
+ fix_type = msg.fix_type
+
+ elif msg_type == "LOCAL_POSITION_NED":
+ x = msg.y
+ y = msg.x
+ z = -msg.z
+ self.signals.update_signal.emit('local_pose', drone_id, {
+ 'x': x,
+ 'y': y,
+ 'z': z
+ })
+ self.signals.update_signal.emit('altitude', drone_id, {
+ 'altitude': z
+ })
+ self.signals.update_signal.emit('velocity', drone_id, {
+ 'vx': msg.vx,
+ 'vy': msg.vy,
+ 'vz': msg.vz
+ })
+
+ elif msg_type == "ATTITUDE":
+ pitch = math.degrees(msg.pitch)
+ self.signals.update_signal.emit('attitude', drone_id, {
+ 'pitch': pitch,
+ 'roll': 0,
+ 'yaw': 0,
+ 'rates': (0, 0, 0)
+ })
+
+ elif msg_type == "VFR_HUD":
+ groundspeed = msg.groundspeed
+ heading = msg.heading
+ self.signals.update_signal.emit('hud', drone_id, {
+ 'heading': heading,
+ 'groundspeed': groundspeed
+ })
+
+ except Exception as e:
+ print(f"Error processing MAVLink message from serial: {e}")
+
+ def stop(self):
+ """停止接收器"""
+ self.running = False
+
+class WebSocketMavlinkReceiver(threading.Thread):
+ """WebSocket MAVLink 接收器"""
+ def __init__(self, url, signals, connection_name):
+ super().__init__(daemon=True)
+ self.url = url
+ self.signals = signals
+ self.connection_name = connection_name
+ self.running = False
+ self.max_retries = 5
+ self.base_delay = 1.0
+
+ def run(self):
+ """執行 WebSocket 接收循環"""
+ self.running = True
+ asyncio.set_event_loop(asyncio.new_event_loop())
+ asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
+
+ async def ws_client_loop(self):
+ """WebSocket 連接的主循環"""
+ retry_count = 0
+
+ print(f"Starting WebSocket client for {self.connection_name} at {self.url}")
+
+ while self.running and retry_count < self.max_retries:
+ try:
+ async with websockets.connect(self.url) as websocket:
+ print(f"WebSocket {self.connection_name} connected to {self.url}")
+ retry_count = 0 # 重置重試計數
+
+ async for message in websocket:
+ if not self.running:
+ break
+
+ try:
+ data = json.loads(message)
+ data['_connection_source'] = self.connection_name
+ self.process_websocket_message(data)
+ except json.JSONDecodeError as e:
+ print(f"WebSocket {self.connection_name} JSON decode error: {e}")
+ except Exception as e:
+ print(f"WebSocket {self.connection_name} message processing error: {e}")
+
+ except websockets.exceptions.ConnectionClosedError:
+ print(f"WebSocket {self.connection_name} connection closed")
+ if self.running:
+ retry_count += 1
+ if retry_count < self.max_retries:
+ delay = self.base_delay * (2 ** min(retry_count, 4))
+ print(f"Reconnecting in {delay}s...")
+ await asyncio.sleep(delay)
+ else:
+ break
+ except Exception as e:
+ retry_count += 1
+ if retry_count < self.max_retries and self.running:
+ delay = self.base_delay * (2 ** min(retry_count, 4))
+ print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})")
+ await asyncio.sleep(delay)
+ else:
+ break
+
+ print(f"WebSocket client {self.connection_name} stopped")
+
+ def process_websocket_message(self, data):
+ """處理 WebSocket 訊息"""
+ try:
+ drone_id = data.get('system_id')
+ drone_id = f"s9_{drone_id}" if drone_id else None
+ if not drone_id:
+ return
+
+ # 模式
+ if 'mode' in data:
+ self.signals.update_signal.emit('state', drone_id, {
+ 'mode': data['mode'],
+ })
+
+ # 電池
+ if 'battery' in data:
+ self.signals.update_signal.emit('battery', drone_id, {
+ 'percentage': data['battery']
+ })
+
+ # 位置
+ if 'position' in data:
+ pos = data['position']
+ self.signals.update_signal.emit('gps', drone_id, {
+ 'lat': pos.get('lat', 0),
+ 'lon': pos.get('lon', 0)
+ })
+
+ # Local position - 設定 x, y 為 0.0
+ self.signals.update_signal.emit('local_pose', drone_id, {
+ 'x': 0.0,
+ 'y': 0.0,
+ 'z': 0.0
+ })
+
+ # Altitude - 設定為 0.0
+ self.signals.update_signal.emit('altitude', drone_id, {
+ 'altitude': 0.0
+ })
+
+ # 航向
+ if 'heading' in data:
+ self.signals.update_signal.emit('hud', drone_id, {
+ 'heading': data['heading'],
+ 'groundspeed': 0.0
+ })
+
+ except Exception as e:
+ print(f"WebSocket message processing error: {e}")
+
+ def stop(self):
+ """停止接收器"""
+ self.running = False
+
+class DroneMonitor(Node):
+ def __init__(self):
+ super().__init__('drone_monitor')
+ self.signals = DroneSignals()
+ self.drone_topics = {}
+ self.lock = Lock()
+
+ self.arm_clients = {}
+ self.takeoff_clients = {}
+ self.setpoint_pubs = {}
+ self.selected_drones = set()
+ self.latest_data = {}
+
+ # 定義需要過濾的模式
+ self.filtered_modes = ['Mode(0x000000c0)']
+
+ # WebSocket 接收器列表
+ self.ws_receivers = []
+
+ # 串口接收器列表
+ self.serial_receivers = []
+
+ # 主题检测定时器
+ self.create_timer(1.0, self.scan_topics)
+
+ def scan_topics(self):
+ topics = self.get_topic_names_and_types()
+ drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)')
+
+ found_drones = set()
+ for topic_name, _ in topics:
+ if match := drone_pattern.match(topic_name):
+ drone_id, topic_type = match.groups()
+ found_drones.add(drone_id)
+ with self.lock:
+ self.drone_topics.setdefault(drone_id, set()).add(topic_type)
+
+ for drone_id in found_drones:
+ if not hasattr(self, f'drone_{drone_id}_subs'):
+ self.setup_drone(drone_id)
+
+ def setup_drone(self, drone_id):
+ base_topic = f'/MavLinkBus/{drone_id}'
+
+ # Add service clients
+ self.arm_clients[drone_id] = self.create_client(
+ CommandBool,
+ f'{base_topic}/cmd/arming'
+ )
+ self.takeoff_clients[drone_id] = self.create_client(
+ CommandTOL,
+ f'{base_topic}/cmd/takeoff'
+ )
+
+ # Add setpoint publisher
+ self.setpoint_pubs[drone_id] = self.create_publisher(
+ Point,
+ f'{base_topic}/setpoint_position/local',
+ 10
+ )
+
+ subs = {
+ 'attitude': self.create_subscription(
+ Imu,
+ f'{base_topic}/attitude',
+ lambda msg, did=drone_id: self.attitude_callback(did, msg),
+ 10
+ ),
+ 'battery': self.create_subscription(
+ BatteryState,
+ f'{base_topic}/battery',
+ lambda msg, did=drone_id: self.battery_callback(did, msg),
+ 10
+ ),
+ 'global': self.create_subscription(
+ NavSatFix,
+ f'{base_topic}/global_position/global',
+ lambda msg, did=drone_id: self.gps_callback(did, msg),
+ 10
+ ),
+ 'rel_alt': self.create_subscription(
+ Float64,
+ f'{base_topic}/global_position/rel_alt',
+ lambda msg, did=drone_id: self.altitude_callback(did, msg),
+ 10
+ ),
+ 'local_pose': self.create_subscription(
+ Point,
+ f'{base_topic}/local_position/pose',
+ lambda msg, did=drone_id: self.local_pose_callback(did, msg),
+ 10
+ ),
+ 'local_vel': self.create_subscription(
+ Vector3,
+ f'{base_topic}/local_position/velocity',
+ lambda msg, did=drone_id: self.local_vel_callback(did, msg),
+ 10
+ ),
+ 'loss_rate': self.create_subscription(
+ Float64,
+ f'{base_topic}/packet_loss_rate',
+ lambda msg, did=drone_id: self.loss_rate_callback(did, msg),
+ 10
+ ),
+ 'state': self.create_subscription(
+ State,
+ f'{base_topic}/state',
+ lambda msg, did=drone_id: self.state_callback(did, msg),
+ 10
+ ),
+ 'ping': self.create_subscription(
+ Float64,
+ f'{base_topic}/ping',
+ lambda msg, did=drone_id: self.ping_callback(did, msg),
+ 10
+ ),
+ 'vfr_hud': self.create_subscription(
+ VfrHud,
+ f'{base_topic}/vfr_hud',
+ lambda msg, did=drone_id: self.hud_callback(did, msg),
+ 10
+ )
+ }
+
+ setattr(self, f'drone_{drone_id}_subs', subs)
+
+ async def arm_drone(self, drone_id, arm):
+ if drone_id not in self.arm_clients:
+ return False
+
+ client = self.arm_clients[drone_id]
+ if not client.wait_for_service(timeout_sec=1.0):
+ return False
+
+ request = CommandBool.Request()
+ request.value = arm
+
+ future = client.call_async(request)
+ try:
+ response = await future
+ return response.success
+ except Exception as e:
+ self.get_logger().error(f'Arm service call failed: {e}')
+ return False
+
+ async def takeoff_drone(self, drone_id, altitude=10.0):
+ if drone_id not in self.takeoff_clients:
+ return False
+
+ client = self.takeoff_clients[drone_id]
+ if not client.wait_for_service(timeout_sec=1.0):
+ return False
+
+ request = CommandTOL.Request()
+ request.altitude = altitude
+ request.min_pitch = 0.0
+ request.yaw = 0.0
+
+ future = client.call_async(request)
+ try:
+ response = await future
+ return response.success
+ except Exception as e:
+ self.get_logger().error(f'Takeoff service call failed: {e}')
+ return False
+
+ def send_setpoint(self, drone_id, x, y, z):
+ """Send setpoint position command"""
+ if drone_id not in self.setpoint_pubs:
+ return False
+
+ msg = Point()
+ msg.x = float(x)
+ msg.y = float(y)
+ msg.z = float(z)
+
+ self.setpoint_pubs[drone_id].publish(msg)
+ return True
+
+ def quaternion_to_euler(self, q):
+ sinr_cosp = 2 * (q.w * q.x + q.y * q.z)
+ cosr_cosp = 1 - 2 * (q.x**2 + q.y**2)
+ roll = math.atan2(sinr_cosp, cosr_cosp)
+
+ sinp = 2 * (q.w * q.y - q.z * q.x)
+ pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp)
+
+ siny_cosp = 2 * (q.w * q.z + q.x * q.y)
+ cosy_cosp = 1 - 2 * (q.y**2 + q.z**2)
+ yaw = math.atan2(siny_cosp, cosy_cosp)
+
+ return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
+
+ # callbacks
+ def attitude_callback(self, drone_id, msg):
+ if hasattr(msg, 'orientation'):
+ roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
+ self.latest_data[(drone_id, 'attitude')] = {
+ 'roll': roll,
+ 'pitch': pitch,
+ 'yaw': yaw,
+ 'rates': (msg.angular_velocity.x,
+ msg.angular_velocity.y,
+ msg.angular_velocity.z)
+ }
+
+ def battery_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'battery')] = {
+ 'voltage': msg.voltage
+ }
+
+ def state_callback(self, drone_id, msg):
+ mode = msg.mode
+ if mode in self.filtered_modes:
+ return
+ self.latest_data[(drone_id, 'state')] = {
+ 'mode': msg.mode,
+ 'armed': msg.armed
+ }
+
+ def gps_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'gps')] = {
+ 'lat': msg.latitude,
+ 'lon': msg.longitude,
+ 'alt': msg.altitude
+ }
+
+ def local_vel_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'velocity')] = {
+ 'vx': msg.x,
+ 'vy': msg.y,
+ 'vz': msg.z
+ }
+
+ def altitude_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'altitude')] = {
+ 'altitude': msg.data
+ }
+
+ def local_pose_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'local_pose')] = {
+ 'x': msg.x,
+ 'y': msg.y,
+ 'z': msg.z
+ }
+
+ def hud_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'hud')] = {
+ 'airspeed': msg.airspeed,
+ 'groundspeed': msg.groundspeed,
+ 'heading': msg.heading,
+ 'throttle': msg.throttle,
+ 'alt': msg.altitude,
+ 'climb': msg.climb
+ }
+
+ def loss_rate_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'loss_rate')] = {
+ 'loss_rate': msg.data
+ }
+
+ def ping_callback(self, drone_id, msg):
+ self.latest_data[(drone_id, 'ping')] = {
+ 'ping': msg.data
+ }
+
+ def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
+ """啟動串口 MAVLink 連接"""
+ connection_name = f"Serial_{port.replace('/', '_')}"
+ receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name)
+ receiver.start()
+ self.serial_receivers.append(receiver)
+ print(f"Started serial connection on {port} at {baudrate} baud")
+ return receiver
\ No newline at end of file
diff --git a/src/unitdev01/unitdev01/drone_panel.py b/src/unitdev01/unitdev01/drone_panel.py
new file mode 100644
index 0000000..a5a7bf5
--- /dev/null
+++ b/src/unitdev01/unitdev01/drone_panel.py
@@ -0,0 +1,378 @@
+#!/usr/bin/env python3
+from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel,
+ QPushButton, QSizePolicy, QCheckBox)
+from PyQt6.QtCore import pyqtSignal
+
+class DronePanel(QWidget):
+ """單個無人機面板類別"""
+
+ # 定義信號
+ mode_change_requested = pyqtSignal(str) # drone_id
+ arm_requested = pyqtSignal(str) # drone_id
+ takeoff_requested = pyqtSignal(str) # drone_id
+ setpoint_requested = pyqtSignal(str) # drone_id
+ selection_changed = pyqtSignal(str, int) # drone_id, state
+
+ def __init__(self, drone_id, parent=None):
+ super().__init__(parent)
+ self.drone_id = drone_id
+ self.display_id = 's' + drone_id.split('_')[1]
+ self._init_ui()
+
+ def _init_ui(self):
+ """初始化UI"""
+ self.setObjectName(f"panel_{self.drone_id}")
+ self.setFixedHeight(140)
+ self.setStyleSheet("""
+ background-color: #2A2A2A;
+ border-radius: 8px;
+ """)
+
+ # 主佈局
+ main_layout = QHBoxLayout(self)
+ main_layout.setContentsMargins(8, 8, 8, 8)
+ main_layout.setSpacing(0)
+
+ # 創建內容容器（包含 info 和 control）
+ content_widget = QWidget()
+ content_widget.setStyleSheet("background-color: #333; border-radius: 6px;")
+ content_layout = QHBoxLayout(content_widget)
+ content_layout.setContentsMargins(8, 8, 8, 8)
+ content_layout.setSpacing(8)
+
+ # 左側資訊區域
+ info_widget = self._create_info_section()
+
+ # 右側控制按鈕區域
+ control_widget = self._create_control_section()
+
+ # 將 info 和 control 加入內容容器
+ content_layout.addWidget(info_widget)
+ content_layout.addWidget(control_widget)
+
+ # 將內容容器加入主佈局
+ main_layout.addWidget(content_widget)
+
+ def _create_info_section(self):
+ """創建資訊顯示區域"""
+ info_widget = QWidget()
+ info_layout = QVBoxLayout(info_widget)
+ info_layout.setContentsMargins(0, 0, 0, 0)
+ info_layout.setSpacing(4)
+
+ # 頂部標題欄
+ header = QWidget()
+ header_layout = QHBoxLayout(header)
+ header_layout.setContentsMargins(0, 0, 0, 0)
+
+ # 勾選框
+ self.checkbox = QCheckBox()
+ self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
+ self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }")
+ self.checkbox.stateChanged.connect(
+ lambda state: self.selection_changed.emit(self.drone_id, state)
+ )
+
+ # ID 顯示
+ id_label = QLabel(self.display_id)
+ id_label.setStyleSheet("""
+ font-weight: bold;
+ font-size: 14px;
+ color: #7FFFD4;
+ min-width: 80px;
+ """)
+
+ header_layout.addWidget(self.checkbox)
+ header_layout.addWidget(id_label)
+ header_layout.addStretch()
+
+ info_layout.addWidget(header)
+
+ # 第一行：狀態 (模式 + ARM狀態)
+ status_row = self._create_status_row()
+ info_layout.addWidget(status_row)
+
+ # 第二行：電池
+ battery_row = self._create_battery_row()
+ info_layout.addWidget(battery_row)
+
+ # 第三行：位置 + 高度
+ position_row = self._create_position_row()
+ info_layout.addWidget(position_row)
+
+ # 第四行：航向 + 速度
+ nav_row = self._create_nav_row()
+ info_layout.addWidget(nav_row)
+
+ return info_widget
+
+ def _create_status_row(self):
+ """創建狀態行"""
+ status_row = QWidget()
+ status_layout = QHBoxLayout(status_row)
+ status_layout.setContentsMargins(0, 0, 0, 0)
+
+ status_title = QLabel("狀態：")
+ status_title.setStyleSheet("color: #888; min-width: 50px;")
+
+ self.mode_label = QLabel("--")
+ self.mode_label.setObjectName(f"{self.drone_id}_mode")
+ self.mode_label.setStyleSheet("color: #DDD;")
+
+ self.armed_label = QLabel("--")
+ self.armed_label.setObjectName(f"{self.drone_id}_armed")
+ self.armed_label.setStyleSheet("color: #DDD;")
+
+ status_layout.addWidget(status_title)
+ status_layout.addWidget(self.mode_label)
+ status_layout.addWidget(self.armed_label)
+ status_layout.addStretch()
+
+ return status_row
+
+ def _create_battery_row(self):
+ """創建電池行"""
+ battery_row = QWidget()
+ battery_layout = QHBoxLayout(battery_row)
+ battery_layout.setContentsMargins(0, 0, 0, 0)
+
+ battery_title = QLabel("電池：")
+ battery_title.setStyleSheet("color: #888; min-width: 50px;")
+
+ self.battery_label = QLabel("--")
+ self.battery_label.setObjectName(f"{self.drone_id}_battery")
+ self.battery_label.setStyleSheet("color: #DDD;")
+
+ battery_layout.addWidget(battery_title)
+ battery_layout.addWidget(self.battery_label)
+ battery_layout.addStretch()
+
+ return battery_row
+
+ def _create_position_row(self):
+ """創建位置行"""
+ position_row = QWidget()
+ position_layout = QHBoxLayout(position_row)
+ position_layout.setContentsMargins(0, 0, 0, 0)
+
+ position_title = QLabel("位置：")
+ position_title.setStyleSheet("color: #888; min-width: 50px;")
+
+ self.local_label = QLabel("--")
+ self.local_label.setObjectName(f"{self.drone_id}_local")
+ self.local_label.setStyleSheet("color: #DDD;")
+
+ altitude_title = QLabel("高度：")
+ altitude_title.setStyleSheet("color: #888; margin-left: 10px;")
+
+ self.altitude_label = QLabel("--")
+ self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
+ self.altitude_label.setStyleSheet("color: #DDD;")
+
+ position_layout.addWidget(position_title)
+ position_layout.addWidget(self.local_label)
+ position_layout.addWidget(altitude_title)
+ position_layout.addWidget(self.altitude_label)
+ position_layout.addStretch()
+
+ return position_row
+
+ def _create_nav_row(self):
+ """創建導航行"""
+ nav_row = QWidget()
+ nav_layout = QHBoxLayout(nav_row)
+ nav_layout.setContentsMargins(0, 0, 0, 0)
+
+ heading_title = QLabel("航向：")
+ heading_title.setStyleSheet("color: #888; min-width: 50px;")
+
+ self.heading_label = QLabel("--")
+ self.heading_label.setObjectName(f"{self.drone_id}_heading")
+ self.heading_label.setStyleSheet("color: #DDD;")
+
+ speed_title = QLabel("速度：")
+ speed_title.setStyleSheet("color: #888; margin-left: 10px;")
+
+ self.groundspeed_label = QLabel("--")
+ self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed")
+ self.groundspeed_label.setStyleSheet("color: #DDD;")
+
+ nav_layout.addWidget(heading_title)
+ nav_layout.addWidget(self.heading_label)
+ nav_layout.addWidget(speed_title)
+ nav_layout.addWidget(self.groundspeed_label)
+ nav_layout.addStretch()
+
+ return nav_row
+
+ def _create_control_section(self):
+ """創建控制按鈕區域"""
+ control_widget = QWidget()
+ control_layout = QVBoxLayout(control_widget)
+ control_layout.setContentsMargins(0, 0, 0, 0)
+ control_layout.setSpacing(6)
+
+ control_widget.setFixedWidth(80)
+
+ btn_style = """
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ border-radius: 4px;
+ font-size: 11px;
+ }
+ QPushButton:hover {
+ background-color: #555;
+ }
+ """
+ # 模式切換按鈕
+ mode_btn = QPushButton("切換模式")
+ mode_btn.setStyleSheet(btn_style)
+ mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+ mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id))
+
+ # 解鎖按鈕
+ arm_btn = QPushButton("解鎖")
+ arm_btn.setStyleSheet(btn_style)
+ arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+ arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id))
+
+ # 起飛按鈕
+ takeoff_btn = QPushButton("起飛")
+ takeoff_btn.setStyleSheet(btn_style)
+ takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+ takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id))
+
+ # Setpoint 按鈕
+ setpoint_btn = QPushButton("Setpoint")
+ setpoint_btn.setStyleSheet(btn_style)
+ setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding)
+ setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id))
+
+ control_layout.addWidget(mode_btn)
+ control_layout.addWidget(arm_btn)
+ control_layout.addWidget(takeoff_btn)
+ control_layout.addWidget(setpoint_btn)
+
+ return control_widget
+
+ def update_field(self, field, text, color=None):
+ """更新指定欄位的值"""
+ label = self.findChild(QLabel, f"{self.drone_id}_{field}")
+ if label and label.text() != text:
+ label.setText(text)
+ if color:
+ label.setStyleSheet(f"color: {color};")
+
+ def get_checkbox(self):
+ """獲取勾選框"""
+ return self.checkbox
+
+ def set_checked(self, checked):
+ """設置勾選狀態"""
+ self.checkbox.setChecked(checked)
+
+ def is_checked(self):
+ """獲取勾選狀態"""
+ return self.checkbox.isChecked()
+
+class SocketGroupPanel(QWidget):
+ # 定義信號
+ group_selection_changed = pyqtSignal(str, int) # socket_id, state
+
+ def __init__(self, socket_id, color='#AAAAAA', parent=None):
+ super().__init__(parent)
+ self.socket_id = socket_id
+ self.color = color
+ self._init_ui()
+
+ def _init_ui(self):
+ """初始化UI"""
+ self.setObjectName(f"socket_group_{self.socket_id}")
+ self.setStyleSheet("""
+ background-color: #1E1E1E;
+ border-radius: 12px;
+ """)
+
+ layout = QVBoxLayout(self)
+ layout.setContentsMargins(10, 10, 10, 10)
+ layout.setSpacing(6)
+
+ # Socket 分組標題行 - 包含勾選框
+ title_row = QWidget()
+ title_layout = QHBoxLayout(title_row)
+ title_layout.setContentsMargins(0, 0, 0, 0)
+
+ # 分組勾選框
+ self.group_checkbox = QCheckBox()
+ self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
+ self.group_checkbox.setStyleSheet(f"""
+ QCheckBox {{ color: #DDD; }}
+ QCheckBox::indicator {{
+ width: 14px;
+ height: 14px;
+ border: 2px solid #888888;
+ border-radius: 3px;
+ background: transparent;
+ }}
+ QCheckBox::indicator:checked {{
+ background-color: {self.color};
+ border: 2px solid #888888;
+ }}
+ QCheckBox::indicator:indeterminate {{
+ background-color: #666;
+ border: 2px solid #888888;
+ }}
+ """)
+ self.group_checkbox.stateChanged.connect(
+ lambda state: self.group_selection_changed.emit(self.socket_id, state)
+ )
+
+ # Socket 分組標題
+ title_label = QLabel(f"Socket {self.socket_id}")
+ title_label.setStyleSheet(f"""
+ font-weight: bold;
+ font-size: 16px;
+ color: {self.color};
+ margin-bottom: 8px;
+ padding: 4px 8px;
+ border-radius: 6px;
+ """)
+
+ title_layout.addWidget(self.group_checkbox)
+ title_layout.addWidget(title_label)
+ title_layout.addStretch()
+
+ layout.addWidget(title_row)
+
+ # 創建子容器用於放置該 socket 下的所有無人機面板
+ self.drones_container = QWidget()
+ self.drones_layout = QVBoxLayout(self.drones_container)
+ self.drones_layout.setContentsMargins(0, 0, 0, 0)
+ self.drones_layout.setSpacing(4)
+
+ layout.addWidget(self.drones_container)
+
+ def add_drone_panel(self, panel):
+ """添加無人機面板到分組"""
+ self.drones_layout.addWidget(panel)
+
+ def clear_drones(self):
+ """清空所有無人機面板"""
+ while self.drones_layout.count():
+ item = self.drones_layout.takeAt(0)
+ if item.widget():
+ item.widget().setParent(None)
+
+ def get_checkbox(self):
+ """獲取分組勾選框"""
+ return self.group_checkbox
+
+ def set_checked(self, checked):
+ """設置分組勾選狀態"""
+ self.group_checkbox.setChecked(checked)
+
+ def set_check_state(self, state):
+ """設置分組勾選狀態（支持半選）"""
+ self.group_checkbox.setCheckState(state)
\ No newline at end of file
diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py
new file mode 100644
index 0000000..615e9ff
--- /dev/null
+++ b/src/unitdev01/unitdev01/gui.py
@@ -0,0 +1,862 @@
+#!/usr/bin/env python3
+import rclpy
+from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout,
+ QWidget, QLabel, QSplitter, QScrollArea,
+ QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem,
+ QHeaderView, QPushButton, QCheckBox, QLineEdit)
+from PyQt6.QtCore import Qt, QTimer
+import sys
+import asyncio
+
+# 導入分離的類別
+from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
+from map_layout import DroneMap
+from drone_panel import DronePanel, SocketGroupPanel
+from comm_panel import CommPanel
+
+class ControlStationUI(QMainWindow):
+ def __init__(self):
+ super().__init__()
+ self.setWindowTitle('GCS')
+ self.resize(1400, 900)
+
+ # 初始化ROS2
+ rclpy.init()
+ self.monitor = DroneMonitor()
+ self.monitor.signals.update_signal.connect(self.update_ui)
+
+ # ROS执行器配置
+ self.executor = rclpy.executors.SingleThreadedExecutor()
+ self.executor.add_node(self.monitor)
+
+ # 定时处理ROS事件
+ self.timer = QTimer()
+ self.timer.timeout.connect(self.spin_ros)
+ self.timer.start(10)
+
+ # 初始化UI
+ self.drones = {}
+ self.socket_groups = {}
+ self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向",
+ "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+ self.info_type_map = {
+ "mode": 0,
+ "armed": 1,
+ "battery": 2,
+ "altitude": 3,
+ "local": 4,
+ "velocity": 5,
+ "groundspeed": 6,
+ "heading": 7,
+ "roll": 8,
+ "pitch": 9,
+ "yaw": 10,
+ "loss_rate": 11,
+ "ping": 12
+ }
+
+ self.socket_colors = {
+ '0': '#00BFFF', # 天藍色 (DeepSkyBlue)
+ '1': '#FFD700', # 金色 (Gold)
+ '2': '#FF6969', # 淺紅色 (Light Red)
+ '3': '#FF69B4', # 熱粉紅 (HotPink)
+ '4': '#00FA9A', # 中春綠 (MediumSpringGreen)
+ '5': '#9370DB', # 中紫色 (MediumPurple) - 串口
+ '6': '#FFA500', # 橙色 (Orange)
+ '7': '#20B2AA', # 淺海綠 (LightSeaGreen)
+ '8': '#7CFC00', # 草綠色 (LawnGreen)
+ '9': '#FF8C00', # 深橙色 (DarkOrange)
+ 'default': '#AAAAAA' # 灰色
+ }
+
+ self.drone_positions = {}
+ self.drone_headings = {}
+ # 初始化地圖
+ self.drone_map = DroneMap()
+ # 初始化連接列表
+ self.udp_receivers = []
+ self.udp_connections = []
+ self.ws_connections = []
+ self.monitor.start_serial_connection('/dev/ttyUSB0', 57600)
+
+ self.init_ui()
+
+ def init_ui(self):
+ # 只呼叫一次
+ main_splitter = QSplitter(Qt.Orientation.Horizontal)
+
+ # 左側 TabWidget
+ self.left_tab = QTabWidget()
+
+ # — 分頁 1：Drone Panel
+ self.drone_panel_container = QWidget()
+ self.drone_panel_layout = QVBoxLayout(self.drone_panel_container)
+ self.drone_panel_layout.setAlignment(Qt.AlignmentFlag.AlignTop)
+ self.drone_panel_layout.setSpacing(0)
+ self.drone_panel_layout.setContentsMargins(10, 10, 10, 10)
+
+ # Wrap drone panel in scroll area
+ scroll = QScrollArea()
+ scroll.setWidget(self.drone_panel_container)
+ scroll.setWidgetResizable(True)
+ self.left_tab.addTab(scroll, "無人載具")
+
+ # — 分頁 2：Overview Table
+ self.overview_table = QTableWidget()
+ self.overview_table.setColumnCount(1)
+ self.overview_table.setRowCount(len(self.info_types))
+ self.overview_table.setHorizontalHeaderLabels(["資訊"])
+ header = self.overview_table.horizontalHeader()
+ header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents)
+ self.overview_table.verticalHeader().setVisible(False)
+ for i, txt in enumerate(self.info_types):
+ item = QTableWidgetItem(txt)
+ item.setFlags(Qt.ItemFlag.ItemIsEnabled)
+ item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
+ self.overview_table.setItem(i, 0, item)
+
+ self.left_tab.addTab(self.overview_table, "總覽")
+
+ # — 分頁 3：通訊設定
+ self.comm_panel = CommPanel()
+ self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added)
+ self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added)
+ self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection)
+ self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection)
+ self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection)
+ self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection)
+ self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout))
+
+ self.left_tab.addTab(self.comm_panel, "通訊")
+
+ # 右侧容器
+ right_container = QWidget()
+ right_layout = QVBoxLayout(right_container)
+ right_layout.setContentsMargins(10, 10, 10, 10)
+ right_layout.setSpacing(10)
+
+ # ========== 批次控制區域（直接使用 layout） ==========
+ batch_control_layout = QHBoxLayout()
+
+ # 添加批次操作標題
+ batch_title = QLabel("批次操作")
+ batch_title.setStyleSheet("""
+ color: #DDD;
+ font-size: 16px;
+ font-weight: bold;
+ padding: 5px;
+ background-color: #333;
+ border-radius: 4px;
+ """)
+ batch_control_layout.addWidget(batch_title)
+
+ # 上方按鈕區域
+ upper_buttons = QHBoxLayout()
+ select_all_btn = QPushButton("全選")
+ select_all_btn.clicked.connect(self.handle_select_all)
+ arm_all_btn = QPushButton("解鎖")
+ arm_all_btn.clicked.connect(self.handle_arm_selected)
+ takeoff_all_btn = QPushButton("起飛")
+ takeoff_all_btn.clicked.connect(self.handle_takeoff_selected)
+ for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]:
+ btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 8px 12px;
+ border-radius: 4px;
+ min-width: 80px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+ upper_buttons.addWidget(btn)
+ upper_buttons.addStretch()
+
+ # 模式切換區域
+ mode_layout = QHBoxLayout()
+ mode_label = QLabel("模式：")
+ mode_label.setStyleSheet("color: #DDD; min-width: 40px;")
+
+ from PyQt6.QtWidgets import QComboBox
+ self.mode_combo = QComboBox()
+ self.mode_combo.addItems(["AUTO", "GUIDED", "LOITER", "LAND"])
+ self.mode_combo.setCurrentIndex(1)
+ self.mode_combo.setStyleSheet("""
+ QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;}
+ """)
+
+ batch_mode_btn = QPushButton("切換")
+ batch_mode_btn.clicked.connect(self.handle_batch_mode_change)
+ batch_mode_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 8px 12px;
+ border-radius: 4px;
+ min-width: 80px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+ mode_layout.addWidget(mode_label)
+ mode_layout.addWidget(self.mode_combo)
+ mode_layout.addWidget(batch_mode_btn)
+ mode_layout.addStretch()
+
+ # 座標輸入區域
+ coord_widget = QWidget()
+ coord_layout = QHBoxLayout(coord_widget)
+
+ self.x_input = QLineEdit()
+ self.y_input = QLineEdit()
+ self.z_input = QLineEdit()
+
+ for input_field in [self.x_input, self.y_input, self.z_input]:
+ input_field.setFixedWidth(60)
+ input_field.setStyleSheet("""
+ QLineEdit {
+ background-color: #333;
+ color: #DDD;
+ border: 1px solid #555;
+ border-radius: 4px;
+ padding: 3px;
+ }
+ """)
+
+ coord_layout.addWidget(QLabel("X:", styleSheet="color: #DDD;"))
+ coord_layout.addWidget(self.x_input)
+ coord_layout.addWidget(QLabel("Y:", styleSheet="color: #DDD;"))
+ coord_layout.addWidget(self.y_input)
+ coord_layout.addWidget(QLabel("Z:", styleSheet="color: #DDD;"))
+ coord_layout.addWidget(self.z_input)
+
+ setpoint_btn = QPushButton("Setpoint")
+ setpoint_btn.clicked.connect(self.handle_setpoint_selected)
+ setpoint_btn.setStyleSheet("""
+ QPushButton {
+ background-color: #444;
+ color: #DDD;
+ border: none;
+ padding: 8px 12px;
+ border-radius: 4px;
+ min-width: 80px;
+ }
+ QPushButton:hover { background-color: #555; }
+ """)
+
+ lower_control = QHBoxLayout()
+ lower_control.addWidget(coord_widget)
+ lower_control.addWidget(setpoint_btn)
+ lower_control.addStretch()
+
+ # 組裝批次控制 layout
+ batch_control_layout.addLayout(upper_buttons)
+ batch_control_layout.addLayout(mode_layout)
+ batch_control_layout.addLayout(lower_control)
+
+ # 將批次控制 layout 添加到右側容器
+ right_layout.addLayout(batch_control_layout)
+
+ # 添加地圖
+ right_layout.addWidget(self.drone_map.get_widget())
+ self.drone_map.get_gps_signal().connect(self.handle_map_click)
+
+
+ # Add widgets to splitter
+ main_splitter.addWidget(self.left_tab)
+ main_splitter.addWidget(right_container)
+ main_splitter.setSizes([400, 1000])
+
+ self.setCentralWidget(main_splitter)
+
+ def handle_map_click(self, lat, lon):
+ print(f"地圖點擊位置: {lat:.6f}, {lon:.6f}")
+
+ def handle_udp_connection_added(self, ip, port):
+ """处理 UDP 连接添加信号"""
+ # 创建新连接
+ new_conn = {
+ 'name': f'UDP {len(self.udp_connections) + 1}',
+ 'ip': ip,
+ 'port': port,
+ 'enabled': True
+ }
+
+ # 启动接收器
+ receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'])
+ receiver.start()
+ self.udp_receivers.append(receiver)
+ new_conn['receiver'] = receiver
+
+ self.udp_connections.append(new_conn)
+
+ # 添加到 UI
+ self.comm_panel.add_udp_panel(new_conn)
+
+ self.statusBar().showMessage(f"已添加 UDP 连接: {ip}:{port}", 3000)
+ print(f"UDP MAVLink receiver added and started on {ip}:{port}")
+
+ def handle_ws_connection_added(self, url):
+ """处理 WebSocket 连接添加信号"""
+ # 创建新连接
+ new_conn = {
+ 'name': f'WS {len(self.ws_connections) + 1}',
+ 'url': url,
+ 'enabled': True
+ }
+
+ # 启动接收器
+ receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'])
+ receiver.start()
+ self.monitor.ws_receivers.append(receiver)
+ new_conn['receiver'] = receiver
+
+ self.ws_connections.append(new_conn)
+
+ # 添加到 UI
+ self.comm_panel.add_ws_panel(new_conn)
+
+ self.statusBar().showMessage(f"已添加 WebSocket 连接: {url}", 3000)
+ print(f"WebSocket receiver added and started: {url}")
+
+ def create_drone_panel(self, drone_id):
+ """創建無人機面板"""
+ panel = DronePanel(drone_id)
+
+ # 連接信號
+ panel.mode_change_requested.connect(self.handle_mode_change)
+ panel.arm_requested.connect(self.handle_arm)
+ panel.takeoff_requested.connect(self.handle_takeoff)
+ panel.setpoint_requested.connect(self.handle_single_setpoint)
+ panel.selection_changed.connect(self.handle_drone_selection)
+
+ return panel
+
+ def toggle_ws_connection(self, conn, btn, status_label):
+ """切換 WebSocket 連接狀態"""
+ if conn.get('enabled', False):
+ # 停止連接
+ if 'receiver' in conn and conn['receiver']:
+ conn['receiver'].stop()
+ conn['enabled'] = False
+ btn.setText("啟動")
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+ self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000)
+ else:
+ # 啟動連接
+ receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'])
+ receiver.start()
+ self.monitor.ws_receivers.append(receiver)
+ conn['receiver'] = receiver
+ conn['enabled'] = True
+ btn.setText("停止")
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000)
+
+ def remove_ws_connection(self, conn, panel):
+ """移除 WebSocket 连接"""
+ # 停止接收器
+ if 'receiver' in conn and conn['receiver']:
+ conn['receiver'].stop()
+ if conn['receiver'] in self.monitor.ws_receivers:
+ self.monitor.ws_receivers.remove(conn['receiver'])
+
+ # 从列表移除
+ if conn in self.ws_connections:
+ self.ws_connections.remove(conn)
+
+ # 从 comm_panel 列表移除
+ self.comm_panel.remove_ws_connection_from_list(conn)
+
+ # 从 UI 移除
+ panel.setParent(None)
+ panel.deleteLater()
+
+ self.statusBar().showMessage(f"已移除 WebSocket 连接: {conn['url']}", 3000)
+
+ def toggle_udp_connection(self, conn, btn, status_label):
+ """切換 UDP 連接狀態"""
+ if conn.get('enabled', False):
+ # 停止連接
+ if 'receiver' in conn and conn['receiver']:
+ conn['receiver'].stop()
+ conn['enabled'] = False
+ btn.setText("啟動")
+ status_label.setStyleSheet("color: #888; font-size: 16px;")
+ status_label.setToolTip("已停止")
+ self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000)
+ else:
+ # 啟動連接
+ receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'])
+ receiver.start()
+ self.udp_receivers.append(receiver)
+ conn['receiver'] = receiver
+ conn['enabled'] = True
+ btn.setText("停止")
+ status_label.setStyleSheet("color: #4CAF50; font-size: 16px;")
+ status_label.setToolTip("運行中")
+ self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000)
+
+ def remove_udp_connection(self, conn, panel):
+ """移除 UDP 连接"""
+ # 停止接收器
+ if 'receiver' in conn and conn['receiver']:
+ conn['receiver'].stop()
+ if conn['receiver'] in self.udp_receivers:
+ self.udp_receivers.remove(conn['receiver'])
+
+ # 从列表移除
+ if conn in self.udp_connections:
+ self.udp_connections.remove(conn)
+
+ # 从 comm_panel 列表移除
+ self.comm_panel.remove_udp_connection_from_list(conn)
+
+ # 从 UI 移除
+ panel.setParent(None)
+ panel.deleteLater()
+
+ self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000)
+
+ def create_socket_group_panel(self, socket_id):
+ """創建 socket 分組面板"""
+ color = self.socket_colors.get(socket_id, self.socket_colors['default'])
+ panel = SocketGroupPanel(socket_id, color)
+ panel.group_selection_changed.connect(self.handle_group_selection)
+ return panel
+
+ def handle_mode_change(self, drone_id):
+ """處理單個無人機的模式切換"""
+ mode = self.mode_combo.currentText()
+ loop = asyncio.get_event_loop()
+ future = self.monitor.set_mode(drone_id, mode)
+ loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}"))
+
+ def handle_arm(self, drone_id):
+ loop = asyncio.get_event_loop()
+ arm_state = not self.monitor.get_arm_state(drone_id) # Toggle arm state
+ future = self.monitor.arm_drone(drone_id, arm_state)
+ loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}"))
+
+ def handle_takeoff(self, drone_id):
+ loop = asyncio.get_event_loop()
+ future = self.monitor.takeoff_drone(drone_id, 10.0) # 10 meters default
+ loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}"))
+
+ def handle_setpoint_selected(self):
+ """處理發送 setpoint 命令"""
+ try:
+ x = float(self.x_input.text() or '0')
+ y = float(self.y_input.text() or '0')
+ z = float(self.z_input.text() or '0')
+
+ for drone_id in self.monitor.selected_drones:
+ if self.monitor.send_setpoint(drone_id, x, y, z):
+ self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000)
+ else:
+ self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000)
+ except ValueError:
+ self.statusBar().showMessage("座標格式錯誤", 3000)
+
+ def handle_single_setpoint(self, drone_id):
+ """處理單個無人機的 setpoint 命令"""
+ try:
+ x = float(self.x_input.text() or '0')
+ y = float(self.y_input.text() or '0')
+ z = float(self.z_input.text() or '0')
+
+ if self.monitor.send_setpoint(drone_id, x, y, z):
+ self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000)
+ else:
+ self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000)
+ except ValueError:
+ self.statusBar().showMessage("座標格式錯誤", 3000)
+
+ async def handle_service_response(self, future, action):
+ try:
+ result = await future
+ if result:
+ self.statusBar().showMessage(f"{action} 成功", 3000)
+ else:
+ self.statusBar().showMessage(f"{action} 失敗", 3000)
+ except Exception as e:
+ self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
+
+ def update_ui(self, msg_type, drone_id, data):
+ # 檢查是否為新無人機，若是則加入無人機面板
+ if drone_id not in self.drones:
+ self.add_drone(drone_id)
+ return
+
+ # 確認無人機面板存在
+ if not (panel := self.drones.get(drone_id)):
+ return
+
+ # 更新特定欄位並在總覽頁面更新
+ if msg_type == 'state':
+ mode = data.get('mode', 'UNKNOWN')
+ armed = data.get('armed', None)
+ mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55'
+ if armed is True:
+ arm_text = "ARMED"
+ arm_color = '#55FF55'
+ elif armed is False:
+ arm_text = "DISARMED"
+ arm_color = '#FF5555'
+ else:
+ arm_text = "--"
+ arm_color = '#AAAAAA' # 未知狀態
+
+ # 更新無人機面板欄位
+ self.update_field(panel, drone_id, 'mode', mode, mode_color)
+ self.update_field(panel, drone_id, 'armed', arm_text, arm_color)
+
+ # 更新總覽頁面欄位
+ self.update_overview_table(drone_id, 'mode', mode)
+ self.update_overview_table(drone_id, 'armed', arm_text)
+
+ elif msg_type == 'battery':
+ voltage = data.get('voltage', 16)
+
+ # 判斷電池節數
+ cells = round(voltage / 3.95)
+
+ # 計算電量百分比
+ percentage = (voltage / cells - 3.7) / 0.5 * 100
+
+ # 根據百分比設置顏色
+ if percentage < 20:
+ voltage_color = '#FF6464' # 紅色 (低電量)
+ elif percentage < 50:
+ voltage_color = '#FFA500' # 橘色 (中低電量)
+ else:
+ voltage_color = '#FFFFFF' # 白色 (正常電量)
+
+ percentage = data.get('percentage', percentage)
+
+ # 顯示總電壓、電池節數、單節電壓和百分比
+ text = f"{percentage:.0f}%"
+ vol = f"{voltage:.2f}V"
+
+ self.update_field(panel, drone_id, 'battery', text, voltage_color)
+ self.update_overview_table(drone_id, 'battery', vol)
+
+ elif msg_type == 'gps':
+ lat, lon = data.get('lat', 0), data.get('lon', 0)
+ self.drone_positions[drone_id] = (lat, lon)
+ text = f"{lat:.6f}°, {lon:.6f}°"
+ self.update_field(panel, drone_id, 'gps', text)
+ self.update_overview_table(drone_id, 'gps', text)
+
+ elif msg_type == 'altitude':
+ altitude = data.get('altitude', 0)
+ text = f"{altitude:.1f} m"
+ self.update_field(panel, drone_id, 'altitude', text)
+ self.update_overview_table(drone_id, 'altitude', text)
+
+ elif msg_type == 'local_pose':
+ text = f"{data['x']:.1f}, {data['y']:.1f}"
+ self.update_field(panel, drone_id, 'local', text)
+ self.update_overview_table(drone_id, 'local', text)
+
+ elif msg_type == 'hud':
+ heading = data.get('heading')
+ self.drone_headings[drone_id] = heading
+ groundspeed = data.get('groundspeed')
+ heading_text = f"{heading:.1f}°"
+ if isinstance(groundspeed, (int, float)):
+ groundspeed_text = f"{groundspeed:.1f} m/s"
+ else:
+ groundspeed_text = "--"
+ self.update_field(panel, drone_id, 'heading', heading_text)
+ self.update_field(panel, drone_id, 'groundspeed', groundspeed_text)
+ self.update_overview_table(drone_id, 'heading', heading_text)
+ self.update_overview_table(drone_id, 'groundspeed', groundspeed_text)
+
+ elif msg_type == 'loss_rate':
+ loss_rate = data.get('loss_rate', 0)
+ text = f"{loss_rate:.1f}%"
+ self.update_field(panel, drone_id, 'loss_rate', text)
+ self.update_overview_table(drone_id, 'loss_rate', text)
+
+ elif msg_type == 'ping':
+ ping = data.get('ping', 0)
+ text = f"{ping:.1f} ms"
+ self.update_field(panel, drone_id, 'ping', text)
+ self.update_overview_table(drone_id, 'ping', text)
+
+ elif msg_type == 'velocity':
+ text = f"{data['vx']:.1f}, {data['vy']:.1f}"
+ self.update_overview_table(drone_id, 'velocity', text)
+
+ elif msg_type == 'attitude':
+ roll = data.get('roll', 0)
+ pitch = data.get('pitch', 0)
+ yaw = data.get('yaw', 0)
+ roll_text = f"{roll:.1f}°"
+ pitch_text = f"{pitch:.1f}°"
+ yaw_text = f"{yaw:.1f}°"
+ self.update_overview_table(drone_id, 'roll', roll_text)
+ self.update_overview_table(drone_id, 'pitch', pitch_text)
+ self.update_overview_table(drone_id, 'yaw', yaw_text)
+
+ # 更新地圖上的無人機位置
+ if drone_id in self.drone_positions and drone_id in self.drone_headings:
+ lat, lon = self.drone_positions[drone_id]
+ heading = self.drone_headings[drone_id]
+ self.drone_map.update_drone_position(drone_id, lat, lon, heading)
+
+ # 新增處理分組勾選的方法
+ def handle_group_selection(self, socket_id, state):
+ """處理 socket 分組勾選狀態變化"""
+ # 獲取該分組下的所有無人機
+ group_drones = [did for did in self.drones.keys()
+ if self.get_socket_id(did) == socket_id]
+
+ # 根據分組勾選狀態更新所有該分組的無人機勾選狀態
+ is_checked = state == Qt.CheckState.Checked.value
+
+ for drone_id in group_drones:
+ checkbox = self.drones[drone_id].get_checkbox()
+ if checkbox:
+ # 暫時斷開信號連接，避免遞迴觸發
+ checkbox.blockSignals(True)
+ checkbox.setChecked(is_checked)
+ checkbox.blockSignals(False)
+
+ # 手動更新選中集合
+ if is_checked:
+ self.monitor.selected_drones.add(drone_id)
+ else:
+ self.monitor.selected_drones.discard(drone_id)
+
+ def handle_drone_selection(self, drone_id, state):
+ """處理個別無人機勾選狀態變化"""
+ if state == Qt.CheckState.Checked.value:
+ self.monitor.selected_drones.add(drone_id)
+ else:
+ self.monitor.selected_drones.discard(drone_id)
+
+ # 更新對應 socket 分組的勾選狀態
+ socket_id = self.get_socket_id(drone_id)
+ self.update_group_checkbox_state(socket_id)
+
+ # 新增更新分組勾選框狀態的方法
+ def update_group_checkbox_state(self, socket_id):
+ """更新指定 socket 分組的勾選框狀態"""
+ # 獲取該分組下的所有無人機
+ group_drones = [did for did in self.drones.keys()
+ if self.get_socket_id(did) == socket_id]
+
+ if not group_drones:
+ return
+
+ # 檢查該分組內有多少無人機被勾選
+ checked_count = sum(1 for did in group_drones
+ if self.drones[did].is_checked())
+
+ # 獲取分組勾選框
+ if socket_id in self.socket_groups:
+ group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox")
+ if group_checkbox:
+ # 暫時斷開信號連接
+ group_checkbox.blockSignals(True)
+
+ if checked_count == 0:
+ # 沒有無人機被勾選
+ group_checkbox.setCheckState(Qt.CheckState.Unchecked)
+ elif checked_count == len(group_drones):
+ # 所有無人機都被勾選
+ group_checkbox.setCheckState(Qt.CheckState.Checked)
+ else:
+ # 部分無人機被勾選，顯示半勾選狀態
+ group_checkbox.setCheckState(Qt.CheckState.PartiallyChecked)
+
+ # 重新連接信號
+ group_checkbox.blockSignals(False)
+
+ def handle_select_all(self):
+ """處理全選按鈕 - 支援分組結構"""
+ # 檢查是否所有無人機都已被選中
+ all_selected = all(
+ self.drones[drone_id].is_checked()
+ for drone_id in self.drones
+ )
+
+ # 如果全部已選中，則取消全選；否則全選
+ new_state = not all_selected
+
+ # 更新所有勾選框狀態（無人機和分組）
+ for drone_id in self.drones:
+ self.drones[drone_id].set_checked(new_state)
+
+ # 更新所有分組勾選框狀態
+ for socket_id in self.socket_groups:
+ group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox")
+ if group_checkbox:
+ group_checkbox.blockSignals(True)
+ group_checkbox.setChecked(new_state)
+ group_checkbox.blockSignals(False)
+
+ def handle_arm_selected(self):
+ """處理批次解鎖"""
+ loop = asyncio.get_event_loop()
+ for drone_id in self.monitor.selected_drones:
+ future = self.monitor.arm_drone(drone_id, True)
+ loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}"))
+
+ def handle_takeoff_selected(self):
+ """處理批次起飛"""
+ loop = asyncio.get_event_loop()
+ for drone_id in self.monitor.selected_drones:
+ future = self.monitor.takeoff_drone(drone_id, 10.0)
+ loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
+
+ def handle_batch_mode_change(self):
+ mode = self.mode_combo.currentText()
+ loop = asyncio.get_event_loop()
+ for drone_id in self.monitor.selected_drones:
+ future = self.monitor.set_mode(drone_id, mode)
+ loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}"))
+
+ def update_field(self, panel, drone_id, field, text, color=None):
+ """更新面板中的指定欄位"""
+ if isinstance(panel, DronePanel):
+ panel.update_field(field, text, color)
+
+ def update_overview_table(self, drone_id=None, field=None, value=None):
+ # Ensure the widget is available
+ if not hasattr(self, 'overview_table') or self.overview_table is None:
+ return
+
+ # Update a specific cell in the overview table
+ if drone_id and field and value:
+ col = 1 + list(self.drones.keys()).index(drone_id)
+ row = self.info_type_map.get(field, -1) # Get row from field mapping
+
+ if row == -1:
+ return # Invalid field, exit
+
+ item = self.overview_table.item(row, col)
+ if not item:
+ item = QTableWidgetItem()
+ self.overview_table.setItem(row, col, item)
+
+ item.setText(value) # Update the cell's text
+ item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) # Center the text
+
+ # If no specific update, refresh entire table
+ if drone_id is None:
+ cols = 1 + len(self.drones)
+ self.overview_table.setColumnCount(cols)
+ headers = ["資訊"] + list(self.drones.keys())
+ self.overview_table.setHorizontalHeaderLabels(headers)
+
+ for col, did in enumerate(self.drones, start=1):
+ panel = self.drones[did]
+ for field, row in self.info_type_map.items():
+ lbl = panel.findChild(QLabel, f"{did}_{field}")
+ val = lbl.text() if lbl else "--"
+ val_item = QTableWidgetItem(val)
+ val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter)
+ self.overview_table.setItem(row, col, val_item)
+
+ def get_socket_id(self, drone_id):
+ """從 drone_id 提取 socket_id (例如 's9_1' -> '9')"""
+ import re
+ match = re.match(r's(\d+)_\d+', drone_id)
+ return match.group(1) if match else 'unknown'
+
+ def add_drone(self, drone_id):
+ if drone_id in self.drones:
+ return
+
+ # 獲取 socket_id
+ socket_id = self.get_socket_id(drone_id)
+
+ # 創建無人機面板
+ panel = self.create_drone_panel(drone_id)
+ self.drones[drone_id] = panel
+
+ # 如果該 socket 分組不存在，創建它
+ if socket_id not in self.socket_groups:
+ group_panel = self.create_socket_group_panel(socket_id)
+ self.socket_groups[socket_id] = group_panel
+
+ # 將無人機面板添加到對應的 socket 分組中
+ group_panel = self.socket_groups[socket_id]
+ group_panel.drones_layout.addWidget(panel)
+
+ # 重新排序並顯示所有 socket 分組
+ self.reorganize_socket_groups()
+
+ # 更新分組勾選框狀態
+ self.update_group_checkbox_state(socket_id)
+
+ # 更新總覽表
+ self.update_overview_table()
+
+ def reorganize_socket_groups(self):
+ """重新組織和排序 socket 分組"""
+ # 先清空主佈局
+ while self.drone_panel_layout.count():
+ w = self.drone_panel_layout.takeAt(0).widget()
+ if w:
+ w.setParent(None)
+
+ # 對每個 socket 分組內的無人機進行排序
+ for socket_id, group_panel in self.socket_groups.items():
+ # 獲取該分組內的所有無人機
+ group_drones = [did for did in self.drones.keys()
+ if self.get_socket_id(did) == socket_id]
+
+ # 清空分組佈局
+ while group_panel.drones_layout.count():
+ w = group_panel.drones_layout.takeAt(0).widget()
+ if w:
+ w.setParent(None)
+
+ # 對該分組內的無人機按數字排序
+ def sort_key(x):
+ parts = x[1:].split('_')
+ return (int(parts[0]), int(parts[1]))
+
+ # 重新添加排序後的無人機面板
+ for did in sorted(group_drones, key=sort_key):
+ group_panel.drones_layout.addWidget(self.drones[did])
+
+ # 按 socket_id 數字順序添加分組到主佈局
+ for socket_id in sorted(self.socket_groups.keys(), key=lambda x: int(x)):
+ self.drone_panel_layout.addWidget(self.socket_groups[socket_id])
+
+ def spin_ros(self):
+ try:
+ self.executor.spin_once(timeout_sec=0.01)
+ for (drone_id, msg_type), data in self.monitor.latest_data.items():
+ self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
+ self.monitor.latest_data.clear()
+ except Exception as e:
+ print(f"ROS spin error: {e}")
+
+ def closeEvent(self, event):
+ # 停止 UDP 接收器
+ for receiver in self.udp_receivers:
+ receiver.stop()
+
+ # 停止 WebSocket 接收器
+ for receiver in self.monitor.ws_receivers:
+ receiver.stop()
+
+ self.monitor.destroy_node()
+ self.executor.shutdown()
+ rclpy.shutdown()
+ event.accept()
+
+if __name__ == '__main__':
+ app = QApplication(sys.argv)
+ station = ControlStationUI()
+ station.show()
+ app.exec()
\ No newline at end of file
diff --git a/src/unitdev01/unitdev01/interface.py b/src/unitdev01/unitdev01/interface.py
index 5699e46..07b2773 100644
--- a/src/unitdev01/unitdev01/interface.py
+++ b/src/unitdev01/unitdev01/interface.py
@@ -144,7 +144,6 @@ class DroneControlApp(QMainWindow):
self.workers.imu_signal.connect(self.update_imu)
self.workers.hdg_signal.connect(self.update_hdg)
self.workers.groundspeed_signal.connect(self.update_groundspeed)
-
self.workers.start()
def initUI(self):
diff --git a/src/unitdev01/unitdev01/map_layout.py b/src/unitdev01/unitdev01/map_layout.py
new file mode 100644
index 0000000..11f7171
--- /dev/null
+++ b/src/unitdev01/unitdev01/map_layout.py
@@ -0,0 +1,292 @@
+#!/usr/bin/env python3
+from PyQt6.QtWebEngineWidgets import QWebEngineView
+from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot
+from PyQt6.QtWebChannel import QWebChannel
+
+class DroneMap:
+ """無人機地圖類別 - 負責管理 Leaflet 地圖顯示"""
+
+ def __init__(self):
+ """初始化地圖"""
+ self.map_view = QWebEngineView()
+ self.map_loaded = False
+ self.pending_map_updates = {}
+
+ # 創建橋接對象
+ self.bridge = MapBridge()
+
+ # 設置 QWebChannel
+ self.channel = QWebChannel()
+ self.channel.registerObject('bridge', self.bridge)
+ self.map_view.page().setWebChannel(self.channel)
+
+ # 設置地圖 HTML
+ inline_html = '''
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ '''
+
+ self.map_view.setHtml(inline_html)
+ self.map_view.loadFinished.connect(self._on_map_loaded)
+
+ # 設置地圖更新計時器
+ self.map_update_timer = QTimer()
+ self.map_update_timer.timeout.connect(self.update_map_positions)
+ self.map_update_timer.start(200) # 每 200ms 更新一次
+
+ def _on_map_loaded(self, ok: bool):
+ """地圖加載完成回調"""
+ if ok:
+ self.map_loaded = True
+ else:
+ print("⚠️ 地圖加載失敗")
+
+ def update_drone_position(self, drone_id, lat, lon, heading):
+ """更新無人機位置（加入待處理隊列）"""
+ self.pending_map_updates[drone_id] = (lat, lon, heading)
+
+ def update_map_positions(self):
+ """批量更新地圖上的無人機位置"""
+ if not self.map_loaded or not self.pending_map_updates:
+ return
+
+ # 批量執行所有待更新的位置
+ js_commands = []
+ for drone_id, (lat, lon, heading) in self.pending_map_updates.items():
+ js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});")
+
+ if js_commands:
+ combined_js = "\n".join(js_commands)
+ self.map_view.page().runJavaScript(combined_js)
+
+ # 清空待更新緩存
+ self.pending_map_updates.clear()
+
+ def clear_trajectories(self):
+ """清除所有軌跡"""
+ if self.map_loaded:
+ self.map_view.page().runJavaScript("clearAllTrajectories();")
+
+ def focus_on_drone(self, drone_id):
+ """聚焦到指定無人機"""
+ if self.map_loaded:
+ self.map_view.page().runJavaScript(f"focusOn('{drone_id}');")
+
+ def get_widget(self):
+ """獲取地圖 widget"""
+ return self.map_view
+
+ def get_gps_signal(self):
+ """獲取 GPS 信號"""
+ return self.bridge.gps_signal
+
+class MapBridge(QObject):
+ """JavaScript 和 Python 之間的橋接類"""
+ gps_signal = pyqtSignal(float, float) # lat, lon
+
+ def __init__(self):
+ super().__init__()
+
+ @pyqtSlot(float, float)
+ def emitGpsSignal(self, lat, lon):
+ """供 JavaScript 調用的方法"""
+ self.gps_signal.emit(lat, lon)
\ No newline at end of file
diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py
index e17aa74..9bb6329 100644
--- a/src/unitdev01/unitdev01/mavlink.py
+++ b/src/unitdev01/unitdev01/mavlink.py
@@ -1,5 +1,6 @@
import sys
-import rclpy
+import time
+import math
from PyQt5 import QtWidgets
from PyQt5.QtCore import QThread, pyqtSignal
from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox
@@ -14,34 +15,119 @@ class MAVLinkWorker(QThread):
imu_signal = pyqtSignal(str, float)
hdg_signal = pyqtSignal(str, float)
groundspeed_signal = pyqtSignal(str, float)
+ ping_signal = pyqtSignal(str, float)
+ loss_signal = pyqtSignal(str, float)
+ frequency_signal = pyqtSignal(str, float)
+ param_signal = pyqtSignal(str, int)
+ kbps_signal = pyqtSignal(str, float)
- def __init__(self, connection_string="udp:127.0.0.1:14550"):
+ def __init__(self, connection_string="udp:127.0.0.1:14560 ", usb='/dev/ttyUSB0', acm ='/dev/ttyACM0'):
super().__init__()
- self.connection = mavutil.mavlink_connection(connection_string)
+ self.sysids = [1, 5, 10]
+ self.namespaces = {}
+ self.namespaces = [f"UAV{sysid}" for sysid in self.sysids]
+
+ self.connection = mavutil.mavlink_connection(connection_string, baud=115200)
+ self.connection.wait_heartbeat()
+ for sysid in self.sysids:
+ self.set_sr_params(sysid)
self.running = True
- self.uav_data = {}
+
+ # 用於計算頻率丟包
+ self.message_count = {}
+ self.frequency = {}
+ self.start_time = {}
+ self.seq_numbers = {}
+ self.packet_loss_count = {}
+ self.total_packet_count = {}
+ self.loss_percentage = {}
+ self.total_bytes_received = {}
+ self.throughput_KBps = {}
+
+ for sysid in self.sysids:
+ self.request_param(sysid, "SR1_EXTRA1")
+ self.connection.mav.timesync_send(
+ 0, #tc1
+ int(time.time() * 1e9) #ts1
+ )
def run(self):
while self.running:
try:
- msg = self.connection.recv_match(blocking=True)
+ msg = self.connection.recv_msg() # Debugging line to see received messages
+ current_time = time.time()
if not msg:
continue
-
- msg_type = msg.get_type()
sysid = msg.get_srcSystem()
- namespace = f"uav{sysid}"
-
- if sysid not in self.uav_data:
- self.uav_data[sysid] = {"sysid": sysid}
+ if sysid == 0:
+ continue
+ namespace = f"UAV{sysid}"
+ msg_type = msg.get_type()
+ if msg_type =="BAD_DATA":
+ continue
+
+ if sysid not in self.total_bytes_received:
+ self.total_bytes_received[sysid] = 0
+ self.throughput_KBps[sysid] = 0
+
+ # 計算訊息大小
+ msg_bytes = msg.get_msgbuf() # 取得封包的 bytes
+ if msg_bytes:
+ self.total_bytes_received[sysid] += len(msg_bytes)
+
+ # Packet loss calculation
+ if sysid not in self.seq_numbers:
+ self.seq_numbers[sysid] = msg.get_seq() # Initialize sequence number tracking
+ self.packet_loss_count[sysid] = 0
+ self.total_packet_count[sysid] = 1
+ else:
+ current_seq = msg.get_seq()
+ expected_seq = (self.seq_numbers[sysid] + 1) % 256
+ self.total_packet_count[sysid] += 1
+
+ if current_seq != expected_seq: # Packet(s) lost
+ lost_packets = (current_seq - expected_seq) % 256
+ self.packet_loss_count[sysid] += lost_packets
+ self.total_packet_count[sysid] += lost_packets
+
+ self.seq_numbers[sysid] = current_seq
+ self.loss_percentage[sysid] = (self.packet_loss_count[sysid] / self.total_packet_count[sysid]) * 100
+ self.loss_signal.emit(namespace, self.loss_percentage[sysid])
+
+ # Frequency calculation
+ if sysid not in self.message_count:
+ self.message_count[sysid] = 0
+ self.start_time[sysid] = current_time
+
+ self.message_count[sysid] += 1
+
+ # 每隔一段時間更新
+ elapsed_time = current_time - self.start_time[sysid]
+ if elapsed_time >= 1:
+ # 每秒頻率
+ self.frequency[sysid] = self.message_count[sysid] / elapsed_time
+ self.frequency_signal.emit(namespace, self.frequency[sysid])
+
+ # 發送 timesync request
+ self.connection.mav.timesync_send(
+ 0, #tc1
+ int(current_time * 1e9) #ts1
+ )
+ #吞吐量
+ self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time
+ self.kbps_signal.emit(namespace, self.throughput_KBps[sysid])
+
+ self.start_time[sysid] = current_time
+ self.message_count[sysid] = 0
+ self.total_bytes_received[sysid] = 0
if msg_type == "HEARTBEAT":
mode = mavutil.mode_string_v10(msg)
self.state_signal.emit(namespace, mode)
elif msg_type == "BATTERY_STATUS":
- percentage = msg.battery_remaining/100
- self.battery_signal.emit(namespace, percentage)
+ voltage = msg.voltages[0] / 1000
+ self.battery_signal.emit(namespace, voltage)
elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7
@@ -60,7 +146,7 @@ class MAVLinkWorker(QThread):
self.local_position_signal.emit(namespace, x, y, z)
elif msg_type == "ATTITUDE":
- pitch = msg.pitch
+ pitch = math.degrees(msg.pitch)
self.imu_signal.emit(namespace, pitch)
elif msg_type == "VFR_HUD":
@@ -69,6 +155,16 @@ class MAVLinkWorker(QThread):
self.hdg_signal.emit(namespace, heading)
self.groundspeed_signal.emit(namespace, groundspeed)
+ elif msg_type == "TIMESYNC":
+ round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6
+ if(round_trip_time<1e6):
+ self.ping_signal.emit(namespace, round_trip_time)
+
+ elif msg_type == 'PARAM_VALUE':
+ param_name = "SR1_EXTRA1"
+ if msg.param_id == param_name:
+ self.param_signal.emit(namespace, int(msg.param_value))
+
except Exception as e:
print(f"Error reading message: {e}")
@@ -76,8 +172,11 @@ class MAVLinkWorker(QThread):
self.running = False
self.connection.close()
+ def get_sysid(self, namespace):
+ return int(namespace.replace('UAV', ''))
+
def set_mode(self, namespace, mode):
- sysid = int(namespace[-1])
+ sysid = self.get_sysid(namespace)
if mode == 'STABILIZE':
mode = 0
elif mode == 'AUTO':
@@ -86,56 +185,47 @@ class MAVLinkWorker(QThread):
mode = 4
elif mode == 'LOITER':
mode = 5
- self.connection.mav.set_mode_send(
+ self.connection.mav.set_mode_send( #SET_MODE (11) — [DEP]
sysid,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
mode
)
def arm(self, namespace, arm):
- sysid = int(namespace[-1])
+ sysid = self.get_sysid(namespace)
self.connection.mav.command_long_send(
sysid,
1, # Component ID
- mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, #MAV_CMD_COMPONENT_ARM_DISARM (400)
0, # Confirmation
1 if arm else 0,
0, 0, 0, 0, 0, 0
)
- # self.connection.mav.command_long_send(
- # sysid,
- # 1, # Component ID
- # mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
- # 0,
- # 33,
- # -1,
- # 0, 0, 0, 0, 0
- # )
def takeoff(self, namespace, altitude):
- sysid = int(namespace[-1])
+ sysid = self.get_sysid(namespace)
self.connection.mav.command_long_send(
sysid,
1, # Component ID
- mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, # Command to execute
+ mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, #MAV_CMD_NAV_TAKEOFF (22)
0, # Confirmation (0 = first transmission)
0, 0, 0, 0, # Parameters 1-4 (Unused in takeoff command)
0, 0, altitude # Latitude, Longitude, Altitude (target_altitude in meters)
)
def land(self, namespace):
- sysid = int(namespace[-1])
+ sysid = self.get_sysid(namespace)
self.connection.mav.command_long_send(
sysid,
1, # Component ID
- mavutil.mavlink.MAV_CMD_NAV_LAND,
+ mavutil.mavlink.MAV_CMD_NAV_LAND, #MAV_CMD_NAV_LAND (21)
0,
0, 0, 0, 0,
0, 0, 0
)
def set_local_position(self, namespace, x, y, z):
- sysid = int(namespace[-1])
+ sysid = self.get_sysid(namespace)
self.connection.mav.set_position_target_local_ned_send(
0, sysid, 1, # time_boot_ms, sysid, compid
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame
@@ -146,12 +236,93 @@ class MAVLinkWorker(QThread):
0, 0 # Yaw, yaw_rate
)
+ def reboot_drone(self, namespace):
+ sysid = self.get_sysid(namespace)
+ self.connection.mav.command_long_send(
+ sysid,
+ 1, # target_component (一般為1)
+ mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, # Reboot 命令
+ 0, # confirmation
+ 1, # 第一個參數 (1 = reboot，0 = 無操作，2 = 關機)
+ 0, 0, 0, 0, 0, 0 # 其餘參數保留
+ )
+
+ def set_param(self, namespace, param_name, value):
+ sysid = self.get_sysid(namespace)
+ try:
+ self.connection.mav.param_set_send(
+ sysid,
+ 1,
+ param_name.encode('utf-8'),
+ float(value),
+ mavutil.mavlink.MAV_PARAM_TYPE_REAL32
+ )
+ except Exception as e:
+ print(f"Failed to set parameter {param_name}: {e}")
+
+ def request_param(self, sysid, param_name):
+ try:
+ self.connection.mav.param_request_read_send(
+ sysid, # 系統 ID
+ 1, # 組件 ID
+ param_name.encode('utf-8'), # 參數名稱
+ -1 # 參數索引，-1 表示根據名稱請求
+ )
+ except Exception as e:
+ print(f"Failed to set parameter {param_name}: {e}")
+
+
+ def set_sr_params(self, sysid):
+ """ 直接設置 MAVLink 訊息頻率 """
+ freqs = [0, 1, 4]
+ params = {
+ "SR1_ADSB": freqs[0],
+ "SR1_EXT_STAT": freqs[1],
+ "SR1_EXTRA1": freqs[2],
+ "SR1_EXTRA2": freqs[1],
+ "SR1_EXTRA3": freqs[1],
+ "SR1_POSITION": freqs[2],
+ "SR1_RAW_SENS": freqs[1],
+ "SR1_RC_CHAN": freqs[1]
+ }
+ for param, value in params.items():
+ self.set_param(f"UAV{sysid}", param, value)
+
+ '''
+ def set_sr_params(self, sysid):
+ """ 直接設置 MAVLink 訊息頻率 """
+ params = {
+ "SERIAL1_BAUD": 38
+ }
+ for param, value in params.items():
+ self.set_param(f"UAV{sysid}", param, value)
+
+ def set_sr_params(self, sysid):
+ """ 直接設置 MAVLink 訊息頻率 """
+ freqs = [0, 3, 3]
+ params = {
+ "SR1_ADSB": freqs[0],
+ "SR1_EXT_STAT": freqs[1],
+ "SR1_EXTRA1": freqs[2],
+ "SR1_EXTRA2": freqs[2],
+ "SR1_EXTRA3": freqs[1],
+ "SR1_POSITION": freqs[1],
+ "SR1_RAW_SENS": freqs[1],
+ "SR1_RC_CHAN": freqs[1]
+ }
+ for param, value in params.items():
+ self.set_param(f"UAV{sysid}", param, value)
+ '''
+ def init_drone(self, namespace):
+ sysid = self.get_sysid(namespace)
+ self.set_local_position(sysid, 0, 0, 0)
class DroneControlApp(QMainWindow):
def __init__(self):
super().__init__()
- self.namespaces = ['uav1', 'uav2', 'uav3']
self.workers = MAVLinkWorker()
+ self.sysids = self.workers.sysids
+ self.namespaces = self.workers.namespaces
self.initUI()
# Connect signals to update the UI
@@ -163,7 +334,11 @@ class DroneControlApp(QMainWindow):
self.workers.imu_signal.connect(self.update_imu)
self.workers.hdg_signal.connect(self.update_hdg)
self.workers.groundspeed_signal.connect(self.update_groundspeed)
-
+ self.workers.ping_signal.connect(self.update_ping)
+ self.workers.loss_signal.connect(self.update_loss)
+ self.workers.frequency_signal.connect(self.update_frequency)
+ self.workers.param_signal.connect(self.update_param)
+ self.workers.kbps_signal.connect(self.update_kbps)
self.workers.start()
def initUI(self):
@@ -193,15 +368,20 @@ class DroneControlApp(QMainWindow):
# 狀態顯示
self.uav_labels[namespace] = {
- "status": QLabel(f"{namespace} 狀態：等待數據..."),
- "battery": QLabel(f"{namespace} 電量：等待數據..."),
- "gps": QLabel(f"{namespace} GPS位置：等待數據...\n\n"),
- "fix": QLabel(f"{namespace} Fix Type：等待數據..."),
- "satellites": QLabel(f"{namespace} 衛星數量：等待數據..."),
- "local_position": QLabel(f"{namespace} Local Position：等待數據..."),
- "groundspeed": QLabel(f"{namespace} 地面速度：等待數據..."),
- "pitch": QLabel(f"{namespace} Pitch：等待數據..."),
- "heading": QLabel(f"{namespace} Heading：等待數據..."),
+ "status": QLabel("狀態：等待數據..."),
+ "battery": QLabel("電壓：等待數據..."),
+ "local_position": QLabel("Local Position：等待數據..."),
+ "gps": QLabel("GPS位置：等待數據...\n\n"),
+ "fix": QLabel("Fix Type：等待數據..."),
+ "satellites": QLabel("衛星數量：等待數據..."),
+ "groundspeed": QLabel("地面速度：等待數據..."),
+ "pitch": QLabel("Pitch：等待數據..."),
+ "heading": QLabel("Heading：等待數據..."),
+ "ping": QLabel("Ping：等待數據..."),
+ "loss": QLabel("丟包：等待數據..."),
+ "frequency": QLabel("頻率：等待數據..."),
+ "kbps": QLabel("吞吐量：等待數據..."),
+ "param": QLabel("SR1_EXTRA1：等待數據...")
}
# 把每個資訊添加到該 UAV 的垂直佈局中
@@ -213,6 +393,16 @@ class DroneControlApp(QMainWindow):
main_layout.addLayout(uav_layout)
+ # SR1_EXTRA1參數設置
+ param_layout = QHBoxLayout()
+ self.paramInput = QLineEdit()
+ self.paramInput.setPlaceholderText("輸入SR1_EXTRA1的新值")
+ self.setParamButton = QPushButton("設置SR1_EXTRA1")
+ self.setParamButton.clicked.connect(self.set_SR1_EXTRA1)
+ param_layout.addWidget(self.paramInput)
+ param_layout.addWidget(self.setParamButton)
+ main_layout.addLayout(param_layout)
+
# 模式切換區域
mode_layout = QHBoxLayout()
self.modeComboBox = QComboBox()
@@ -224,6 +414,23 @@ class DroneControlApp(QMainWindow):
mode_layout.addWidget(self.modeButton)
main_layout.addLayout(mode_layout)
+ # 解鎖按鈕
+ fly_layout = QHBoxLayout()
+ self.armButton = QPushButton("解鎖")
+ self.armButton.clicked.connect(self.arm_drone)
+
+ # 起飛按鈕
+ self.takeoffButton = QPushButton("起飛")
+ self.takeoffButton.clicked.connect(self.takeoff_drone)
+
+ # 降落按鈕
+ self.landButton = QPushButton("降落")
+ self.landButton.clicked.connect(self.land_drone)
+ fly_layout.addWidget(self.armButton)
+ fly_layout.addWidget(self.takeoffButton)
+ fly_layout.addWidget(self.landButton)
+ main_layout.addLayout(fly_layout)
+
# XYZ 設置欄位
xyz_layout = QHBoxLayout()
self.positionX = QLineEdit()
@@ -232,31 +439,30 @@ class DroneControlApp(QMainWindow):
self.positionY.setPlaceholderText("Y")
self.positionZ = QLineEdit()
self.positionZ.setPlaceholderText("Z")
+ self.setPositionButton = QPushButton("設置位置")
+ self.setPositionButton.clicked.connect(self.set_local_position)
xyz_layout.addWidget(QLabel("輸入位置："))
xyz_layout.addWidget(self.positionX)
xyz_layout.addWidget(self.positionY)
xyz_layout.addWidget(self.positionZ)
+ xyz_layout.addWidget(self.setPositionButton)
main_layout.addLayout(xyz_layout)
# 設置 XYZ 的按鈕
- self.setPositionButton = QPushButton("設置位置")
- self.setPositionButton.clicked.connect(self.set_local_position)
- main_layout.addWidget(self.setPositionButton)
- # 解鎖按鈕
- self.armButton = QPushButton("解鎖")
- self.armButton.clicked.connect(self.arm_drone)
- main_layout.addWidget(self.armButton)
-
- # 起飛按鈕
- self.takeoffButton = QPushButton("起飛")
- self.takeoffButton.clicked.connect(self.takeoff_drone)
- main_layout.addWidget(self.takeoffButton)
+ #設置重啟按鈕
+ self.rebootButton = QPushButton("重啟")
+ self.rebootButton.clicked.connect(self.reboot_drone)
+ main_layout.addWidget(self.rebootButton)
- # 降落按鈕
- self.landButton = QPushButton("降落")
- self.landButton.clicked.connect(self.land_drone)
- main_layout.addWidget(self.landButton)
+ mission_layout = QHBoxLayout()
+ self.initButton = QPushButton("重設位置")
+ self.initButton.clicked.connect(self.init_drone)
+ self.startButton = QPushButton("開始任務")
+ self.startButton.clicked.connect(self.start_mission)
+ mission_layout.addWidget(self.initButton)
+ mission_layout.addWidget(self.startButton)
+ main_layout.addLayout(mission_layout)
# 設置主佈局
central_widget = QWidget(self)
@@ -271,13 +477,13 @@ class DroneControlApp(QMainWindow):
event.accept()
def update_state(self, namespace, mode):
- self.uav_labels[namespace]["status"].setText(f"{namespace} 狀態：{mode}")
+ self.uav_labels[namespace]["status"].setText(f"狀態：{mode}")
- def update_battery(self, namespace, percentage):
- self.uav_labels[namespace]["battery"].setText(f"{namespace} 電量：{percentage * 100:.2f}%")
+ def update_battery(self, namespace, voltage):
+ self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V")
def update_gps(self, namespace, latitude, longitude):
- self.uav_labels[namespace]["gps"].setText(f"{namespace} GPS位置： \nLat:{latitude:.6f}° \nLon:{longitude:.6f}°")
+ self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°")
def update_gps_status(self, namespace, fix_type, satellites_visible):
fix_type_str = {
@@ -288,20 +494,35 @@ class DroneControlApp(QMainWindow):
4: "RTK Float",
5: "RTK Fix"
}.get(fix_type, "Unknown")
- self.uav_labels[namespace]["fix"].setText(f"{namespace} Fix Type：{fix_type_str}")
- self.uav_labels[namespace]["satellites"].setText(f"{namespace} 衛星數量：{satellites_visible}")
+ self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}")
+ self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}")
def update_local_position(self, namespace, x, y, z):
- self.uav_labels[namespace]["local_position"].setText(f"{namespace} Local：({x:.2f}, {y:.2f}, {z:.2f})")
+ self.uav_labels[namespace]["local_position"].setText(f"Local：({x:.2f}, {y:.2f}, {z:.2f})")
def update_imu(self, namespace, pitch):
- self.uav_labels[namespace]["pitch"].setText(f"{namespace} Pitch：{pitch:.2f}°")
+ self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°")
def update_hdg(self, namespace, heading):
- self.uav_labels[namespace]["heading"].setText(f"{namespace} Heading：{heading:.2f}°")
+ self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°")
def update_groundspeed(self, namespace, groundspeed):
- self.uav_labels[namespace]["groundspeed"].setText(f"{namespace} 地面速度：{groundspeed:.2f} m/s")
+ self.uav_labels[namespace]["groundspeed"].setText(f"地面速度：{groundspeed:.2f} m/s")
+
+ def update_ping(self, namespace, ping):
+ self.uav_labels[namespace]["ping"].setText(f"Ping：{ping:.2f} ms")
+
+ def update_loss(self, namespace, loss):
+ self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%")
+
+ def update_frequency(self, namespace, frequency):
+ self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz")
+
+ def update_kbps(self, namespace, kbps):
+ self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s")
+
+ def update_param(self, namespace, value):
+ self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}")
def change_mode(self):
try:
@@ -323,10 +544,12 @@ class DroneControlApp(QMainWindow):
def takeoff_drone(self):
try:
z_text = self.positionZ.text().strip()
- altitude = float(z_text) if z_text else 5.0
- for namespace in self.namespaces:
+ z = float(z_text) if z_text else 5.0
+ h = 2
+ for i, namespace in enumerate(self.namespaces):
if self.uav_checkboxes[namespace].isChecked():
- self.workers.takeoff(namespace, altitude)
+ self.workers.takeoff(namespace, z + h * i)
+
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
@@ -340,14 +563,51 @@ class DroneControlApp(QMainWindow):
x = float(self.positionX.text().strip())
y = float(self.positionY.text().strip())
z = float(self.positionZ.text().strip())
+ h = 2
+ for i, namespace in enumerate(self.namespaces):
+ if self.uav_checkboxes[namespace].isChecked():
+ self.workers.set_local_position(namespace, x, y, z + h * i)
+
+ except ValueError:
+ QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
+
+ def reboot_drone(self):
+ try:
+ for namespace in self.namespaces:
+ if self.uav_checkboxes[namespace].isChecked():
+ self.workers.reboot_drone(namespace)
+ except Exception as e:
+ QtWidgets.QMessageBox.warning(self, "錯誤", f"重啟失敗：{e}")
+
+ def set_SR1_EXTRA1(self):
+ param_value = self.paramInput.text().strip()
+ try:
for namespace in self.namespaces:
if self.uav_checkboxes[namespace].isChecked():
- self.workers.set_local_position(namespace, x, y, z)
+ self.workers.set_param(namespace, "SR1_EXTRA1", param_value)
+ self.workers.request_param(namespace, "SR1_EXTRA1")
except ValueError:
QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
-def main(args=None):
- rclpy.init(args=args)
+ def init_drone(self):
+ try:
+ for namespace in self.namespaces:
+ if self.uav_checkboxes[namespace].isChecked():
+ self.workers.init_drone(namespace)
+ print(f"重設位置：{namespace}")
+ except Exception as e:
+ QtWidgets.QMessageBox.warning(self, "錯誤", f"重設位置失敗：{e}")
+
+ def start_mission(self):
+ try:
+ for namespace in self.namespaces:
+ if self.uav_checkboxes[namespace].isChecked():
+ self.workers.start_mission(namespace)
+ print(f"開始任務：{namespace}")
+ except Exception as e:
+ QtWidgets.QMessageBox.warning(self, "錯誤", f"開始任務失敗：{e}")
+
+def main():
app = QtWidgets.QApplication(sys.argv)
window = DroneControlApp()
window.show()
diff --git a/src/unitdev02/setup.py b/src/unitdev02/setup.py
index 62d265b..f5d6e64 100644
--- a/src/unitdev02/setup.py
+++ b/src/unitdev02/setup.py
@@ -21,7 +21,8 @@ setup(
entry_points={
'console_scripts': [
'showmavsimple = unitdev02.test01:mavlink_analyzer_simple',
- 'fdm_switch_one = unitdev02.test01:fdm_swicth_example_one',
+ 'fdm_switch_one = unitdev02.test01:fdm_switch_example_one',
+ 'fdm_switch_two = unitdev02.test01:fdm_switch_example_two',
],
},
)
diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt
new file mode 100644
index 0000000..d8309aa
--- /dev/null
+++ b/src/unitdev02/unitdev02/devnote.txt
@@ -0,0 +1,57 @@
+備選需要的功能
+ - serail 對於 telemetry 的支援
+ - serial_manager.serial_object.transport 這些變數可能不需要
+
+不用動
+ - mavlink_object 的 send_message 確認一下 mavlink_bridge 的 _send_to_socket 是不是應該做成 async
+ - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複)
+
+這一步
+ 研究 ros2 service
+
+下一步
+
+下下一步
+
+
+後面
+ rssi 資訊提取s
+
+
+
+自己的常用指令
+python -m fc_network_adapter.tests.test_vehicleStatusPublisher
+python -m fc_network_adapter.tests.test_ringBuffer
+python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+
+python -m someotherpkg.src.example_takeoff_land
+python -m someotherpkg.src.example_change_mode
+
+ros2 topic list
+ros2 topic echo /fc_network/vehicle/sys3/battery
+
+
+/home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7
+
+mavproxy.py --master=tcp:127.0.0.1:5790 --out=udp:127.0.0.1:14560
+
+ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5,b: 8}"
+ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"
+
+ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}"
+ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
+
+ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}"
+
+
+sudo tcpdump -i lo 'udp dst port 14561' -X
+sudo tcpdump -i lo 'udp dst port 14550' -X -vv
+sudo tcpdump -i lo -X udp port 14550
+
+幾個要討論的
+1. 專案文件中 .mat 檔案是幹嘛的?
+2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
+3. readme 那串文字來源? 用途?
+4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
+5. pitch yaw roll 出來了 弄一下
+6.
diff --git a/src/unitdev02/unitdev02/lifecycle_node.py b/src/unitdev02/unitdev02/lifecycle_node.py
new file mode 100644
index 0000000..bebc82e
--- /dev/null
+++ b/src/unitdev02/unitdev02/lifecycle_node.py
@@ -0,0 +1,45 @@
+
+
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import time
+
+# import mavros_msgs.srv
+
+class TalkerNode(Node):
+ def __init__(self):
+ start_time = time.time()
+ super().__init__('talker_node')
+ end_time = time.time()
+ print(f"Node initialization took {end_time - start_time:.2f} seconds")
+
+ self.publisher_ = self.create_publisher(String, 'hahatest/_1', 10)
+ self.timer = self.create_timer(1.0, self.timer_callback) # 每秒執行一次
+ self.get_logger().info('TalkerNode has been started.')
+
+ def timer_callback(self):
+ msg = String()
+ msg.data = 'Hello, ROS 2!'
+ self.publisher_.publish(msg)
+ self.get_logger().info(f'Published: "{msg.data}"')
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = TalkerNode()
+
+ print("Before sleep")
+ time.sleep(5) # 等待 5 秒鐘
+ print("After sleep")
+ try:
+ start_time = time.time()
+ while time.time() - start_time < 10: # 持續 10 秒鐘
+ rclpy.spin_once(node)
+ time.sleep(1) # 每秒執行一次
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/unitdev02/unitdev02/sslChech.sh b/src/unitdev02/unitdev02/sslChech.sh
new file mode 100644
index 0000000..9f38904
--- /dev/null
+++ b/src/unitdev02/unitdev02/sslChech.sh
@@ -0,0 +1,27 @@
+#!/bin/bash
+
+# 網站清單
+DOMAINS=("google.com" "smarter.nchu.edu.tw")
+
+echo "網站 SSL 憑證剩餘天數："
+echo "---------------------------"
+
+for domain in "${DOMAINS[@]}"; do
+ end_date=$(echo | openssl s_client -servername "$domain" -connect "$domain:443" 2>/dev/null |
+ openssl x509 -noout -enddate | cut -d= -f2)
+
+ end_timestamp=$(date -d "$end_date" +%s)
+ now_timestamp=$(date +%s)
+
+ remaining_days=$(( (end_timestamp - now_timestamp) / 86400 ))
+
+ if [ $remaining_days -lt 0 ]; then
+ status="已過期 ❌"
+ elif [ $remaining_days -lt 15 ]; then
+ status="即將到期 ⚠️"
+ else
+ status="正常 ✅"
+ fi
+
+ printf "%-20s 到期日：%-25s 剩餘天數：%3d 天 %s\n" "$domain" "$end_date" "$remaining_days" "$status"
+done
diff --git a/src/unitdev02/unitdev02/test01.md b/src/unitdev02/unitdev02/test01.md
index d265f70..af13f0b 100644
--- a/src/unitdev02/unitdev02/test01.md
+++ b/src/unitdev02/unitdev02/test01.md
@@ -46,4 +46,60 @@ RC_CHANNELS：遙控器 (RC) 的通道數據，代表從操控者 (或模擬器)
'MISSION_CURRENT': 73, 'SIMSTATE': 72,
7. 任務與導航
MISSION_CURRENT：目前正在執行的飛行任務 (Mission) 的編號。
-SIMSTATE：SITL 模擬器的狀態，如飛行模式、地面速度等。
\ No newline at end of file
+SIMSTATE：SITL 模擬器的狀態，如飛行模式、地面速度等。
+
+
+
+================================
+
+
+['__annotations__', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__getitem__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__',
+
+'_crc', '_fieldnames', '_header', '_instance_field', '_instance_offset', '_instances', '_link_id', '_msgbuf', '_pack', '_payload', '_posted', '_signed', '_timestamp', ,
+
+, 'crc_extra', 'fieldunits_by_name', 'format_attr', 'get_crc', , 'get_header', 'get_link_id', , 'get_msgbuf', 'get_payload', 'get_seq', 'get_signed', 'instance_field', 'instance_offset', 'lengths', 'name', 'native_format', 'ordered_fieldnames', 'orders', 'pack', 'pitch', 'pitchspeed', 'roll', 'rollspeed', 'sign_packet', 'time_boot_ms', 'to_dict', 'to_json', 'unpacker', 'yaw', 'yawspeed']
+
+mavlink 封包 種類名稱
+'get_type()' '_type' 'msgname'
+'get_msgId()' 'id'
+'fieldenums_by_name'
+
+
+發送端的 sysid get_srcComponent()
+發送端的 compid get_srcComponent()
+
+這邊是一組的 mavlink 封包 內容的
+資料長度 'array_lengths'
+資料名稱 'fieldnames' 'get_fieldnames()'
+資料格式 'fieldtypes'
+
+=====================================
+
+ HEARTBEAT {type : 2, autopilot : 3, base_mode : 81, custom_mode : 0, system_status : 3, mavlink_version : 3}
+
+=====================================
+
+
+?
+
+https://github.com/ros/angles
+https://github.com/ros-geographic-info/geographic_info
+
+
+指令
+
+mkdir -p ~/ros2_geographic_info/src
+cd ~/ros2_geographic_info/src
+git clone https://github.com/ros/angles.git -b ros2
+git clone https://github.com/ros-geographic-info/geographic_info.git -b ros2
+
+cd ..
+rosdep check --from-paths src --ignore-src
+
+colcon build --packages-select angles
+. ./install/setup.bash
+colcon build --packages-select geographic_info
+. ./install/setup.bash
+colcon build --packages-select mavros_msgs
+
+
diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py
index c3cb82c..5beda2e 100644
--- a/src/unitdev02/unitdev02/test01.py
+++ b/src/unitdev02/unitdev02/test01.py
@@ -6,6 +6,10 @@ import time
import socket
import struct
+# used at fdm_switch_example_two
+import threading
+import queue
+
def mavlink_analyzer_simple(count = 500):
# rclpy.init()
# node = rclpy.create_node('mavlink_analyzer_simple')
@@ -32,11 +36,17 @@ def mavlink_analyzer_simple(count = 500):
if msg.get_type() == 'SERVO_OUTPUT_RAW':
print(msg)
+ pass
else:
print('No message yet, 1 second for data to fill')
sleep(1)
+ print("markA ------")
+ print(msg.get_type())
+ print(msg.ordered_fieldnames)
+ print("markA ------ END")
+
print('Messages Type received:')
print(msg_count)
@@ -52,7 +62,6 @@ def fdm_parser_example(data=None):
data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
# 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
-
parse_format = 'HHI16H'
if len(data) != struct.calcsize(parse_format):
@@ -103,7 +112,8 @@ def fdm_switch_example_one():
elapsed_time = current_time - start_time
if elapsed_time >= 1.0:
- print(f"Packets received in the last second: {packet_count}")
+ # print(f"Packets received in the last second: {packet_count}")
+ print(f'JSON Pack : {data_g2s}')
packet_count = 0
start_time = current_time
fdm_parser_example(data_s2g)
@@ -113,34 +123,140 @@ def fdm_switch_example_two():
'''
這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
有轉換數據格式
+
+ time_usec
+ servo1_raw
+ servo16_raw
'''
# read info from SITL via MAVLink
- connection_string="udp:127.0.0.1:14550"
+ connection_string="udp:127.0.0.1:14550" # uart mavlink # MATLAB
connection = mavutil.mavlink_connection(connection_string)
# set a target link pass JSON to Gazebo, as a client
sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
server_address = ('127.0.0.1', 19002)
+ data_queue = queue.Queue()
+ servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....]
+ data_queue.put(servo_out)
+
+ def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
+ interval = 1.0 / frame_rate_fdm
+ while True:
+ start_time = time.time()
+ try:
+ data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
+ if data is None:
+ break
+ last_data = data
+ except queue.Empty:
+ data = last_data
+ data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
+ sock.sendto(data_fdm, server_address)
+ frame_count_fdm += 3
+ # fdm_parser_example(data_fdm) # debug
+
+ elapsed_time = time.time() - start_time
+ sleep_time = interval - elapsed_time
+ if sleep_time > 0:
+ sleep(sleep_time)
+
+ Running = True
+ thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+ thread.start()
+ print('Start sending data to Gazebo')
+
+ while Running:
+ msgb1 = None
+ msg = connection.recv_msg()
+ while msg :
+ if msg.get_type() == 'SERVO_OUTPUT_RAW':
+ msgb1 = msg
+ # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+ msg = connection.recv_msg()
+
+ if msgb1:
+ for i in range(16):
+ servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+ data_queue.put(servo_out)
+ msgb1 = None
+ # Running = False # debug
+ else:
+ # print('No message yet, 10 ms for data to fill')
+ sleep(0.01)
+
+ # Example of putting data into the queue
+ # data_queue.put(data)
+ # To stop the thread, you can put None into the queue
+ # data_queue.put(None)
+
+from pymavlink.dialects.v20 import common # 使用 MAVLink 2.0
+
+def mavlink_btye_generator_test():
+
+ # 創建一個 MAVLink 對象
+ mav = common.MAVLink(None) # 不透過 connection，直接使用 None
+
+ # 創建一個 HEARTBEAT 訊息
+ msg = mav.heartbeat_encode(
+ type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
+ autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+ base_mode=0,
+ custom_mode=0,
+ system_status=mavutil.mavlink.MAV_STATE_ACTIVE
+ )
+ msg = mav.command_long_encode(
+ target_system=1,
+ target_component=1,
+ command=mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+ confirmation=0,
+ param1=0, # Custom mode
+ param2=0, # Custom sub-mode
+ param3=0,
+ param4=0,
+ param5=0,
+ param6=0,
+ param7=0
+ )
+
+
+
+ # 取得 MAVLink 訊息的 bytes
+ mavlink_bytes = msg.pack(mav)
+
+ # 回傳 bytes
+ print(mavlink_bytes)
+ print(type(mavlink_bytes))
+
+ mav2 = common.MAVLink(None)
+ # my_msg = mav2.decode(mavlink_bytes)
+ print("========")
+ print(dir(mav2))
+ # print(my_msg)
+
+def simple_getMavlink():
+ connection_string="udp:127.0.0.1:14550"
+ # connection_string='/dev/ttyUSB0'
+ connection = mavutil.mavlink_connection(connection_string)
+
+ while True:
+ msg = connection.recv_msg()
+ if msg:
+ print(msg)
+ else:
+ print('No message yet, 0.1 second for data to fill')
+ sleep(0.1)
+
+
print('Start')
-mavlink_analyzer_simple()
-
-# connection_string="udp:127.0.0.1:14550"
-# connection = mavutil.mavlink_connection(connection_string)
-
-# connection.mav.command_long_send(
-# 1,
-# 1,
-# mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
-# 0, 33, 1000000, 0, 0, 0, 0, 0
-# )
-
-# connection.mav.command_long_send(
-# 1,
-# 1, # Component ID
-# mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-# 0, # Confirmation
-# 1, # 0: disarm, 1: arm
-# 0, 0, 0, 0, 0, 0
-# )
\ No newline at end of file
+# fdm_switch_example_two()
+# fdm_parser_example()
+# mavlink_analyzer_simple(8)
+# mavlink_btye_generator_test()
+simple_getMavlink()
+
+
+
+
+# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out, source_system=17, source_component=200)
diff --git a/src/unitdev02/unitdev02/unix_client.py b/src/unitdev02/unitdev02/unix_client.py
new file mode 100644
index 0000000..ce00e92
--- /dev/null
+++ b/src/unitdev02/unitdev02/unix_client.py
@@ -0,0 +1,14 @@
+import socket
+
+SOCKET_PATH = "/tmp/unix_socket_example"
+
+# 建立 socket
+client = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+client.connect(SOCKET_PATH)
+
+# 傳送資料
+client.sendall(b"Hello from client!")
+data = client.recv(1024)
+print("📤 Server replied:", data.decode())
+
+client.close()
diff --git a/src/unitdev02/unitdev02/unix_server.py b/src/unitdev02/unitdev02/unix_server.py
new file mode 100644
index 0000000..30ef524
--- /dev/null
+++ b/src/unitdev02/unitdev02/unix_server.py
@@ -0,0 +1,94 @@
+import socket
+import os
+
+# # socket file path
+# SOCKET_PATH = "/tmp/unix_socket_example"
+
+# # 若檔案存在就先刪除
+# if os.path.exists(SOCKET_PATH):
+# os.remove(SOCKET_PATH)
+
+# # 建立 socket
+# server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+
+# # 綁定 socket 到檔案
+# server.bind(SOCKET_PATH)
+# server.listen(1)
+
+# print("🔌 Server waiting for connection...")
+
+# # 等待 client 連線
+# conn, _ = server.accept()
+# print("✅ Client connected.")
+
+# while True:
+# data = conn.recv(1024)
+# if not data:
+# break
+# print("📥 Received:", data.decode())
+# conn.sendall(b"Echo: " + data)
+
+# conn.close()
+# server.close()
+# os.remove(SOCKET_PATH)
+
+
+# =====================
+
+# from pymavlink import mavutil
+
+
+# def create_unix_socket_connection():
+# # 建立一個 mavtcpin 實例
+# mav_conn = mavutil.mavtcpin("127.0.0.250:9999", source_system=1, source_component=1)
+
+# # 替換底層 socket
+# # 關閉原有的 socket
+# mav_conn.listen.close()
+
+# # 創建 Unix socket 並替換
+# unix_socket = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+# unix_socket_path = "/tmp/unix_socket_mavlink.sock"
+
+# # 確保先前的 unix socket 已被移除
+# if os.path.exists(unix_socket_path):
+# os.remove(unix_socket_path)
+
+# # 綁定並設定 Unix socket
+# unix_socket.bind(unix_socket_path)
+# unix_socket.listen(1)
+# unix_socket.setblocking(0)
+# mavutil.set_close_on_exec(unix_socket.fileno())
+
+# # 替換 listen socket
+# mav_conn.listen = unix_socket
+# mav_conn.fd = unix_socket.fileno()
+
+# # mav_conn.port = unix_socket
+
+# return mav_conn
+
+
+# a = create_unix_socket_connection()
+
+# # print(a.port)
+
+
+
+
+# ===============================
+
+import mavunixin
+mav_conn = mavunixin.mavunixin("unix:/tmp/unix_socket_mavlink.sock", source_system=1, source_component=1)
+
+import time
+print("🔌 Server waiting for connection...")
+while True:
+ time.sleep(1)
+ mavlinkMsg = mav_conn.recv_msg()
+ if mavlinkMsg is not None:
+ print("📥 Received:", mavlinkMsg)
+ # a.send(mavlinkMsg)
+ # print("📤 Server replied:", data.decode())
+ else:
+ print("No message received.")
\ No newline at end of file
diff --git a/src/unitdev03/0228/gazebo_native_manager.py b/src/unitdev03/0228/gazebo_native_manager.py
new file mode 100644
index 0000000..8699188
--- /dev/null
+++ b/src/unitdev03/0228/gazebo_native_manager.py
@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+import subprocess
+import argparse
+import time
+import psutil
+import h5py
+import numpy as np
+
+class GazeboModelManager:
+ def __init__(self):
+ self.base_cmd = "gz service -s /world/map"
+ self.timeout = 5000
+
+ def add_iris(self, number, x_pos, y_pos, yaw_angle):
+ cmd = f'''gz service -s /world/map/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 5000 --req 'sdf: "model://iris{number}{x_pos} {y_pos} 0.195 0 0 {yaw_angle}"' '''
+
+ try:
+ print(f"正在新增 iris{number}...")
+ print(f"位置: x={x_pos}, y={y_pos}, yaw={yaw_angle}度")
+ subprocess.run(cmd, shell=True, check=True)
+ print(f"成功新增 iris{number}")
+
+ sitl_instance = number - 1
+ port = 14550 + sitl_instance
+
+ sitl_cmd = f"sim_vehicle.py -v ArduCopter -f gazebo-iris{sitl_instance} -L NCHU --model JSON --map --console -I{sitl_instance} --out=127.0.0.1:{port}"
+ terminal_cmd = f'gnome-terminal -- bash -c "{sitl_cmd}; exec bash"'
+ print(f"正在啟動 SITL...")
+ subprocess.Popen(terminal_cmd, shell=True)
+
+ return True
+ except subprocess.CalledProcessError as e:
+ print(f"新增失敗: {e}")
+ return False
+
+ def cleanup_sitl(self, number):
+ instance_number = number - 1
+ print(f"正在清理 SITL 進程 {number}...")
+
+ cleanup_commands = [
+ f"pkill -f 'sim_vehicle.py.*-I{instance_number}'",
+ f"pkill -f 'arducopter.*-I{instance_number}'",
+ "pkill -f 'mavproxy.py.*map'",
+ "pkill -f 'mavproxy.py.*console'",
+ f"pkill -f 'mavproxy.*{instance_number}'",
+ "pkill -f 'python.*map.py'",
+ "pkill -f 'MAVProxy.py.*map'"
+ ]
+
+ for cmd in cleanup_commands:
+ try:
+ subprocess.run(cmd, shell=True)
+ except subprocess.CalledProcessError:
+ pass
+
+ time.sleep(2)
+ print(f"SITL 進程清理完成")
+
+ def remove_iris(self, number):
+ cmd = f'gz service -s /world/map/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 1000 --req \'name: "iris{number}"\''
+
+ try:
+ print(f"正在移除 iris{number}...")
+ subprocess.run(cmd, shell=True, check=True)
+ print(f"成功移除 iris{number}")
+ return True
+ except subprocess.CalledProcessError as e:
+ print(f"移除失敗: {e}")
+ return False
+
+ def add_obstacle(self, number, x_pos, y_pos, radius=0.5, height=2.0):
+ cmd = f'''gz service -s /world/map/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 5000 --req 'sdf: "true{x_pos} {y_pos} {height/2} 0 0 0{radius}{height}{radius}{height}0.3 0.3 0.3 10.7 0.7 0.7 1"' '''
+
+ try:
+ print(f"正在新增障礙物 {number}...")
+ print(f"位置: x={x_pos}, y={y_pos}, radius={radius}, height={height}")
+ subprocess.run(cmd, shell=True, check=True)
+ return True
+ except subprocess.CalledProcessError as e:
+ print(f"新增失敗: {e}")
+ return False
+
+ def remove_obstacle(self, number):
+ cmd = f'gz service -s /world/map/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 1000 --req \'name: "obstacle_{number}"\''
+
+ try:
+ print(f"正在移除障礙物 {number}...")
+ subprocess.run(cmd, shell=True, check=True)
+ print(f"成功移除障礙物 {number}")
+ return True
+ except subprocess.CalledProcessError as e:
+ print(f"移除失敗: {e}")
+ return False
+
+ def add_obstacles_from_mat(self, mat_file):
+ """從 .mat 檔案逐一生成障礙物"""
+ try:
+ with h5py.File(mat_file, 'r') as f:
+ x_data = f['#refs#/d'][()]
+ y_data = f['#refs#/e'][()]
+ z_data = f['#refs#/f'][()]
+ radius_data = f['#refs#/g'][()]
+ height_data = f['#refs#/h'][()]
+
+ data_matrix = np.vstack((x_data, y_data, z_data, radius_data, height_data)).T
+
+ for i, obstacle in enumerate(data_matrix):
+ x, y, _, radius, height = obstacle
+ success = self.add_obstacle(i+1, float(x), float(y), float(radius), float(height))
+ if not success:
+ print(f"警告：障礙物 {i+1} 新增失敗")
+ time.sleep(0.1)
+
+ print("所有障礙物新增完成")
+ return True
+
+ except Exception as e:
+ print(f"錯誤：讀取或處理 .mat 檔案時發生問題: {e}")
+ return False
+
+def main():
+ parser = argparse.ArgumentParser(description='Gazebo Iris and Obstacle Manager')
+ parser.add_argument('action', choices=['add', 'remove', 'cleanup', 'add_from_mat'],
+ help='Action to perform (add, remove, cleanup, or add_from_mat)')
+ parser.add_argument('type', choices=['iris', 'obstacle'],
+ help='Type of object to manage (iris or obstacle)')
+ parser.add_argument('number', type=int, nargs='?',
+ help='Object number (e.g., 4 for iris4 or obstacle_4)')
+ parser.add_argument('--x-pos', type=float, default=15.0,
+ help='X position for new object')
+ parser.add_argument('--y-pos', type=float, default=0.0,
+ help='Y position for new object')
+ parser.add_argument('--yaw', type=float, default=90.0,
+ help='Yaw angle in degrees for iris')
+ parser.add_argument('--radius', type=float, default=0.5,
+ help='Radius for obstacle cylinder')
+ parser.add_argument('--height', type=float, default=2.0,
+ help='Height for obstacle cylinder')
+ parser.add_argument('--mat-file', type=str,
+ help='Path to .mat file containing obstacles data')
+
+ args = parser.parse_args()
+ manager = GazeboModelManager()
+
+ if args.action == 'add_from_mat':
+ if args.type == 'obstacle' and args.mat_file:
+ manager.add_obstacles_from_mat(args.mat_file)
+ else:
+ print("錯誤：使用 add_from_mat 時需指定 --mat-file 參數")
+ elif args.action == 'add':
+ if args.number is None:
+ print("錯誤：使用 add 時需指定 number 參數")
+ return
+ if args.type == 'iris':
+ manager.add_iris(args.number, args.x_pos, args.y_pos, args.yaw)
+ else: # obstacle
+ manager.add_obstacle(args.number, args.x_pos, args.y_pos, args.radius, args.height)
+ elif args.action == 'cleanup':
+ if args.number is None:
+ print("錯誤：使用 cleanup 時需指定 number 參數")
+ return
+ if args.type == 'iris':
+ manager.cleanup_sitl(args.number)
+ else: # remove
+ if args.number is None:
+ print("錯誤：使用 remove 時需指定 number 參數")
+ return
+ if args.type == 'iris':
+ manager.remove_iris(args.number)
+ else:
+ manager.remove_obstacle(args.number)
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/unitdev03/0228/pymavlink_01.py b/src/unitdev03/0228/pymavlink_01.py
new file mode 100644
index 0000000..dc8ebb1
--- /dev/null
+++ b/src/unitdev03/0228/pymavlink_01.py
@@ -0,0 +1,232 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import time
+import math
+import threading
+
+class DroneController:
+ def __init__(self, connection_string="udp:127.0.0.1:14550"):
+ # 建立 MAVLink 連接
+ self.master = mavutil.mavlink_connection(connection_string)
+
+ # 等待心跳包
+ self.master.wait_heartbeat()
+ print("已建立連接!")
+
+ def set_mode(self, mode):
+ """設置飛行模式"""
+ mode_id = self.master.mode_mapping()[mode]
+ self.master.mav.set_mode_send(
+ self.master.target_system,
+ mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+ mode_id
+ )
+
+ for _ in range(5):
+ msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+ if msg and msg.custom_mode == mode_id:
+ return True
+ return False
+
+ def arm(self, arm_state=True):
+ """解鎖/上鎖無人機"""
+ self.master.mav.command_long_send(
+ self.master.target_system,
+ self.master.target_component,
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+ 0,
+ 1 if arm_state else 0, 0, 0, 0, 0, 0, 0
+ )
+
+ for _ in range(5):
+ msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+ if msg:
+ armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
+ if armed == arm_state:
+ return True
+ return False
+
+ def takeoff(self, altitude):
+ """起飛到指定高度"""
+ self.master.mav.command_long_send(
+ self.master.target_system,
+ self.master.target_component,
+ mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+ 0,
+ 0, 0, 0, 0, 0, 0,
+ altitude
+ )
+
+ while True:
+ msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
+ if msg:
+ current_alt = msg.relative_alt / 1000.0
+ if abs(current_alt - altitude) < 0.5:
+ return True
+ time.sleep(0.1)
+
+ def goto_position_local(self, x, y, z, vx=2, vy=2, vz=0, yaw=0, yaw_rate=0):
+ """
+ 飛到指定的本地座標位置
+ x, y, z: 目標位置(公尺)
+ """
+ # NED系統中，向上飛需要給負值
+ z = -z # 將輸入的z值轉換為NED系統
+
+ # 重要：type_mask 設為 0 表示使用該維度
+ # 我們只想使用位置控制 (x,y,z)，所以其他都設為 ignore
+ type_mask = (
+ # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE | # 8
+ # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE | # 16
+ # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE | # 32
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | # 64
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | # 128
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | # 256
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE | # 1024
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE # 2048
+ )
+
+ # 或直接使用二進制: type_mask = 0b0000111111111000
+
+ print(f"發送位置指令: x={x}, y={y}, z={-z}")
+
+ self.master.mav.set_position_target_local_ned_send(
+ 0, # timestamp
+ self.master.target_system,
+ self.master.target_component,
+ mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+ type_mask,
+ x, y, z, # 位置
+ 0, 0, 0, # 速度 (設為 0)
+ 5, 5, 0, # 加速度 (設為 0)
+ 0, 0 # yaw, yaw_rate (設為 0)
+ )
+
+ # 監控位置變化
+ print("開始監控位置變化...")
+ start_time = time.time()
+ while time.time() - start_time < 20: # 20秒超時
+ msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+ if msg:
+ dist = math.sqrt((msg.x - x)**2 + (msg.y - y)**2 + (msg.z - z)**2)
+ print(f"當前位置: x={msg.x:.2f}, y={msg.y:.2f}, z={-msg.z:.2f}, 距離目標: {dist:.2f}m")
+ if dist < 0.5:
+ print("到達目標位置")
+ return True
+ time.sleep(0.1)
+
+ print("移動超時")
+ return False
+
+ def get_current_state(self):
+ """獲取當前狀態"""
+ msg = self.master.recv_match(type='HEARTBEAT', blocking=True)
+ if msg:
+ mode = list(self.master.mode_mapping().keys())[list(self.master.mode_mapping().values()).index(msg.custom_mode)]
+ armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
+ return {
+ 'mode': mode,
+ 'armed': armed
+ }
+ return None
+
+def print_menu():
+ """列印功能表"""
+ menu = [
+ "\n=== 無人機控制界面 ===",
+ "1. 切換至 GUIDED 模式",
+ "2. 切換至 LAND 模式",
+ "3. 解鎖",
+ "4. 上鎖",
+ "5. 起飛",
+ "6. 飛到指定位置",
+ "7. 切換至 RTL 模式(返航)",
+ "8. 查看當前狀態",
+ "9. 退出",
+ "0. 顯示此清單",
+ "===================="
+ ]
+ for item in menu:
+ print(item)
+
+def main():
+ drone = DroneController()
+ print_menu()
+
+ while True:
+ try:
+ choice = input("\n請輸入指令編號: ")
+
+ if choice == '0':
+ print_menu()
+
+ elif choice == '1':
+ if drone.set_mode("GUIDED"):
+ print("已切換至 GUIDED 模式")
+ else:
+ print("切換模式失敗")
+
+ elif choice == '2':
+ if drone.set_mode("LAND"):
+ print("已切換至 LAND 模式")
+ else:
+ print("切換模式失敗")
+
+ elif choice == '3':
+ if drone.arm(True):
+ print("無人機已解鎖")
+ else:
+ print("解鎖失敗")
+
+ elif choice == '4':
+ if drone.arm(False):
+ print("無人機已上鎖")
+ else:
+ print("上鎖失敗")
+
+ elif choice == '5':
+ altitude = float(input("請輸入起飛高度(公尺): "))
+ if drone.takeoff(altitude):
+ print(f"已到達目標高度: {altitude}公尺")
+ else:
+ print("起飛失敗")
+
+ elif choice == '6':
+ x = float(input("請輸入X座標(公尺): "))
+ y = float(input("請輸入Y座標(公尺): "))
+ z = float(input("請輸入Z座標(公尺,負值代表向下): "))
+ if drone.goto_position_local(x, y, z):
+ print("已到達目標位置")
+ else:
+ print("移動失敗")
+
+ elif choice == '7':
+ if drone.set_mode("RTL"):
+ print("已切換至 RTL 模式，無人機正在返航")
+ else:
+ print("切換至 RTL 模式失敗")
+
+ elif choice == '8':
+ state = drone.get_current_state()
+ if state:
+ print("\n=== 無人機狀態 ===")
+ print(f"模式: {state['mode']}")
+ print(f"解鎖狀態: {'已解鎖' if state['armed'] else '已上鎖'}")
+ print("==================")
+ else:
+ print("無法獲取狀態")
+
+ elif choice == '9':
+ print("程式結束")
+ break
+
+ else:
+ print("無效的指令，請重新輸入")
+
+ except ValueError as e:
+ print(f"輸入錯誤: {e}")
+ except Exception as e:
+ print(f"發生錯誤: {e}")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/unitdev03/0328/close_loop.py b/src/unitdev03/0328/close_loop.py
new file mode 100644
index 0000000..639e0cf
--- /dev/null
+++ b/src/unitdev03/0328/close_loop.py
@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import socket
+import re
+import time
+import math
+
+def main():
+ # 建立MAVLink連接
+ print("建立MAVLink連接...")
+ connection_string = "udp:127.0.0.1:14550"
+ master = mavutil.mavlink_connection(connection_string)
+ master.wait_heartbeat()
+ print("MAVLink連接已建立")
+
+ # 切換到GUIDED模式
+ master.mav.set_mode_send(
+ master.target_system,
+ mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+ 4 # GUIDED模式ID
+ )
+ print("已切換到GUIDED模式")
+
+ # 解鎖無人機
+ master.mav.command_long_send(
+ master.target_system,
+ master.target_component,
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+ 0,
+ 1, 0, 0, 0, 0, 0, 0 # 1表示解鎖
+ )
+ print("無人機已解鎖")
+
+ # 設置UDP服務器
+ server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_socket.bind(('0.0.0.0', 5000))
+ server_socket.settimeout(0.1) # 非阻塞操作
+ print("UDP服務器已啟動，監聽端口5000")
+
+ # 主循環，持續處理數據
+ while True:
+ try:
+ # 接收UDP數據
+ try:
+ data, _ = server_socket.recvfrom(1024)
+ decoded_data = data.decode('utf-8')
+
+ # 提取velocity數據
+ vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data)
+
+ if vel_match:
+ velocity_str = vel_match.group(1)
+ velocity = [float(x) for x in velocity_str.split(',')]
+ vx, vy, vz = velocity
+
+ # 獲取當前位置
+ pos_msg = master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=0.5)
+ if pos_msg:
+ x, y, z = pos_msg.x, pos_msg.y, pos_msg.z
+
+ # 設置type_mask - 僅使用速度控制
+ type_mask = (
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
+ )
+
+ # 發送位置目標命令
+ print(f"發送速度指令: vx={vx}, vy={vy}, vz={vz}")
+ master.mav.set_position_target_local_ned_send(
+ 0,
+ master.target_system,
+ master.target_component,
+ mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+ type_mask,
+ 0, 0, 0,
+ vx, vy, vz, # 速度
+ 0, 0, 0, # 加速度
+ 0, 0 # 航向
+ )
+ print("success")
+ # 要再去偵錯沒有data進來的狀況
+ except socket.timeout:
+ pass
+
+ time.sleep(0.05) # 控制循環速率
+
+ except KeyboardInterrupt:
+ print("程序終止")
+ server_socket.close()
+ break
+ except Exception as e:
+ print(f"錯誤: {e}")
+ time.sleep(1)
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/unitdev03/0328/pymavlink_02.py b/src/unitdev03/0328/pymavlink_02.py
new file mode 100644
index 0000000..5dba35b
--- /dev/null
+++ b/src/unitdev03/0328/pymavlink_02.py
@@ -0,0 +1,497 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import time
+import math
+import threading
+import sys
+
+class DroneController:
+ def __init__(self, connection_string="udp:127.0.0.1:14550", drone_id=0):
+ self.master = mavutil.mavlink_connection(connection_string)
+ self.drone_id = drone_id
+ self.connection_string = connection_string
+ self.master.wait_heartbeat()
+ print(f"無人機 #{self.drone_id} 已建立連接!")
+
+ def set_mode(self, mode):
+ if mode == 'STABILIZE':
+ mode_id = 0
+ elif mode == 'AUTO':
+ mode_id = 3
+ elif mode == 'GUIDED':
+ mode_id = 4
+ elif mode == 'LOITER':
+ mode_id = 5
+ elif mode == 'RTL':
+ mode_id = 6
+ elif mode == 'LAND':
+ mode_id = 9
+ else:
+ print(f"不支援的模式: {mode}")
+ return False
+
+ self.master.mav.set_mode_send(
+ self.master.target_system,
+ mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+ mode_id
+ )
+
+ for _ in range(5):
+ msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+ if msg and msg.custom_mode == mode_id:
+ return True
+ return False
+
+ def arm(self, arm_state=True):
+ self.master.mav.command_long_send(
+ self.master.target_system,
+ self.master.target_component,
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+ 0,
+ 1 if arm_state else 0, 0, 0, 0, 0, 0, 0
+ )
+
+ for _ in range(5):
+ msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+ if msg:
+ armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
+ if (arm_state and armed) or (not arm_state and not armed):
+ return True
+ return False
+
+ def takeoff(self, altitude):
+ self.master.mav.command_long_send(
+ self.master.target_system,
+ self.master.target_component,
+ mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+ 0,
+ 0, 0, 0, 0, 0, 0,
+ altitude
+ )
+
+ timeout = time.time() + 30
+ while time.time() < timeout:
+ msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+ if msg:
+ current_alt = msg.relative_alt / 1000.0
+ if abs(current_alt - altitude) < 0.5:
+ return True
+ time.sleep(0.1)
+ return False
+
+ def goto_position_local(self, x, y, z, vx=0, vy=0, vz=0, yaw=0, yaw_rate=0):
+ z = -z
+
+ type_mask = (
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE
+ )
+
+ print(f"無人機 #{self.drone_id} 發送位置指令: x={x}, y={y}, z={-z}, vx={vx}, vy={vy}, vz={vz}")
+
+ self.master.mav.set_position_target_local_ned_send(
+ 0,
+ self.master.target_system,
+ self.master.target_component,
+ mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+ type_mask,
+ x, y, z,
+ vx, vy, vz,
+ 0, 0, 0,
+ yaw, yaw_rate
+ )
+
+ print(f"無人機 #{self.drone_id} 開始監控位置變化...")
+ start_time = time.time()
+ last_progress_time = start_time
+ min_dist = float('inf')
+
+ while time.time() - start_time < 30:
+ msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+ if msg:
+ dist = math.sqrt((msg.x - x)**2 + (msg.y - y)**2 + (msg.z - z)**2)
+ min_dist = min(min_dist, dist)
+
+ current_time = time.time()
+ if current_time - last_progress_time >= 5:
+ if dist > min_dist + 0.1:
+ print(f"無人機 #{self.drone_id} 偏離目標，重新調整")
+ self.master.mav.set_position_target_local_ned_send(
+ 0, self.master.target_system, self.master.target_component,
+ mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+ type_mask,
+ x, y, z, vx, vy, vz, 0, 0, 0, yaw, yaw_rate
+ )
+ last_progress_time = current_time
+
+ print(f"無人機 #{self.drone_id} 當前位置: x={msg.x:.2f}, y={msg.y:.2f}, z={-msg.z:.2f}, 距離目標: {dist:.2f}m")
+
+ if dist < 0.5:
+ print(f"無人機 #{self.drone_id} 到達目標位置")
+ return True
+
+ vel_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=False)
+ if vel_msg:
+ vel_total = math.sqrt(vel_msg.vx**2 + vel_msg.vy**2 + vel_msg.vz**2)
+ if dist < 1.0 and vel_total < 0.1:
+ print(f"無人機 #{self.drone_id} 已穩定懸停在目標位置附近")
+ return True
+
+ time.sleep(0.1)
+
+ print(f"無人機 #{self.drone_id} 移動超時")
+ return False
+
+ def get_current_state(self):
+ state = {}
+
+ # Mode and armed status
+ hb_msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=0.5)
+ if hb_msg:
+ mode_id = hb_msg.custom_mode
+ mode_name = {
+ 0: "STABILIZE",
+ 3: "AUTO",
+ 4: "GUIDED",
+ 5: "LOITER",
+ 6: "RTL",
+ 9: "LAND"
+ }.get(mode_id, f"Unknown({mode_id})")
+
+ state['mode'] = mode_name
+ state['armed'] = bool(hb_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
+
+ # Position
+ pos_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1.0)
+ if pos_msg:
+ state['position'] = {
+ 'x': round(pos_msg.x, 2),
+ 'y': round(pos_msg.y, 2),
+ 'z': round(-pos_msg.z, 2)
+ }
+
+ # GPS
+ gps_msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1.0)
+ if gps_msg:
+ state['gps_position'] = {
+ 'lat': gps_msg.lat / 1e7,
+ 'lon': gps_msg.lon / 1e7,
+ 'alt': gps_msg.alt / 1000,
+ 'relative_alt': gps_msg.relative_alt / 1000
+ }
+
+ return state
+
+class MultiDroneController:
+ def __init__(self, num_drones=5):
+ self.drones = []
+ self.num_drones = num_drones
+
+ for i in range(num_drones):
+ port = 14550 + i
+ connection_string = f"udp:127.0.0.1:{port}"
+ drone = DroneController(connection_string, i)
+ self.drones.append(drone)
+ print(f"已初始化無人機 #{i} 在端口 {port}")
+
+ def get_drone(self, drone_id):
+ if 0 <= drone_id < len(self.drones):
+ return self.drones[drone_id]
+ return None
+
+ def all_drones_command(self, command, *args):
+ results = []
+ threads = []
+
+ for drone in self.drones:
+ thread = threading.Thread(
+ target=lambda d, cmd, args, results: results.append((d.drone_id, getattr(d, cmd)(*args))),
+ args=(drone, command, args, results)
+ )
+ threads.append(thread)
+ thread.start()
+
+ for thread in threads:
+ thread.join()
+
+ for drone_id, success in results:
+ print(f"無人機 #{drone_id} 執行 {command} 結果: {'成功' if success else '失敗'}")
+
+ return all(success for _, success in results)
+
+def print_menu():
+ menu = [
+ "\n=== 多機無人機控制界面 ===",
+ "1. 選擇操作的無人機",
+ "2. 對所有無人機執行命令",
+ "3. 查看所有無人機狀態",
+ "4. 退出",
+ "0. 顯示此清單",
+ "=========================="
+ ]
+ for item in menu:
+ print(item)
+
+def print_drone_menu():
+ menu = [
+ "\n=== 單機無人機控制界面 ===",
+ "1. 切換至 GUIDED 模式",
+ "2. 切換至 LAND 模式",
+ "3. 解鎖",
+ "4. 上鎖",
+ "5. 起飛",
+ "6. 飛到指定位置",
+ "7. 切換至 RTL 模式(返航)",
+ "8. 查看當前狀態",
+ "9. 返回主菜單",
+ "0. 顯示此清單",
+ "=========================="
+ ]
+ for item in menu:
+ print(item)
+
+def print_all_drones_menu():
+ menu = [
+ "\n=== 所有無人機控制界面 ===",
+ "1. 全部切換至 GUIDED 模式",
+ "2. 全部切換至 LAND 模式",
+ "3. 全部解鎖",
+ "4. 全部上鎖",
+ "5. 全部起飛",
+ "6. 全部返航",
+ "7. 返回主菜單",
+ "0. 顯示此清單",
+ "=========================="
+ ]
+ for item in menu:
+ print(item)
+
+def control_single_drone(drone):
+ print_drone_menu()
+ current_drone_id = drone.drone_id
+
+ while True:
+ try:
+ choice = input(f"\n無人機 #{current_drone_id} - 請輸入指令編號: ")
+
+ if choice == '0':
+ print_drone_menu()
+
+ elif choice == '1':
+ if drone.set_mode("GUIDED"):
+ print(f"無人機 #{current_drone_id} 已切換至 GUIDED 模式")
+ else:
+ print(f"無人機 #{current_drone_id} 切換模式失敗")
+
+ elif choice == '2':
+ if drone.set_mode("LAND"):
+ print(f"無人機 #{current_drone_id} 已切換至 LAND 模式")
+ else:
+ print(f"無人機 #{current_drone_id} 切換模式失敗")
+
+ elif choice == '3':
+ if drone.arm(True):
+ print(f"無人機 #{current_drone_id} 已解鎖")
+ else:
+ print(f"無人機 #{current_drone_id} 解鎖失敗")
+
+ elif choice == '4':
+ if drone.arm(False):
+ print(f"無人機 #{current_drone_id} 已上鎖")
+ else:
+ print(f"無人機 #{current_drone_id} 上鎖失敗")
+
+ elif choice == '5':
+ altitude = float(input("請輸入起飛高度(公尺): "))
+ if drone.takeoff(altitude):
+ print(f"無人機 #{current_drone_id} 已到達目標高度: {altitude}公尺")
+ else:
+ print(f"無人機 #{current_drone_id} 起飛失敗")
+
+ elif choice == '6':
+ x = float(input("請輸入X座標(公尺): "))
+ y = float(input("請輸入Y座標(公尺): "))
+ z = float(input("請輸入Z座標(公尺,高度為正值): "))
+ vx = float(input("請輸入X方向速度(公尺/秒，可選): ") or "0")
+ vy = float(input("請輸入Y方向速度(公尺/秒，可選): ") or "0")
+ if drone.goto_position_local(x, y, z, vx, vy):
+ print(f"無人機 #{current_drone_id} 已到達目標位置")
+ else:
+ print(f"無人機 #{current_drone_id} 移動失敗")
+
+ elif choice == '7':
+ if drone.set_mode("RTL"):
+ print(f"無人機 #{current_drone_id} 已切換至 RTL 模式，正在返航")
+ else:
+ print(f"無人機 #{current_drone_id} 切換至 RTL 模式失敗")
+
+ elif choice == '8':
+ state = drone.get_current_state()
+ if state:
+ print(f"\n=== 無人機 #{current_drone_id} 狀態 ===")
+ print(f"模式: {state.get('mode', '未知')}")
+ print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}")
+
+ if 'gps_position' in state:
+ gps = state['gps_position']
+ print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m")
+
+ if 'position' in state:
+ pos = state['position']
+ print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m")
+ print("=============================")
+ else:
+ print(f"無法獲取無人機 #{current_drone_id} 狀態")
+
+ elif choice == '9':
+ print("返回主菜單")
+ break
+
+ else:
+ print("無效的指令，請重新輸入")
+
+ except ValueError as e:
+ print(f"輸入錯誤: {e}")
+ except Exception as e:
+ print(f"發生錯誤: {e}")
+
+def control_all_drones(controller):
+ print_all_drones_menu()
+
+ while True:
+ try:
+ choice = input("\n請輸入指令編號: ")
+
+ if choice == '0':
+ print_all_drones_menu()
+
+ elif choice == '1':
+ if controller.all_drones_command("set_mode", "GUIDED"):
+ print("所有無人機已切換至 GUIDED 模式")
+ else:
+ print("部分無人機切換模式失敗")
+
+ elif choice == '2':
+ if controller.all_drones_command("set_mode", "LAND"):
+ print("所有無人機已切換至 LAND 模式")
+ else:
+ print("部分無人機切換模式失敗")
+
+ elif choice == '3':
+ if controller.all_drones_command("arm", True):
+ print("所有無人機已解鎖")
+ else:
+ print("部分無人機解鎖失敗")
+
+ elif choice == '4':
+ if controller.all_drones_command("arm", False):
+ print("所有無人機已上鎖")
+ else:
+ print("部分無人機上鎖失敗")
+
+ elif choice == '5':
+ altitude = float(input("請輸入所有無人機起飛高度(公尺): "))
+ if controller.all_drones_command("takeoff", altitude):
+ print(f"所有無人機已到達目標高度: {altitude}公尺")
+ else:
+ print("部分無人機起飛失敗")
+
+ elif choice == '6':
+ if controller.all_drones_command("set_mode", "RTL"):
+ print("所有無人機已切換至 RTL 模式，正在返航")
+ else:
+ print("部分無人機切換至 RTL 模式失敗")
+
+ elif choice == '7':
+ print("返回主菜單")
+ break
+
+ else:
+ print("無效的指令，請重新輸入")
+
+ except ValueError as e:
+ print(f"輸入錯誤: {e}")
+ except Exception as e:
+ print(f"發生錯誤: {e}")
+
+def display_all_drone_states(controller):
+ """顯示所有無人機狀態"""
+ print("\n=== 所有無人機狀態 ===")
+
+ for drone in controller.drones:
+ max_attempts = 5
+ state = None
+
+ for attempt in range(max_attempts):
+ state = drone.get_current_state()
+ if state and 'position' in state:
+ break
+ time.sleep(0.2)
+
+ print(f"\n--- 無人機 #{drone.drone_id} ---")
+ print(f"連接: {drone.connection_string}")
+
+ if not state:
+ print("狀態: 無法獲取完整數據")
+ continue
+
+ print(f"飛行模式: {state.get('mode', '未知')}")
+ print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}")
+
+ if 'gps_position' in state:
+ gps = state['gps_position']
+ print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m")
+ print(f"相對高度: {gps['relative_alt']:.2f}m")
+
+ if 'position' in state:
+ pos = state['position']
+ print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m")
+
+ print("-----------------------")
+
+ print("=========================")
+
+def main():
+ controller = MultiDroneController(5)
+ print_menu()
+
+ while True:
+ try:
+ choice = input("\n請輸入指令編號: ")
+
+ if choice == '0':
+ print_menu()
+
+ elif choice == '1':
+ drone_id = int(input("請輸入要操作的無人機ID (0-4): "))
+ drone = controller.get_drone(drone_id)
+ if drone:
+ control_single_drone(drone)
+ else:
+ print(f"無效的無人機ID: {drone_id}")
+
+ elif choice == '2':
+ control_all_drones(controller)
+
+ elif choice == '3':
+ display_all_drone_states(controller)
+
+ elif choice == '4':
+ print("程式結束")
+ break
+
+ else:
+ print("無效的指令，請重新輸入")
+
+ except ValueError as e:
+ print(f"輸入錯誤: {e}")
+ except Exception as e:
+ print(f"發生錯誤: {e}")
+
+if __name__ == "__main__":
+ try:
+ main()
+ except KeyboardInterrupt:
+ print("\n程式被使用者中斷")
+ sys.exit(0)
\ No newline at end of file
diff --git a/src/unitdev03/0328/test_transform.py b/src/unitdev03/0328/test_transform.py
new file mode 100644
index 0000000..af72a2d
--- /dev/null
+++ b/src/unitdev03/0328/test_transform.py
@@ -0,0 +1,331 @@
+import rclpy
+from pymavlink import mavutil
+from time import sleep
+import time
+
+import socket
+import struct
+
+# used at fdm_switch_example_two
+import threading
+import queue
+
+import json
+
+def mavlink_analyzer_simple(count = 500):
+ # rclpy.init()
+ # node = rclpy.create_node('mavlink_analyzer_simple')
+
+ print('Inintializing connection')
+
+ connection_string="udp:127.0.0.1:14550"
+ connection = mavutil.mavlink_connection(connection_string)
+
+ print('Catch messages')
+ msg_count = {}
+
+ start_time = time.time()
+
+ for _ in range(count):
+ msg = connection.recv_msg()
+ # msg = connection.recv_match(blocking=True)
+ if msg:
+ msg_type = msg.get_type()
+ if msg_type in msg_count:
+ msg_count[msg_type] += 1
+ else:
+ msg_count[msg_type] = 1
+
+ if msg.get_type() == 'SERVO_OUTPUT_RAW':
+ print(msg)
+ pass
+
+ else:
+ print('No message yet, 1 second for data to fill')
+ sleep(1)
+
+ print('Messages Type received:')
+ print(msg_count)
+
+ end_time = time.time()
+ print('Time elapsed: ', end_time - start_time)
+
+ print('Closing connection')
+ connection.close()
+
+
+def fdm_parser_example(data=None):
+ if data is None:
+ data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
+ # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
+
+
+ parse_format = 'HHI16H'
+
+ if len(data) != struct.calcsize(parse_format):
+ print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
+ exit(1)
+
+ decoded = struct.unpack(parse_format,data)
+
+ magic = decoded[0]
+ frame_rate_hz = decoded[1]
+ frame_count = decoded[2]
+ pwm = decoded[3:]
+
+
+ print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
+
+def fdm_switch_example_one():
+
+ '''
+ 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
+ 沒有轉換數據格式
+ '''
+ # make a link get fdm from SITL, as a server
+ sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ sock_s2g.bind(('', 9002))
+ sock_s2g.settimeout(0.1)
+
+ # set a target link pass JSON to Gazebo, as a client
+ sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_address = ('127.0.0.1', 19002)
+
+ packet_count = 0
+ start_time = time.time()
+
+ while True:
+ try:
+ # 接到SITL的數據 並轉發給Gazebo
+ data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+ sock_g2s.sendto(data_s2g, server_address)
+ # 得到Gazebo的回應 並轉發給SITL
+ data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+ sock_s2g.sendto(data_g2s, addr_s2g)
+
+ # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+ try:
+ # 解析從 SITL 到 Gazebo 的數據
+ decoded = struct.unpack('HHI16H', data_s2g)
+ s2g_json = {
+ "magic": decoded[0],
+ "frame_rate_hz": decoded[1],
+ "frame_count": decoded[2],
+ "pwm": list(decoded[3:])
+ }
+
+ # 解析從 Gazebo 到 SITL 的數據
+ # 注意：假設從 Gazebo 返回的數據是字串形式的 JSON
+ try:
+ g2s_data_str = data_g2s.decode('utf-8')
+ # 保存原始數據，適合後續分析
+ with open('gazebo_response_raw.txt', 'a') as f:
+ f.write(g2s_data_str + '\n')
+ except:
+ # 如果不是 UTF-8 格式，可能是二進制數據
+ g2s_data_str = str(data_g2s)
+
+ # 合併數據到一個 JSON 對象
+ combined_data = {
+ "sitl_to_gazebo": s2g_json,
+ "gazebo_to_sitl": g2s_data_str,
+ "timestamp": time.time()
+ }
+
+ # 寫入 JSON 檔案
+ with open('fdm_one_data.json', 'w') as f:
+ json.dump(combined_data, f, indent=2)
+
+ except Exception as e:
+ print(f"JSON 處理錯誤: {e}")
+
+ packet_count += 1
+ except socket.timeout:
+ print(f'no value')
+ #print(f'address:{addr_s2g}')
+ #pass
+
+ current_time = time.time()
+ elapsed_time = current_time - start_time
+
+ if elapsed_time >= 1.0:
+ #print(f"Packets received in the last second: {packet_count}")
+ print(f'JSON Pack:{data_g2s}')
+ packet_count = 0
+ start_time = current_time
+ fdm_parser_example(data_s2g)
+
+
+def fdm_switch_example_two():
+ '''
+ 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
+ 有轉換數據格式
+
+ time_usec
+ servo1_raw
+ servo16_raw
+ '''
+ # read info from SITL via MAVLink
+ connection_string="udp:127.0.0.1:14550" # uart mavlink # MATLAB
+ connection = mavutil.mavlink_connection(connection_string)
+
+ # set a target link pass JSON to Gazebo, as a client
+ sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_address = ('127.0.0.1', 19002)
+
+ data_queue = queue.Queue()
+ servo_out = [0] * 16
+ data_queue.put(servo_out)
+
+ def send_data_from_queue(sock, server_address, q, frame_rate_fdm=600, frame_count_fdm=1):
+ interval = 1.0 / frame_rate_fdm
+ while True:
+ start_time = time.time()
+ try:
+ data = q.get(timeout=0.1)
+ if data is None:
+ break
+ last_data = data
+ except queue.Empty:
+ data = last_data
+ data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
+ sock.sendto(data, server_address)
+
+ # 新增：寫入 JSON 檔案
+ data_json = {
+ "magic": 0x481a,
+ "frame_rate_hz": frame_rate_fdm,
+ "frame_count": frame_count_fdm,
+ "pwm": list(servo_out)
+ }
+ with open('fdm_data.json', 'w') as f:
+ json.dump(data_json, f)
+
+ frame_count_fdm += 1
+ fdm_parser_example(data) # debug
+
+ elapsed_time = time.time() - start_time
+ sleep_time = interval - elapsed_time
+ if sleep_time > 0:
+ sleep(sleep_time)
+
+ Running = True
+ thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+ thread.start()
+
+ while Running:
+ msgb1 = None
+ msg = connection.recv_msg()
+ while msg :
+ if msg.get_type() == 'SERVO_OUTPUT_RAW':
+ msgb1 = msg
+ # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+ msg = connection.recv_msg()
+
+ if msgb1:
+ for i in range(16):
+ servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+ data_queue.put(servo_out)
+ msgb1 = None
+ # Running = False # debug
+ else:
+ # print('No message yet, 10 ms for data to fill')
+ sleep(0.01)
+
+ # Example of putting data into the queue
+ # data_queue.put(data)
+ # To stop the thread, you can put None into the queue
+ # data_queue.put(None)
+
+
+def fdm_switch_example_one_with_remote_forwarding():
+ '''
+ 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
+ 並且將JSON數據轉發到指定的遠端IP
+ '''
+ # make a link get fdm from SITL, as a server
+ sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ sock_s2g.bind(('', 9002))
+ sock_s2g.settimeout(0.1)
+
+ # set a target link pass JSON to Gazebo, as a client
+ sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_address = ('127.0.0.1', 19002)
+
+ # 建立與遠端IP的socket連接
+ remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ remote_address = ('140.120.31.130', 9003) # 遠端IP與埠號，您可以根據需要更改埠號
+ print(f'Setting up forwarding to remote address: {remote_address}')
+
+ packet_count = 0
+ start_time = time.time()
+
+ while True:
+ try:
+ # 接到SITL的數據 並轉發給Gazebo
+ data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+ sock_g2s.sendto(data_s2g, server_address)
+
+ # 得到Gazebo的回應 並轉發給SITL
+ data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+ sock_s2g.sendto(data_g2s, addr_s2g)
+
+ # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+ try:
+ # 解析從 SITL 到 Gazebo 的數據
+ decoded = struct.unpack('HHI16H', data_s2g)
+ s2g_json = {
+ "magic": decoded[0],
+ "frame_rate_hz": decoded[1],
+ "frame_count": decoded[2],
+ "pwm": list(decoded[3:])
+ }
+
+ # 解析從 Gazebo 到 SITL 的數據
+ try:
+ g2s_data_str = data_g2s.decode('utf-8')
+ # 保存原始數據，適合後續分析
+ with open('gazebo_response_raw.txt', 'a') as f:
+ f.write(g2s_data_str + '\n')
+ except:
+ # 如果不是 UTF-8 格式，可能是二進制數據
+ g2s_data_str = str(data_g2s)
+
+ # 合併數據到一個 JSON 對象
+ combined_data = {
+ "sitl_to_gazebo": s2g_json,
+ "gazebo_to_sitl": g2s_data_str,
+ "timestamp": time.time()
+ }
+
+ # 寫入 JSON 檔案
+ with open('fdm_one_data.json', 'w') as f:
+ json.dump(combined_data, f, indent=2)
+
+ # 將JSON數據轉發到遠端IP
+ remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+
+ except Exception as e:
+ print(f"JSON 處理錯誤: {e}")
+
+ packet_count += 1
+ except socket.timeout:
+ print(f'no value')
+ #pass
+
+ current_time = time.time()
+ elapsed_time = current_time - start_time
+
+ if elapsed_time >= 1.0:
+ print(f'JSON Pack:{data_g2s}')
+ print(f'Forwarded data to remote IP: 140.120.31.130')
+ packet_count = 0
+ start_time = current_time
+ fdm_parser_example(data_s2g)
+
+print('Start')
+#fdm_switch_example_two()
+#mavlink_analyzer_simple()
+#fdm_switch_example_one()
+fdm_switch_example_one_with_remote_forwarding()
+#fdm_parser_example()
\ No newline at end of file
diff --git a/src/unitdev03/0328/testserver.py b/src/unitdev03/0328/testserver.py
new file mode 100644
index 0000000..cce1478
--- /dev/null
+++ b/src/unitdev03/0328/testserver.py
@@ -0,0 +1,55 @@
+import socket
+import re
+
+def start_udp_server(host='0.0.0.0', port=5000, buffer_size=1024):
+ server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_socket.bind((host, port))
+
+ print(f"UDP Server running on {host}:{port}")
+ print("Waiting for data...")
+
+ position = None
+ velocity = None
+
+ try:
+ while True:
+ data, client_address = server_socket.recvfrom(buffer_size)
+
+ print(f"\n----- 接收到的資料 -----")
+ print(f"從 {client_address} 接收:")
+
+ try:
+ decoded_data = data.decode('utf-8')
+ print(f"文本數據: {decoded_data}")
+
+ # 直接使用正則表達式提取數據
+ pos_match = re.search(r'"position":\[([-\d\.\,]+)\]', decoded_data)
+ vel_match = re.search(r'"velocity":\[([-\d\.\,]+)\]', decoded_data)
+
+ if pos_match:
+ position_str = pos_match.group(1)
+ position = [float(x) for x in position_str.split(',')]
+ print(f"Position: {position}")
+
+ if vel_match:
+ velocity_str = vel_match.group(1)
+ velocity = [float(x) for x in velocity_str.split(',')]
+ print(f"Velocity: {velocity}")
+
+ # 這裡可以對 position 和 velocity 進行進一步操作
+
+ except UnicodeDecodeError:
+ print(f"二進制數據: {data.hex()}")
+ except Exception as e:
+ print(f"處理數據時出錯: {e}")
+
+ print(f"數據大小: {len(data)} 字節")
+ print("-----------------------")
+
+ except KeyboardInterrupt:
+ print("Server shutting down...")
+ finally:
+ server_socket.close()
+
+if __name__ == "__main__":
+ start_udp_server(port=5000)
\ No newline at end of file
diff --git a/src/unitdev03/0519/close_loop_multi_1.py b/src/unitdev03/0519/close_loop_multi_1.py
new file mode 100644
index 0000000..5542372
--- /dev/null
+++ b/src/unitdev03/0519/close_loop_multi_1.py
@@ -0,0 +1,297 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import socket
+import re
+import time
+import math
+import threading
+import json
+import os
+import datetime
+import queue
+import argparse
+from select import select
+
+class Drone:
+ def __init__(self, instance_id, mavlink_port, udp_port):
+ self.instance_id = instance_id
+ self.mavlink_port = mavlink_port
+ self.udp_port = udp_port
+ self.running = True
+ self.connection = None
+
+ # 創建數據緩存
+ self.latest_position = None
+
+ # 創建日誌佇列
+ self.log_queue = queue.Queue()
+
+ # 創建日誌文件目錄
+ self.log_dir = "drone_logs"
+ if not os.path.exists(self.log_dir):
+ os.makedirs(self.log_dir)
+ # 創建一個含時間戳的日誌文件名
+ timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
+ self.log_file = os.path.join(self.log_dir, f"drone_{self.instance_id}_{timestamp}.log")
+
+ # 通訊統計
+ self.packet_counter = 0
+ self.start_time = time.time()
+ self.packets_per_second = 0
+
+ def connect(self):
+ """僅建立MAVLink連接"""
+ print(f"Drone {self.instance_id}: 建立MAVLink連接 (Port {self.mavlink_port})...")
+ connection_string = f"udp:127.0.0.1:{self.mavlink_port}"
+ self.connection = mavutil.mavlink_connection(connection_string)
+ self.connection.wait_heartbeat()
+ print(f"Drone {self.instance_id}: MAVLink連接已建立")
+
+ def set_guided_mode(self):
+ """切換到GUIDED模式"""
+ self.connection.mav.set_mode_send(
+ self.connection.target_system,
+ mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+ 4 # GUIDED模式ID
+ )
+ print(f"Drone {self.instance_id}: 已切換到GUIDED模式")
+
+ def arm(self):
+ """解鎖無人機"""
+ self.connection.mav.command_long_send(
+ self.connection.target_system,
+ self.connection.target_component,
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+ 0, 1, 0, 0, 0, 0, 0, 0 # 1表示解鎖
+ )
+ print(f"Drone {self.instance_id}: 無人機已解鎖")
+
+ def takeoff(self, altitude=3.0):
+ """起飛到指定高度"""
+ self.connection.mav.command_long_send(
+ self.connection.target_system,
+ self.connection.target_component,
+ mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+ 0, 0, 0, 0, 0, 0, 0, altitude
+ )
+ print(f"Drone {self.instance_id}: 起飛命令已發送，目標高度{altitude}公尺")
+
+ def mavlink_listener(self):
+ """事件驅動方式監聽MAVLink訊息"""
+ # 獲取底層socket
+ mavlink_socket = self.connection.port.fileno()
+
+ while self.running:
+ # 使用select等待數據，無須sleep
+ readable, _, _ = select([mavlink_socket], [], [], 0.001)
+ if readable:
+ msg = self.connection.recv_msg()
+ if msg and msg.get_type() == 'LOCAL_POSITION_NED':
+ self.latest_position = msg
+
+ def log_worker(self):
+ """事件驅動方式處理日誌寫入"""
+ while self.running:
+ if not self.log_queue.empty():
+ try:
+ # 批量處理日誌
+ batch = []
+ while not self.log_queue.empty() and len(batch) < 100:
+ try:
+ entry = self.log_queue.get_nowait()
+ batch.append(entry)
+ except queue.Empty:
+ break
+
+ # 寫入日誌
+ if batch:
+ with open(self.log_file, 'a', encoding='utf-8') as f:
+ for entry in batch:
+ f.write(json.dumps(entry, ensure_ascii=False) + "\n")
+ except Exception as e:
+ print(f"Drone {self.instance_id}: 日誌寫入錯誤: {e}")
+
+ # 每秒計算一次統計數據
+ now = time.time()
+ if now - self.start_time >= 1.0:
+ self.packets_per_second = self.packet_counter
+ print(f"Drone {self.instance_id} - 每秒封包數: {self.packets_per_second}")
+ self.packet_counter = 0
+ self.start_time = now
+
+ # 使用select等待短暫時間而非sleep
+ select([], [], [], 0.01) # 10ms的微小等待
+
+ def log_packet(self, packet_data, packet_num):
+ """將數據加入日誌佇列"""
+ try:
+ timestamp = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
+ log_entry = {
+ "timestamp": timestamp,
+ "packet_number": packet_num,
+ "data": packet_data
+ }
+ self.log_queue.put(log_entry)
+ except Exception as e:
+ print(f"Drone {self.instance_id}: 佇列添加錯誤: {e}")
+
+ def run(self):
+ """使用事件驅動方式處理UDP通訊"""
+ # 啟動背景線程
+ mavlink_thread = threading.Thread(target=self.mavlink_listener)
+ mavlink_thread.daemon = True
+ mavlink_thread.start()
+
+ log_thread = threading.Thread(target=self.log_worker)
+ log_thread.daemon = True
+ log_thread.start()
+
+ # 設置UDP服務器
+ server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 262144) # 增大接收緩衝區
+ server_socket.bind(('0.0.0.0', self.udp_port))
+ server_socket.setblocking(0) # 非阻塞模式
+
+ print(f"Drone {self.instance_id}: UDP服務器已啟動，監聽端口{self.udp_port}")
+ print(f"Drone {self.instance_id}: 日誌將保存至 {self.log_file}")
+
+ # 主事件循環
+ while self.running:
+ # 使用select監聽UDP
+ readable, _, _ = select([server_socket], [], [], 0.001)
+
+ if readable:
+ try:
+ data, _ = server_socket.recvfrom(4096)
+ decoded_data = data.decode('utf-8')
+
+ # 增加計數器
+ self.packet_counter += 1
+ total_packets = self.packet_counter
+
+ try:
+ # 解析JSON
+ json_data = json.loads(decoded_data)
+
+ # 寫入日誌
+ self.log_packet(json_data, total_packets)
+
+ # 提取速度數據
+ vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data)
+ if vel_match:
+ velocity_str = vel_match.group(1)
+ velocity = [float(x) for x in velocity_str.split(',')]
+ vx, vy, vz = velocity
+
+ # 設置type_mask - 僅使用速度控制
+ type_mask = (
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
+ mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
+ )
+
+ # 直接發送速度指令，不需等待位置數據
+ self.connection.mav.set_position_target_local_ned_send(
+ 0,
+ self.connection.target_system,
+ self.connection.target_component,
+ mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+ type_mask,
+ 0, 0, 0, # 位置
+ vx, vy, -vz, # 速度
+ 0, 0, 0, # 加速度
+ 0, 0 # 航向
+ )
+ print(f"Drone {self.instance_id}: 發送速度指令: vx={vx}, vy={vy}, vz={-vz}")
+
+ except json.JSONDecodeError as je:
+ print(f"Drone {self.instance_id}: JSON解析錯誤: {je}")
+ self.log_packet({"raw_data": decoded_data}, total_packets)
+
+ except Exception as e:
+ print(f"Drone {self.instance_id}: 處理錯誤: {e}")
+
+ # 清理資源
+ server_socket.close()
+ print(f"Drone {self.instance_id}: 線程結束")
+
+def main():
+ # 解析命令行參數
+ parser = argparse.ArgumentParser(description='多無人機控制程序')
+ parser.add_argument('--drones', type=str, default='1,2,3',
+ help='要控制的無人機ID，用逗號分隔 (例如: 1,2,3)')
+ args = parser.parse_args()
+
+ # 所有可用的無人機配置
+ all_drone_configs = [
+ (1, 14550, 5003),
+ (2, 14560, 5013),
+ (3, 14570, 5023)
+ ]
+
+ # 解析要使用的無人機ID
+ drone_ids = [int(id_str) for id_str in args.drones.split(',')]
+ drone_configs = [config for config in all_drone_configs if config[0] in drone_ids]
+
+ drones = []
+ threads = []
+
+ try:
+ # 創建所有無人機實例
+ for instance_id, mavlink_port, udp_port in drone_configs:
+ drone = Drone(instance_id, mavlink_port, udp_port)
+ drones.append(drone)
+
+ # 步驟1: 所有無人機連接
+ for drone in drones:
+ drone.connect()
+
+ # 適當延遲確保連接穩定
+ time.sleep(1)
+
+ # 步驟2: 所有無人機同時切換到GUIDED模式
+ for drone in drones:
+ drone.set_guided_mode()
+
+ time.sleep(1)
+
+ # 步驟3: 所有無人機同時解鎖
+ for drone in drones:
+ drone.arm()
+
+ time.sleep(1)
+
+ # 步驟4: 所有無人機同時起飛
+ for drone in drones:
+ drone.takeoff()
+
+ # 啟動監聽線程
+ for drone in drones:
+ thread = threading.Thread(target=drone.run)
+ thread.daemon = True
+ threads.append(thread)
+ thread.start()
+
+ print(f"已啟動 {len(drones)} 台無人機 (ID: {args.drones})，按Ctrl+C結束程序")
+
+ # 主線程事件循環
+ while True:
+ select([], [], [], 0.1) # 使用select代替time.sleep
+
+ except KeyboardInterrupt:
+ print("正在停止所有無人機...")
+ for drone in drones:
+ drone.running = False
+
+ for thread in threads:
+ thread.join(timeout=2)
+
+ print("程序已結束")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/unitdev03/0519/enemy_status.py b/src/unitdev03/0519/enemy_status.py
new file mode 100644
index 0000000..c4b96a6
--- /dev/null
+++ b/src/unitdev03/0519/enemy_status.py
@@ -0,0 +1,340 @@
+#!/usr/bin/env python3
+
+from pymavlink import mavutil
+import time
+import sys
+import json
+import socket
+import threading
+import os
+from datetime import datetime
+
+# 固定初始位置設定 (5,5,0)
+INITIAL_POSITION = [5, 5, 0]
+
+# MAVLink 連接設定
+connection_string = 'udp:127.0.0.1:14553'
+
+# 網路傳輸設定
+target_ip = '140.120.31.130'
+target_port = 9053
+
+# 建立 UDP socket 用於傳送數據
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+# 封包編號計數器
+packet_counter = 0
+
+# 共享變數 - 存儲最新位置
+current_position = None
+initial_position = INITIAL_POSITION.copy()
+position_lock = threading.Lock()
+
+# 連接到無人機
+print(f"嘗試連接到 MAVLink: {connection_string}")
+try:
+ connection = mavutil.mavlink_connection(connection_string, autoreconnect=True)
+ connection.wait_heartbeat()
+ print(f"成功連接到系統 {connection.target_system}!")
+except Exception as e:
+ print(f"MAVLink 連接失敗: {e}")
+ sys.exit(1)
+
+print(f"使用指定初始位置: 北={initial_position[0]}, 東={initial_position[1]}, 下={initial_position[2]}")
+
+# 請求位置數據流
+connection.mav.request_data_stream_send(
+ connection.target_system,
+ connection.target_component,
+ mavutil.mavlink.MAV_DATA_STREAM_POSITION,
+ 10,
+ 1
+)
+
+# 日誌記錄函數
+def start_logging():
+ """創建並開啟日誌文件"""
+ log_dir = "logs"
+ if not os.path.exists(log_dir):
+ os.makedirs(log_dir)
+
+ timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+ log_file = os.path.join(log_dir, f"drone_log_{timestamp}.csv")
+
+ file = open(log_file, "w")
+ file.write("packet_id,timestamp,x,y,z,vx,vy,vz\n") # CSV標頭
+ print(f"日誌記錄已開始: {log_file}")
+ return file
+
+# 控制函數
+def set_guided_mode():
+ """設置為 GUIDED 模式"""
+ connection.mav.command_long_send(
+ connection.target_system,
+ connection.target_component,
+ mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+ 0,
+ mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+ 4,
+ 0, 0, 0, 0, 0
+ )
+ ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+ return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def arm_throttle():
+ """解鎖油門"""
+ connection.mav.command_long_send(
+ connection.target_system,
+ connection.target_component,
+ mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+ 0, 1, 0, 0, 0, 0, 0, 0
+ )
+ ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+ return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def takeoff(altitude):
+ """起飛到指定高度"""
+ connection.mav.command_long_send(
+ connection.target_system,
+ connection.target_component,
+ mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+ 0, 0, 0, 0, 0, 0, 0, altitude
+ )
+ ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+ return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+
+def set_speed_limit(speed_type, speed):
+ """
+ 設置速度限制
+
+ Parameters:
+ -----------
+ speed_type : int
+ 速度類型 (0=空速, 1=地速, 2=爬升速度, 3=下降速度)
+ speed : float
+ 速度值 (米/秒)
+ """
+ connection.mav.command_long_send(
+ connection.target_system,
+ connection.target_component,
+ mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
+ 0, # 確認
+ speed_type, # 參數1: 速度類型
+ speed, # 參數2: 速度 (m/s)
+ -1, # 參數3: 油門 (-1 表示不變)
+ 0, 0, 0, 0 # 參數4-7: 未使用
+ )
+ ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
+ success = ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED
+ if success:
+ print(f"已設置速度限制為 {speed:.2f} m/s")
+ else:
+ print("設置速度限制失敗")
+ return success
+
+def goto_position(north, east, down):
+ """前往指定位置 (世界座標系)"""
+ # 將世界座標轉換為相對座標
+ rel_north = north - initial_position[0]
+ rel_east = east - initial_position[1]
+ rel_down = down - initial_position[2]
+
+ connection.mav.set_position_target_local_ned_send(
+ 0,
+ connection.target_system,
+ connection.target_component,
+ mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+ 0b0000111111111000,
+ rel_north, rel_east, rel_down,
+ 0, 0, 0,
+ 0, 0, 0,
+ 0, 0
+ )
+ # print(f"相對位置: 北={rel_north:.2f}, 東={rel_east:.2f}, 下={rel_down:.2f}")
+
+def get_current_position():
+ """獲取存儲的當前位置"""
+ with position_lock:
+ if current_position is None:
+ return None
+ return current_position.copy()
+
+def control_interface():
+ """命令行控制界面"""
+ print("無人機控制介面已啟動，輸入命令：")
+ print(" arm - 解鎖油門")
+ print(" takeoff [高度] - 起飛")
+ print(" goto [北] [東] [高度] - 前往位置 (世界座標系)")
+ print(" pos - 顯示當前位置 (相對座標和世界座標)")
+ print(" init - 顯示初始位置")
+ print(" exit - 退出控制")
+
+ while True:
+ try:
+ cmd = input("命令> ").strip().split()
+ if not cmd:
+ continue
+
+ if cmd[0] == "arm":
+ print("嘗試解鎖油門...")
+ if set_guided_mode() and arm_throttle():
+ print("成功解鎖油門！")
+ else:
+ print("解鎖失敗")
+
+ elif cmd[0] == "takeoff":
+ alt = float(cmd[1]) if len(cmd) > 1 else 3.0
+ print(f"嘗試起飛到 {alt} 米高...")
+ if takeoff(alt):
+ print("起飛命令已發送")
+ else:
+ print("起飛失敗")
+
+ elif cmd[0] == "goto":
+ if len(cmd) >= 4:
+ north = float(cmd[1])
+ east = float(cmd[2])
+ down = -float(cmd[3]) # 高度是負的下
+ print(f"前往世界位置: 北:{north} 東:{east} 下:{-down} 米")
+ goto_position(north, east, down)
+ else:
+ print("用法: goto [北] [東] [高度]")
+
+ elif cmd[0] == "pos":
+ pos = get_current_position()
+ if pos:
+ rel_x, rel_y, rel_z = pos
+ # 計算世界座標
+ world_x = initial_position[0] + rel_x
+ world_y = initial_position[1] + rel_y
+ world_z = initial_position[2] + rel_z
+ # NED 轉 ENU
+ enu_x = rel_y
+ enu_y = rel_x
+ enu_z = -rel_z
+
+ print("\n當前位置:")
+ #print(f" 相對座標 (NED): 北={rel_x:.2f}m, 東={rel_y:.2f}m, 下={rel_z:.2f}m")
+ print(f" 世界座標 : 北={world_x:.2f}m, 東={world_y:.2f}m, 下={-world_z:.2f}m")
+ #print(f" 相對座標 (ENU): 東={enu_x:.2f}m, 北={enu_y:.2f}m, 上={enu_z:.2f}m")
+ #print(f" 高度: {-rel_z:.2f}m")
+ else:
+ print("尚未獲取到位置信息")
+
+ elif cmd[0] == "speed":
+ if len(cmd) > 1:
+ speed = float(cmd[1])
+ set_speed_limit(1, speed) # 1 = 地速
+ else:
+ print("用法: speed [速度m/s]")
+
+ elif cmd[0] == "climbspeed":
+ if len(cmd) > 1:
+ speed = float(cmd[1])
+ set_speed_limit(2, speed) # 2 = 爬升速度
+ set_speed_limit(3, speed) # 3 = 下降速度
+ else:
+ print("用法: climbspeed [速度m/s]")
+
+ elif cmd[0] == "init":
+ print(f"初始位置 (NED): 北={initial_position[0]:.2f}m, 東={initial_position[1]:.2f}m, 下={initial_position[2]:.2f}m")
+
+ elif cmd[0] == "exit":
+ print("退出控制介面")
+ break
+
+ else:
+ print(f"未知命令: {cmd[0]}")
+ except Exception as e:
+ print(f"命令執行錯誤: {e}")
+
+# 數據傳輸線程函數
+def data_sending_thread():
+ global packet_counter, current_position
+
+ # 開啟日誌文件
+ log_file = start_logging()
+
+ try:
+ print(f"開始接收數據並傳送到 {target_ip}:{target_port}...")
+ while True:
+ msg = connection.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+ if msg is not None:
+ # 更新當前位置（線程安全）
+ with position_lock:
+ current_position = [msg.x, msg.y, msg.z]
+
+ # 增加封包編號
+ packet_counter += 1
+
+ # 計算世界座標
+ world_x = initial_position[0] + msg.x
+ world_y = initial_position[1] + msg.y
+ world_z = initial_position[2] + msg.z
+
+ # NED 轉換為 ENU 並使用世界座標
+ enu_position = {
+ "x": world_x, # 東 (從 NED 的 x)
+ "y": world_y, # 北 (從 NED 的 y)
+ "z": -world_z # 上 (從 NED 的 -z)
+ }
+
+ enu_velocity = {
+ "vx": msg.vy, # 東向速度
+ "vy": msg.vx, # 北向速度
+ "vz": -msg.vz # 上向速度
+ }
+
+ # 創建JSON對象 (使用世界座標)
+ json_data = {
+ "packet_id": packet_counter,
+ "timestamp": msg.time_boot_ms,
+ "enemy_position": enu_position,
+ "enemy_velocity": enu_velocity
+ }
+
+ # 寫入日誌文件 (CSV格式)
+ try:
+ log_file.write(f"{packet_counter},{msg.time_boot_ms},{enu_position['x']},{enu_position['y']},{enu_position['z']},{enu_velocity['vx']},{enu_velocity['vy']},{enu_velocity['vz']}\n")
+ log_file.flush() # 確保數據即時寫入文件
+ except Exception as e:
+ print(f"寫入日誌失敗: {e}")
+
+ json_str = json.dumps(json_data)
+
+ try:
+ sock.sendto(json_str.encode('utf-8'), (target_ip, target_port))
+ except Exception as e:
+ print(f"傳送數據失敗: {e}")
+
+ time.sleep(0.1)
+ except Exception as e:
+ print(f"數據傳輸線程錯誤: {e}")
+ finally:
+ # 確保關閉日誌文件
+ log_file.close()
+ print("日誌文件已關閉")
+
+# 創建並啟動數據發送線程
+sender_thread = threading.Thread(target=data_sending_thread)
+sender_thread.daemon = True
+sender_thread.start()
+
+# 啟動命令行控制界面
+try:
+ control_interface()
+except KeyboardInterrupt:
+ print("\n程序已被用戶中斷")
+finally:
+ if connection:
+ connection.mav.request_data_stream_send(
+ connection.target_system,
+ connection.target_component,
+ mavutil.mavlink.MAV_DATA_STREAM_ALL,
+ 0, 0
+ )
+ print("數據流已停止")
+ sock.close()
+ print("網絡連接已關閉")
+ print(f"總共傳送了 {packet_counter} 個封包")
+ print("日誌文件已儲存")
\ No newline at end of file
diff --git a/src/unitdev03/0519/test_transform.py b/src/unitdev03/0519/test_transform.py
new file mode 100644
index 0000000..96b22be
--- /dev/null
+++ b/src/unitdev03/0519/test_transform.py
@@ -0,0 +1,663 @@
+import rclpy
+from pymavlink import mavutil
+from time import sleep
+import time
+
+import socket
+import struct
+
+# used at fdm_switch_example_two
+import threading
+import queue
+
+import json
+
+def mavlink_analyzer_simple(count = 500):
+ # rclpy.init()
+ # node = rclpy.create_node('mavlink_analyzer_simple')
+
+ print('Inintializing connection')
+
+ connection_string="udp:127.0.0.1:14550"
+ connection = mavutil.mavlink_connection(connection_string)
+
+ print('Catch messages')
+ msg_count = {}
+
+ start_time = time.time()
+
+ for _ in range(count):
+ msg = connection.recv_msg()
+ # msg = connection.recv_match(blocking=True)
+ if msg:
+ msg_type = msg.get_type()
+ if msg_type in msg_count:
+ msg_count[msg_type] += 1
+ else:
+ msg_count[msg_type] = 1
+
+ if msg.get_type() == 'SERVO_OUTPUT_RAW':
+ print(msg)
+ pass
+
+ else:
+ print('No message yet, 1 second for data to fill')
+ sleep(1)
+
+ print('Messages Type received:')
+ print(msg_count)
+
+ end_time = time.time()
+ print('Time elapsed: ', end_time - start_time)
+
+ print('Closing connection')
+ connection.close()
+
+
+def fdm_parser_example(data=None):
+ if data is None:
+ data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
+ # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
+
+
+ parse_format = 'HHI16H'
+
+ if len(data) != struct.calcsize(parse_format):
+ print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
+ exit(1)
+
+ decoded = struct.unpack(parse_format,data)
+
+ magic = decoded[0]
+ frame_rate_hz = decoded[1]
+ frame_count = decoded[2]
+ pwm = decoded[3:]
+
+
+ print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
+
+def fdm_switch_example_one():
+
+ '''
+ 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
+ 沒有轉換數據格式
+ '''
+ # make a link get fdm from SITL, as a server
+ sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ sock_s2g.bind(('', 9012))
+ sock_s2g.settimeout(0.1)
+
+ # set a target link pass JSON to Gazebo, as a client
+ sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_address = ('127.0.0.1', 19012)
+
+ packet_count = 0
+ start_time = time.time()
+
+ while True:
+ try:
+ # 接到SITL的數據 並轉發給Gazebo
+ data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+ sock_g2s.sendto(data_s2g, server_address)
+ # 得到Gazebo的回應 並轉發給SITL
+ data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+ sock_s2g.sendto(data_g2s, addr_s2g)
+
+ # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+ try:
+ # 解析從 SITL 到 Gazebo 的數據
+ decoded = struct.unpack('HHI16H', data_s2g)
+ s2g_json = {
+ "magic": decoded[0],
+ "frame_rate_hz": decoded[1],
+ "frame_count": decoded[2],
+ "pwm": list(decoded[3:])
+ }
+
+ # 解析從 Gazebo 到 SITL 的數據
+ # 注意：假設從 Gazebo 返回的數據是字串形式的 JSON
+ try:
+ g2s_data_str = data_g2s.decode('utf-8')
+ # 保存原始數據，適合後續分析
+ with open('gazebo_response_raw.txt', 'a') as f:
+ f.write(g2s_data_str + '\n')
+ except:
+ # 如果不是 UTF-8 格式，可能是二進制數據
+ g2s_data_str = str(data_g2s)
+
+ # 合併數據到一個 JSON 對象
+ combined_data = {
+ "sitl_to_gazebo": s2g_json,
+ "gazebo_to_sitl": g2s_data_str,
+ "timestamp": time.time()
+ }
+
+ # 寫入 JSON 檔案
+ with open('fdm_one_data.json', 'w') as f:
+ json.dump(combined_data, f, indent=2)
+
+ except Exception as e:
+ print(f"JSON 處理錯誤: {e}")
+
+ packet_count += 1
+ except socket.timeout:
+ print(f'no value')
+ #print(f'address:{addr_s2g}')
+ #pass
+
+ current_time = time.time()
+ elapsed_time = current_time - start_time
+
+ if elapsed_time >= 1.0:
+ #print(f"Packets received in the last second: {packet_count}")
+ print(f'JSON Pack:{data_g2s}')
+ packet_count = 0
+ start_time = current_time
+ fdm_parser_example(data_s2g)
+
+
+def fdm_switch_example_two():
+ '''
+ 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
+ 有轉換數據格式
+
+ time_usec
+ servo1_raw
+ servo16_raw
+ '''
+ # read info from SITL via MAVLink
+ connection_string="udp:127.0.0.1:14550" # uart mavlink # MATLAB
+ connection = mavutil.mavlink_connection(connection_string)
+
+ # set a target link pass JSON to Gazebo, as a client
+ sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_address = ('127.0.0.1', 19012)
+
+ data_queue = queue.Queue()
+ servo_out = [0] * 16
+ data_queue.put(servo_out)
+
+ def send_data_from_queue(sock, server_address, q, frame_rate_fdm=600, frame_count_fdm=1):
+ interval = 1.0 / frame_rate_fdm
+ while True:
+ start_time = time.time()
+ try:
+ data = q.get(timeout=0.1)
+ if data is None:
+ break
+ last_data = data
+ except queue.Empty:
+ data = last_data
+ data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
+ sock.sendto(data, server_address)
+
+ # 新增：寫入 JSON 檔案
+ data_json = {
+ "magic": 0x481a,
+ "frame_rate_hz": frame_rate_fdm,
+ "frame_count": frame_count_fdm,
+ "pwm": list(servo_out)
+ }
+ with open('fdm_data.json', 'w') as f:
+ json.dump(data_json, f)
+
+ frame_count_fdm += 1
+ fdm_parser_example(data) # debug
+
+ elapsed_time = time.time() - start_time
+ sleep_time = interval - elapsed_time
+ if sleep_time > 0:
+ sleep(sleep_time)
+
+ Running = True
+ thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+ thread.start()
+
+ while Running:
+ msgb1 = None
+ msg = connection.recv_msg()
+ while msg :
+ if msg.get_type() == 'SERVO_OUTPUT_RAW':
+ msgb1 = msg
+ # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+ msg = connection.recv_msg()
+
+ if msgb1:
+ for i in range(16):
+ servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+ data_queue.put(servo_out)
+ msgb1 = None
+ # Running = False # debug
+ else:
+ # print('No message yet, 10 ms for data to fill')
+ sleep(0.01)
+
+ # Example of putting data into the queue
+ # data_queue.put(data)
+ # To stop the thread, you can put None into the queue
+ # data_queue.put(None)
+
+
+def fdm_switch_example_one_with_remote_forwarding():
+ '''
+ 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
+ 並且將JSON數據轉發到指定的遠端IP
+ '''
+ # make a link get fdm from SITL, as a server
+ sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ sock_s2g.bind(('', 9012))
+ sock_s2g.settimeout(0.1)
+
+ # set a target link pass JSON to Gazebo, as a client
+ sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ server_address = ('127.0.0.1', 19012)
+
+ # 建立與遠端IP的socket連接
+ remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ remote_address = ('140.120.31.130', 9003) # 遠端IP與埠號，您可以根據需要更改埠號
+ print(f'Setting up forwarding to remote address: {remote_address}')
+
+ packet_count = 0
+ start_time = time.time()
+
+ while True:
+ try:
+ # 接到SITL的數據 並轉發給Gazebo
+ data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+ sock_g2s.sendto(data_s2g, server_address)
+
+ # 得到Gazebo的回應 並轉發給SITL
+ data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+ sock_s2g.sendto(data_g2s, addr_s2g)
+
+ # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo)
+ try:
+ # 解析從 SITL 到 Gazebo 的數據
+ decoded = struct.unpack('HHI16H', data_s2g)
+ s2g_json = {
+ "magic": decoded[0],
+ "frame_rate_hz": decoded[1],
+ "frame_count": decoded[2],
+ "pwm": list(decoded[3:])
+ }
+
+ # 解析從 Gazebo 到 SITL 的數據
+ try:
+ g2s_data_str = data_g2s.decode('utf-8')
+ # 保存原始數據，適合後續分析
+ with open('gazebo_response_raw.txt', 'a') as f:
+ f.write(g2s_data_str + '\n')
+ except:
+ # 如果不是 UTF-8 格式，可能是二進制數據
+ g2s_data_str = str(data_g2s)
+
+ # 合併數據到一個 JSON 對象
+ combined_data = {
+ "sitl_to_gazebo": s2g_json,
+ "gazebo_to_sitl": g2s_data_str,
+ "timestamp": time.time()
+ }
+
+ # 寫入 JSON 檔案
+ with open('fdm_one_data.json', 'w') as f:
+ json.dump(combined_data, f, indent=2)
+
+ # 將JSON數據轉發到遠端IP
+ remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+
+ except Exception as e:
+ print(f"JSON 處理錯誤: {e}")
+
+ packet_count += 1
+ except socket.timeout:
+ print(f'no value')
+ #pass
+
+ current_time = time.time()
+ elapsed_time = current_time - start_time
+
+ if elapsed_time >= 1.0:
+ print(f'JSON Pack:{data_g2s}')
+ print(f'Forwarded data to remote IP: 140.120.31.130')
+ packet_count = 0
+ start_time = current_time
+ fdm_parser_example(data_s2g)
+
+
+def fdm_switch_multiple(num_systems=3):
+ '''
+ Create multiple UDP forwarders between SITL and Gazebo using threading
+ Each forwarder uses its own set of ports following the pattern:
+ - SITL ports: 9002, 9012, 9022, ...
+ - Gazebo ports: 19002, 19012, 19022, ...
+
+ Parameters:
+ num_systems (int): Number of forwarding pairs to create
+ '''
+ # Flag to control thread execution
+ running = True
+
+ def forwarder_thread(system_id):
+ sitl_port = 9002 + (system_id * 10)
+ gazebo_port = 19002 + (system_id * 10)
+
+ # Create SITL socket (server)
+ sitl_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ sitl_socket.bind(('', sitl_port))
+ sitl_socket.settimeout(0.1)
+
+ # Create Gazebo socket (client)
+ gazebo_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ gazebo_socket.settimeout(0.1)
+ gazebo_address = ('127.0.0.1', gazebo_port)
+
+ print(f"System {system_id}: Forwarding {sitl_port} → {gazebo_port}")
+
+ packet_count = 0
+ start_time = time.time()
+ last_data_s2g = None
+
+ try:
+ while running:
+ try:
+ # Receive from SITL and forward to Gazebo
+ data_s2g, addr_s2g = sitl_socket.recvfrom(1024)
+ last_data_s2g = data_s2g
+ gazebo_socket.sendto(data_s2g, gazebo_address)
+
+ try:
+ # Get response from Gazebo and send back to SITL
+ data_g2s, addr_g2s = gazebo_socket.recvfrom(1024)
+ sitl_socket.sendto(data_g2s, addr_s2g)
+
+ # Process and store data
+ try:
+ # Parse SITL to Gazebo data
+ decoded = struct.unpack('HHI16H', data_s2g)
+ s2g_json = {
+ "magic": decoded[0],
+ "frame_rate_hz": decoded[1],
+ "frame_count": decoded[2],
+ "pwm": list(decoded[3:])
+ }
+
+ # Try to parse Gazebo to SITL data
+ try:
+ g2s_data_str = data_g2s.decode('utf-8')
+ if packet_count % 50 == 0:
+ with open(f'gazebo_response_raw_{system_id}.txt', 'a') as f:
+ f.write(g2s_data_str + '\n')
+ except:
+ g2s_data_str = str(data_g2s)
+
+ # Combine data
+ combined_data = {
+ "sitl_to_gazebo": s2g_json,
+ "gazebo_to_sitl": g2s_data_str,
+ "timestamp": time.time(),
+ "system_id": system_id,
+ "sitl_port": sitl_port,
+ "gazebo_port": gazebo_port
+ }
+
+ # Save to JSON file (once per second)
+ current_time = time.time()
+ elapsed_time = current_time - start_time
+ if elapsed_time >= 1.0:
+ with open(f'fdm_data_system_{system_id}.json', 'w') as f:
+ json.dump(combined_data, f, indent=2)
+
+ except Exception as e:
+ print(f"System {system_id} - JSON processing error: {e}")
+
+ packet_count += 1
+
+ except socket.timeout:
+ # No response from Gazebo, continue
+ pass
+
+ except socket.timeout:
+ # No data from SITL, continue
+ pass
+
+ # Print stats periodically
+ current_time = time.time()
+ elapsed_time = current_time - start_time
+
+ if elapsed_time >= 1.0:
+ # print(f"System {system_id} - Packets: {packet_count} in the last second")
+ if packet_count > 0 and last_data_s2g:
+ print(f'System {system_id} - JSON Pack: {data_g2s}')
+ fdm_parser_example(last_data_s2g)
+ packet_count = 0
+ start_time = current_time
+
+ except Exception as e:
+ print(f"System {system_id} - Error in forwarder thread: {e}")
+ finally:
+ # Clean up resources
+ sitl_socket.close()
+ gazebo_socket.close()
+ print(f"System {system_id} - Forwarder thread terminated")
+
+ # Create and start threads for each system
+ threads = []
+ for i in range(num_systems):
+ thread = threading.Thread(target=forwarder_thread, args=(i,))
+ thread.daemon = True
+ threads.append(thread)
+ thread.start()
+
+ # Keep the main thread running
+ try:
+ while True:
+ time.sleep(1)
+ except KeyboardInterrupt:
+ print("\nShutting down all forwarders...")
+ running = False
+ time.sleep(1)
+ print("Done.")
+ return
+
+
+def fdm_switch_multiple_with_remote(num_systems=3, remote_ip='140.120.31.130'):
+ '''
+ Create multiple UDP forwarders between SITL and Gazebo using threading
+ Also forwards the combined data to a remote IP address
+
+ Each forwarder uses its own set of ports following the pattern:
+ - SITL ports: 9002, 9012, 9022, ...
+ - Gazebo ports: 19002, 19012, 19022, ...
+ - Remote ports: 9003, 9013, 9023, ... (for the remote forwarding)
+
+ Parameters:
+ num_systems (int): Number of forwarding pairs to create
+ remote_ip (str): IP address for remote forwarding
+ '''
+ # Flag to control thread execution
+ running = True
+
+ # Add shared counters for total packets
+ total_packets_sent = 0
+ total_packets_lock = threading.Lock() # For thread safety
+
+ def forwarder_thread(system_id):
+ nonlocal total_packets_sent
+ sitl_port = 9002 + (system_id * 10)
+ gazebo_port = 19002 + (system_id * 10)
+ remote_port = 9003 + (system_id * 10)
+
+ # Create SITL socket (server)
+ sitl_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ sitl_socket.bind(('', sitl_port))
+ sitl_socket.settimeout(0.1)
+
+ # Create Gazebo socket (client)
+ gazebo_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ gazebo_socket.settimeout(0.1)
+ gazebo_address = ('127.0.0.1', gazebo_port)
+
+ # Create remote forwarding socket
+ remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ remote_address = (remote_ip, remote_port)
+
+ print(f"System {system_id}: SITL:{sitl_port} → Gazebo:{gazebo_port}, Remote:{remote_ip}:{remote_port}")
+
+ packet_count = 0
+ start_time = time.time()
+ last_data_s2g = None
+ forward_counter = 0 # 添加轉發計數器
+ system_packets_sent = 0 # Local counter for this system
+
+ try:
+ while running:
+ try:
+ # Receive from SITL and forward to Gazebo
+ data_s2g, addr_s2g = sitl_socket.recvfrom(1024)
+ last_data_s2g = data_s2g
+ gazebo_socket.sendto(data_s2g, gazebo_address)
+
+ try:
+ # Get response from Gazebo and send back to SITL
+ data_g2s, addr_g2s = gazebo_socket.recvfrom(1024)
+ sitl_socket.sendto(data_g2s, addr_s2g)
+
+ # Process and store data
+ try:
+ # Parse SITL to Gazebo data
+ decoded = struct.unpack('HHI16H', data_s2g)
+ s2g_json = {
+ "magic": decoded[0],
+ "frame_rate_hz": decoded[1],
+ "frame_count": decoded[2],
+ "pwm": list(decoded[3:])
+ }
+
+ # Try to parse Gazebo to SITL data
+ try:
+ g2s_data_str = data_g2s.decode('utf-8')
+ if packet_count % 50 == 0:
+ with open(f'gazebo_response_raw_{system_id}.txt', 'a') as f:
+ f.write(g2s_data_str + '\n')
+ except:
+ g2s_data_str = str(data_g2s)
+
+ # Combine data
+ combined_data = {
+ "sitl_to_gazebo": s2g_json,
+ "gazebo_to_sitl": g2s_data_str,
+ "timestamp": time.time(),
+ "system_id": system_id,
+ "sitl_port": sitl_port,
+ "gazebo_port": gazebo_port
+ }
+
+ '''
+ # 每n筆才轉發一次
+ forward_counter += 1
+ if forward_counter >= 18:
+ # Forward data to remote server
+ remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+ #print(f"System {system_id} - Send data to remote (第{packet_count}個包)")
+ forward_counter = 0 # 重置計數器
+ '''
+
+ # 使用模運算代替計數器重置
+ forward_counter += 1
+ if packet_count % 1 == 0:
+ # Forward data to remote server
+ remote_socket.sendto(json.dumps(combined_data).encode(), remote_address)
+ system_packets_sent += 1
+ with total_packets_lock:
+ total_packets_sent += 1
+ # Debug用，看每秒傳了多少筆出去
+ #print(f"System {system_id} - Send data to remote (第{packet_count}個包)")
+
+ # Save to JSON file (once per second)
+ current_time = time.time()
+ elapsed_time = current_time - start_time
+ if elapsed_time >= 1.0:
+ with open(f'fdm_data_system_{system_id}.json', 'w') as f:
+ json.dump(combined_data, f, indent=2)
+
+ except Exception as e:
+ print(f"System {system_id} - JSON processing error: {e}")
+
+ packet_count += 1
+
+ except socket.timeout:
+ # No response from Gazebo, continue
+ pass
+
+ except socket.timeout:
+ # No data from SITL, continue
+ pass
+
+ # Print stats periodically
+ current_time = time.time()
+ elapsed_time = current_time - start_time
+
+ if elapsed_time >= 1.0:
+ print(f"System {system_id} - Packets: {packet_count} in the last second")
+ if packet_count > 0 and last_data_s2g:
+ fdm_parser_example(last_data_s2g)
+ packet_count = 0
+ start_time = current_time
+
+ except Exception as e:
+ print(f"System {system_id} - Error in forwarder thread: {e}")
+ finally:
+ # Clean up resources
+ sitl_socket.close()
+ gazebo_socket.close()
+ remote_socket.close()
+ print(f"System {system_id} - Forwarder thread terminated")
+
+ # Create statistics display thread
+ def stats_display_thread():
+ start_time = time.time()
+ while running:
+ time.sleep(5) # Display stats every 5 seconds
+ elapsed = time.time() - start_time
+ with total_packets_lock:
+ rate = total_packets_sent / elapsed if elapsed > 0 else 0
+ print(f"\n===== STATISTICS =====")
+ print(f"Total packets sent: {total_packets_sent}")
+ print(f"Average rate: {rate:.2f} packets/sec")
+ print(f"Running time: {elapsed:.1f} seconds")
+ print(f"======================\n")
+
+ # Create and start threads for each system
+ threads = []
+ for i in range(num_systems):
+ thread = threading.Thread(target=forwarder_thread, args=(i,))
+ thread.daemon = True
+ threads.append(thread)
+ thread.start()
+
+ # Start stats display thread
+ stats_thread = threading.Thread(target=stats_display_thread)
+ stats_thread.daemon = True
+ stats_thread.start()
+
+ # Keep the main thread running
+ try:
+ while True:
+ time.sleep(1)
+ except KeyboardInterrupt:
+ print("\nShutting down all forwarders...")
+ running = False
+ time.sleep(1)
+ print(f"Final total packets sent: {total_packets_sent}")
+ print("Done.")
+ return
+
+
+print('Start')
+#fdm_switch_example_two()
+#mavlink_analyzer_simple()
+#fdm_switch_example_one()
+#fdm_switch_example_one_with_remote_forwarding()
+#fdm_switch_multiple(1) # Create 3 forwarding pairs
+fdm_switch_multiple_with_remote(3) # Create 1 forwarding pair with remote forwarding
+#fdm_parser_example()
\ No newline at end of file
diff --git a/src/unitdev04/change_mode.py b/src/unitdev04/change_mode.py
new file mode 100644
index 0000000..53c96f3
--- /dev/null
+++ b/src/unitdev04/change_mode.py
@@ -0,0 +1,109 @@
+import curses
+import serial
+import struct
+from pymavlink.dialects.v20 import ardupilotmega as mavlink2
+
+PORT = "COM5" # or "/dev/ttyUSB0"
+BAUD = 57600
+target_system = 5
+target_component = 1
+MAV_CMD_DO_SET_MODE = 176
+
+mode_list = [
+ ("STABILIZE", 0),
+ ("AUTO", 3),
+ ("GUIDED", 4),
+ ("LOITER", 5)
+]
+
+ser = serial.Serial(PORT, BAUD)
+
+class PacketCapture:
+ def __init__(self):
+ self.data = bytearray()
+ def write(self, b):
+ self.data.extend(b)
+ return len(b)
+
+def build_api_tx_frame(data: bytes, frame_id=0x01):
+ frame_type = 0x10
+ dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播
+ dest_addr16 = b'\xFF\xFE'
+ broadcast_radius = 0x00
+ options = 0x00
+
+ frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
+ frame += dest_addr64 + dest_addr16
+ frame += struct.pack(">BB", broadcast_radius, options) + data
+ checksum = 0xFF - (sum(frame) & 0xFF)
+ return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+def curses_main(stdscr):
+ curses.curs_set(0)
+ selected = 0
+
+ while True:
+ stdscr.clear()
+ h, w = stdscr.getmaxyx()
+
+ stdscr.addstr(1, 2, "模式選單（使用:箭頭選擇，Enter:發送，q:離開）")
+
+ for i, (name, _) in enumerate(mode_list):
+ if i == selected:
+ stdscr.attron(curses.A_REVERSE)
+ stdscr.addstr(i + 3, 4, f"> {name}")
+ stdscr.attroff(curses.A_REVERSE)
+ else:
+ stdscr.addstr(i + 3, 4, f" {name}")
+
+ key = stdscr.getch()
+
+ if key == curses.KEY_UP:
+ selected = (selected - 1) % len(mode_list)
+ elif key == curses.KEY_DOWN:
+ selected = (selected + 1) % len(mode_list)
+ elif key in [10, 13]: # Enter
+ name, custom_mode = mode_list[selected]
+ capture = PacketCapture()
+ mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1)
+ mav.version = 2
+
+ msg = mav.command_long_encode(
+ target_system=target_system,
+ target_component=target_component,
+ command=MAV_CMD_DO_SET_MODE,
+ confirmation=0,
+ param1=1,
+ param2=custom_mode,
+ param3=0, param4=0, param5=0, param6=0, param7=0
+ )
+ print("msg =", msg) # 確認封包物件生成
+
+ mav.send(msg) # 改為 send() 會寫入 capture
+ print("RAW HEX:", capture.data.hex())
+
+ api_frame = build_api_tx_frame(capture.data)
+ ser.write(api_frame)
+
+ # 顯示封包資訊
+ msg_line = min(h - 4, len(mode_list) + 5)
+ stdscr.addstr(msg_line, 2, f"發送模式切換：{name} ({custom_mode})")
+ stdscr.addstr(msg_line + 1, 2, f"MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}")
+ stdscr.addstr(msg_line + 2, 2, f"XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}")
+ stdscr.refresh()
+ curses.napms(1500)
+
+ elif key in [ord('q'), ord('Q')]:
+ break
+
+ stdscr.refresh()
+
+
+try:
+ curses.wrapper(lambda stdscr: curses_main(stdscr)) # 使用 lambda 函數來傳遞 ser
+
+except Exception as e:
+ print(f"發生錯誤: {e}")
+finally:
+ ser.close()
+ print("程式結束，串口已關閉")
diff --git a/src/unitdev04/endpoint_at.py b/src/unitdev04/endpoint_at.py
index b6e0bfb..21c53f4 100644
--- a/src/unitdev04/endpoint_at.py
+++ b/src/unitdev04/endpoint_at.py
@@ -38,4 +38,4 @@ try:
pass
except KeyboardInterrupt:
print("\nProgram terminated.")
- ser.close()
+ ser.close()
\ No newline at end of file
diff --git a/src/unitdev04/receive.py b/src/unitdev04/receive.py
new file mode 100644
index 0000000..eab6436
--- /dev/null
+++ b/src/unitdev04/receive.py
@@ -0,0 +1,60 @@
+from xbee import XBee
+import serial
+from pymavlink import mavutil
+import time
+
+# 配置 XBee 串口連接
+ser = serial.Serial('COM5', 57600) # XBee 串口
+
+
+# 無人機系統 ID（假設為 5）和目標系統 ID（假設為 1）
+system_id = 5
+target_system_id = 1 # 設定目標系統 ID，這裡假設目標系統 ID 是 1
+
+# 目標設備的 64 位地址（請替換為實際的無人機地址）
+target_address = b'\x00\x13\xa2\x00\x40\x5f\x88\x56' # 例如：00 13 A2 00 40 5F 88 56
+
+def decode_mavlink_data(data,master):
+ """解碼 MAVLink 的原始數據並處理 HEARTBEAT 訊息"""
+
+ try:
+ msg = master.parse_char(data)
+ if msg:
+ if msg.get_type() == "HEARTBEAT":
+ print(f"Raw MAVLink Data (Hex): {data.hex()}")
+ print(f"Received MAVLink message: {msg}")
+ print(f"System status: {msg.system_status}")
+ print(f"Flight mode: {mavutil.mode_string_v10(msg)}")
+ print(f"System ID: {msg.get_srcSystem()}") # 使用 get_srcSystem() 獲取 system_id
+ except Exception as e:
+ print(f"Failed to decode MAVLink data: {e}")
+
+def receive_packets(ser):
+ xbee = XBee(ser)
+ # 創建 MAVLink 解析器並與 XBee 串口連接
+ master = mavutil.mavlink.MAVLink(ser)
+
+ while True:
+ try:
+ # 從 XBee 接收數據
+ xbee_data = xbee.wait_read_frame()
+
+ # 讀取 `rf_data` 而非 `payload`
+ raw_data = xbee_data.get('rf_data', None)
+ if raw_data is None:
+ print("Warning: No 'rf_data' found in received XBee data!")
+ continue # 跳過此次循環
+
+ # 解碼 MAVLink 訊息
+ decode_mavlink_data(raw_data,master)
+
+ # 根據需要觸發模式切換，例如根據用戶輸入更改模式
+
+
+ except KeyboardInterrupt:
+ print("Exiting...")
+ break
+ except Exception as e:
+ print(f"Error: {e}")
+
+ time.sleep(0.1)