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

Compare commits

base: chiyu1468:8c62f891a8d55d967d21ddfb7fc1d06540d88ceb
Branches Tags
chiyu1468:chiyu
chiyu1468:master
chiyu1468:lunu
chiyu1468:wenchun
chiyu1468:ken
..
compare: chiyu1468:730c3e2420667a1bec858e514aa0135ac4958c04
Branches Tags
chiyu1468:chiyu
chiyu1468:master
chiyu1468:lunu
chiyu1468:wenchun
chiyu1468:ken

No commits in common. '8c62f891a8d55d967d21ddfb7fc1d06540d88ceb' and '730c3e2420667a1bec858e514aa0135ac4958c04' have entirely different histories.
8c62f891a8 ... 730c3e2420

25 changed files with 561 additions and 1399 deletions
Show Stats

27
README.md
Unescape Escape View File

@ -73,30 +73,25 @@ python gui.py
=== ===
## 資料夾說明 ## 資料夾說明
1. fc_network_adapter (核心) 1. unitdev 為各自協作者做開發時的測試區
01 -> 晉凱(ken)
02 -> 其宇(chiyu)
03 -> 文鈞
04 -> 倫渝
2. fc_network_adapter (核心)
建立、維護與飛控韌體的連接 建立、維護與飛控韌體的連接
構築 mavlink 封包 構築 mavlink 封包
處理無線模組的通訊格式 (XBee) 處理無線模組的通訊格式 (XBee)
--同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)-- --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
2. fc_interfaces (重要) 3. fc_interfaces (重要)
自定義的 ROS2 介面檔 沒啥好說的 沒有這個核心會運作不了 自定義的 ROS2 介面檔
3. fc_network_module (重要)
非核心 但是支援載具的重要附屬功能
需要該功能時 作為一個 ros2 節點打開
例如 : ntrip rtk 訊號轉接
4. fc_network_apps 4. fc_network_apps
與 fc_network_adapter 銜接做高階功能包裝的應用小程式 與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
利於開發GUI或其他應用 使用者的外層包裝 使用者的外層包裝
這裡的定位是 "核心功能的高階包裝" 可以完全不去用
5. someotherpkg 5. someotherpkg
如何使用 fc_network_apps 的範例檔案 如何使用 fc_network_apps 的範例檔案
6. GUI 6. GUI
由 PyQt6 開發的互動式介面 由 PyQt6 開發的互動式介面
7. unitdev 為各自協作者做開發時的測試區 N. logs 是執行時期的記錄檔
01 -> 晉凱(ken)
02 -> 其宇(chiyu)
03 -> 文鈞
04 -> 倫渝
8. logs 是執行時期的記錄檔
=== ===

410
src/GUI/communication.py
Unescape Escape View File

@ -12,10 +12,6 @@ import socket
import sys import sys
import os import os
import traceback import traceback
try:
import serial
except ImportError:
serial = None
from pymavlink import mavutil from pymavlink import mavutil
from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
from sensor_msgs.msg import BatteryState, NavSatFix, Imu from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@ -62,163 +58,9 @@ except ImportError as e:
_log("ERROR", f"錯誤: {e}") _log("ERROR", f"錯誤: {e}")
AttitudeRaw = None AttitudeRaw = None
try:
from fc_interfaces.msg import GnssRaw
except ImportError as e:
_log("WARN", "無法導入 GnssRaw")
_log("ERROR", f"錯誤: {e}")
GnssRaw = None
class DroneSignals(QObject): class DroneSignals(QObject):
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
class JsonTelemetryProcessor:
"""共用 WebSocket JSON telemetry 轉換器。
Canonical JSON fields:
{
"system_id": 1,
"mode": "GUIDED",
"battery": 85,
"position": {"lat": 24.0, "lon": 120.0},
"heading": 90
}
Serial JSON uses this same shape; only the transport/framing differs.
"""
def _emit_json_connection_type(self, drone_id):
self.signals.update_signal.emit('connection_type', drone_id, {
'type': self.source_type
})
def process_json_telemetry_message(self, data):
"""處理 WebSocket JSON 格式的遙測資料。"""
try:
if isinstance(data, list):
for item in data:
if isinstance(item, dict):
self.process_json_telemetry_message(item)
return
if not isinstance(data, dict):
return
system_id = data.get('system_id', data.get('sysid'))
if system_id is None:
return
drone_id = f"s{self.socket_id}_{system_id}"
self._emit_json_connection_type(drone_id)
mode = data.get('mode', data.get('mode_name'))
state = {}
if mode is not None:
state['mode'] = mode
if 'armed' in data:
state['armed'] = data.get('armed')
if state:
self.signals.update_signal.emit('state', drone_id, state)
if 'battery' in data:
battery = data['battery']
if isinstance(battery, dict):
battery_data = {}
if 'percentage' in battery:
battery_data['percentage'] = battery.get('percentage')
elif 'percent' in battery:
battery_data['percentage'] = battery.get('percent')
if 'voltage' in battery:
battery_data['voltage'] = battery.get('voltage')
elif 'voltage_v' in battery:
battery_data['voltage'] = battery.get('voltage_v')
if battery_data:
self.signals.update_signal.emit('battery', drone_id, battery_data)
else:
self.signals.update_signal.emit('battery', drone_id, {
'percentage': battery
})
elif 'battery_percentage' in data or 'battery_voltage' in data:
battery_data = {}
if 'battery_percentage' in data:
battery_data['percentage'] = data.get('battery_percentage')
if 'battery_voltage' in data:
battery_data['voltage'] = data.get('battery_voltage')
self.signals.update_signal.emit('battery', drone_id, battery_data)
pos = data.get('position')
if isinstance(pos, dict):
gps_data = {
'lat': pos.get('lat', pos.get('latitude', 0)),
'lon': pos.get('lon', pos.get('longitude', 0)),
'alt': pos.get('alt', pos.get('altitude', 0))
}
self.signals.update_signal.emit('gps', drone_id, gps_data)
elif 'lat' in data or 'latitude' in data:
self.signals.update_signal.emit('gps', drone_id, {
'lat': data.get('lat', data.get('latitude', 0)),
'lon': data.get('lon', data.get('longitude', 0)),
'alt': data.get('alt', data.get('altitude', 0))
})
local = data.get('local_position', data.get('local_pose', data.get('local')))
if isinstance(local, dict):
x = local.get('x', 0.0)
y = local.get('y', 0.0)
z = local.get('z', 0.0)
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
})
elif isinstance(pos, dict):
alt = pos.get('alt', pos.get('altitude', 0.0))
self.signals.update_signal.emit('local_pose', drone_id, {
'x': 0.0,
'y': 0.0,
'z': alt
})
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': alt
})
velocity = data.get('velocity')
if isinstance(velocity, dict):
self.signals.update_signal.emit('velocity', drone_id, {
'vx': velocity.get('vx', velocity.get('x', 0.0)),
'vy': velocity.get('vy', velocity.get('y', 0.0)),
'vz': velocity.get('vz', velocity.get('z', 0.0))
})
attitude = data.get('attitude')
if isinstance(attitude, dict):
self.signals.update_signal.emit('attitude', drone_id, {
'roll': attitude.get('roll', 0.0),
'pitch': attitude.get('pitch', 0.0),
'yaw': attitude.get('yaw', 0.0),
'rates': attitude.get('rates', (0.0, 0.0, 0.0))
})
hud = data.get('hud', {})
if not isinstance(hud, dict):
hud = {}
if 'heading' in data:
hud['heading'] = data.get('heading')
if hud:
self.signals.update_signal.emit('hud', drone_id, {
'heading': hud.get('heading', 0.0),
'groundspeed': hud.get('groundspeed', 0.0),
'airspeed': hud.get('airspeed', 0.0),
'throttle': hud.get('throttle', 0.0),
'alt': hud.get('alt', hud.get('altitude', 0.0)),
'climb': hud.get('climb', 0.0)
})
except Exception as e:
print(f"{self.source_type} JSON telemetry processing error: {e}")
class UDPMavlinkReceiver(threading.Thread): class UDPMavlinkReceiver(threading.Thread):
"""UDP MAVLink 接收器""" """UDP MAVLink 接收器"""
def __init__(self, ip, port, signals, connection_name, monitor=None): def __init__(self, ip, port, signals, connection_name, monitor=None):
@ -287,11 +129,9 @@ class UDPMavlinkReceiver(threading.Thread):
elif msg_type == "GLOBAL_POSITION_INT": elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7 latitude = msg.lat / 1e7
longitude = msg.lon / 1e7 longitude = msg.lon / 1e7
relative_alt = msg.relative_alt / 1000.0
self.signals.update_signal.emit('gps', drone_id, { self.signals.update_signal.emit('gps', drone_id, {
'lat': latitude, 'lat': latitude,
'lon': longitude, 'lon': longitude
'alt': relative_alt,
}) })
elif msg_type == "GPS_RAW_INT": elif msg_type == "GPS_RAW_INT":
@ -344,8 +184,8 @@ class UDPMavlinkReceiver(threading.Thread):
"""停止接收器""" """停止接收器"""
self.running = False self.running = False
class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): class SerialMavlinkReceiver(threading.Thread):
"""串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。""" """串口 MAVLink 接收器"""
def __init__(self, port, baudrate, signals, connection_name, monitor=None): def __init__(self, port, baudrate, signals, connection_name, monitor=None):
super().__init__(daemon=True) super().__init__(daemon=True)
self.port = port self.port = port
@ -354,125 +194,47 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self.connection_name = connection_name self.connection_name = connection_name
self.monitor = monitor # 保存 monitor 引用 self.monitor = monitor # 保存 monitor 引用
self.socket_id = monitor.get_next_socket_id() if monitor else 0 self.socket_id = monitor.get_next_socket_id() if monitor else 0
self.source_type = 'Serial'
self.running = False self.running = False
self.serial_conn = None self.mav = None
self.mav_parser = mavutil.mavlink.MAVLink(None)
self.mav_parser.robust_parsing = True
self.json_buffer = []
self.json_depth = 0
self.json_in_string = False
self.json_escape = False
self._detected_protocols = set()
def run(self): def run(self):
"""執行串口接收循環。""" """執行串口接收循環"""
self.running = True self.running = True
try: try:
print(f"Serial receiver started on {self.port} at {self.baudrate} baud (MAVLink/JSON auto detect)") print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
if serial is None:
raise RuntimeError("pyserial 未安裝，無法啟動 Serial 連線")
self.serial_conn = serial.Serial( # 創建 MAVLink 串口連接
self.mav = mavutil.mavlink_connection(
self.port, self.port,
self.baudrate, baud=self.baudrate,
timeout=0.2 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: while self.running:
try: try:
chunk = self.serial_conn.read(256) msg = self.mav.recv_match(blocking=True, timeout=1.0)
if not chunk: if msg is None:
continue continue
for raw_byte in chunk: self.process_mavlink_message(msg)
byte = bytes([raw_byte])
self._process_json_byte(byte)
self._process_mavlink_byte(byte)
except Exception as e: except Exception as e:
if self.running: if self.running:
print(f"Error receiving serial telemetry: {e}") print(f"Error receiving MAVLink message from serial: {e}")
except Exception as e: except Exception as e:
print(f"Serial receiver error: {e}") print(f"Serial receiver error: {e}")
finally: finally:
if self.serial_conn: if self.mav:
try: try:
self.serial_conn.close() self.mav.close()
except Exception: except:
pass pass
def _process_mavlink_byte(self, byte):
try:
msg = self.mav_parser.parse_char(byte)
if msg is None:
return
if 'MAVLink' not in self._detected_protocols:
print(f"Serial {self.connection_name} detected MAVLink")
self._detected_protocols.add('MAVLink')
self.process_mavlink_message(msg)
except Exception:
# MAVLink parser is deliberately fed the whole stream, including JSON bytes.
return
def _process_json_byte(self, byte):
try:
char = byte.decode('utf-8')
except UnicodeDecodeError:
if self.json_buffer:
self._reset_json_framing()
return
if not self.json_buffer:
if char.isspace():
return
if char not in ('{', '['):
return
self.json_depth = 1
self.json_in_string = False
self.json_escape = False
self.json_buffer = [char]
return
self.json_buffer.append(char)
if self.json_in_string:
if self.json_escape:
self.json_escape = False
elif char == '\\':
self.json_escape = True
elif char == '"':
self.json_in_string = False
return
if char == '"':
self.json_in_string = True
elif char in ('{', '['):
self.json_depth += 1
elif char in ('}', ']'):
self.json_depth -= 1
if self.json_depth > 0:
return
payload = ''.join(self.json_buffer)
self._reset_json_framing()
try:
data = json.loads(payload)
if 'JSON' not in self._detected_protocols:
print(f"Serial {self.connection_name} detected JSON")
self._detected_protocols.add('JSON')
self.process_json_telemetry_message(data)
except json.JSONDecodeError as e:
print(f"Serial {self.connection_name} JSON decode error: {e}")
def _reset_json_framing(self):
self.json_buffer = []
self.json_depth = 0
self.json_in_string = False
self.json_escape = False
def process_mavlink_message(self, msg): def process_mavlink_message(self, msg):
"""處理 MAVLink 訊息""" """處理 MAVLink 訊息"""
try: try:
@ -501,11 +263,9 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
elif msg_type == "GLOBAL_POSITION_INT": elif msg_type == "GLOBAL_POSITION_INT":
latitude = msg.lat / 1e7 latitude = msg.lat / 1e7
longitude = msg.lon / 1e7 longitude = msg.lon / 1e7
relative_alt = msg.relative_alt / 1000.0
self.signals.update_signal.emit('gps', drone_id, { self.signals.update_signal.emit('gps', drone_id, {
'lat': latitude, 'lat': latitude,
'lon': longitude, 'lon': longitude
'alt': relative_alt,
}) })
elif msg_type == "GPS_RAW_INT": elif msg_type == "GPS_RAW_INT":
@ -557,23 +317,15 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
def stop(self): def stop(self):
"""停止接收器""" """停止接收器"""
self.running = False self.running = False
if self.serial_conn:
try:
self.serial_conn.close()
except Exception:
pass
class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): class WebSocketMavlinkReceiver(threading.Thread):
"""WebSocket MAVLink 接收器""" """WebSocket MAVLink 接收器"""
def __init__(self, url, signals, connection_name, monitor=None): def __init__(self, url, signals, connection_name, monitor=None):
super().__init__(daemon=True) super().__init__(daemon=True)
self.url = url self.url = url
self.signals = signals self.signals = signals
self.connection_name = connection_name self.connection_name = connection_name
self.monitor = monitor # 保存 monitor 引用 self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False
self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id
self.source_type = 'WS'
self.running = False
self.max_retries = 5 self.max_retries = 5
self.base_delay = 1.0 self.base_delay = 1.0
@ -601,8 +353,7 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
try: try:
data = json.loads(message) data = json.loads(message)
if isinstance(data, dict): data['_connection_source'] = self.connection_name
data['_connection_source'] = self.connection_name
self.process_websocket_message(data) self.process_websocket_message(data)
except json.JSONDecodeError as e: except json.JSONDecodeError as e:
print(f"WebSocket {self.connection_name} JSON decode error: {e}") print(f"WebSocket {self.connection_name} JSON decode error: {e}")
@ -632,7 +383,57 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
def process_websocket_message(self, data): def process_websocket_message(self, data):
"""處理 WebSocket 訊息""" """處理 WebSocket 訊息"""
self.process_json_telemetry_message(data) 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): def stop(self):
"""停止接收器""" """停止接收器"""
@ -803,21 +604,8 @@ class DroneMonitor(Node):
if not hasattr(self, subs_attr): if not hasattr(self, subs_attr):
self.setup_drone(sys_id) self.setup_drone(sys_id)
else: else:
# 檢查既有訂閱是否包含 position_gnss / position_ned / attitude，如果不包含就添加（兼容舊訂閱） # 檢查既有訂閱是否包含 position_ned / attitude，如果不包含就添加（兼容舊訂閱）
subs = getattr(self, subs_attr, {}) subs = getattr(self, subs_attr, {})
if isinstance(subs, dict) and 'position_gnss' not in subs and GnssRaw is not None:
base_topic = f'/fc_network/vehicle/{sys_id}'
try:
position_gnss_sub = self.create_subscription(
GnssRaw,
f'{base_topic}/position_gnss',
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
10
)
subs['position_gnss'] = position_gnss_sub
setattr(self, subs_attr, subs)
except Exception:
pass
if isinstance(subs, dict) and 'position_ned' not in subs: if isinstance(subs, dict) and 'position_ned' not in subs:
base_topic = f'/fc_network/vehicle/{sys_id}' base_topic = f'/fc_network/vehicle/{sys_id}'
try: try:
@ -873,6 +661,12 @@ class DroneMonitor(Node):
lambda msg, sid=sys_id: self.battery_callback(sid, msg), lambda msg, sid=sys_id: self.battery_callback(sid, msg),
10 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( 'summary': self.create_subscription(
String, String,
f'{base_topic}/summary', f'{base_topic}/summary',
@ -893,21 +687,6 @@ class DroneMonitor(Node):
) )
} }
if GnssRaw is not None:
subs['position_gnss'] = self.create_subscription(
GnssRaw,
f'{base_topic}/position_gnss',
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
10
)
else:
subs['position'] = self.create_subscription(
NavSatFix,
f'{base_topic}/position',
lambda msg, sid=sys_id: self.gps_callback(sid, msg),
10
)
if AttitudeRaw is not None: if AttitudeRaw is not None:
subs['attitude'] = self.create_subscription( subs['attitude'] = self.create_subscription(
AttitudeRaw, AttitudeRaw,
@ -1211,27 +990,20 @@ class DroneMonitor(Node):
if actual_drone_id is None: if actual_drone_id is None:
return return
gps_data = { self.latest_data[(actual_drone_id, 'gps')] = {
'lat': msg.latitude, 'lat': msg.latitude,
'lon': msg.longitude, 'lon': msg.longitude,
'alt': msg.altitude 'alt': msg.altitude
} }
if hasattr(msg, 'fix_type'):
gps_data['fix_type'] = msg.fix_type
if hasattr(msg, 'satellites_visible'):
gps_data['satellites_visible'] = msg.satellites_visible
if hasattr(msg, 'eph'):
gps_data['eph'] = msg.eph
if hasattr(msg, 'epv'):
gps_data['epv'] = msg.epv
self.latest_data[(actual_drone_id, 'gps')] = gps_data
# ================================================================================ # ================================================================================
# 【新增】儲存 GPS 資料到 drone_gps 字典 # 【新增】儲存 GPS 資料到 drone_gps 字典
# ================================================================================ # ================================================================================
self.drone_gps[actual_drone_id] = gps_data.copy() self.drone_gps[actual_drone_id] = {
'lat': msg.latitude,
'lon': msg.longitude,
'alt': msg.altitude
}
# ================================================================================ # ================================================================================
def local_vel_callback(self, drone_id, msg): def local_vel_callback(self, drone_id, msg):
@ -1329,10 +1101,10 @@ class DroneMonitor(Node):
} }
def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
"""啟動串口遙測連接（自動辨識 MAVLink / JSON）""" """啟動串口 MAVLink 連接"""
connection_name = f"Serial_{port.replace('/', '_')}" connection_name = f"Serial_{port.replace('/', '_')}"
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self) receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
receiver.start() receiver.start()
self.serial_receivers.append(receiver) self.serial_receivers.append(receiver)
_log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud (MAVLink/JSON)") _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud")
return receiver return receiver

36
src/GUI/drone_panel.py
Unescape Escape View File

@ -104,7 +104,7 @@ class DronePanel(QWidget):
def _init_ui(self): def _init_ui(self):
"""初始化UI""" """初始化UI"""
self.setObjectName(f"panel_{self.drone_id}") self.setObjectName(f"panel_{self.drone_id}")
self.setFixedHeight(160) self.setFixedHeight(140)
_set_scaled_stylesheet(self, """ _set_scaled_stylesheet(self, """
background-color: #2A2A2A; background-color: #2A2A2A;
border-radius: 8px; border-radius: 8px;
@ -197,11 +197,7 @@ class DronePanel(QWidget):
altitude_row = self._create_altitude_row() altitude_row = self._create_altitude_row()
info_layout.addWidget(altitude_row) info_layout.addWidget(altitude_row)
# 第四行：GNSS 定位狀態 # 第四行：航向 + 速度
gnss_row = self._create_gnss_row()
info_layout.addWidget(gnss_row)
# 第五行：航向 + 速度
nav_row = self._create_nav_row() nav_row = self._create_nav_row()
info_layout.addWidget(nav_row) info_layout.addWidget(nav_row)
@ -344,34 +340,6 @@ class DronePanel(QWidget):
return altitude_row return altitude_row
def _create_gnss_row(self):
"""創建 GNSS 定位狀態行。"""
gnss_row = QWidget()
gnss_layout = QHBoxLayout(gnss_row)
gnss_layout.setContentsMargins(0, 0, 0, 0)
fix_title = QLabel("定位：")
_set_scaled_stylesheet(fix_title, "color: #888; min-width: 50px;")
self.fix_type_label = QLabel("--")
self.fix_type_label.setObjectName(f"{self.drone_id}_fix_type_label")
_set_scaled_stylesheet(self.fix_type_label, "color: #DDD;")
error_title = QLabel("誤差(H/V)：")
_set_scaled_stylesheet(error_title, "color: #888; margin-left: 8px;")
self.gnss_error_label = QLabel("--")
self.gnss_error_label.setObjectName(f"{self.drone_id}_gnss_error")
_set_scaled_stylesheet(self.gnss_error_label, "color: #DDD;")
gnss_layout.addWidget(fix_title)
gnss_layout.addWidget(self.fix_type_label)
gnss_layout.addWidget(error_title)
gnss_layout.addWidget(self.gnss_error_label)
gnss_layout.addStretch()
return gnss_row
def _create_position_row(self): def _create_position_row(self):
"""位置行已移除（位置座標不再顯示於面板）。""" """位置行已移除（位置座標不再顯示於面板）。"""
return QWidget() return QWidget()

593
src/GUI/gui.py
Unescape Escape View File

@ -14,8 +14,6 @@ import subprocess
import time import time
import traceback import traceback
import re import re
import threading
from concurrent.futures import ThreadPoolExecutor
def _log(level, message): def _log(level, message):
@ -145,9 +143,7 @@ class ToggleSwitch(QWidget):
class ControlStationUI(QMainWindow): class ControlStationUI(QMainWindow):
planning_finished = pyqtSignal(object) VERSION = '2.2.0'
VERSION = '2.4.1'
FONT_SCALE_MIN = 70 FONT_SCALE_MIN = 70
FONT_SCALE_MAX = 180 FONT_SCALE_MAX = 180
FONT_SCALE_DEFAULT = 100 FONT_SCALE_DEFAULT = 100
@ -156,7 +152,6 @@ class ControlStationUI(QMainWindow):
super().__init__() super().__init__()
self.setWindowTitle(f'GCS v{self.VERSION}') self.setWindowTitle(f'GCS v{self.VERSION}')
self.resize(1400, 900) self.resize(1400, 900)
self._closing = False
# 初始化消息隊列（用於線程安全的 GUI 更新） # 初始化消息隊列（用於線程安全的 GUI 更新）
import queue import queue
@ -173,16 +168,10 @@ class ControlStationUI(QMainWindow):
# 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加 # 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加
self.monitor.executor = self.executor self.monitor.executor = self.executor
# 在背景執行緒處理 ROS2 spin，避免佔用 Qt 主執行緒時間 # 定时处理ROS事件
self.ros_thread_running = True self.timer = QTimer()
self.ros_thread = threading.Thread(target=self._ros_spin_thread, daemon=True) self.timer.timeout.connect(self.spin_ros)
self.ros_thread.start() self.timer.start(10)
# 任務規劃移到 worker thread，避免大量航點計算卡住 Qt 主執行緒
self.planner_pool = ThreadPoolExecutor(max_workers=2)
self._plan_request_counter = 0
self._latest_plan_request_by_group = {}
self.planning_finished.connect(self._on_plan_done)
# 驅動 asyncio 事件循環的定時器（新增 - 關鍵！） # 驅動 asyncio 事件循環的定時器（新增 - 關鍵！）
# 這允許異步任務（如 arm_drone）能夠執行 # 這允許異步任務（如 arm_drone）能夠執行
@ -190,16 +179,11 @@ class ControlStationUI(QMainWindow):
self.asyncio_timer.timeout.connect(self._spin_asyncio) self.asyncio_timer.timeout.connect(self._spin_asyncio)
self.asyncio_timer.start(10) # 每 10ms 驅動一次 asyncio self.asyncio_timer.start(10) # 每 10ms 驅動一次 asyncio
# 初始化 panel 更新（10Hz） # 初始化 panel 和 map 更新（10Hz）
self.panel_map_timer = QTimer() self.panel_map_timer = QTimer()
self.panel_map_timer.timeout.connect(self._update_panel_and_map) self.panel_map_timer.timeout.connect(self._update_panel_and_map)
self.panel_map_timer.start(100) # 10Hz self.panel_map_timer.start(100) # 10Hz
# Attitude 顯示器獨立高頻更新（30Hz），避免被 panel/table/map 節奏拖住
self.attitude_timer = QTimer()
self.attitude_timer.timeout.connect(self._update_attitude_only)
self.attitude_timer.start(33)
# 消息隊列處理定時器（來自線程的 GUI 更新） # 消息隊列處理定時器（來自線程的 GUI 更新）
self.msg_queue_timer = QTimer() self.msg_queue_timer = QTimer()
self.msg_queue_timer.timeout.connect(self._process_message_queue) self.msg_queue_timer.timeout.connect(self._process_message_queue)
@ -207,9 +191,6 @@ class ControlStationUI(QMainWindow):
# 快取消息數據，以便在沒有新消息時使用上一次的值 # 快取消息數據，以便在沒有新消息時使用上一次的值
self._message_cache = {} self._message_cache = {}
self._attitude_cache = {}
self._overview_cache = {}
self._map_dirty_drones = set()
self.message_history = [] self.message_history = []
self.max_message_history = 500 self.max_message_history = 500
@ -236,17 +217,6 @@ class ControlStationUI(QMainWindow):
self.drone_headings = {} self.drone_headings = {}
# 初始化地圖 # 初始化地圖
self.drone_map = DroneMap() self.drone_map = DroneMap()
# 地圖更新獨立節流（10Hz），避免 WebEngine / JS map 拖慢 panel 更新
self.map_timer = QTimer()
self.map_timer.timeout.connect(self._update_map_only)
self.map_timer.start(100)
# Overview table 獨立批次更新（5Hz），避免每筆資料都重繪 Qt table
self.overview_timer = QTimer()
self.overview_timer.timeout.connect(self._flush_overview_table)
self.overview_timer.start(200)
# 初始化連接列表 # 初始化連接列表
self.udp_receivers = [] self.udp_receivers = []
self.udp_connections = [] self.udp_connections = []
@ -421,7 +391,7 @@ class ControlStationUI(QMainWindow):
self.right_vertical_splitter.setChildrenCollapsible(False) self.right_vertical_splitter.setChildrenCollapsible(False)
self.right_vertical_splitter.setStretchFactor(0, 0) self.right_vertical_splitter.setStretchFactor(0, 0)
self.right_vertical_splitter.setStretchFactor(1, 1) self.right_vertical_splitter.setStretchFactor(1, 1)
self.right_vertical_splitter.setSizes([180, 640]) self.right_vertical_splitter.setSizes([170, 700])
right_layout.addWidget(self.right_vertical_splitter) right_layout.addWidget(self.right_vertical_splitter)
# 添加地圖 # 添加地圖
@ -756,7 +726,7 @@ class ControlStationUI(QMainWindow):
self.message_history = self.message_history[-self.max_message_history:] self.message_history = self.message_history[-self.max_message_history:]
if hasattr(self, 'message_history_view'): if hasattr(self, 'message_history_view'):
self.message_history_view.appendPlainText(entry) self.message_history_view.setPlainText("\n".join(self.message_history))
scrollbar = self.message_history_view.verticalScrollBar() scrollbar = self.message_history_view.verticalScrollBar()
scrollbar.setValue(scrollbar.maximum()) scrollbar.setValue(scrollbar.maximum())
@ -830,7 +800,7 @@ class ControlStationUI(QMainWindow):
status_label.setToolTip("已停止") status_label.setToolTip("已停止")
self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000) self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000)
else: else:
receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'], self.monitor) receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'])
receiver.start() receiver.start()
self.monitor.ws_receivers.append(receiver) self.monitor.ws_receivers.append(receiver)
conn['receiver'] = receiver conn['receiver'] = receiver
@ -862,7 +832,7 @@ class ControlStationUI(QMainWindow):
status_label.setToolTip("已停止") status_label.setToolTip("已停止")
self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000) self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000)
else: else:
receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'], self.monitor) receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'])
receiver.start() receiver.start()
self.udp_receivers.append(receiver) self.udp_receivers.append(receiver)
conn['receiver'] = receiver conn['receiver'] = receiver
@ -1079,7 +1049,7 @@ class ControlStationUI(QMainWindow):
self.group_tab_widget.show() self.group_tab_widget.show()
if hasattr(self, 'right_vertical_splitter'): if hasattr(self, 'right_vertical_splitter'):
total = sum(self.right_vertical_splitter.sizes()) or self.height() total = sum(self.right_vertical_splitter.sizes()) or self.height()
group_height = getattr(self, '_last_group_panel_height', 230) group_height = getattr(self, '_last_group_panel_height', 170)
group_height = min(max(group_height, 120), max(120, total - 120)) group_height = min(max(group_height, 120), max(120, total - 120))
self.right_vertical_splitter.setSizes([group_height, max(1, total - group_height)]) self.right_vertical_splitter.setSizes([group_height, max(1, total - group_height)])
self.toggle_group_btn.setText("▼") self.toggle_group_btn.setText("▼")
@ -1610,8 +1580,6 @@ class ControlStationUI(QMainWindow):
self._message_cache[drone_id] = {} self._message_cache[drone_id] = {}
self._message_cache[drone_id][msg_type] = data self._message_cache[drone_id][msg_type] = data
if msg_type == 'attitude':
self._attitude_cache[drone_id] = data
# ================================================================================ # ================================================================================
@ -1765,141 +1733,6 @@ class ControlStationUI(QMainWindow):
"""取得群組的無人機 ID 列表（排序後）""" """取得群組的無人機 ID 列表（排序後）"""
return sorted(group.selected_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1]))) return sorted(group.selected_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
@staticmethod
def _run_plan_formation_mission(planner_config, drone_gps_positions,
target_gps, mission_type, params):
planner = FormationPlanner(**planner_config)
return planner.plan_formation_mission(
drone_gps_positions, target_gps, mission_type, params=params
)
def _submit_mission_plan(self, group, selected_drones, drone_gps_positions,
target_gps, mission_type, params, context):
self._plan_request_counter += 1
request_id = self._plan_request_counter
self._latest_plan_request_by_group[group.group_id] = request_id
planner_config = {
'spacing': self.mission_planner.spacing,
'base_altitude': self.mission_planner.base_altitude,
'altitude_diff': self.mission_planner.altitude_diff,
}
params = dict(params)
future = self.planner_pool.submit(
self._run_plan_formation_mission,
planner_config,
list(drone_gps_positions),
tuple(target_gps),
mission_type,
params,
)
future.add_done_callback(
lambda f: self._emit_plan_result(
f, request_id, group.group_id, list(selected_drones),
list(drone_gps_positions), tuple(target_gps), context
)
)
def _emit_plan_result(self, future, request_id, group_id, selected_drones,
drone_gps_positions, target_gps, context):
if getattr(self, '_closing', False):
return
try:
waypoints_per_drone, center_origin, rendezvous_indices = future.result()
payload = {
'ok': True,
'request_id': request_id,
'group_id': group_id,
'selected_drones': selected_drones,
'drone_gps_positions': drone_gps_positions,
'target_gps': target_gps,
'waypoints_per_drone': waypoints_per_drone,
'center_origin': center_origin,
'rendezvous_indices': rendezvous_indices,
'context': context,
}
except Exception as e:
payload = {
'ok': False,
'request_id': request_id,
'group_id': group_id,
'error': str(e),
'traceback': traceback.format_exc(),
'context': context,
}
self.planning_finished.emit(payload)
def _on_plan_done(self, payload):
if getattr(self, '_closing', False):
return
group_id = payload['group_id']
context = payload.get('context', {})
if self._latest_plan_request_by_group.get(group_id) != payload['request_id']:
return
group = self.mission_groups.get(group_id)
if not group:
return
if not payload['ok']:
self.statusBar().showMessage(
f"Group {group_id}: {context.get('failure_label', '規劃')}失敗: {payload['error']}", 5000
)
_log("ERROR", payload.get('traceback', ''))
return
selected_drones = payload['selected_drones']
drone_gps_positions = payload['drone_gps_positions']
target_gps = payload['target_gps']
waypoints_per_drone = payload['waypoints_per_drone']
center_origin = payload['center_origin']
rendezvous_indices = payload['rendezvous_indices']
expected_mission_type = context.get('expected_mission_type')
if set(group.selected_drone_ids) != set(selected_drones):
self.statusBar().showMessage(
f"Group {group_id}: 無人機分配已變更，忽略舊規劃結果", 3000
)
return
if expected_mission_type and group.mission_type != expected_mission_type:
self.statusBar().showMessage(
f"Group {group_id}: 任務模式已變更，忽略舊規劃結果", 3000
)
return
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
target_lat, target_lon = context['draw_target']
self.drone_map.draw_mission_plan_for_group(
group_id, group.color,
center_lat, center_lon, target_lat, target_lon
)
self._launch_verification(
context['verification_type'], drone_gps_positions, selected_drones,
waypoints_per_drone, center_origin,
target_gps=target_gps if context.get('use_target_gps') else None,
rect_corners=context.get('rect_corners'),
route_waypoints=context.get('route_waypoints'),
)
panel = self.group_panels.get(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_id}: {context['success_label']}，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
)
def handle_map_click(self, lat, lon): def handle_map_click(self, lat, lon):
"""處理地圖點擊事件 — 根據 active group 的任務類型規劃""" """處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
group = self._get_active_group() group = self._get_active_group()
@ -1928,22 +1761,46 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage( self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000) f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000)
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) try:
if drone_gps_positions is None: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
return if drone_gps_positions is None:
return
context = { waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
'success_label': f"{group.mission_type} 規劃完成", drone_gps_positions, target_gps, mission_type, params=params
'failure_label': '規劃', )
'verification_type': group.mission_type,
'draw_target': (lat, lon), group.planned_waypoints = {
'use_target_gps': True, 'drone_ids': selected_drones,
'expected_mission_type': group.mission_type, 'waypoints': waypoints_per_drone,
} 'rendezvous_indices': rendezvous_indices,
self._submit_mission_plan( }
group, selected_drones, drone_gps_positions, group.center_origin = center_origin
target_gps, mission_type, params, context 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) # 任務規劃 — 矩形選取 (Grid Sweep)
@ -1974,27 +1831,51 @@ class ControlStationUI(QMainWindow):
base_alt = params.get('altitude', 10.0) base_alt = params.get('altitude', 10.0)
self.statusBar().showMessage( self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000) f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000)
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) try:
if drone_gps_positions is None: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
return if drone_gps_positions is None:
return
target_lat = sum(c[0] for c in rect_corners) / 4 target_lat = sum(c[0] for c in rect_corners) / 4
target_lon = sum(c[1] 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) target_gps = (target_lat, target_lon, base_alt)
params['rect_corners'] = rect_corners params['rect_corners'] = rect_corners
context = { waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
'success_label': 'Grid Sweep 規劃完成', drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
'failure_label': 'Grid Sweep 規劃', params=params
'verification_type': 'grid_sweep', )
'draw_target': (target_lat, target_lon),
'rect_corners': rect_corners, group.planned_waypoints = {
'expected_mission_type': group.mission_type, 'drone_ids': selected_drones,
} 'waypoints': waypoints_per_drone,
self._submit_mission_plan( 'rendezvous_indices': rendezvous_indices,
group, selected_drones, drone_gps_positions, }
target_gps, MissionType.GRID_SWEEP, params, context 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) # 任務規劃 — 路徑確認 (Leader-Follower)
@ -2036,28 +1917,53 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage( self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000) f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000)
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) try:
if drone_gps_positions is None: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
return if drone_gps_positions is None:
return
target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints) 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_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints)
target_gps = (target_lat, target_lon, base_alt) target_gps = (target_lat, target_lon, base_alt)
params['route_waypoints'] = route_waypoints params['route_waypoints'] = route_waypoints
context = { waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
'success_label': '跟隨模式規劃完成', drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER,
'failure_label': '跟隨模式規劃', params=params
'verification_type': 'LEADER_FOLLOWER', )
'draw_target': (target_lat, target_lon),
'use_target_gps': True, group.planned_waypoints = {
'route_waypoints': route_waypoints, 'drone_ids': selected_drones,
'expected_mission_type': group.mission_type, 'waypoints': waypoints_per_drone,
} 'rendezvous_indices': rendezvous_indices,
self._submit_mission_plan( }
group, selected_drones, drone_gps_positions, group.center_origin = center_origin
target_gps, MissionType.LEADER_FOLLOWER, params, context 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()
# ================================================================================ # ================================================================================
# 任務執行回呼 # 任務執行回呼
@ -2153,51 +2059,11 @@ class ControlStationUI(QMainWindow):
if isinstance(panel, DronePanel): if isinstance(panel, DronePanel):
panel.update_field(field, text, color) panel.update_field(field, text, color)
@staticmethod
def _format_gnss_fix_type(fix_type):
fix_labels = {
0: "No GPS",
1: "No GPS",
2: "2D Fix",
3: "3D Fix",
4: "DGPS",
5: "RTK Float",
6: "RTK Fixed",
}
try:
return fix_labels.get(int(fix_type), f"Fix {fix_type}")
except (TypeError, ValueError):
return "--"
def update_overview_table(self, drone_id=None, field=None, value=None): 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 if not hasattr(self, 'overview_table') or self.overview_table is None: return
self.overview_table.set_drones(self.drones) self.overview_table.set_drones(self.drones)
self.overview_table.update_table(drone_id, field, value) self.overview_table.update_table(drone_id, field, value)
def queue_overview_update(self, drone_id, field, value):
if not hasattr(self, '_overview_cache'):
self._overview_cache = {}
self._overview_cache.setdefault(drone_id, {})[field] = value
def _flush_overview_table(self):
if not hasattr(self, 'overview_table') or self.overview_table is None:
return
if not getattr(self, '_overview_cache', None):
return
pending_updates = self._overview_cache
self._overview_cache = {}
self.overview_table.set_drones(self.drones)
self.overview_table.setUpdatesEnabled(False)
try:
for drone_id, fields in pending_updates.items():
for field, value in fields.items():
self.overview_table.update_table(drone_id, field, value)
finally:
self.overview_table.setUpdatesEnabled(True)
self.overview_table.viewport().update()
def get_socket_id(self, drone_id): def get_socket_id(self, drone_id):
import re import re
match = re.match(r's(\d+)_(\d+)', drone_id) match = re.match(r's(\d+)_(\d+)', drone_id)
@ -2234,7 +2100,7 @@ class ControlStationUI(QMainWindow):
self.drone_panel_layout.addWidget(self.socket_groups[socket_id]) self.drone_panel_layout.addWidget(self.socket_groups[socket_id])
def _update_panel_and_map(self): def _update_panel_and_map(self):
"""定時更新 panel / table，批量更新 UI 以避免過度重繪。""" """30Hz 定時更新 panel 和 map，批量更新 UI 以避免過度重繪"""
if not hasattr(self, '_message_cache') or not self._message_cache: if not hasattr(self, '_message_cache') or not self._message_cache:
return return
@ -2249,17 +2115,20 @@ class ControlStationUI(QMainWindow):
self._map_update_time = now self._map_update_time = now
self._map_update_count = 0 self._map_update_count = 0
# ✅ 步驟 1: 暫停表格的即時重繪
if hasattr(self, 'overview_table') and self.overview_table:
self.overview_table.setUpdatesEnabled(False)
try: try:
start_time = time.time() start_time = time.time()
pending_messages = self._message_cache
self._message_cache = {}
# ✅ 步驟 2: 只處理本批新資料，避免每個 tick 重跑舊資料造成週期性卡頓 # ✅ 步驟 2: 遍歷快取中最新的資料來更新 UI
for drone_id, cached_data in pending_messages.items(): for drone_id in list(self._message_cache.keys()):
if drone_id not in self.drones: if drone_id not in self.drones:
continue continue
panel = self.drones[drone_id] panel = self.drones[drone_id]
cached_data = self._message_cache[drone_id]
# 處理所有快取的消息類型 # 處理所有快取的消息類型
for msg_type, data in cached_data.items(): for msg_type, data in cached_data.items():
@ -2275,8 +2144,8 @@ class ControlStationUI(QMainWindow):
arm_text, arm_color = "--", '#AAAAAA' arm_text, arm_color = "--", '#AAAAAA'
self.update_field(panel, drone_id, 'mode', mode, mode_color) self.update_field(panel, drone_id, 'mode', mode, mode_color)
self.update_field(panel, drone_id, 'armed', arm_text, arm_color) self.update_field(panel, drone_id, 'armed', arm_text, arm_color)
self.queue_overview_update(drone_id, 'mode', mode) self.update_overview_table(drone_id, 'mode', mode)
self.queue_overview_update(drone_id, 'armed', arm_text) self.update_overview_table(drone_id, 'armed', arm_text)
elif msg_type == 'battery': elif msg_type == 'battery':
voltage = data.get('voltage', 16) voltage = data.get('voltage', 16)
@ -2289,147 +2158,90 @@ class ControlStationUI(QMainWindow):
self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color) 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_vol', f"{voltage:.2f}V")
self.update_field(panel, drone_id, 'battery_cells', f"{cells}S") self.update_field(panel, drone_id, 'battery_cells', f"{cells}S")
self.queue_overview_update(drone_id, 'battery', f"{voltage:.2f}V") self.update_overview_table(drone_id, 'battery', f"{voltage:.2f}V")
elif msg_type == 'altitude': elif msg_type == 'altitude':
altitude = data.get('altitude', 0) altitude = data.get('altitude', 0)
text = f"{altitude:.1f} m" text = f"{altitude:.1f} m"
self.update_field(panel, drone_id, 'altitude', text) self.update_field(panel, drone_id, 'altitude', text)
self.queue_overview_update(drone_id, 'altitude', text) self.update_overview_table(drone_id, 'altitude', text)
elif msg_type == 'local_pose': elif msg_type == 'local_pose':
x, y = data.get('x', 0), data.get('y', 0) x, y = data.get('x', 0), data.get('y', 0)
if not hasattr(self.monitor, 'drone_local'): if not hasattr(self.monitor, 'drone_local'):
self.monitor.drone_local = {} self.monitor.drone_local = {}
self.monitor.drone_local[drone_id] = {'x': x, 'y': y} self.monitor.drone_local[drone_id] = {'x': x, 'y': y}
self.queue_overview_update(drone_id, 'local', f"{x:.1f}, {y:.1f}") self.update_overview_table(drone_id, 'local', f"{x:.1f}, {y:.1f}")
elif msg_type == 'loss_rate': elif msg_type == 'loss_rate':
text = f"{data.get('loss_rate', 0):.1f}%" text = f"{data.get('loss_rate', 0):.1f}%"
self.update_field(panel, drone_id, 'loss_rate', text) self.update_field(panel, drone_id, 'loss_rate', text)
self.queue_overview_update(drone_id, 'loss_rate', text) self.update_overview_table(drone_id, 'loss_rate', text)
elif msg_type == 'ping': elif msg_type == 'ping':
text = f"{data.get('ping', 0):.1f} ms" text = f"{data.get('ping', 0):.1f} ms"
self.update_field(panel, drone_id, 'ping', text) self.update_field(panel, drone_id, 'ping', text)
self.queue_overview_update(drone_id, 'ping', text) self.update_overview_table(drone_id, 'ping', text)
elif msg_type == 'velocity': elif msg_type == 'velocity':
self.queue_overview_update(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}") self.update_overview_table(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}")
elif msg_type == 'attitude': elif msg_type == 'attitude':
roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0) roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)
self.queue_overview_update(drone_id, 'roll', f"{roll:.1f}°") self.update_overview_table(drone_id, 'roll', f"{roll:.1f}°")
self.queue_overview_update(drone_id, 'pitch', f"{pitch:.1f}°") self.update_overview_table(drone_id, 'pitch', f"{pitch:.1f}°")
self.queue_overview_update(drone_id, 'yaw', f"{yaw:.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, yaw)
panel.update_attitude(heading, roll, pitch)
elif msg_type == 'gps': elif msg_type == 'gps':
gps_data = data gps_data = data
lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0) lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0)
self.drone_positions[drone_id] = (lat, lon) self.drone_positions[drone_id] = (lat, lon)
self._map_dirty_drones.add(drone_id)
alt = gps_data.get('alt', 0) alt = gps_data.get('alt', 0)
if not hasattr(self.monitor, 'drone_gps'): if not hasattr(self.monitor, 'drone_gps'):
self.monitor.drone_gps = {} self.monitor.drone_gps = {}
self.monitor.drone_gps[drone_id] = gps_data.copy() self.monitor.drone_gps[drone_id] = {'lat': lat, 'lon': lon, 'alt': alt}
self.queue_overview_update(drone_id, 'latitude', f"{lat:.6f}°") self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°")
self.queue_overview_update(drone_id, 'longitude', f"{lon:.6f}°") self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°")
fix_type = gps_data.get('fix_type')
satellites_visible = gps_data.get('satellites_visible')
eph = gps_data.get('eph')
epv = gps_data.get('epv')
fix_text = self._format_gnss_fix_type(fix_type)
self.update_field(panel, drone_id, 'fix_type_label', fix_text)
if isinstance(eph, (int, float)) and isinstance(epv, (int, float)):
error_text = f"{eph:.1f}/{epv:.1f}"
else:
error_text = "--"
self.update_field(panel, drone_id, 'gnss_error', error_text)
self.queue_overview_update(
drone_id, 'fix_type',
fix_text
)
self.queue_overview_update(
drone_id, 'satellites_visible',
str(satellites_visible) if satellites_visible is not None else "--"
)
self.queue_overview_update(
drone_id, 'eph',
f"{eph:.2f}" if isinstance(eph, (int, float)) else "--"
)
self.queue_overview_update(
drone_id, 'epv',
f"{epv:.2f}" if isinstance(epv, (int, float)) else "--"
)
elif msg_type == 'hud': elif msg_type == 'hud':
hud_data = data hud_data = data
heading = hud_data.get('heading', 0) heading = hud_data.get('heading', 0)
self.drone_headings[drone_id] = heading self.drone_headings[drone_id] = heading
self._map_dirty_drones.add(drone_id)
groundspeed = hud_data.get('groundspeed', 0) groundspeed = hud_data.get('groundspeed', 0)
airspeed = hud_data.get('airspeed', 0) airspeed = hud_data.get('airspeed', 0)
throttle = hud_data.get('throttle', 0) throttle = hud_data.get('throttle', 0)
hud_alt = hud_data.get('alt', 0) hud_alt = hud_data.get('alt', 0)
climb = hud_data.get('climb', 0) climb = hud_data.get('climb', 0)
self.queue_overview_update(drone_id, 'heading', f"{heading:.1f}°") self.update_overview_table(drone_id, 'heading', f"{heading:.1f}°")
self.queue_overview_update(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") self.update_overview_table(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
self.queue_overview_update(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--") self.update_overview_table(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--")
self.queue_overview_update(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--") self.update_overview_table(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--")
self.queue_overview_update(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--") self.update_overview_table(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--")
self.queue_overview_update(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (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, '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, '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 "--") 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 elapsed = (time.time() - start_time) * 1000
if elapsed > 33: if elapsed > 33:
_log("WARN", f"UI 更新耗時 {elapsed:.1f}ms (目標 <33ms)") _log("WARN", f"UI 更新耗時 {elapsed:.1f}ms (目標 <33ms)")
except Exception as e:
_log("ERROR", f"Panel 更新錯誤: {e}")
traceback.print_exc()
def _update_map_only(self):
"""獨立降頻更新地圖，避免地圖 JS 呼叫拖住 panel / table。"""
dirty_drones = getattr(self, '_map_dirty_drones', set())
if not dirty_drones:
return
pending_drones = list(dirty_drones)
self._map_dirty_drones = set()
for drone_id in pending_drones:
pos = self.drone_positions.get(drone_id)
if not pos:
continue
heading = self.drone_headings.get(drone_id, 0)
lat, lon = pos
self.drone_map.update_drone_position(drone_id, lat, lon, heading)
def _update_attitude_only(self):
"""高頻更新 drone panel 的 attitude 顯示器。"""
if not getattr(self, '_attitude_cache', None):
return
latest_attitudes = self._attitude_cache
self._attitude_cache = {}
for drone_id, data in latest_attitudes.items(): finally:
panel = self.drones.get(drone_id) # ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪）
if not panel or not hasattr(panel, 'update_attitude'): if hasattr(self, 'overview_table') and self.overview_table:
continue self.overview_table.setUpdatesEnabled(True)
self.overview_table.viewport().update()
roll = data.get('roll', 0)
pitch = data.get('pitch', 0)
yaw = data.get('yaw', 0)
heading = self.drone_headings.get(drone_id, yaw)
panel._last_roll = roll
panel._last_pitch = pitch
panel.update_attitude(heading, roll, pitch)
@ -2463,41 +2275,28 @@ class ControlStationUI(QMainWindow):
# 但仍然打印詳細的堆棧跟踪以便調試 # 但仍然打印詳細的堆棧跟踪以便調試
traceback.print_exc() traceback.print_exc()
def _ros_spin_thread(self): def spin_ros(self):
while self.ros_thread_running and rclpy.ok(): try:
try: # 仅在 ROS2 正常工作时才尝试 spin
if rclpy.ok():
self.executor.spin_once(timeout_sec=0.01) self.executor.spin_once(timeout_sec=0.01)
for (drone_id, msg_type), data in self.monitor.latest_data.items():
latest_items = list(self.monitor.latest_data.items())
self.monitor.latest_data.clear()
for (drone_id, msg_type), data in latest_items:
self.monitor.signals.update_signal.emit(msg_type, drone_id, data) self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
except RuntimeError as e: self.monitor.latest_data.clear()
if "Context" in str(e) or "context" in str(e).lower(): except RuntimeError as e:
_log("WARN", f"ROS2 context 錯誤（已忽略）: {e}") # ROS2 context 被破坏或不可用
break if "Context" in str(e) or "context" in str(e).lower():
_log("ERROR", f"ROS spin thread error: {e}") _log("WARN", f"ROS2 context 錯誤（已忽略）: {e}")
traceback.print_exc() else:
except Exception as e: _log("ERROR", f"ROS spin error: {e}")
_log("ERROR", f"ROS spin thread error: {e}")
traceback.print_exc() traceback.print_exc()
except Exception as e:
def _stop_ros_thread(self): _log("ERROR", f"ROS spin error: {e}")
self.ros_thread_running = False traceback.print_exc()
ros_thread = getattr(self, 'ros_thread', None)
if ros_thread and ros_thread.is_alive():
ros_thread.join(timeout=1.0)
def closeEvent(self, event): def closeEvent(self, event):
self._closing = True
self._restore_stream_redirector() self._restore_stream_redirector()
self._stop_ros_thread()
try: try:
try:
self.planner_pool.shutdown(wait=False, cancel_futures=True)
except TypeError:
self.planner_pool.shutdown(wait=False)
for group in self.mission_groups.values(): for group in self.mission_groups.values():
if group.executor: if group.executor:
group.executor.stop() group.executor.stop()

10
src/GUI/map_layout.py
Unescape Escape View File

@ -340,7 +340,6 @@ class DroneMap:
var markers = {}; var markers = {};
var idLabels = {}; var idLabels = {};
var trajectories = {}; var trajectories = {};
const maxTrajectoryPoints = 300;
var trajectoryLines = {}; var trajectoryLines = {};
var focusedId = null; var focusedId = null;
var initialized = false; var initialized = false;
@ -703,12 +702,11 @@ class DroneMap:
const point = [lat, lon]; const point = [lat, lon];
trajectories[id].push(point); trajectories[id].push(point);
if (trajectories[id].length > maxTrajectoryPoints) { if (trajectories[id].length > 1000) {
trajectories[id].shift(); trajectories[id].shift();
trajectoryLines[id].setLatLngs(trajectories[id]);
} else {
trajectoryLines[id].addLatLng(point);
} }
trajectoryLines[id].setLatLngs([...trajectories[id]]);
} }
function focusOn(id) { function focusOn(id) {
@ -880,7 +878,7 @@ class DroneMap:
# 設置地圖更新計時器 # 設置地圖更新計時器
self.map_update_timer = QTimer() self.map_update_timer = QTimer()
self.map_update_timer.timeout.connect(self.update_map_positions) self.map_update_timer.timeout.connect(self.update_map_positions)
self.map_update_timer.start(100) # 每 100ms 更新一次（10Hz） self.map_update_timer.start(200) # 每 200ms 更新一次
def _on_map_loaded(self, ok: bool): def _on_map_loaded(self, ok: bool):
"""地圖加載完成回調""" """地圖加載完成回調"""

247
src/GUI/mission_planner.py
Unescape Escape View File

@ -83,11 +83,10 @@ class FormationPlanner:
center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions) 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_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)
# origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT self.current_origin = (center_lat, center_lon, center_alt)
# 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移 self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
self.current_origin = (center_lat, center_lon, 0.0)
self.converter = CoordinateConverter(center_lat, center_lon, 0.0)
drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions] drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
target_local = self.converter.gps_to_local(*target_gps) target_local = self.converter.gps_to_local(*target_gps)
@ -207,17 +206,29 @@ class FormationPlanner:
def _calculate_leader_follower(self, drone_positions, route_wps_local, params): def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
""" """
路徑跟隨編隊 — 虛擬領隊 / 弧長參數化 路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
center_path = 使用者畫的路徑點（直線段連接，不插弧）， 核心想法：
每架 drone 沿弧長 s 參數化做隊形偏移： - center_path 建好後計算每個 sample 的累積弧長 s
s_follower = s_leader - lon_k - 每架 drone 給 (lat_k, lon_k) 的隊形偏移
position = lookup(s_follower) + lat_k × perp(tangent) - 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k
s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。 - follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent)
- 超過 path 起點/終點以 clip 處理，避免倒退或衝出
Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
隊形 (俯視): 解決前版「空間偏移」的三個 bug：
D0 (lat=-L, lon=0) ← leader 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) D1 (lat=+L, lon=5)
D2 (lat=-L, lon=10) D2 (lat=-L, lon=10)
D3 (lat=+L, lon=15) D3 (lat=+L, lon=15)
@ -226,16 +237,29 @@ class FormationPlanner:
lateral_offset = params.get('lateral_offset', 3.0) lateral_offset = params.get('lateral_offset', 3.0)
longitudinal_spacing = params.get('longitudinal_spacing', 5.0) longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
altitude = params.get('altitude', self.base_altitude) altitude = params.get('altitude', self.base_altitude)
min_inner_radius = params.get('min_inner_radius', 3.0)
# Step 1: 建立中心路徑（直線段連接，每個中間航點設 barrier） arc_resolution = params.get('arc_resolution', 12)
center_path, barrier_indices = self._build_center_path(route_wps_local) 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_list = [pt['s'] for pt in center_path]
s_max = s_list[-1] s_max = s_list[-1]
n_pts = len(center_path) n_pts = len(center_path)
def lookup(s_target): def lookup(s_target):
"""在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。超出範圍時沿端點方向外推。""" """
在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
s<0 → clip 到 start；s>s_max → clip 到 end。
tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
"""
if n_pts == 1: if n_pts == 1:
pt = center_path[0] pt = center_path[0]
return pt['x'], pt['y'], pt['dir'] return pt['x'], pt['y'], pt['dir']
@ -246,8 +270,7 @@ class FormationPlanner:
sdx, sdy = b['x'] - a['x'], b['y'] - a['y'] sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy) slen = math.hypot(sdx, sdy)
if slen > 1e-6: if slen > 1e-6:
dx, dy = sdx / slen, sdy / slen return a['x'], a['y'], (sdx / slen, sdy / slen)
return a['x'] + s_target * dx, a['y'] + s_target * dy, (dx, dy)
return a['x'], a['y'], a['dir'] return a['x'], a['y'], a['dir']
if s_target >= s_max: if s_target >= s_max:
@ -256,9 +279,7 @@ class FormationPlanner:
sdx, sdy = b['x'] - a['x'], b['y'] - a['y'] sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy) slen = math.hypot(sdx, sdy)
if slen > 1e-6: if slen > 1e-6:
dx, dy = sdx / slen, sdy / slen return b['x'], b['y'], (sdx / slen, sdy / slen)
overshoot = s_target - s_max
return b['x'] + overshoot * dx, b['y'] + overshoot * dy, (dx, dy)
return b['x'], b['y'], b['dir'] return b['x'], b['y'], b['dir']
# Binary search：s_list[lo] <= s_target < s_list[hi] # Binary search：s_list[lo] <= s_target < s_list[hi]
@ -287,6 +308,16 @@ class FormationPlanner:
# 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定 # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
all_waypoints = [None] * N 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): for rank in range(N):
lat_sign = -1 if rank % 2 == 0 else 1 lat_sign = -1 if rank % 2 == 0 else 1
lat = lat_sign * lateral_offset lat = lat_sign * lateral_offset
@ -303,16 +334,46 @@ class FormationPlanner:
altitude, altitude,
)) ))
# 收尾：全員到 path 終點的 rank 位置（lon 歸零）
waypoints.append((
end['x'] + lat * (-end_dy),
end['y'] + lat * end_dx,
altitude,
))
all_waypoints[rank] = waypoints all_waypoints[rank] = waypoints
# barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
# 不觸及末尾收尾點，直接沿用即可
return all_waypoints, barrier_indices return all_waypoints, barrier_indices
def _build_center_path(self, waypoints): def _build_center_path(self, waypoints,
formation_max_lateral,
min_inner_radius,
arc_resolution,
sharp_turn_cos_threshold):
""" """
建立中心路徑 — 直接用路徑點連接，不插弧。 建立以「圓弧」連接直線段的中心路徑。
每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。 每個中間 waypoint 的處理：
飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。 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: Returns:
(path, barrier_indices) (path, barrier_indices)
@ -324,35 +385,129 @@ class FormationPlanner:
'seg': 'straight', 'seg': 'straight',
'x': waypoints[0][0], 'y': waypoints[0][1], 'x': waypoints[0][0], 'y': waypoints[0][1],
'dir': (0.0, 1.0), 'dir': (0.0, 1.0),
's': 0.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 = [] path = []
barrier_indices = [] barrier_indices = []
for i in range(num_wps): # 起點
if i < num_wps - 1: path.append({
dx = waypoints[i + 1][0] - waypoints[i][0] 'seg': 'straight',
dy = waypoints[i + 1][1] - waypoints[i][1] 'x': waypoints[0][0], 'y': waypoints[0][1],
else: 'dir': segments[0][0],
dx = waypoints[i][0] - waypoints[i - 1][0] })
dy = waypoints[i][1] - waypoints[i - 1][1]
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],
)
length = math.hypot(dx, dy) theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
if length < 0.01:
direction = (0.0, 1.0)
else:
direction = (dx / length, dy / length)
# T_in (straight, 方向 u_in)
path.append({ path.append({
'seg': 'straight', 'seg': 'straight',
'x': waypoints[i][0], 'y': waypoints[i][1], 'x': T_in[0], 'y': T_in[1],
'dir': direction, 'dir': u_in,
}) })
if 0 < i < num_wps - 1: # 弧段採樣 k=1..arc_resolution-1（不含兩端）
barrier_indices.append(len(path) - 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 path[0]['s'] = 0.0
for i in range(1, len(path)): for i in range(1, len(path)):
prev = path[i - 1] prev = path[i - 1]

37
src/GUI/overview_table.py
Unescape Escape View File

@ -6,9 +6,8 @@ class OverviewTable(QTableWidget):
"""總覽表格，顯示所有無人機的狀態資訊""" """總覽表格，顯示所有無人機的狀態資訊"""
# 默認的資訊類型和映射 # 默認的資訊類型和映射
DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "FixType", "衛星數", "EPH", "EPV", DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
"高度", "XY位置", "XY速度", "地速", "航向", "空速", "油門", "海拔高度", "空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
"爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
DEFAULT_INFO_TYPE_MAP = { DEFAULT_INFO_TYPE_MAP = {
"mode": 0, "mode": 0,
@ -16,24 +15,20 @@ class OverviewTable(QTableWidget):
"battery": 2, "battery": 2,
"longitude": 3, "longitude": 3,
"latitude": 4, "latitude": 4,
"fix_type": 5, "altitude": 5,
"satellites_visible": 6, "local": 6,
"eph": 7, "velocity": 7,
"epv": 8, "groundspeed": 8,
"altitude": 9, "heading": 9,
"local": 10, "airspeed": 10,
"velocity": 11, "throttle": 11,
"groundspeed": 12, "hud_alt": 12,
"heading": 13, "climb": 13,
"airspeed": 14, "roll": 14,
"throttle": 15, "pitch": 15,
"hud_alt": 16, "yaw": 16,
"climb": 17, "loss_rate": 17,
"roll": 18, "ping": 18
"pitch": 19,
"yaw": 20,
"loss_rate": 21,
"ping": 22
} }
def __init__(self, info_types=None, info_type_map=None, parent=None): def __init__(self, info_types=None, info_type_map=None, parent=None):

4
src/fc_interfaces/CMakeLists.txt
Unescape Escape View File

@ -9,15 +9,13 @@ endif()
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED) find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(builtin_interfaces REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME} rosidl_generate_interfaces(${PROJECT_NAME}
"msg/AttitudeRaw.msg" "msg/AttitudeRaw.msg"
"msg/GnssRaw.msg"
"srv/MavPing.srv" "srv/MavPing.srv"
"srv/MavCommandLong.srv" "srv/MavCommandLong.srv"
"srv/MavPositionTargetGlobalInt.srv" "srv/MavPositionTargetGlobalInt.srv"
DEPENDENCIES std_msgs builtin_interfaces DEPENDENCIES std_msgs
) )
ament_package() ament_package()

8
src/fc_interfaces/msg/GnssRaw.msg
Unescape Escape View File

@ -1,8 +0,0 @@
builtin_interfaces/Time stamp
float64 latitude
float64 longitude
float64 altitude
uint8 fix_type
uint8 satellites_visible
float32 eph
float32 epv

2
src/fc_interfaces/package.xml
Unescape Escape View File

@ -17,8 +17,6 @@
<build_depend>std_msgs</build_depend> <build_depend>std_msgs</build_depend>
<exec_depend>std_msgs</exec_depend> <exec_depend>std_msgs</exec_depend>
<build_depend>builtin_interfaces</build_depend>
<exec_depend>builtin_interfaces</exec_depend>
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>

21
src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
Unescape Escape View File

@ -176,15 +176,13 @@
這裡支持了所有 ROS2 框架的對應功能 包涵 這裡支持了所有 ROS2 框架的對應功能 包涵
1. 將載具的串流更新的狀態更新到對應的 topic 1. 將載具的串流更新的狀態更新到對應的 topic
2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面 2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
3. 訂閱外部 RTCM 資料並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
### **[Class]** fc_ros_manager ### **[Class]** fc_ros_manager
MAVLink ROS2 節點管理器 管理三個獨立的 Node MAVLink ROS2 節點管理器 管理兩個獨立的 Node
會開一個執行緒 讓三個 Node 都跑在裡面 會開一個執行緒 讓兩個 Node 都跑在裡面
然後用 spin_once 保持 Node 的活性 然後用 spin_once 保持 Node 的活性
- *self.status_publisher* 物件實例 - *self.status_publisher* 物件實例
- *self.command_service* 物件實例 - *self.command_service* 物件實例
- *self.rtcm_relay* 物件實例
- *self.spin_thread* 執行緒 - *self.spin_thread* 執行緒
--- ---
- *start()* 啟動自己 - *start()* 啟動自己
@ -212,20 +210,6 @@
- *__init__()* 這邊登入要創建的 service 到 ros2 系統 - *__init__()* 這邊登入要創建的 service 到 ros2 系統
- *handle_XXX()* 這邊是實現 service 的具體方法 - *handle_XXX()* 這邊是實現 service 的具體方法
### **[Class]** RtcmRelay
訂閱 /fc_network/rtcm_in (mavros_msgs/msg/RTCM) 並轉發為 MAVLink GPS_RTCM_DATA
topic 的 QoS：BEST_EFFORT / KEEP_LAST depth=1 / VOLATILE（低延遲優先，丟舊包不排隊）
處理順序：過期丟棄(1s) → blake2s hash 去重 → 最短轉發間隔節流 → 分片轉發
分片：GPS_RTCM_DATA 每片 180 bytes，超過自動分片，最多 4 片 (720 bytes)，超長直接丟棄
失敗語意：不做舊包重送，送失敗就放棄等下一包
- *self.counters* 觀測用計數器 (received/forwarded/dropped_stale/dropped_duplicate/dropped_throttle/dropped_oversize/no_route/send_fail)
- *self.clock_offset_sec* 與 RTCM 來源的時鐘偏移量（秒）
---
- *_rtcm_callback()* 收到 RTCM 封包時的處理流程
- *_send_rtcm()* 對單一載具編碼並送出 GPS_RTCM_DATA（含分片邏輯）
- *set_clock_offset()* 設定時鐘偏移量
- *get_counters()* 取得觀測計數器
# 開發記錄 # 開發記錄
@ -241,7 +225,6 @@
9. 載具狀態收集與彙整 9. 載具狀態收集與彙整
10. a. ros2 topic 應用開發介面 (基礎框架) 10. a. ros2 topic 應用開發介面 (基礎框架)
b. ros2 service 應用開發介面 (基礎框架) b. ros2 service 應用開發介面 (基礎框架)
11. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
### 待開發功能 ### 待開發功能
5-1. 建立 serial 連線 並可以對接收器下達AT指令 5-1. 建立 serial 連線 並可以對接收器下達AT指令

6
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
Unescape Escape View File

@ -1145,9 +1145,9 @@ class Orchestrator:
self.fc_ros_manager = mros.ros2_manager self.fc_ros_manager = mros.ros2_manager
self.fc_ros_manager.initialize() self.fc_ros_manager.initialize()
self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = { self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
'position_gnss': 0.2, 'position': 1.0,
'position_ned': 0.5, 'position_ned': 1.0,
'attitude': 0.1, 'attitude': 1.0,
'velocity': 0.0, 'velocity': 0.0,
'battery': 1.0, 'battery': 1.0,
'vfr_hud': 1.0, 'vfr_hud': 1.0,

13
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
Unescape Escape View File

@ -109,7 +109,6 @@ class mavlink_bridge:
"""初始化訊息處理器映射表，提高處理效率""" """初始化訊息處理器映射表，提高處理效率"""
self.message_handlers = { self.message_handlers = {
0: self._handle_heartbeat, # HEARTBEAT 0: self._handle_heartbeat, # HEARTBEAT
24: self._handle_gps_raw_int, # GPS_RAW_INT
30: self._handle_attitude, # ATTITUDE 30: self._handle_attitude, # ATTITUDE
32: self._handle_local_position, # LOCAL_POSITION_NED 32: self._handle_local_position, # LOCAL_POSITION_NED
33: self._handle_global_position, # GLOBAL_POSITION_INT 33: self._handle_global_position, # GLOBAL_POSITION_INT
@ -240,14 +239,6 @@ class mavlink_bridge:
component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺 component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺
component.status.position.timestamp = timestamp component.status.position.timestamp = timestamp
def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
"""處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
component.status.gps.fix_type = msg.fix_type
component.status.gps.satellites_visible = msg.satellites_visible
component.status.gps.eph = None if msg.eph == 65535 else msg.eph
component.status.gps.epv = None if msg.epv == 65535 else msg.epv
component.status.gps.timestamp = timestamp
def _handle_vfr_hud(self, vehicle, component, msg, timestamp): def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
"""處理 VFR_HUD 訊息 (msg_id: 74)""" """處理 VFR_HUD 訊息 (msg_id: 74)"""
component.status.vfr.airspeed = msg.airspeed component.status.vfr.airspeed = msg.airspeed
@ -395,9 +386,7 @@ class mavlink_object:
self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191) self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
# 記錄訊息過濾類型 (可選) # 記錄訊息過濾類型 (可選)
# 0 HEARTBEAT, 24 GPS_RAW_INT, 30 ATTITUDE, 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
# 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147])
self.return_msg_types = set([]) self.return_msg_types = set([])
# 轉發到別的 mavlink object 作為目標端口 的列表 # 轉發到別的 mavlink object 作為目標端口 的列表

228
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
Unescape Escape View File

@ -1,13 +1,11 @@
""" """
MAVLink ROS2 Nodes MAVLink ROS2 Nodes
主要包含三個獨立的 ROS2 Node : 主要包含兩個獨立的 ROS2 Node :
1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計 從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
2. MavlinkCommandService - 提供 MAVLink 指令 service 介面 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
以最基礎的 mavlink microservice 會實現目標 以最基礎的 mavlink microservice 會實現目標
並不會包含額外的功能 並不會包含額外的功能
3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
過期丟棄、去重、節流、分片
與一個節點管理器 與一個節點管理器
- fc_ros_manager - fc_ros_manager
@ -17,7 +15,6 @@ MAVLink ROS2 Nodes
import os import os
import time import time
import math import math
import hashlib
import threading import threading
from typing import Dict, Optional from typing import Dict, Optional
@ -25,7 +22,6 @@ from typing import Dict, Optional
import rclpy import rclpy
from rclpy.node import Node from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor from rclpy.executors import MultiThreadedExecutor
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
# ROS2 Message imports # ROS2 Message imports
import std_msgs.msg import std_msgs.msg
@ -65,7 +61,7 @@ class PublishRateController:
# 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉 # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
# 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段 # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
self.topic_intervals = { self.topic_intervals = {
'position_gnss': 0.0, # GNSS位置 'position': 0.0, # GNSS位置
'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度) 'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度)
'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態) 'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態)
'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除) 'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除)
@ -135,8 +131,8 @@ class VehicleStatusPublisher(Node):
self.fc_publishers: Dict[tuple, any] = {} self.fc_publishers: Dict[tuple, any] = {}
# 定時器：以較高頻率檢查 vehicle_registry 並發布 # 定時器：以較高頻率檢查 vehicle_registry 並發布
# 20Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率 # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
self.timer_period = 0.05 # 50ms self.timer_period = 0.1 # 100ms
self.timer = self.create_timer(self.timer_period, self.timer_callback) self.timer = self.create_timer(self.timer_period, self.timer_callback)
# 狀態標誌 # 狀態標誌
@ -175,7 +171,7 @@ class VehicleStatusPublisher(Node):
# 例如：self._publish_ekf_status(sysid, status) # 例如：self._publish_ekf_status(sysid, status)
# ═══════════════════════════════════════════════════════════════ # ═══════════════════════════════════════════════════════════════
# 發布各種狀態（通過頻率控制器判斷是否發布） # 發布各種狀態（通過頻率控制器判斷是否發布）
self._publish_position_gnss(sysid, status) self._publish_position(sysid, status)
self._publish_position_ned(sysid, status) self._publish_position_ned(sysid, status)
self._publish_attitude(sysid, status) self._publish_attitude(sysid, status)
self._publish_velocity(sysid, status) self._publish_velocity(sysid, status)
@ -206,9 +202,9 @@ class VehicleStatusPublisher(Node):
logger.info(f"Created publisher: {topic_name}") logger.info(f"Created publisher: {topic_name}")
return self.fc_publishers[key] return self.fc_publishers[key]
def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus): def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
"""發布 GPS 位置""" """發布 GPS 位置"""
if not self.rate_controller.should_publish(sysid, 'position_gnss'): if not self.rate_controller.should_publish(sysid, 'position'):
return return
pos = status.position pos = status.position
@ -216,27 +212,22 @@ class VehicleStatusPublisher(Node):
return return
publisher = self._get_or_create_publisher( publisher = self._get_or_create_publisher(
sysid, 'position_gnss', fcmsg.GnssRaw sysid, 'position', sensor_msgs.msg.NavSatFix
) )
# 檢查是否有訂閱者 # 檢查是否有訂閱者
if publisher.get_subscription_count() == 0: if publisher.get_subscription_count() == 0:
return return
msg = fcmsg.GnssRaw() msg = sensor_msgs.msg.NavSatFix()
msg.stamp = self.get_clock().now().to_msg() msg.latitude = pos.latitude
msg.latitude = float(pos.latitude) msg.longitude = pos.longitude
msg.longitude = float(pos.longitude) msg.altitude = pos.altitude if pos.altitude is not None else 0.0
msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
# GPS 狀態資訊 # GPS 狀態資訊
gps = status.gps gps = status.gps
if gps.fix_type is not None: if gps.fix_type is not None:
msg.fix_type = int(gps.fix_type) msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus
if gps.satellites_visible is not None:
msg.satellites_visible = int(gps.satellites_visible)
msg.eph = float(gps.eph) / 100.0 if gps.eph is not None else float('nan')
msg.epv = float(gps.epv) / 100.0 if gps.epv is not None else float('nan')
publisher.publish(msg) publisher.publish(msg)
@ -440,7 +431,7 @@ class VehicleStatusPublisher(Node):
# 'longitude': status.position.longitude if status.position.longitude 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, # 'altitude': status.position.altitude if status.position.altitude else 0.0,
# 'battery_percent': status.battery.remaining if status.battery.remaining else 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, 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
'connection_type': vehicle.connected_via.value, 'connection_type': vehicle.connected_via.value,
'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0, 'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
} }
@ -959,179 +950,6 @@ class MavlinkCommandService(Node):
logger.info("MavlinkCommandService stopped") logger.info("MavlinkCommandService stopped")
# ============================================================================
# RtcmRelay Node
# ============================================================================
class RtcmRelay(Node):
"""
RTCM 轉發節點 - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
職責:
- 訂閱外部 RTCM 資料（mavros_msgs/msg/RTCM）
- 過期丟棄：封包 Header.stamp 超過 1 秒則丟棄
- 去重：與上一筆 payload blake2s hash 相同則丟棄
- 節流：最短轉發間隔保護（可參數化）
- 對所有在線載具直接編碼並送出 MAVLink GPS_RTCM_DATA（含分片）
MAVLink GPS_RTCM_DATA (msg_id=233) 每片 payload 上限 180 bytes
超過 180 bytes 需分片，最多 4 片 = 720 bytes
"""
FRAGMENT_MAX_LEN = 180
MAX_TOTAL_LEN = 4 * 180 # 720 bytes
STALE_THRESHOLD_SEC = 1.0
def __init__(self, min_forward_interval_ms: float = 0.0, clock_offset_ms: float = 0.0):
super().__init__('rtcm_relay')
self.running = True
self.min_forward_interval_sec = min_forward_interval_ms / 1000.0
self.clock_offset_sec = clock_offset_ms / 1000.0
# 去重：上一筆 payload 的短 hash
self._last_hash: Optional[bytes] = None
# 節流：上次成功轉發的時間
self._last_forward_time: float = 0.0
# MAVLink GPS_RTCM_DATA 分片序號 (0~31 循環，同一筆 RTCM 的所有分片共用)
self._seq: int = 0
# 觀測 counters
self.counters = {
'received': 0,
'forwarded': 0,
'dropped_stale': 0,
'dropped_duplicate': 0,
'dropped_throttle': 0,
'dropped_oversize': 0,
'no_route': 0,
'send_fail': 0,
}
# QoS: 低延遲、弱可靠 — 寧可掉舊包也不排隊
rtcm_qos = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=1,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE,
)
self.subscription = self.create_subscription(
mavros_msgs.msg.RTCM,
'/fc_network/rtcm_input',
self._rtcm_callback,
rtcm_qos,
)
logger.info("RtcmRelay initialized")
# ── callback ──────────────────────────────────────────────────────
def _rtcm_callback(self, msg):
"""處理順序：過期檢查 → 去重 → 節流 → 轉發"""
if not self.running:
return
self.counters['received'] += 1
data = bytes(msg.data)
now = time.time()
# 1) 過期丟棄
stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
age = (now + self.clock_offset_sec) - stamp_sec
if age > self.STALE_THRESHOLD_SEC:
self.counters['dropped_stale'] += 1
return
# 2) 去重：blake2s 短 hash 與上一筆比對
h = hashlib.blake2s(data, digest_size=8).digest()
if h == self._last_hash:
self.counters['dropped_duplicate'] += 1
return
self._last_hash = h
# 3) 節流：最短轉發間隔
if self.min_forward_interval_sec > 0:
if (now - self._last_forward_time) < self.min_forward_interval_sec:
self.counters['dropped_throttle'] += 1
return
# 4) 超長丟棄
if len(data) > self.MAX_TOTAL_LEN:
self.counters['dropped_oversize'] += 1
logger.warning(f"RTCM data too large ({len(data)} bytes), max {self.MAX_TOTAL_LEN}")
return
# 5) 取得本次的分片序號，然後遞增給下一筆
seq = self._seq
self._seq = (self._seq + 1) & 0x1F
# 6) 轉發給所有載具
forwarded_any = False
for sysid, vehicle in mvv.vehicle_registry.read_all():
socket_id = vehicle.custom_meta.get("socket_id")
if socket_id is None:
self.counters['no_route'] += 1
continue
mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
if mav_obj is None:
self.counters['no_route'] += 1
continue
try:
self._send_rtcm(mav_obj, data, seq)
forwarded_any = True
except Exception as e:
self.counters['send_fail'] += 1
logger.debug(f"RTCM send fail sysid={sysid}: {e}")
if forwarded_any:
self.counters['forwarded'] += 1
self._last_forward_time = now
# ── MAVLink 編碼與分片 ────────────────────────────────────────────
def _send_rtcm(self, mav_obj, data: bytes, seq: int):
"""
對單一載具送出 GPS_RTCM_DATA 必要時做分片
GPS_RTCM_DATA flags 欄位:
bit 0 : 1=fragmented
bit 1-2 : fragment_id (0~3)
bit 3-7 : sequence number (0~31)
"""
if len(data) <= self.FRAGMENT_MAX_LEN:
flags = (seq & 0x1F) << 3
padded = data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(data))
mav_obj.MAVLink.gps_rtcm_data_send(flags, len(data), padded)
else:
fragments = [
data[i:i + self.FRAGMENT_MAX_LEN]
for i in range(0, len(data), self.FRAGMENT_MAX_LEN)
]
for frag_id, frag_data in enumerate(fragments):
flags = 1 | ((frag_id & 0x03) << 1) | ((seq & 0x1F) << 3)
padded = frag_data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(frag_data))
mav_obj.MAVLink.gps_rtcm_data_send(flags, len(frag_data), padded)
# ── 外部介面 ──────────────────────────────────────────────────────
def set_clock_offset(self, offset_ms: float):
"""更新時鐘偏移量（毫秒），用於與 RTCM 來源的時間對齊"""
self.clock_offset_sec = offset_ms / 1000.0
def get_counters(self) -> dict:
return dict(self.counters)
def stop(self):
"""停止轉發"""
self.running = False
logger.info("RtcmRelay stopped")
# ============================================================================ # ============================================================================
# ROS2 節點管理器 # ROS2 節點管理器
# ============================================================================ # ============================================================================
@ -1145,10 +963,9 @@ class fc_ros_manager:
stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。 stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
shutdown 是完全關閉 ROS2 並銷毀節點實例。 shutdown 是完全關閉 ROS2 並銷毀節點實例。
管理三個獨立的 ROS2 Node： 管理兩個獨立的 ROS2 Node：
- VehicleStatusPublisher - VehicleStatusPublisher
- MavlinkCommandService - MavlinkCommandService
- RtcmRelay
提供統一的啟動/停止介面給 mainOrchestrator 提供統一的啟動/停止介面給 mainOrchestrator
""" """
@ -1157,10 +974,9 @@ class fc_ros_manager:
self.initialized = False self.initialized = False
self.running = False self.running = False
# node 實例 # 两个 node 實例
self.status_publisher: Optional[VehicleStatusPublisher] = None self.status_publisher: Optional[VehicleStatusPublisher] = None
self.command_service: Optional[MavlinkCommandService] = None self.command_service: Optional[MavlinkCommandService] = None
self.rtcm_relay: Optional[RtcmRelay] = None
# Executor & Thread # Executor & Thread
self.spin_thread: Optional[threading.Thread] = None self.spin_thread: Optional[threading.Thread] = None
@ -1180,13 +996,11 @@ class fc_ros_manager:
# 創建節點 node # 創建節點 node
self.status_publisher = VehicleStatusPublisher() self.status_publisher = VehicleStatusPublisher()
self.command_service = MavlinkCommandService() self.command_service = MavlinkCommandService()
self.rtcm_relay = RtcmRelay()
# 創建執行者 MultiThreadedExecutor 並把 node 加入其中 # 創建執行者 MultiThreadedExecutor 並把 node 加入其中
self.executor = MultiThreadedExecutor() self.executor = MultiThreadedExecutor()
self.executor.add_node(self.status_publisher) self.executor.add_node(self.status_publisher)
self.executor.add_node(self.command_service) self.executor.add_node(self.command_service)
self.executor.add_node(self.rtcm_relay)
self.initialized = True self.initialized = True
# logger.info("fc_ros_manager initialized") # logger.info("fc_ros_manager initialized")
@ -1250,8 +1064,6 @@ class fc_ros_manager:
self.status_publisher.stop() self.status_publisher.stop()
if self.command_service: if self.command_service:
self.command_service.stop() self.command_service.stop()
if self.rtcm_relay:
self.rtcm_relay.stop()
# 等待 spin 執行緒結束 # 等待 spin 執行緒結束
if self.spin_thread and self.spin_thread.is_alive(): if self.spin_thread and self.spin_thread.is_alive():
@ -1276,8 +1088,6 @@ class fc_ros_manager:
self.status_publisher.destroy_node() self.status_publisher.destroy_node()
if self.command_service: if self.command_service:
self.command_service.destroy_node() self.command_service.destroy_node()
if self.rtcm_relay:
self.rtcm_relay.destroy_node()
# 關閉 ROS2 # 關閉 ROS2
if rclpy.ok(): if rclpy.ok():
@ -1295,7 +1105,6 @@ class fc_ros_manager:
'running': self.running, 'running': self.running,
'status_publisher_active': self.status_publisher is not None and self.status_publisher.running, 'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
'command_service_active': self.command_service is not None, 'command_service_active': self.command_service is not None,
'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running,
} }
@ -1326,11 +1135,6 @@ ros2_manager = fc_ros_manager()
3. 新增 _publish_position_ned() 發布剛體座標的訊息 3. 新增 _publish_position_ned() 發布剛體座標的訊息
4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息 4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息
2026.05.11
1. 新增 RtcmRelay node - 訂閱 mavros_msgs/RTCM topic 轉發為 MAVLink GPS_RTCM_DATA
2. 實作過期丟棄(1s)、blake2s hash 去重、最短轉發間隔節流、分片(180 bytes/片)
3. 接入 fc_ros_manager 的 initialize/start/stop/shutdown 生命週期
TODO TODO
1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題 2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題

17
src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
Unescape Escape View File

@ -17,7 +17,6 @@ import std_msgs.msg
import sensor_msgs.msg import sensor_msgs.msg
import geometry_msgs.msg import geometry_msgs.msg
import mavros_msgs.msg import mavros_msgs.msg
import fc_interfaces.msg
# 專案 imports # 專案 imports
from ..fc_network_adapter.mavlinkROS2Nodes import ( from ..fc_network_adapter.mavlinkROS2Nodes import (
@ -40,7 +39,7 @@ class TestSubscriber(Node):
self.sysid = sysid self.sysid = sysid
self.received_messages = { self.received_messages = {
'position_gnss': [], 'position': [],
'attitude': [], 'attitude': [],
'velocity': [], 'velocity': [],
'battery': [], 'battery': [],
@ -61,9 +60,9 @@ class TestSubscriber(Node):
# Position # Position
self.create_subscription( self.create_subscription(
fc_interfaces.msg.GnssRaw, sensor_msgs.msg.NavSatFix,
f'{base_topic}/position_gnss', f'{base_topic}/position',
lambda msg: self._on_message('position_gnss', msg), lambda msg: self._on_message('position', msg),
10 10
) )
@ -122,7 +121,7 @@ class TestSubscriber(Node):
def _format_msg(self, topic_name: str, msg) -> str: def _format_msg(self, topic_name: str, msg) -> str:
"""格式化消息以便顯示""" """格式化消息以便顯示"""
if topic_name == 'position_gnss': if topic_name == 'position':
return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m" return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
elif topic_name == 'attitude': elif topic_name == 'attitude':
return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})" return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
@ -265,7 +264,7 @@ def test_basic_publishing():
# 檢查收到的消息 # 檢查收到的消息
print("\n--- 消息統計 ---") print("\n--- 消息統計 ---")
total_messages = 0 total_messages = 0
for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']: for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
count = test_sub.get_message_count(topic) count = test_sub.get_message_count(topic)
total_messages += count total_messages += count
print(f" {topic:15s}: {count:3d} 條消息") print(f" {topic:15s}: {count:3d} 條消息")
@ -305,7 +304,7 @@ def test_frequency_control():
# 修改頻率設定 # 修改頻率設定
publisher_node = ros2_manager.status_publisher publisher_node = ros2_manager.status_publisher
publisher_node.rate_controller.topic_intervals = { publisher_node.rate_controller.topic_intervals = {
'position_gnss': 1.5, 'position': 1.5,
'attitude': 1.0, 'attitude': 1.0,
'velocity': 1.0, 'velocity': 1.0,
'battery': 1.0, 'battery': 1.0,
@ -330,7 +329,7 @@ def test_frequency_control():
print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布") print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
print("2Hz Topics (預期 ~6 條):") print("2Hz Topics (預期 ~6 條):")
for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']: for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
count = test_sub.get_message_count(topic) count = test_sub.get_message_count(topic)
print(f" {topic:15s}: {count:3d} 條") print(f" {topic:15s}: {count:3d} 條")
for topic in ['battery', 'mode', 'summary']: for topic in ['battery', 'mode', 'summary']:

0
src/fc_network_module/fc_network_module/__init__.py
Unescape Escape View File

146
src/fc_network_module/fc_network_module/ntrip_client.py
Unescape Escape View File

@ -1,146 +0,0 @@
import socket
import base64
import threading
import time
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
from mavros_msgs.msg import RTCM
class NtripClientNode(Node):
"""連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。"""
RECONNECT_BASE_SEC = 2.0
RECONNECT_MAX_SEC = 60.0
def __init__(self):
super().__init__('ntrip_client')
self.declare_parameter('host', 'rtk2go.com')
self.declare_parameter('port', 2101)
self.declare_parameter('mountpoint', '')
self.declare_parameter('username', '')
self.declare_parameter('password', '')
rtcm_qos = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=1,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE,
)
self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
self._running = True
self._thread = threading.Thread(target=self._receive_loop, daemon=True)
self._thread.start()
self.get_logger().info('NtripClientNode 已啟動')
# ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
def _receive_loop(self):
backoff = self.RECONNECT_BASE_SEC
while self._running:
host = self.get_parameter('host').value
port = self.get_parameter('port').value
mount = self.get_parameter('mountpoint').value
user = self.get_parameter('username').value
pwd = self.get_parameter('password').value
if not mount:
self.get_logger().error('mountpoint 參數未設定，等待重試...')
time.sleep(backoff)
continue
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.settimeout(10)
try:
self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...')
sock.connect((host, port))
auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
request = (
f'GET /{mount} HTTP/1.0\r\n'
f'User-Agent: NTRIP PythonClient\r\n'
f'Authorization: Basic {auth}\r\n'
f'Connection: close\r\n\r\n'
)
sock.sendall(request.encode())
resp = sock.recv(4096)
if b'ICY 200 OK' not in resp:
self.get_logger().error(
f'NTRIP 握手失敗: {resp.decode(errors="ignore").strip()}'
)
raise ConnectionError('handshake failed')
self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流')
backoff = self.RECONNECT_BASE_SEC
self._read_stream(sock)
except Exception as e:
if self._running:
self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
time.sleep(backoff)
backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
finally:
sock.close()
def _read_stream(self, sock: socket.socket):
buf = b''
while self._running:
try:
chunk = sock.recv(4096)
except socket.timeout:
continue
if not chunk:
break
buf += chunk
while len(buf) >= 6:
if buf[0] != 0xD3:
buf = buf[1:]
continue
payload_len = ((buf[1] & 0x03) << 8) | buf[2]
frame_len = payload_len + 6 # header(3) + payload + CRC(3)
if len(buf) < frame_len:
break
packet = buf[:frame_len]
buf = buf[frame_len:]
self._publish_rtcm(packet)
def _publish_rtcm(self, raw_packet: bytes):
msg = RTCM()
msg.header.stamp = self.get_clock().now().to_msg()
msg.data = list(raw_packet)
self.publisher_.publish(msg)
def destroy_node(self):
self._running = False
self._thread.join(timeout=3.0)
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = NtripClientNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

22
src/fc_network_module/package.xml
Unescape Escape View File

@ -1,22 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>fc_network_module</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="chiyu1468@hotmail.com">picars</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<depend>mavros_msgs</depend>
<depend>std_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
src/fc_network_module/resource/fc_network_module
Unescape Escape View File

4
src/fc_network_module/setup.cfg
Unescape Escape View File

@ -1,4 +0,0 @@
[develop]
script_dir=$base/lib/fc_network_module
[install]
install_scripts=$base/lib/fc_network_module

26
src/fc_network_module/setup.py
Unescape Escape View File

@ -1,26 +0,0 @@
from setuptools import find_packages, setup
package_name = 'fc_network_module'
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': [
'ntrip_client = fc_network_module.ntrip_client:main',
],
},
)

25
src/fc_network_module/test/test_copyright.py
Unescape Escape View File

@ -1,25 +0,0 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'

25
src/fc_network_module/test/test_flake8.py
Unescape Escape View File

@ -1,25 +0,0 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)

23
src/fc_network_module/test/test_pep257.py
Unescape Escape View File

@ -1,23 +0,0 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'

14
src/unitdev02/unitdev02/devnote.txt
Unescape Escape View File

@ -7,17 +7,9 @@
- 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複)
這一步 這一步
希望 ackEnum 可以擺到正確的位置 研究 ros2 service
node 增加 GNSS 訊息 (2d fix 3d fix rtk float...)
RTK
跟 RTK2go 抓取列表
從特定 mount point 得到數據
做一個 ros2 service(action?) 接到數據並包裝
下一步 下一步
NODE 重啟
可以 refresh node topic
下下一步 下下一步
@ -26,8 +18,6 @@
rssi 資訊提取s rssi 資訊提取s
mavlink timeout - CommLONG 這個是 mavlink bus 沒有回應 可能是丟包了
自己的常用指令 自己的常用指令
python -m fc_network_adapter.tests.test_vehicleStatusPublisher python -m fc_network_adapter.tests.test_vehicleStatusPublisher
@ -37,8 +27,6 @@ python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
python -m someotherpkg.src.example_takeoff_land python -m someotherpkg.src.example_takeoff_land
python -m someotherpkg.src.example_change_mode python -m someotherpkg.src.example_change_mode
python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpoint:=No1bio_02 -p username:=chiyu1468@hotmail.com
ros2 topic list ros2 topic list
ros2 topic echo /fc_network/vehicle/sys3/battery ros2 topic echo /fc_network/vehicle/sys3/battery

Write Preview
Loading…
Cancel
Save