|
|
from rclpy.node import Node
|
|
|
from PyQt6.QtCore import QObject, pyqtSignal
|
|
|
import math
|
|
|
import re
|
|
|
import threading
|
|
|
from threading import Lock
|
|
|
from concurrent.futures import ThreadPoolExecutor
|
|
|
import asyncio
|
|
|
import websockets
|
|
|
import json
|
|
|
import socket
|
|
|
import sys
|
|
|
import os
|
|
|
import time
|
|
|
import traceback
|
|
|
try:
|
|
|
import serial
|
|
|
except ImportError:
|
|
|
serial = None
|
|
|
from pymavlink import mavutil
|
|
|
from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
|
|
|
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
|
|
|
from std_msgs.msg import Float64, String
|
|
|
from mavros_msgs.msg import State, VfrHud
|
|
|
from nav_msgs.msg import Odometry
|
|
|
from mavros_msgs.srv import CommandBool, CommandTOL
|
|
|
|
|
|
|
|
|
def _log(level, message):
|
|
|
print(f"[{level}] {message}")
|
|
|
|
|
|
|
|
|
def build_xbee_json_command(system_id, command, position=None):
|
|
|
"""Build the shared newline-free XBee command used by Serial and WebSocket."""
|
|
|
if isinstance(system_id, bool) or not isinstance(system_id, int):
|
|
|
raise ValueError("XBee system_id 必須為整數")
|
|
|
if not isinstance(command, str):
|
|
|
raise ValueError("XBee 指令必須為大寫字串")
|
|
|
if command not in {'START', 'STOP', 'GOTO'}:
|
|
|
raise ValueError(f"XBee 指令必須為大寫 START、STOP 或 GOTO: {command}")
|
|
|
|
|
|
payload_data = {
|
|
|
's': system_id,
|
|
|
'c': command,
|
|
|
}
|
|
|
if command == 'GOTO':
|
|
|
if not isinstance(position, (list, tuple)) or len(position) < 2:
|
|
|
raise ValueError("GOTO 缺少有效的經緯度")
|
|
|
payload_data['p'] = [float(position[0]), float(position[1])]
|
|
|
|
|
|
return json.dumps(
|
|
|
payload_data, ensure_ascii=False, separators=(',', ':'))
|
|
|
|
|
|
|
|
|
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
|
|
|
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
|
|
|
if _src_path not in sys.path:
|
|
|
sys.path.insert(0, _src_path)
|
|
|
|
|
|
# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API）
|
|
|
try:
|
|
|
from fc_network_apps.longCommand import CommandLongClient
|
|
|
except ImportError as e:
|
|
|
import traceback
|
|
|
_log("WARN", "無法導入 CommandLongClient")
|
|
|
_log("ERROR", f"錯誤: {e}")
|
|
|
_log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整")
|
|
|
traceback.print_exc()
|
|
|
CommandLongClient = None
|
|
|
|
|
|
# 導入 fc_network_apps 的 navigation（PositionTargetGlobalInt / Offboard goto）
|
|
|
try:
|
|
|
from fc_network_apps.navigation import PositionTargetGlobalIntClient
|
|
|
except ImportError as e:
|
|
|
import traceback
|
|
|
_log("WARN", "無法導入 PositionTargetGlobalIntClient")
|
|
|
_log("ERROR", f"錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
PositionTargetGlobalIntClient = None
|
|
|
|
|
|
try:
|
|
|
from fc_interfaces.msg import AttitudeRaw
|
|
|
except ImportError as e:
|
|
|
_log("WARN", "無法導入 AttitudeRaw")
|
|
|
_log("ERROR", f"錯誤: {e}")
|
|
|
AttitudeRaw = None
|
|
|
|
|
|
try:
|
|
|
from fc_interfaces.msg import GnssRaw
|
|
|
except ImportError as e:
|
|
|
_log("WARN", "無法導入 GnssRaw")
|
|
|
_log("ERROR", f"錯誤: {e}")
|
|
|
GnssRaw = None
|
|
|
|
|
|
try:
|
|
|
from fc_interfaces.msg import SystemDiagnosticsRaw
|
|
|
except ImportError as e:
|
|
|
_log("WARN", f"SystemDiagnosticsRaw 尚不可用，略過 sys_diags 訂閱: {e}")
|
|
|
SystemDiagnosticsRaw = None
|
|
|
|
|
|
try:
|
|
|
from fc_interfaces.msg import FcNetworkLog
|
|
|
except ImportError as e:
|
|
|
_log("WARN", f"FcNetworkLog 尚不可用，略過 FC Network 紀錄訂閱: {e}")
|
|
|
FcNetworkLog = None
|
|
|
|
|
|
class DroneSignals(QObject):
|
|
|
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 and WebSocket JSON also accept compact UAV status shapes:
|
|
|
{"s": 1, "m": "GUIDED", "b": 85, "p": [24.0, 120.0], "y": 90.0}
|
|
|
or the full shape:
|
|
|
{"s": 1, "m": "GUIDED", "a": 1, "b": 85, "h": 10.0,
|
|
|
"v": 4.2, "p": [24.0, 120.0], "ypr": [90.0, 0.0, 0.0],
|
|
|
"g": 3, "d": [0.8, 1.2]}
|
|
|
"""
|
|
|
|
|
|
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
|
|
|
|
|
|
# XBee command acknowledgement does not carry a UAV system id.
|
|
|
# Route it to the GUI log/status area instead of silently dropping it.
|
|
|
if all(key in data for key in ('result', 'command', 'message')):
|
|
|
self.signals.update_signal.emit(
|
|
|
'xbee_ack',
|
|
|
self.connection_name,
|
|
|
{
|
|
|
'result': data.get('result'),
|
|
|
'command': data.get('command'),
|
|
|
'message': data.get('message'),
|
|
|
}
|
|
|
)
|
|
|
return
|
|
|
|
|
|
system_id = data.get('system_id', data.get('sysid', data.get('s')))
|
|
|
if system_id is None:
|
|
|
return
|
|
|
|
|
|
drone_id = f"s{self.socket_id}_{system_id}"
|
|
|
self._emit_json_connection_type(drone_id)
|
|
|
|
|
|
# Compact XBee/UAV packets are complete snapshots. Emit every field
|
|
|
# group, using None for omitted values, so the GUI can show '-'
|
|
|
# instead of inventing zero-valued telemetry or retaining stale data.
|
|
|
if 's' in data and 'c' not in data:
|
|
|
self._process_compact_uav_telemetry(data, drone_id)
|
|
|
return
|
|
|
|
|
|
mode = data.get('mode', data.get('mode_name', data.get('m')))
|
|
|
state = {}
|
|
|
if mode is not None:
|
|
|
state['mode'] = mode
|
|
|
if 'armed' in data:
|
|
|
state['armed'] = data.get('armed')
|
|
|
elif 'a' in data:
|
|
|
state['armed'] = bool(data.get('a'))
|
|
|
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)
|
|
|
elif 'b' in data:
|
|
|
self.signals.update_signal.emit('battery', drone_id, {
|
|
|
'percentage': data.get('b')
|
|
|
})
|
|
|
|
|
|
pos = data.get('position')
|
|
|
if isinstance(pos, dict):
|
|
|
gps_data = {
|
|
|
'lat': pos.get('lat', pos.get('latitude')),
|
|
|
'lon': pos.get('lon', pos.get('longitude')),
|
|
|
'alt': pos.get('alt', pos.get('altitude'))
|
|
|
}
|
|
|
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')),
|
|
|
'lon': data.get('lon', data.get('longitude')),
|
|
|
'alt': data.get('alt', data.get('altitude'))
|
|
|
})
|
|
|
elif isinstance(data.get('p'), (list, tuple)) and len(data.get('p')) >= 2:
|
|
|
dop = data.get('d') if isinstance(data.get('d'), (list, tuple)) else []
|
|
|
gps_data = {
|
|
|
'lat': data['p'][0],
|
|
|
'lon': data['p'][1],
|
|
|
'alt': data.get('h')
|
|
|
}
|
|
|
if 'g' in data:
|
|
|
gps_data['fix_type'] = data.get('g')
|
|
|
if len(dop) >= 1:
|
|
|
gps_data['eph'] = dop[0]
|
|
|
gps_data['hdop'] = dop[0]
|
|
|
if len(dop) >= 2:
|
|
|
gps_data['epv'] = dop[1]
|
|
|
gps_data['vdop'] = dop[1]
|
|
|
self.signals.update_signal.emit('gps', drone_id, gps_data)
|
|
|
|
|
|
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
|
|
|
})
|
|
|
elif 'h' in data:
|
|
|
height = data.get('h', 0.0)
|
|
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
|
|
'x': 0.0,
|
|
|
'y': 0.0,
|
|
|
'z': height
|
|
|
})
|
|
|
self.signals.update_signal.emit('altitude', drone_id, {
|
|
|
'altitude': height
|
|
|
})
|
|
|
elif (
|
|
|
isinstance(data.get('p'), (list, tuple))
|
|
|
or 'lat' in data
|
|
|
or 'latitude' in data
|
|
|
):
|
|
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
|
|
'x': 0.0,
|
|
|
'y': 0.0,
|
|
|
'z': 0.0
|
|
|
})
|
|
|
self.signals.update_signal.emit('altitude', drone_id, {
|
|
|
'altitude': 0.0
|
|
|
})
|
|
|
|
|
|
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))
|
|
|
})
|
|
|
elif 'v' in data:
|
|
|
self.signals.update_signal.emit('velocity', drone_id, {
|
|
|
'vx': data.get('v', 0.0),
|
|
|
'vy': 0.0,
|
|
|
'vz': 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))
|
|
|
})
|
|
|
elif isinstance(data.get('ypr'), (list, tuple)) and len(data.get('ypr')) >= 3:
|
|
|
yaw, pitch, roll = data['ypr'][0], data['ypr'][1], data['ypr'][2]
|
|
|
self.signals.update_signal.emit('attitude', drone_id, {
|
|
|
'roll': roll,
|
|
|
'pitch': pitch,
|
|
|
'yaw': yaw,
|
|
|
'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')
|
|
|
elif 'y' in data:
|
|
|
hud['heading'] = data.get('y')
|
|
|
elif isinstance(data.get('ypr'), (list, tuple)) and len(data.get('ypr')) >= 1:
|
|
|
hud['heading'] = data['ypr'][0]
|
|
|
if 'v' in data and 'groundspeed' not in hud:
|
|
|
hud['groundspeed'] = data.get('v')
|
|
|
if 'h' in data and 'alt' not in hud and 'altitude' not in hud:
|
|
|
hud['alt'] = data.get('h')
|
|
|
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}")
|
|
|
|
|
|
def _process_compact_uav_telemetry(self, data, drone_id):
|
|
|
"""Convert one compact XBee UAV snapshot into existing GUI events."""
|
|
|
snapshot = {'_snapshot': True}
|
|
|
|
|
|
self.signals.update_signal.emit('state', drone_id, {
|
|
|
**snapshot,
|
|
|
'mode': data.get('m'),
|
|
|
'armed': bool(data.get('a')) if data.get('a') is not None else None,
|
|
|
})
|
|
|
self.signals.update_signal.emit('battery', drone_id, {
|
|
|
**snapshot,
|
|
|
'percentage': data.get('b'),
|
|
|
'voltage': None,
|
|
|
})
|
|
|
|
|
|
position = data.get('p')
|
|
|
has_position = isinstance(position, (list, tuple)) and len(position) >= 2
|
|
|
dop = data.get('d')
|
|
|
has_dop = isinstance(dop, (list, tuple))
|
|
|
ypr = data.get('ypr')
|
|
|
has_ypr = isinstance(ypr, (list, tuple)) and len(ypr) >= 3
|
|
|
has_yaw = data.get('y') is not None
|
|
|
# In the legacy compact shape, a lone 'h' is heading. In the newer
|
|
|
# y/ypr shapes it is height, matching the original ground-station parser.
|
|
|
height = data.get('h') if has_ypr or has_yaw else None
|
|
|
yaw = ypr[0] if has_ypr else data.get('y', data.get('h'))
|
|
|
pitch = ypr[1] if has_ypr else None
|
|
|
roll = ypr[2] if has_ypr else None
|
|
|
|
|
|
self.signals.update_signal.emit('gps', drone_id, {
|
|
|
**snapshot,
|
|
|
'lat': position[0] if has_position else None,
|
|
|
'lon': position[1] if has_position else None,
|
|
|
'alt': height,
|
|
|
'fix_type': data.get('g'),
|
|
|
'eph': dop[0] if has_dop and len(dop) >= 1 else None,
|
|
|
'epv': dop[1] if has_dop and len(dop) >= 2 else None,
|
|
|
'satellites_visible': None,
|
|
|
})
|
|
|
|
|
|
self.signals.update_signal.emit('altitude', drone_id, {
|
|
|
**snapshot,
|
|
|
'altitude': height,
|
|
|
})
|
|
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
|
|
**snapshot,
|
|
|
'x': None,
|
|
|
'y': None,
|
|
|
'z': height,
|
|
|
})
|
|
|
|
|
|
# Compact 'v' is scalar ground speed, not an XY velocity vector.
|
|
|
self.signals.update_signal.emit('velocity', drone_id, {
|
|
|
**snapshot,
|
|
|
'vx': None,
|
|
|
'vy': None,
|
|
|
'vz': None,
|
|
|
})
|
|
|
|
|
|
self.signals.update_signal.emit('attitude', drone_id, {
|
|
|
**snapshot,
|
|
|
'roll': roll,
|
|
|
'pitch': pitch,
|
|
|
'yaw': yaw,
|
|
|
'rates': None,
|
|
|
})
|
|
|
self.signals.update_signal.emit('hud', drone_id, {
|
|
|
**snapshot,
|
|
|
'heading': yaw,
|
|
|
'groundspeed': data.get('v'),
|
|
|
'airspeed': None,
|
|
|
'throttle': None,
|
|
|
'alt': height,
|
|
|
'climb': None,
|
|
|
})
|
|
|
|
|
|
class UDPMavlinkReceiver(threading.Thread):
|
|
|
"""UDP MAVLink 接收器"""
|
|
|
def __init__(self, ip, port, signals, connection_name, monitor=None):
|
|
|
super().__init__(daemon=True)
|
|
|
self.ip = ip
|
|
|
self.port = port
|
|
|
self.signals = signals
|
|
|
self.connection_name = connection_name
|
|
|
self.monitor = monitor # 保存 monitor 引用
|
|
|
self.socket_id = monitor.get_next_socket_id() if monitor else 0
|
|
|
self._socket_id_released = False
|
|
|
self.running = False
|
|
|
self.sock = None
|
|
|
|
|
|
def run(self):
|
|
|
"""執行 UDP 接收循環"""
|
|
|
self.running = True
|
|
|
try:
|
|
|
print(f"UDP MAVLink receiver started on {self.ip}:{self.port}")
|
|
|
|
|
|
# 創建 MAVLink 連接
|
|
|
mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}')
|
|
|
while self.running:
|
|
|
try:
|
|
|
msg = mav.recv_match(blocking=True, timeout=1.0)
|
|
|
if msg is None:
|
|
|
continue
|
|
|
|
|
|
self.process_mavlink_message(msg)
|
|
|
|
|
|
except socket.timeout:
|
|
|
continue
|
|
|
except Exception as e:
|
|
|
print(f"Error receiving MAVLink message: {e}")
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"UDP receiver error: {e}")
|
|
|
finally:
|
|
|
if self.sock:
|
|
|
self.sock.close()
|
|
|
self._release_socket_id()
|
|
|
|
|
|
def process_mavlink_message(self, msg):
|
|
|
"""處理 MAVLink 訊息"""
|
|
|
try:
|
|
|
msg_type = msg.get_type()
|
|
|
system_id = msg.get_srcSystem()
|
|
|
drone_id = f"s{self.socket_id}_{system_id}"
|
|
|
|
|
|
if msg_type == "HEARTBEAT":
|
|
|
# 先發送連接類型資訊
|
|
|
self.signals.update_signal.emit('connection_type', drone_id, {
|
|
|
'type': 'UDP'
|
|
|
})
|
|
|
mode = mavutil.mode_string_v10(msg)
|
|
|
armed = bool(msg.base_mode & 128)
|
|
|
self.signals.update_signal.emit('state', drone_id, {
|
|
|
'mode': mode,
|
|
|
'armed': armed
|
|
|
})
|
|
|
|
|
|
elif msg_type == "BATTERY_STATUS":
|
|
|
voltage = msg.voltages[0] / 1000
|
|
|
self.signals.update_signal.emit('battery', drone_id, {
|
|
|
'voltage': voltage
|
|
|
})
|
|
|
|
|
|
elif msg_type == "GLOBAL_POSITION_INT":
|
|
|
latitude = msg.lat / 1e7
|
|
|
longitude = msg.lon / 1e7
|
|
|
relative_alt = msg.relative_alt / 1000.0
|
|
|
self.signals.update_signal.emit('gps', drone_id, {
|
|
|
'lat': latitude,
|
|
|
'lon': longitude,
|
|
|
'alt': relative_alt,
|
|
|
})
|
|
|
|
|
|
elif msg_type == "GPS_RAW_INT":
|
|
|
fix_type = msg.fix_type
|
|
|
|
|
|
elif msg_type == "LOCAL_POSITION_NED":
|
|
|
x = msg.y
|
|
|
y = msg.x
|
|
|
z = -msg.z
|
|
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
|
|
'x': x,
|
|
|
'y': y,
|
|
|
'z': z
|
|
|
})
|
|
|
self.signals.update_signal.emit('altitude', drone_id, {
|
|
|
'altitude': z
|
|
|
})
|
|
|
self.signals.update_signal.emit('velocity', drone_id, {
|
|
|
'vx': msg.vx,
|
|
|
'vy': msg.vy,
|
|
|
'vz': msg.vz
|
|
|
})
|
|
|
|
|
|
elif msg_type == "ATTITUDE":
|
|
|
# 從 MAVLink 訊息中提取並轉為角度
|
|
|
pitch = math.degrees(msg.pitch)
|
|
|
roll = math.degrees(msg.roll)
|
|
|
yaw = math.degrees(msg.yaw)
|
|
|
self.signals.update_signal.emit('attitude', drone_id, {
|
|
|
'pitch': pitch,
|
|
|
'roll': roll,
|
|
|
'yaw': yaw,
|
|
|
'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
|
|
|
})
|
|
|
|
|
|
elif msg_type == "VFR_HUD":
|
|
|
self.signals.update_signal.emit('hud', drone_id, {
|
|
|
'airspeed': msg.airspeed,
|
|
|
'groundspeed': msg.groundspeed,
|
|
|
'heading': msg.heading,
|
|
|
'throttle': msg.throttle,
|
|
|
'alt': msg.alt,
|
|
|
'climb': msg.climb
|
|
|
})
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"Error processing MAVLink message: {e}")
|
|
|
|
|
|
def stop(self):
|
|
|
"""停止接收器"""
|
|
|
self.running = False
|
|
|
self._release_socket_id()
|
|
|
|
|
|
def _release_socket_id(self):
|
|
|
if self.monitor and not self._socket_id_released:
|
|
|
self.monitor.release_socket_id(self.socket_id)
|
|
|
self._socket_id_released = True
|
|
|
|
|
|
class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
|
|
|
"""串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。"""
|
|
|
def __init__(self, port, baudrate, signals, connection_name, monitor=None):
|
|
|
super().__init__(daemon=True)
|
|
|
self.port = port
|
|
|
self.baudrate = baudrate
|
|
|
self.signals = signals
|
|
|
self.connection_name = connection_name
|
|
|
self.monitor = monitor # 保存 monitor 引用
|
|
|
self.socket_id = monitor.get_next_socket_id() if monitor else 0
|
|
|
self._socket_id_released = False
|
|
|
self.source_type = 'Serial'
|
|
|
self.running = False
|
|
|
self.serial_conn = None
|
|
|
self.serial_write_lock = Lock()
|
|
|
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):
|
|
|
"""執行串口接收循環。"""
|
|
|
self.running = True
|
|
|
try:
|
|
|
print(f"Serial receiver started on {self.port} at {self.baudrate} baud (MAVLink/JSON auto detect)")
|
|
|
if serial is None:
|
|
|
raise RuntimeError("pyserial 未安裝，無法啟動 Serial 連線")
|
|
|
|
|
|
self.serial_conn = serial.Serial(
|
|
|
self.port,
|
|
|
self.baudrate,
|
|
|
timeout=0.2
|
|
|
)
|
|
|
|
|
|
while self.running:
|
|
|
try:
|
|
|
chunk = self.serial_conn.read(256)
|
|
|
if not chunk:
|
|
|
continue
|
|
|
|
|
|
for raw_byte in chunk:
|
|
|
byte = bytes([raw_byte])
|
|
|
self._process_json_byte(byte)
|
|
|
self._process_mavlink_byte(byte)
|
|
|
|
|
|
except Exception as e:
|
|
|
if self.running:
|
|
|
print(f"Error receiving serial telemetry: {e}")
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"Serial receiver error: {e}")
|
|
|
finally:
|
|
|
if self.serial_conn:
|
|
|
try:
|
|
|
with self.serial_write_lock:
|
|
|
self.serial_conn.close()
|
|
|
except Exception:
|
|
|
pass
|
|
|
self._release_socket_id()
|
|
|
|
|
|
def send_json_command(self, system_id, command, position=None):
|
|
|
"""Send one newline-delimited XBee JSON command on this serial link."""
|
|
|
payload = build_xbee_json_command(system_id, command, position) + '\n'
|
|
|
with self.serial_write_lock:
|
|
|
if not self.running or self.serial_conn is None:
|
|
|
raise RuntimeError("Serial 連線尚未啟動")
|
|
|
if hasattr(self.serial_conn, 'is_open') and not self.serial_conn.is_open:
|
|
|
raise RuntimeError("Serial 連線已關閉")
|
|
|
self.serial_conn.write(payload.encode('utf-8'))
|
|
|
self.serial_conn.flush()
|
|
|
return payload.strip()
|
|
|
|
|
|
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):
|
|
|
"""處理 MAVLink 訊息"""
|
|
|
try:
|
|
|
msg_type = msg.get_type()
|
|
|
system_id = msg.get_srcSystem()
|
|
|
drone_id = f"s{self.socket_id}_{system_id}"
|
|
|
|
|
|
if msg_type == "HEARTBEAT":
|
|
|
# 先發送連接類型資訊
|
|
|
self.signals.update_signal.emit('connection_type', drone_id, {
|
|
|
'type': 'Serial'
|
|
|
})
|
|
|
mode = mavutil.mode_string_v10(msg)
|
|
|
armed = bool(msg.base_mode & 128)
|
|
|
self.signals.update_signal.emit('state', drone_id, {
|
|
|
'mode': mode,
|
|
|
'armed': armed
|
|
|
})
|
|
|
|
|
|
elif msg_type == "BATTERY_STATUS":
|
|
|
voltage = msg.voltages[0] / 1000
|
|
|
self.signals.update_signal.emit('battery', drone_id, {
|
|
|
'voltage': voltage
|
|
|
})
|
|
|
|
|
|
elif msg_type == "GLOBAL_POSITION_INT":
|
|
|
latitude = msg.lat / 1e7
|
|
|
longitude = msg.lon / 1e7
|
|
|
relative_alt = msg.relative_alt / 1000.0
|
|
|
self.signals.update_signal.emit('gps', drone_id, {
|
|
|
'lat': latitude,
|
|
|
'lon': longitude,
|
|
|
'alt': relative_alt,
|
|
|
})
|
|
|
|
|
|
elif msg_type == "GPS_RAW_INT":
|
|
|
fix_type = msg.fix_type
|
|
|
|
|
|
elif msg_type == "LOCAL_POSITION_NED":
|
|
|
x = msg.y
|
|
|
y = msg.x
|
|
|
z = -msg.z
|
|
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
|
|
'x': x,
|
|
|
'y': y,
|
|
|
'z': z
|
|
|
})
|
|
|
self.signals.update_signal.emit('altitude', drone_id, {
|
|
|
'altitude': z
|
|
|
})
|
|
|
self.signals.update_signal.emit('velocity', drone_id, {
|
|
|
'vx': msg.vx,
|
|
|
'vy': msg.vy,
|
|
|
'vz': msg.vz
|
|
|
})
|
|
|
|
|
|
elif msg_type == "ATTITUDE":
|
|
|
# 從 MAVLink 訊息中提取並轉為角度
|
|
|
pitch = math.degrees(msg.pitch)
|
|
|
roll = math.degrees(msg.roll)
|
|
|
yaw = math.degrees(msg.yaw)
|
|
|
self.signals.update_signal.emit('attitude', drone_id, {
|
|
|
'pitch': pitch,
|
|
|
'roll': roll,
|
|
|
'yaw': yaw,
|
|
|
'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed)
|
|
|
})
|
|
|
|
|
|
elif msg_type == "VFR_HUD":
|
|
|
self.signals.update_signal.emit('hud', drone_id, {
|
|
|
'airspeed': msg.airspeed,
|
|
|
'groundspeed': msg.groundspeed,
|
|
|
'heading': msg.heading,
|
|
|
'throttle': msg.throttle,
|
|
|
'alt': msg.alt,
|
|
|
'climb': msg.climb
|
|
|
})
|
|
|
|
|
|
except Exception as e:
|
|
|
print(f"Error processing MAVLink message from serial: {e}")
|
|
|
|
|
|
def stop(self):
|
|
|
"""停止接收器"""
|
|
|
self.running = False
|
|
|
if self.serial_conn:
|
|
|
try:
|
|
|
with self.serial_write_lock:
|
|
|
self.serial_conn.close()
|
|
|
except Exception:
|
|
|
pass
|
|
|
self._release_socket_id()
|
|
|
|
|
|
def _release_socket_id(self):
|
|
|
if self.monitor and not self._socket_id_released:
|
|
|
self.monitor.release_socket_id(self.socket_id)
|
|
|
self._socket_id_released = True
|
|
|
|
|
|
class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
|
|
|
"""WebSocket MAVLink 接收器"""
|
|
|
def __init__(self, url, signals, connection_name, monitor=None):
|
|
|
super().__init__(daemon=True)
|
|
|
self.url = url
|
|
|
self.signals = signals
|
|
|
self.connection_name = connection_name
|
|
|
self.monitor = monitor # 保存 monitor 引用
|
|
|
self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id
|
|
|
self._socket_id_released = False
|
|
|
self.source_type = 'WS'
|
|
|
self.running = False
|
|
|
self.max_retries = 5
|
|
|
self.base_delay = 1.0
|
|
|
self.loop = None
|
|
|
self.websocket = None
|
|
|
self.websocket_lock = Lock()
|
|
|
|
|
|
def run(self):
|
|
|
"""執行 WebSocket 接收循環"""
|
|
|
self.running = True
|
|
|
self.loop = asyncio.new_event_loop()
|
|
|
asyncio.set_event_loop(self.loop)
|
|
|
try:
|
|
|
self.loop.run_until_complete(self.ws_client_loop())
|
|
|
finally:
|
|
|
self.loop.close()
|
|
|
|
|
|
async def ws_client_loop(self):
|
|
|
"""WebSocket 連接的主循環"""
|
|
|
retry_count = 0
|
|
|
|
|
|
print(f"Starting WebSocket client for {self.connection_name} at {self.url}")
|
|
|
|
|
|
while self.running and retry_count < self.max_retries:
|
|
|
try:
|
|
|
async with websockets.connect(self.url) as websocket:
|
|
|
with self.websocket_lock:
|
|
|
self.websocket = websocket
|
|
|
print(f"WebSocket {self.connection_name} connected to {self.url}")
|
|
|
retry_count = 0 # 重置重試計數
|
|
|
try:
|
|
|
async for message in websocket:
|
|
|
if not self.running:
|
|
|
break
|
|
|
|
|
|
try:
|
|
|
data = json.loads(message)
|
|
|
if isinstance(data, dict):
|
|
|
data['_connection_source'] = self.connection_name
|
|
|
self.process_websocket_message(data)
|
|
|
except json.JSONDecodeError as e:
|
|
|
print(f"WebSocket {self.connection_name} JSON decode error: {e}")
|
|
|
except Exception as e:
|
|
|
print(f"WebSocket {self.connection_name} message processing error: {e}")
|
|
|
finally:
|
|
|
with self.websocket_lock:
|
|
|
if self.websocket is websocket:
|
|
|
self.websocket = None
|
|
|
|
|
|
except websockets.exceptions.ConnectionClosedError:
|
|
|
print(f"WebSocket {self.connection_name} connection closed")
|
|
|
if self.running:
|
|
|
retry_count += 1
|
|
|
if retry_count < self.max_retries:
|
|
|
delay = self.base_delay * (2 ** min(retry_count, 4))
|
|
|
print(f"Reconnecting in {delay}s...")
|
|
|
await asyncio.sleep(delay)
|
|
|
else:
|
|
|
break
|
|
|
except Exception as e:
|
|
|
retry_count += 1
|
|
|
if retry_count < self.max_retries and self.running:
|
|
|
delay = self.base_delay * (2 ** min(retry_count, 4))
|
|
|
print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})")
|
|
|
await asyncio.sleep(delay)
|
|
|
else:
|
|
|
break
|
|
|
|
|
|
print(f"WebSocket client {self.connection_name} stopped")
|
|
|
self._release_socket_id()
|
|
|
|
|
|
def send_json_command(self, system_id, command, position=None):
|
|
|
"""Schedule an XBee JSON command on this receiver's WebSocket loop."""
|
|
|
payload = build_xbee_json_command(system_id, command, position)
|
|
|
with self.websocket_lock:
|
|
|
loop = self.loop
|
|
|
websocket = self.websocket
|
|
|
|
|
|
if not self.running or loop is None or not loop.is_running():
|
|
|
raise RuntimeError("WebSocket 連線尚未啟動")
|
|
|
if websocket is None:
|
|
|
raise RuntimeError("WebSocket 尚未連線")
|
|
|
|
|
|
async def _send_on_current_connection():
|
|
|
with self.websocket_lock:
|
|
|
if self.websocket is not websocket:
|
|
|
raise RuntimeError("WebSocket 連線已變更")
|
|
|
await websocket.send(payload)
|
|
|
|
|
|
future = asyncio.run_coroutine_threadsafe(
|
|
|
_send_on_current_connection(), loop)
|
|
|
|
|
|
def _report_send_error(done_future):
|
|
|
try:
|
|
|
done_future.result()
|
|
|
except Exception as e:
|
|
|
print(
|
|
|
f"WebSocket {self.connection_name} command send error: {e}")
|
|
|
|
|
|
future.add_done_callback(_report_send_error)
|
|
|
return payload
|
|
|
|
|
|
def process_websocket_message(self, data):
|
|
|
"""處理 WebSocket 訊息"""
|
|
|
self.process_json_telemetry_message(data)
|
|
|
|
|
|
def stop(self):
|
|
|
"""停止接收器"""
|
|
|
self.running = False
|
|
|
with self.websocket_lock:
|
|
|
loop = self.loop
|
|
|
websocket = self.websocket
|
|
|
if loop is not None and loop.is_running() and websocket is not None:
|
|
|
asyncio.run_coroutine_threadsafe(websocket.close(), loop)
|
|
|
self._release_socket_id()
|
|
|
|
|
|
def _release_socket_id(self):
|
|
|
if self.monitor and not self._socket_id_released:
|
|
|
self.monitor.release_socket_id(self.socket_id)
|
|
|
self._socket_id_released = True
|
|
|
|
|
|
class DroneMonitor(Node):
|
|
|
# Subscribe to drone ROS2 topics
|
|
|
_instance = None # Singleton pattern to prevent duplicate nodes
|
|
|
|
|
|
def __init__(self):
|
|
|
# Use a unique node name with timestamp to avoid conflicts on restart
|
|
|
node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
|
|
|
super().__init__(node_name)
|
|
|
self.signals = DroneSignals()
|
|
|
self.drone_topics = {}
|
|
|
self.lock = Lock()
|
|
|
|
|
|
self.arm_clients = {}
|
|
|
self.takeoff_clients = {}
|
|
|
self.setpoint_pubs = {}
|
|
|
self.selected_drones = set()
|
|
|
self.latest_data = {}
|
|
|
|
|
|
# 定義需要過濾的模式
|
|
|
self.filtered_modes = ['Mode(0x000000c0)']
|
|
|
|
|
|
# WebSocket 接收器列表
|
|
|
self.ws_receivers = []
|
|
|
|
|
|
# 串口接收器列表
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】儲存 GPS 資料的字典
|
|
|
# ================================================================================
|
|
|
self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
|
|
|
# GnssRaw.altitude 是 AMSL；交戰/航點指令的 alt 則是相對 Home 高度。
|
|
|
self.drone_relative_alt = {}
|
|
|
self.drone_relative_alt_updated_at = {}
|
|
|
# ================================================================================
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】Socket ID 重新分配機制 (從 0 開始)
|
|
|
# ================================================================================
|
|
|
self.socket_id_mapping = {} # {原始socket_id: 重新分配的socket_id}
|
|
|
self.active_socket_ids = set() # 目前通訊連線使用中的 socket_id
|
|
|
self.socket_id_lock = Lock() # 線程安全鎖
|
|
|
# ================================================================================
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取)
|
|
|
# ================================================================================
|
|
|
self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'}
|
|
|
self.sys_to_socket_id = {} # {sys_id: assigned_socket_id} e.g. {'sys11': 0}
|
|
|
# ================================================================================
|
|
|
self.serial_receivers = []
|
|
|
|
|
|
# ================================================================================
|
|
|
# 共用 command / position client（各一個節點，靠 request 的 target_sysid 路由）
|
|
|
# ================================================================================
|
|
|
# 兩個 client 都只打到單一 service（send_command_long / pos_global_int），
|
|
|
# 沒有任何 per-drone 狀態，故不需要 per-drone 節點。改成「啟動時各預建一個、
|
|
|
# 執行期只重用」，避免：
|
|
|
# (1) 切 mode / 首次 goto 時於執行期新建 ROS2 node = 新 DDS participant，
|
|
|
# 其 discovery handshake 撞到壞掉的外來 participant → FastRTPS bad_alloc → OOM。
|
|
|
# (2) 從 asyncio/Qt 執行緒對「正在被 _ros_spin_thread spin 的 executor」
|
|
|
# 跨執行緒 add_node → wait set 損毀 → CPU 空轉 100%。
|
|
|
self.command_long_client = None # CommandLongClient（共用）
|
|
|
self.position_target_client = None # PositionTargetGlobalIntClient（共用）
|
|
|
self.client_lock = Lock() # 保護共用 client 的建立
|
|
|
self.executor = None # 將在 gui.py 中設置，用於 add_node
|
|
|
# ================================================================================
|
|
|
|
|
|
# 主题检测定时器
|
|
|
self.create_timer(1.0, self.scan_topics)
|
|
|
|
|
|
# FC Network 使用專用結構化紀錄 topic，不再解析 /rosout。
|
|
|
self.fc_network_log_sub = None
|
|
|
if FcNetworkLog is not None:
|
|
|
self.fc_network_log_sub = self.create_subscription(
|
|
|
FcNetworkLog,
|
|
|
'/fc_network/logs',
|
|
|
self.fc_network_log_callback,
|
|
|
200
|
|
|
)
|
|
|
|
|
|
def get_drone_gps_snapshot(self):
|
|
|
"""回傳執行緒安全的定位快照。"""
|
|
|
with self.lock:
|
|
|
return {
|
|
|
drone_id: dict(position)
|
|
|
for drone_id, position in self.drone_gps.items()
|
|
|
if isinstance(position, dict)
|
|
|
}
|
|
|
|
|
|
def record_relative_gps(self, drone_id, gps_data, updated_at=None):
|
|
|
"""記錄已知以 Home 為基準的 GPS/高度（UDP、Serial、WebSocket）。"""
|
|
|
now = time.monotonic() if updated_at is None else float(updated_at)
|
|
|
stored = dict(gps_data)
|
|
|
stored['_gps_updated_at'] = now
|
|
|
alt = stored.get('alt')
|
|
|
if isinstance(alt, (int, float)) and math.isfinite(float(alt)):
|
|
|
stored['alt'] = float(alt)
|
|
|
stored['_relative_alt_updated_at'] = now
|
|
|
stored['_altitude_reference'] = 'relative_home'
|
|
|
else:
|
|
|
stored['alt'] = None
|
|
|
|
|
|
with self.lock:
|
|
|
if stored.get('_altitude_reference') == 'relative_home':
|
|
|
self.drone_relative_alt[drone_id] = stored['alt']
|
|
|
self.drone_relative_alt_updated_at[drone_id] = now
|
|
|
self.drone_gps[drone_id] = stored
|
|
|
return stored
|
|
|
|
|
|
def get_next_socket_id(self):
|
|
|
"""取得目前最小的未使用 socket_id（從 0 開始）。"""
|
|
|
with self.socket_id_lock:
|
|
|
socket_id = 0
|
|
|
while socket_id in self.active_socket_ids:
|
|
|
socket_id += 1
|
|
|
self.active_socket_ids.add(socket_id)
|
|
|
return socket_id
|
|
|
|
|
|
def release_socket_id(self, socket_id):
|
|
|
"""釋放通訊連線使用的 socket_id，讓後續連線可重用最小空缺。"""
|
|
|
try:
|
|
|
socket_id = int(socket_id)
|
|
|
except (TypeError, ValueError):
|
|
|
return
|
|
|
|
|
|
with self.socket_id_lock:
|
|
|
if socket_id in getattr(self, 'socket_id_mapping', {}).values():
|
|
|
return
|
|
|
if socket_id in getattr(self, 'sys_to_socket_id', {}).values():
|
|
|
return
|
|
|
self.active_socket_ids.discard(socket_id)
|
|
|
|
|
|
def get_or_assign_socket_id(self, original_socket_id):
|
|
|
"""ROS2 socket_id 映射。
|
|
|
|
|
|
有原始 socket_id=N 時優先使用 N；若 N 已被其他通訊占用，
|
|
|
才改用目前最小未使用 ID。同一個原始 socket_id 會得到同一個映射。
|
|
|
"""
|
|
|
original_socket_id = str(original_socket_id)
|
|
|
|
|
|
with self.socket_id_lock:
|
|
|
if original_socket_id not in self.socket_id_mapping:
|
|
|
try:
|
|
|
preferred_socket_id = int(original_socket_id)
|
|
|
except (TypeError, ValueError):
|
|
|
preferred_socket_id = None
|
|
|
|
|
|
if (
|
|
|
preferred_socket_id is not None
|
|
|
and preferred_socket_id >= 0
|
|
|
and preferred_socket_id not in self.active_socket_ids
|
|
|
):
|
|
|
socket_id = preferred_socket_id
|
|
|
else:
|
|
|
socket_id = 0
|
|
|
while socket_id in self.active_socket_ids:
|
|
|
socket_id += 1
|
|
|
self.active_socket_ids.add(socket_id)
|
|
|
self.socket_id_mapping[original_socket_id] = socket_id
|
|
|
|
|
|
return self.socket_id_mapping[original_socket_id]
|
|
|
|
|
|
def init_shared_command_clients(self):
|
|
|
"""啟動時預建共用的 command / position client（各一個節點）。
|
|
|
|
|
|
必須在 _ros_spin_thread 啟動「之前」於主執行緒呼叫：此時 executor 已建立
|
|
|
但尚未被 spin，add_node 不會與 spin_once 跨執行緒競爭。之後執行期送指令
|
|
|
一律重用這兩個節點，不再於飛行中新建 DDS participant。"""
|
|
|
with self.client_lock:
|
|
|
if self.command_long_client is None and CommandLongClient is not None:
|
|
|
try:
|
|
|
self.command_long_client = CommandLongClient(node_name="cmd_long_client_shared")
|
|
|
if self.executor:
|
|
|
self.executor.add_node(self.command_long_client)
|
|
|
_log("INFO", "已預建共用 CommandLongClient (node=cmd_long_client_shared)")
|
|
|
except Exception as e:
|
|
|
_log("WARN", f"預建 CommandLongClient 失敗: {e}")
|
|
|
if self.position_target_client is None and PositionTargetGlobalIntClient is not None:
|
|
|
try:
|
|
|
self.position_target_client = PositionTargetGlobalIntClient(node_name="pos_target_client_shared")
|
|
|
if self.executor:
|
|
|
self.executor.add_node(self.position_target_client)
|
|
|
_log("INFO", "已預建共用 PositionTargetGlobalIntClient (node=pos_target_client_shared)")
|
|
|
except Exception as e:
|
|
|
_log("WARN", f"預建 PositionTargetGlobalIntClient 失敗: {e}")
|
|
|
|
|
|
def get_or_create_client(self, drone_id=None):
|
|
|
"""回傳共用的 CommandLongClient（單一節點，靠 request 的 target_sysid 路由）。
|
|
|
|
|
|
正常情況已於啟動時由 init_shared_command_clients() 預建；此處僅保留一次性
|
|
|
lazy fallback（例如 init 尚未被呼叫），不會在執行期重複新建 participant。
|
|
|
drone_id 參數保留以相容既有呼叫端，實際不影響路由（路由在 request 內）。"""
|
|
|
if self.command_long_client is not None:
|
|
|
return self.command_long_client
|
|
|
with self.client_lock:
|
|
|
if self.command_long_client is None and CommandLongClient is not None:
|
|
|
try:
|
|
|
self.command_long_client = CommandLongClient(node_name="cmd_long_client_shared")
|
|
|
if self.executor:
|
|
|
self.executor.add_node(self.command_long_client)
|
|
|
_log("INFO", "已建立共用 CommandLongClient (lazy fallback)")
|
|
|
except Exception as e:
|
|
|
_log("WARN", f"無法建立共用 CommandLongClient: {e}")
|
|
|
return None
|
|
|
return self.command_long_client
|
|
|
|
|
|
def get_or_create_position_client(self, drone_id=None):
|
|
|
"""回傳共用的 PositionTargetGlobalIntClient（單一節點，靠 target_sysid 路由）。
|
|
|
|
|
|
同 get_or_create_client：正常已於啟動預建，此處僅一次性 lazy fallback。"""
|
|
|
if self.position_target_client is not None:
|
|
|
return self.position_target_client
|
|
|
if PositionTargetGlobalIntClient is None:
|
|
|
return None
|
|
|
with self.client_lock:
|
|
|
if self.position_target_client is None:
|
|
|
try:
|
|
|
self.position_target_client = PositionTargetGlobalIntClient(node_name="pos_target_client_shared")
|
|
|
if self.executor:
|
|
|
self.executor.add_node(self.position_target_client)
|
|
|
_log("INFO", "已建立共用 PositionTargetGlobalIntClient (lazy fallback)")
|
|
|
except Exception as e:
|
|
|
_log("WARN", f"無法建立共用 PositionTargetGlobalIntClient: {e}")
|
|
|
return None
|
|
|
return self.position_target_client
|
|
|
|
|
|
def scan_topics(self):
|
|
|
topics = self.get_topic_names_and_types()
|
|
|
drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
|
|
|
|
|
|
found_drones = set()
|
|
|
for topic_name, _ in topics:
|
|
|
if match := drone_pattern.match(topic_name):
|
|
|
sys_id, topic_type = match.groups()
|
|
|
found_drones.add(sys_id)
|
|
|
with self.lock:
|
|
|
self.drone_topics.setdefault(sys_id, set()).add(topic_type)
|
|
|
|
|
|
for sys_id in found_drones:
|
|
|
# 为每个 sys_id 分配 socket_id（如果还没有分配）
|
|
|
# 注意：如果后续 summary 提供了 socket_id，会使用 summary 的映射覆盖
|
|
|
if sys_id not in self.sys_to_socket_id:
|
|
|
self.sys_to_socket_id[sys_id] = self.get_next_socket_id()
|
|
|
|
|
|
subs_attr = f'drone_{sys_id}_subs'
|
|
|
if not hasattr(self, subs_attr):
|
|
|
self.setup_drone(sys_id)
|
|
|
else:
|
|
|
# 檢查既有訂閱是否包含 position_gnss / position_ned / attitude，如果不包含就添加（兼容舊訂閱）
|
|
|
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:
|
|
|
base_topic = f'/fc_network/vehicle/{sys_id}'
|
|
|
try:
|
|
|
pos_ned_sub = self.create_subscription(
|
|
|
Odometry,
|
|
|
f'{base_topic}/position_ned',
|
|
|
lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
subs['position_ned'] = pos_ned_sub
|
|
|
setattr(self, subs_attr, subs) # 明確保存更新後的字典
|
|
|
except Exception as e:
|
|
|
pass
|
|
|
if isinstance(subs, dict) and 'attitude' not in subs and AttitudeRaw is not None:
|
|
|
base_topic = f'/fc_network/vehicle/{sys_id}'
|
|
|
try:
|
|
|
attitude_sub = self.create_subscription(
|
|
|
AttitudeRaw,
|
|
|
f'{base_topic}/attitude',
|
|
|
lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
subs['attitude'] = attitude_sub
|
|
|
setattr(self, subs_attr, subs)
|
|
|
except Exception:
|
|
|
pass
|
|
|
if isinstance(subs, dict) and 'sys_diags' not in subs and SystemDiagnosticsRaw is not None:
|
|
|
base_topic = f'/fc_network/vehicle/{sys_id}'
|
|
|
try:
|
|
|
sys_diags_sub = self.create_subscription(
|
|
|
SystemDiagnosticsRaw,
|
|
|
f'{base_topic}/sys_diags',
|
|
|
lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
subs['sys_diags'] = sys_diags_sub
|
|
|
setattr(self, subs_attr, subs)
|
|
|
except Exception:
|
|
|
pass
|
|
|
if isinstance(subs, dict) and 'status_text' not in subs:
|
|
|
base_topic = f'/fc_network/vehicle/{sys_id}'
|
|
|
try:
|
|
|
status_text_sub = self.create_subscription(
|
|
|
String,
|
|
|
f'{base_topic}/status_text',
|
|
|
lambda msg, sid=sys_id: self.status_text_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
subs['status_text'] = status_text_sub
|
|
|
setattr(self, subs_attr, subs)
|
|
|
except Exception:
|
|
|
pass
|
|
|
|
|
|
def setup_drone(self, sys_id):
|
|
|
# sys_id 格式： sys11, sys12, ...
|
|
|
base_topic = f'/fc_network/vehicle/{sys_id}'
|
|
|
|
|
|
# Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務)
|
|
|
self.arm_clients[sys_id] = self.create_client(
|
|
|
CommandBool,
|
|
|
f'{base_topic}/cmd/arming'
|
|
|
)
|
|
|
self.takeoff_clients[sys_id] = self.create_client(
|
|
|
CommandTOL,
|
|
|
f'{base_topic}/cmd/takeoff'
|
|
|
)
|
|
|
|
|
|
# Add setpoint publisher
|
|
|
self.setpoint_pubs[sys_id] = self.create_publisher(
|
|
|
Point,
|
|
|
f'{base_topic}/setpoint_position/local',
|
|
|
10
|
|
|
)
|
|
|
|
|
|
subs = {
|
|
|
'battery': self.create_subscription(
|
|
|
BatteryState,
|
|
|
f'{base_topic}/battery',
|
|
|
lambda msg, sid=sys_id: self.battery_callback(sid, msg),
|
|
|
10
|
|
|
),
|
|
|
'summary': self.create_subscription(
|
|
|
String,
|
|
|
f'{base_topic}/summary',
|
|
|
lambda msg, sid=sys_id: self.summary_callback(sid, msg),
|
|
|
10
|
|
|
),
|
|
|
'vfr_hud': self.create_subscription(
|
|
|
VfrHud,
|
|
|
f'{base_topic}/vfr_hud',
|
|
|
lambda msg, sid=sys_id: self.hud_callback(sid, msg),
|
|
|
10
|
|
|
),
|
|
|
'position_ned': self.create_subscription(
|
|
|
Odometry,
|
|
|
f'{base_topic}/position_ned',
|
|
|
lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
}
|
|
|
|
|
|
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:
|
|
|
subs['attitude'] = self.create_subscription(
|
|
|
AttitudeRaw,
|
|
|
f'{base_topic}/attitude',
|
|
|
lambda msg, sid=sys_id: self.attitude_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
if SystemDiagnosticsRaw is not None:
|
|
|
subs['sys_diags'] = self.create_subscription(
|
|
|
SystemDiagnosticsRaw,
|
|
|
f'{base_topic}/sys_diags',
|
|
|
lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
subs['status_text'] = self.create_subscription(
|
|
|
String,
|
|
|
f'{base_topic}/status_text',
|
|
|
lambda msg, sid=sys_id: self.status_text_callback(sid, msg),
|
|
|
10
|
|
|
)
|
|
|
|
|
|
setattr(self, f'drone_{sys_id}_subs', subs)
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式）
|
|
|
# ================================================================================
|
|
|
MODE_MAPPING = {
|
|
|
"STABILIZE": 0,
|
|
|
"ACRO": 1,
|
|
|
"ALT_HOLD": 2,
|
|
|
"AUTO": 3,
|
|
|
"GUIDED": 4,
|
|
|
"LOITER": 5,
|
|
|
"RTL": 6,
|
|
|
"CIRCLE": 7,
|
|
|
"POSITION": 8,
|
|
|
"LAND": 9,
|
|
|
"OF_LOITER": 10,
|
|
|
"DRIFT": 11,
|
|
|
"SPORT": 13,
|
|
|
"FLIP": 14,
|
|
|
"AUTOTUNE": 15,
|
|
|
"POSHOLD": 16,
|
|
|
"BRAKE": 17,
|
|
|
"THROW": 18,
|
|
|
"AVOID_ADSB": 19,
|
|
|
"GUIDED_NOGPS": 20,
|
|
|
"SMART_RTL": 21,
|
|
|
}
|
|
|
# ================================================================================
|
|
|
|
|
|
async def set_mode(self, drone_id, mode_name):
|
|
|
"""使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
|
|
|
try:
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
parts = drone_id.split('_')
|
|
|
if len(parts) < 2:
|
|
|
_log("ERROR", f"[SET_MODE] 無效的 drone_id 格式: {drone_id}")
|
|
|
return False
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
# 獲取模式對應的 custom_mode 值
|
|
|
custom_mode = self.MODE_MAPPING.get(mode_name)
|
|
|
if custom_mode is None:
|
|
|
_log("ERROR", f"[SET_MODE] 未知模式: {mode_name}")
|
|
|
return False
|
|
|
|
|
|
_log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})")
|
|
|
|
|
|
# 取得共用 command_long client（OOM 修復後已非 per-drone，靠 request 的
|
|
|
# target_sysid 路由；drone_id 僅用於一次性 lazy fallback，無 per-drone 狀態）
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
if not client:
|
|
|
_log("ERROR", "[SET_MODE] CommandLongClient 無法初始化")
|
|
|
return False
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
result = await client.change_mode_async(
|
|
|
target_sysid=sysid,
|
|
|
custom_mode=float(custom_mode),
|
|
|
target_compid=0,
|
|
|
base_mode=1.0,
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
)
|
|
|
|
|
|
if result and result.success:
|
|
|
_log("INFO", f"[SET_MODE] {drone_id} 模式切換成功")
|
|
|
return True
|
|
|
else:
|
|
|
_log("ERROR", f"[SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
|
|
|
return False
|
|
|
except Exception as e:
|
|
|
_log("ERROR", f"[SET_MODE] 例外錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
return False
|
|
|
|
|
|
async def set_speed(self, drone_id, speed_mps, speed_type=1):
|
|
|
"""對指定無人機下 DO_CHANGE_SPEED(178) 設定/限制最大速度（非阻塞 async）。
|
|
|
|
|
|
speed_type: 0=Airspeed, 1=Ground Speed（Copter 用 1）, 2=Climb, 3=Descent。
|
|
|
speed_mps: 目標速度 (m/s)。DO_CHANGE_SPEED 常數定義在 scope 外的 longCommand.py，
|
|
|
故此處用共用 client 的泛用方法 _send_command_long_async 直接發 command=178，
|
|
|
不修改 fc_network_apps（維持 GUI-only scope）。"""
|
|
|
try:
|
|
|
parts = drone_id.split('_')
|
|
|
if len(parts) < 2:
|
|
|
_log("ERROR", f"[SET_SPEED] 無效的 drone_id 格式: {drone_id}")
|
|
|
return False
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
if not client:
|
|
|
_log("ERROR", "[SET_SPEED] CommandLongClient 無法初始化")
|
|
|
return False
|
|
|
|
|
|
_log("INFO", f"[SET_SPEED] {drone_id} -> {speed_mps} m/s (type={speed_type})")
|
|
|
result = await client._send_command_long_async(
|
|
|
target_sysid=sysid,
|
|
|
target_compid=0,
|
|
|
command=178, # MAV_CMD_DO_CHANGE_SPEED
|
|
|
confirmation=0,
|
|
|
param1=float(speed_type),
|
|
|
param2=float(speed_mps),
|
|
|
param3=-1.0, # throttle 不變
|
|
|
param4=0.0, # 0=absolute
|
|
|
param5=0.0,
|
|
|
param6=0.0,
|
|
|
param7=0.0,
|
|
|
timeout_sec=5.0,
|
|
|
)
|
|
|
if result and result.success:
|
|
|
_log("INFO", f"[SET_SPEED] {drone_id} 速度設定成功")
|
|
|
return True
|
|
|
_log("ERROR", f"[SET_SPEED] 速度設定失敗 (message={result.message if result else 'None'})")
|
|
|
return False
|
|
|
except Exception as e:
|
|
|
_log("ERROR", f"[SET_SPEED] 例外錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
return False
|
|
|
|
|
|
async def arm_drone(self, drone_id, arm):
|
|
|
"""使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
|
|
|
try:
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
parts = drone_id.split('_')
|
|
|
if len(parts) < 2:
|
|
|
_log("ERROR", f"[ARM] 無效的 drone_id 格式: {drone_id}")
|
|
|
return False
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
action_name = "解鎖" if arm else "上鎖"
|
|
|
_log("INFO", f"[ARM] {drone_id} -> {action_name}")
|
|
|
|
|
|
# 取得共用 command_long client（OOM 修復後已非 per-drone，靠 request 的
|
|
|
# target_sysid 路由；drone_id 僅用於一次性 lazy fallback，無 per-drone 狀態）
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
if not client:
|
|
|
_log("ERROR", "[ARM] CommandLongClient 無法初始化")
|
|
|
return False
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
result = await client.arm_disarm_async(
|
|
|
target_sysid=sysid,
|
|
|
arm=arm,
|
|
|
target_compid=0,
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
)
|
|
|
|
|
|
if result and result.success:
|
|
|
_log("INFO", f"[ARM] {drone_id} {action_name}成功")
|
|
|
return True
|
|
|
else:
|
|
|
_log("ERROR", f"[ARM] {drone_id} {action_name}失敗 (message={result.message if result else 'None'})")
|
|
|
return False
|
|
|
except Exception as e:
|
|
|
_log("ERROR", f"[ARM] 例外錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
return False
|
|
|
|
|
|
async def takeoff_drone(self, drone_id, altitude):
|
|
|
"""使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
|
|
|
try:
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
parts = drone_id.split('_')
|
|
|
if len(parts) < 2:
|
|
|
_log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}")
|
|
|
return False
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
_log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)")
|
|
|
|
|
|
# 取得共用 command_long client（OOM 修復後已非 per-drone，靠 request 的
|
|
|
# target_sysid 路由；drone_id 僅用於一次性 lazy fallback，無 per-drone 狀態）
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
if not client:
|
|
|
_log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化")
|
|
|
return False
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
result = await client.takeoff_async(
|
|
|
target_sysid=sysid,
|
|
|
altitude_m=float(altitude),
|
|
|
target_compid=0,
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
)
|
|
|
|
|
|
if result and result.success:
|
|
|
_log("INFO", f"[TAKEOFF] {drone_id} 起飛成功")
|
|
|
return True
|
|
|
else:
|
|
|
_log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
|
|
|
return False
|
|
|
except Exception as e:
|
|
|
_log("ERROR", f"[TAKEOFF] 例外錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
return False
|
|
|
|
|
|
async def reboot_drone(self, drone_id):
|
|
|
"""使用 CommandLongClient 執行飛控重啟。"""
|
|
|
try:
|
|
|
parts = drone_id.split('_')
|
|
|
if len(parts) < 2:
|
|
|
_log("ERROR", f"[REBOOT] 無效的 drone_id 格式: {drone_id}")
|
|
|
return False
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
_log("INFO", f"[REBOOT] {drone_id} -> 飛控重啟")
|
|
|
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
if not client:
|
|
|
_log("ERROR", "[REBOOT] CommandLongClient 無法初始化")
|
|
|
return False
|
|
|
|
|
|
result = await client.reboot_autopilot_async(
|
|
|
target_sysid=sysid,
|
|
|
target_compid=0,
|
|
|
timeout_sec=5.0,
|
|
|
)
|
|
|
|
|
|
if result and result.success:
|
|
|
_log("INFO", f"[REBOOT] {drone_id} 重啟命令已送出")
|
|
|
return True
|
|
|
|
|
|
_log("ERROR", f"[REBOOT] 重啟失敗 (message={result.message if result else 'None'})")
|
|
|
return False
|
|
|
except Exception as e:
|
|
|
_log("ERROR", f"[REBOOT] 例外錯誤: {e}")
|
|
|
traceback.print_exc()
|
|
|
return False
|
|
|
|
|
|
def send_setpoint(self, drone_id, x, y, z):
|
|
|
"""Send setpoint position command"""
|
|
|
if drone_id not in self.setpoint_pubs:
|
|
|
return False
|
|
|
|
|
|
msg = Point()
|
|
|
msg.x = float(x)
|
|
|
msg.y = float(y)
|
|
|
msg.z = float(z)
|
|
|
|
|
|
self.setpoint_pubs[drone_id].publish(msg)
|
|
|
return True
|
|
|
|
|
|
def quaternion_to_euler(self, q):
|
|
|
sinr_cosp = 2 * (q.w * q.x + q.y * q.z)
|
|
|
cosr_cosp = 1 - 2 * (q.x**2 + q.y**2)
|
|
|
roll = math.atan2(sinr_cosp, cosr_cosp)
|
|
|
|
|
|
sinp = 2 * (q.w * q.y - q.z * q.x)
|
|
|
pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp)
|
|
|
|
|
|
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
|
|
|
cosy_cosp = 1 - 2 * (q.y**2 + q.z**2)
|
|
|
yaw = math.atan2(siny_cosp, cosy_cosp)
|
|
|
|
|
|
return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
|
|
|
|
|
|
# callbacks
|
|
|
def attitude_callback(self, sys_id, msg):
|
|
|
"""處理姿態 topic，支援 AttitudeRaw 與 IMU 四元數格式。"""
|
|
|
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
|
|
|
if actual_drone_id is None:
|
|
|
return
|
|
|
|
|
|
try:
|
|
|
if hasattr(msg, 'roll') and hasattr(msg, 'pitch') and hasattr(msg, 'yaw'):
|
|
|
data = {
|
|
|
'roll': math.degrees(msg.roll),
|
|
|
'pitch': math.degrees(msg.pitch),
|
|
|
'yaw': math.degrees(msg.yaw),
|
|
|
'rates': (
|
|
|
getattr(msg, 'rollspeed', 0.0),
|
|
|
getattr(msg, 'pitchspeed', 0.0),
|
|
|
getattr(msg, 'yawspeed', 0.0),
|
|
|
)
|
|
|
}
|
|
|
elif hasattr(msg, 'orientation'):
|
|
|
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
|
|
|
data = {
|
|
|
'roll': roll,
|
|
|
'pitch': pitch,
|
|
|
'yaw': yaw,
|
|
|
'rates': (
|
|
|
msg.angular_velocity.x,
|
|
|
msg.angular_velocity.y,
|
|
|
msg.angular_velocity.z,
|
|
|
)
|
|
|
}
|
|
|
else:
|
|
|
return
|
|
|
|
|
|
self.latest_data[(actual_drone_id, 'attitude')] = data
|
|
|
except Exception as e:
|
|
|
print(f"Error parsing attitude msg for {sys_id}: {e}")
|
|
|
|
|
|
def battery_callback(self, sys_id, msg):
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
|
|
|
# 如果還沒有從 summary 獲取到映射，則不處理
|
|
|
if actual_drone_id is None:
|
|
|
return
|
|
|
|
|
|
self.latest_data[(actual_drone_id, 'battery')] = {
|
|
|
'voltage': msg.voltage
|
|
|
}
|
|
|
|
|
|
def sys_diags_callback(self, sys_id, msg):
|
|
|
"""轉送 /fc_network/vehicle/sysN/sys_diags 到 GUI 紀錄區。"""
|
|
|
stamp = getattr(msg, 'stamp', None)
|
|
|
data = {
|
|
|
'stamp': {
|
|
|
'sec': getattr(stamp, 'sec', 0),
|
|
|
'nanosec': getattr(stamp, 'nanosec', 0),
|
|
|
},
|
|
|
'sensors_install_mask': int(msg.sensors_install_mask),
|
|
|
'sensors_enabled_mask': int(msg.sensors_enabled_mask),
|
|
|
'sensors_health_mask': int(msg.sensors_health_mask),
|
|
|
'mcu_load': int(msg.mcu_load),
|
|
|
'mcu_load_percent': float(msg.mcu_load) / 10.0,
|
|
|
'bus_error_rate': int(msg.bus_error_rate),
|
|
|
'bus_error_rate_percent': float(msg.bus_error_rate) / 10.0,
|
|
|
'bus_error_count': int(msg.bus_error_count),
|
|
|
'errors_count1': int(msg.errors_count1),
|
|
|
'errors_count2': int(msg.errors_count2),
|
|
|
'errors_count3': int(msg.errors_count3),
|
|
|
'errors_count4': int(msg.errors_count4),
|
|
|
}
|
|
|
self.signals.update_signal.emit('sys_diags', sys_id, data)
|
|
|
|
|
|
def status_text_callback(self, sys_id, msg):
|
|
|
"""轉送飛控 STATUSTEXT（飛行檢查、警告與錯誤）。"""
|
|
|
raw = msg.data.strip()
|
|
|
if not raw:
|
|
|
return
|
|
|
|
|
|
match = re.match(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$', raw)
|
|
|
if match:
|
|
|
vehicle_timestamp = float(match.group(1))
|
|
|
severity = int(match.group(2))
|
|
|
text = match.group(3)
|
|
|
else:
|
|
|
vehicle_timestamp = None
|
|
|
severity = -1
|
|
|
text = raw
|
|
|
|
|
|
self.signals.update_signal.emit('status_text', sys_id, {
|
|
|
'severity': severity,
|
|
|
'text': text,
|
|
|
'vehicle_timestamp': vehicle_timestamp,
|
|
|
'raw': raw,
|
|
|
})
|
|
|
|
|
|
def fc_network_log_callback(self, msg):
|
|
|
"""接收 /fc_network/logs 的結構化紀錄。"""
|
|
|
stamp = getattr(msg, 'stamp', None)
|
|
|
self.signals.update_signal.emit('fc_network_log', msg.source or 'fc_network', {
|
|
|
'stamp': {
|
|
|
'sec': getattr(stamp, 'sec', 0),
|
|
|
'nanosec': getattr(stamp, 'nanosec', 0),
|
|
|
},
|
|
|
'level': int(msg.level),
|
|
|
'source': msg.source,
|
|
|
'event_code': msg.event_code,
|
|
|
'sysid': int(msg.sysid),
|
|
|
'message': msg.message,
|
|
|
})
|
|
|
|
|
|
def state_callback(self, drone_id, msg):
|
|
|
mode = msg.mode
|
|
|
if mode in self.filtered_modes:
|
|
|
return
|
|
|
self.latest_data[(drone_id, 'state')] = {
|
|
|
'mode': msg.mode,
|
|
|
'armed': msg.armed
|
|
|
}
|
|
|
|
|
|
def summary_callback(self, sys_id, msg):
|
|
|
"""處理 summary topic (JSON 格式)"""
|
|
|
try:
|
|
|
data = json.loads(msg.data)
|
|
|
mode = data.get('mode_name', 'UNKNOWN')
|
|
|
if mode in self.filtered_modes:
|
|
|
return
|
|
|
|
|
|
# 從 summary 獲取原始 socket_id，並映射到分配的 socket_id
|
|
|
original_socket_id = data.get('socket_id')
|
|
|
if original_socket_id is not None:
|
|
|
fallback_socket_id = self.sys_to_socket_id.pop(sys_id, None)
|
|
|
if fallback_socket_id is not None:
|
|
|
self.release_socket_id(fallback_socket_id)
|
|
|
# 使用原始 socket_id 獲取或分配統一的 socket_id
|
|
|
assigned_socket_id = self.get_or_assign_socket_id(original_socket_id)
|
|
|
self.sys_to_socket_id[sys_id] = assigned_socket_id
|
|
|
else:
|
|
|
# 如果沒有 socket_id，使用 sys_to_socket_id 映射
|
|
|
assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0)
|
|
|
|
|
|
sysid = data.get('sysid')
|
|
|
if sysid is not None:
|
|
|
actual_drone_id = f's{assigned_socket_id}_{sysid}'
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【關鍵】保存 sys_id 到 actual_drone_id 的映射
|
|
|
# ================================================================================
|
|
|
self.sys_to_actual_id[sys_id] = actual_drone_id
|
|
|
# ================================================================================
|
|
|
else:
|
|
|
# 如果沒有 sysid，使用 sys_id 中的數字
|
|
|
sys_num = sys_id.replace('sys', '')
|
|
|
actual_drone_id = f's{assigned_socket_id}_{sys_num}'
|
|
|
self.sys_to_actual_id[sys_id] = actual_drone_id
|
|
|
_log("INFO", f"summary_callback 已建立映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
|
|
|
|
|
|
# 先發送連接類型資訊
|
|
|
self.signals.update_signal.emit('connection_type', actual_drone_id, {
|
|
|
'type': 'ROS2'
|
|
|
})
|
|
|
|
|
|
self.latest_data[(actual_drone_id, 'state')] = {
|
|
|
'mode': mode,
|
|
|
'armed': data.get('armed', False),
|
|
|
'socket_id': original_socket_id,
|
|
|
'sysid': sysid,
|
|
|
'vehicle_type': data.get('vehicle_type'),
|
|
|
'autopilot': data.get('autopilot'),
|
|
|
'gps_fix': data.get('gps_fix'),
|
|
|
'gps_fix_type': data.get('gps_fix'),
|
|
|
'connected': data.get('connected')
|
|
|
}
|
|
|
except json.JSONDecodeError as e:
|
|
|
print(f"Error parsing summary JSON for {sys_id}: {e}")
|
|
|
except Exception as e:
|
|
|
print(f"Error in summary_callback for {sys_id}: {e}")
|
|
|
|
|
|
def gps_callback(self, sys_id, msg):
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
|
|
|
# 如果還沒有從 summary 獲取到映射，則不處理
|
|
|
if actual_drone_id is None:
|
|
|
return
|
|
|
|
|
|
# GnssRaw.altitude 是 GLOBAL_POSITION_INT.alt（AMSL）。交戰航點使用
|
|
|
# GLOBAL_RELATIVE_ALT_INT，因此 alt 僅能使用 position_ned 的相對高度。
|
|
|
now = time.monotonic()
|
|
|
gps_data = {
|
|
|
'lat': msg.latitude,
|
|
|
'lon': msg.longitude,
|
|
|
'alt': None,
|
|
|
'amsl_alt': msg.altitude,
|
|
|
'_gps_updated_at': now,
|
|
|
}
|
|
|
|
|
|
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
|
|
|
|
|
|
# ================================================================================
|
|
|
# 【新增】儲存 GPS 資料到 drone_gps 字典
|
|
|
# ================================================================================
|
|
|
with self.lock:
|
|
|
relative_alt = self.drone_relative_alt.get(actual_drone_id)
|
|
|
relative_alt_updated_at = self.drone_relative_alt_updated_at.get(
|
|
|
actual_drone_id
|
|
|
)
|
|
|
if relative_alt_updated_at is not None:
|
|
|
gps_data['alt'] = relative_alt
|
|
|
gps_data['_relative_alt_updated_at'] = relative_alt_updated_at
|
|
|
gps_data['_altitude_reference'] = 'relative_home'
|
|
|
self.drone_gps[actual_drone_id] = gps_data.copy()
|
|
|
self.latest_data[(actual_drone_id, 'gps')] = gps_data
|
|
|
# ================================================================================
|
|
|
|
|
|
def local_vel_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'velocity')] = {
|
|
|
'vx': msg.x,
|
|
|
'vy': msg.y,
|
|
|
'vz': msg.z
|
|
|
}
|
|
|
|
|
|
def altitude_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'altitude')] = {
|
|
|
'altitude': msg.data
|
|
|
}
|
|
|
|
|
|
def local_pose_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'local_pose')] = {
|
|
|
'x': msg.x,
|
|
|
'y': msg.y,
|
|
|
'z': msg.z
|
|
|
}
|
|
|
|
|
|
def hud_callback(self, sys_id, msg):
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
|
|
|
# 如果還沒有從 summary 獲取到映射，則不處理
|
|
|
if actual_drone_id is None:
|
|
|
return
|
|
|
|
|
|
self.latest_data[(actual_drone_id, 'hud')] = {
|
|
|
'airspeed': msg.airspeed,
|
|
|
'groundspeed': msg.groundspeed,
|
|
|
'heading': msg.heading,
|
|
|
'throttle': msg.throttle,
|
|
|
'alt': msg.altitude,
|
|
|
'climb': msg.climb
|
|
|
}
|
|
|
|
|
|
def position_ned_callback(self, sys_id, msg):
|
|
|
"""處理 position_ned topic (nav_msgs/msg/Odometry)
|
|
|
NED 座標系中的位置（在 msg.pose.pose.position）：
|
|
|
- x: 北向位移 (m)
|
|
|
- y: 東向位移 (m)
|
|
|
- z: 向下位移 (m) - 負值表示向下，轉換為高度需要 * (-1)
|
|
|
"""
|
|
|
try:
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
|
|
|
# 如果還沒有從 summary 獲取到映射，則不處理
|
|
|
if actual_drone_id is None:
|
|
|
return
|
|
|
|
|
|
# 從 Odometry 消息中提取位置數據 (msg.pose.pose.position)
|
|
|
# Odometry 結構：header, child_frame_id, pose(包含PoseWithCovariance), twist
|
|
|
x = msg.pose.pose.position.y # NED 座標系中交換 x/y（與 local_pose 對齐）
|
|
|
y = msg.pose.pose.position.x
|
|
|
z = -msg.pose.pose.position.z # 將向下的 NED z 轉換為向上的高度（z 為負表示向下）
|
|
|
vx = msg.twist.twist.linear.y
|
|
|
vy = msg.twist.twist.linear.x
|
|
|
vz = -msg.twist.twist.linear.z
|
|
|
|
|
|
now = time.monotonic()
|
|
|
with self.lock:
|
|
|
self.drone_relative_alt[actual_drone_id] = z
|
|
|
self.drone_relative_alt_updated_at[actual_drone_id] = now
|
|
|
current_gps = self.drone_gps.get(actual_drone_id)
|
|
|
if isinstance(current_gps, dict):
|
|
|
current_gps = dict(current_gps)
|
|
|
current_gps['alt'] = z
|
|
|
current_gps['_relative_alt_updated_at'] = now
|
|
|
current_gps['_altitude_reference'] = 'relative_home'
|
|
|
self.drone_gps[actual_drone_id] = current_gps
|
|
|
|
|
|
# 儲存高度信息
|
|
|
self.latest_data[(actual_drone_id, 'altitude')] = {
|
|
|
'altitude': z
|
|
|
}
|
|
|
|
|
|
# 儲存本地位置信息
|
|
|
self.latest_data[(actual_drone_id, 'local_pose')] = {
|
|
|
'x': x,
|
|
|
'y': y,
|
|
|
'z': z
|
|
|
}
|
|
|
|
|
|
# 儲存速度資訊，供總覽頁「XY速度」欄位顯示
|
|
|
self.latest_data[(actual_drone_id, 'velocity')] = {
|
|
|
'vx': vx,
|
|
|
'vy': vy,
|
|
|
'vz': vz
|
|
|
}
|
|
|
|
|
|
# 發送信號給 GUI 更新高度顯示
|
|
|
self.signals.update_signal.emit('altitude', actual_drone_id, {
|
|
|
'altitude': z
|
|
|
})
|
|
|
except Exception as e:
|
|
|
pass
|
|
|
|
|
|
def loss_rate_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'loss_rate')] = {
|
|
|
'loss_rate': msg.data
|
|
|
}
|
|
|
|
|
|
def ping_callback(self, drone_id, msg):
|
|
|
self.latest_data[(drone_id, 'ping')] = {
|
|
|
'ping': msg.data
|
|
|
}
|
|
|
|
|
|
def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=115200):
|
|
|
"""啟動串口遙測連接（自動辨識 MAVLink / JSON）"""
|
|
|
connection_name = f"Serial_{port.replace('/', '_')}"
|
|
|
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
|
|
|
receiver.start()
|
|
|
self.serial_receivers.append(receiver)
|
|
|
_log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud (MAVLink/JSON)")
|
|
|
return receiver
|