|
|
|
@ -12,23 +12,13 @@ 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
|
|
|
|
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
|
|
|
|
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
|
|
|
|
from std_msgs.msg import Float64, String
|
|
|
|
from std_msgs.msg import Float64, String
|
|
|
|
from mavros_msgs.msg import State, VfrHud
|
|
|
|
from mavros_msgs.msg import State, VfrHud
|
|
|
|
from nav_msgs.msg import Odometry
|
|
|
|
|
|
|
|
from mavros_msgs.srv import CommandBool, CommandTOL
|
|
|
|
from mavros_msgs.srv import CommandBool, CommandTOL
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _log(level, message):
|
|
|
|
|
|
|
|
print(f"[{level}] {message}")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
|
|
|
|
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
|
|
|
|
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
|
|
|
|
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
|
|
|
|
if _src_path not in sys.path:
|
|
|
|
if _src_path not in sys.path:
|
|
|
|
@ -39,179 +29,16 @@ try:
|
|
|
|
from fc_network_apps.longCommand import CommandLongClient
|
|
|
|
from fc_network_apps.longCommand import CommandLongClient
|
|
|
|
except ImportError as e:
|
|
|
|
except ImportError as e:
|
|
|
|
import traceback
|
|
|
|
import traceback
|
|
|
|
_log("WARN", "無法導入 CommandLongClient")
|
|
|
|
print(f"⚠️ 警告: 無法導入 CommandLongClient")
|
|
|
|
_log("ERROR", f"錯誤: {e}")
|
|
|
|
print(f" 错误: {e}")
|
|
|
|
_log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整")
|
|
|
|
print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
|
|
|
|
|
|
|
|
print(f" 完整堆栈跟踪:")
|
|
|
|
traceback.print_exc()
|
|
|
|
traceback.print_exc()
|
|
|
|
CommandLongClient = None
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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):
|
|
|
|
@ -335,8 +162,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
|
|
|
|
@ -345,125 +172,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:
|
|
|
|
@ -546,23 +295,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
|
|
|
|
|
|
|
|
|
|
|
|
@ -590,7 +331,6 @@ 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:
|
|
|
|
@ -621,7 +361,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):
|
|
|
|
"""停止接收器"""
|
|
|
|
"""停止接收器"""
|
|
|
|
@ -677,20 +467,14 @@ class DroneMonitor(Node):
|
|
|
|
self.serial_receivers = []
|
|
|
|
self.serial_receivers = []
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
# 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client）
|
|
|
|
# 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
# 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件
|
|
|
|
self.command_long_client = None
|
|
|
|
self.command_long_clients = {} # {drone_id: CommandLongClient}
|
|
|
|
try:
|
|
|
|
self.client_lock = Lock() # 保護 clients 字典的訪問
|
|
|
|
self.command_long_client = CommandLongClient()
|
|
|
|
self.client_counter = 0 # 用於生成唯一的 client 節點名稱
|
|
|
|
except Exception as e:
|
|
|
|
self.executor = None # 將在 gui.py 中設置，用於添加新的 clients
|
|
|
|
print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
|
|
|
|
# ================================================================================
|
|
|
|
self.command_long_client = None
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
# PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto）
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
self.position_target_clients = {} # {drone_id: PositionTargetGlobalIntClient}
|
|
|
|
|
|
|
|
self.pos_client_counter = 0
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
# 主题检测定时器
|
|
|
|
# 主题检测定时器
|
|
|
|
@ -717,58 +501,6 @@ class DroneMonitor(Node):
|
|
|
|
|
|
|
|
|
|
|
|
return self.socket_id_mapping[original_socket_id]
|
|
|
|
return self.socket_id_mapping[original_socket_id]
|
|
|
|
|
|
|
|
|
|
|
|
def get_or_create_client(self, drone_id):
|
|
|
|
|
|
|
|
"""為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件"""
|
|
|
|
|
|
|
|
with self.client_lock:
|
|
|
|
|
|
|
|
if drone_id not in self.command_long_clients:
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
|
|
|
# 生成唯一的 client 節點名稱
|
|
|
|
|
|
|
|
self.client_counter += 1
|
|
|
|
|
|
|
|
unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
|
|
|
|
|
|
|
|
client = CommandLongClient(node_name=unique_name)
|
|
|
|
|
|
|
|
self.command_long_clients[drone_id] = client
|
|
|
|
|
|
|
|
_log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 將新 client 添加到主執行器（這樣它的回調才能被處理）
|
|
|
|
|
|
|
|
if self.executor:
|
|
|
|
|
|
|
|
self.executor.add_node(client)
|
|
|
|
|
|
|
|
_log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
except TypeError:
|
|
|
|
|
|
|
|
# 舊版 CommandLongClient 不支持 node_name 參數，使用預設
|
|
|
|
|
|
|
|
client = CommandLongClient()
|
|
|
|
|
|
|
|
self.command_long_clients[drone_id] = client
|
|
|
|
|
|
|
|
_log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if self.executor:
|
|
|
|
|
|
|
|
self.executor.add_node(client)
|
|
|
|
|
|
|
|
_log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
|
|
|
|
_log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}")
|
|
|
|
|
|
|
|
return None
|
|
|
|
|
|
|
|
return self.command_long_clients[drone_id]
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def get_or_create_position_client(self, drone_id):
|
|
|
|
|
|
|
|
"""為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。"""
|
|
|
|
|
|
|
|
if PositionTargetGlobalIntClient is None:
|
|
|
|
|
|
|
|
return None
|
|
|
|
|
|
|
|
with self.client_lock:
|
|
|
|
|
|
|
|
if drone_id not in self.position_target_clients:
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
|
|
|
self.pos_client_counter += 1
|
|
|
|
|
|
|
|
unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
|
|
|
|
|
|
|
|
client = PositionTargetGlobalIntClient(node_name=unique_name)
|
|
|
|
|
|
|
|
self.position_target_clients[drone_id] = client
|
|
|
|
|
|
|
|
_log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})")
|
|
|
|
|
|
|
|
if self.executor:
|
|
|
|
|
|
|
|
self.executor.add_node(client)
|
|
|
|
|
|
|
|
_log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器")
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
|
|
|
|
_log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}")
|
|
|
|
|
|
|
|
return None
|
|
|
|
|
|
|
|
return self.position_target_clients[drone_id]
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def scan_topics(self):
|
|
|
|
def scan_topics(self):
|
|
|
|
topics = self.get_topic_names_and_types()
|
|
|
|
topics = self.get_topic_names_and_types()
|
|
|
|
drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
|
|
|
|
drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
|
|
|
|
@ -788,38 +520,8 @@ class DroneMonitor(Node):
|
|
|
|
# 暂时所有 ROS2 topic 共享同一个 socket_id = 0
|
|
|
|
# 暂时所有 ROS2 topic 共享同一个 socket_id = 0
|
|
|
|
self.sys_to_socket_id[sys_id] = 0
|
|
|
|
self.sys_to_socket_id[sys_id] = 0
|
|
|
|
|
|
|
|
|
|
|
|
subs_attr = f'drone_{sys_id}_subs'
|
|
|
|
if not hasattr(self, f'drone_{sys_id}_subs'):
|
|
|
|
if not hasattr(self, subs_attr):
|
|
|
|
|
|
|
|
self.setup_drone(sys_id)
|
|
|
|
self.setup_drone(sys_id)
|
|
|
|
else:
|
|
|
|
|
|
|
|
# 檢查既有訂閱是否包含 position_ned / attitude，如果不包含就添加（兼容舊訂閱）
|
|
|
|
|
|
|
|
subs = getattr(self, subs_attr, {})
|
|
|
|
|
|
|
|
if isinstance(subs, dict) and 'position_ned' not in subs:
|
|
|
|
|
|
|
|
base_topic = f'/fc_network/vehicle/{sys_id}'
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
|
|
|
pos_ned_sub = self.create_subscription(
|
|
|
|
|
|
|
|
Odometry,
|
|
|
|
|
|
|
|
f'{base_topic}/position_ned',
|
|
|
|
|
|
|
|
lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
|
|
|
|
|
|
|
|
10
|
|
|
|
|
|
|
|
)
|
|
|
|
|
|
|
|
subs['position_ned'] = pos_ned_sub
|
|
|
|
|
|
|
|
setattr(self, subs_attr, subs) # 明確保存更新後的字典
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
|
|
|
|
pass
|
|
|
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def setup_drone(self, sys_id):
|
|
|
|
def setup_drone(self, sys_id):
|
|
|
|
# sys_id 格式： sys11, sys12, ...
|
|
|
|
# sys_id 格式： sys11, sys12, ...
|
|
|
|
@ -866,23 +568,9 @@ class DroneMonitor(Node):
|
|
|
|
f'{base_topic}/vfr_hud',
|
|
|
|
f'{base_topic}/vfr_hud',
|
|
|
|
lambda msg, sid=sys_id: self.hud_callback(sid, msg),
|
|
|
|
lambda msg, sid=sys_id: self.hud_callback(sid, msg),
|
|
|
|
10
|
|
|
|
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 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
|
|
|
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
setattr(self, f'drone_{sys_id}_subs', subs)
|
|
|
|
setattr(self, f'drone_{sys_id}_subs', subs)
|
|
|
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
@ -919,41 +607,39 @@ class DroneMonitor(Node):
|
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
|
parts = drone_id.split('_')
|
|
|
|
parts = drone_id.split('_')
|
|
|
|
if len(parts) < 2:
|
|
|
|
if len(parts) < 2:
|
|
|
|
_log("ERROR", f"[SET_MODE] 無效的 drone_id 格式: {drone_id}")
|
|
|
|
print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
sysid = int(parts[-1])
|
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
|
|
|
|
|
|
# 獲取模式對應的 custom_mode 值
|
|
|
|
# 獲取模式對應的 custom_mode 值
|
|
|
|
custom_mode = self.MODE_MAPPING.get(mode_name)
|
|
|
|
custom_mode = self.MODE_MAPPING.get(mode_name)
|
|
|
|
if custom_mode is None:
|
|
|
|
if custom_mode is None:
|
|
|
|
_log("ERROR", f"[SET_MODE] 未知模式: {mode_name}")
|
|
|
|
print(f"❌ [SET_MODE] 未知模式: {mode_name}")
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
_log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})")
|
|
|
|
print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
|
|
|
|
|
|
|
|
|
|
|
|
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
|
|
|
|
if not self.command_long_client:
|
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
|
print(f"❌ [SET_MODE] CommandLongClient 未初始化")
|
|
|
|
if not client:
|
|
|
|
|
|
|
|
_log("ERROR", "[SET_MODE] CommandLongClient 無法初始化")
|
|
|
|
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
|
result = await client.change_mode_async(
|
|
|
|
result = await self.command_long_client.change_mode_async(
|
|
|
|
target_sysid=sysid,
|
|
|
|
target_sysid=sysid,
|
|
|
|
custom_mode=float(custom_mode),
|
|
|
|
custom_mode=float(custom_mode),
|
|
|
|
target_compid=0,
|
|
|
|
target_compid=0,
|
|
|
|
base_mode=1.0,
|
|
|
|
base_mode=1.0,
|
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
|
timeout_sec=2.0,
|
|
|
|
)
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
if result and result.success:
|
|
|
|
if result and result.success:
|
|
|
|
_log("INFO", f"[SET_MODE] {drone_id} 模式切換成功")
|
|
|
|
print(f"✅ [SET_MODE] 模式切換成功")
|
|
|
|
return True
|
|
|
|
return True
|
|
|
|
else:
|
|
|
|
else:
|
|
|
|
_log("ERROR", f"[SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
|
|
|
|
print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
except Exception as e:
|
|
|
|
except Exception as e:
|
|
|
|
_log("ERROR", f"[SET_MODE] 例外錯誤: {e}")
|
|
|
|
print(f"❌ [SET_MODE] 例外錯誤: {e}")
|
|
|
|
traceback.print_exc()
|
|
|
|
traceback.print_exc()
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
@ -963,35 +649,33 @@ class DroneMonitor(Node):
|
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
|
parts = drone_id.split('_')
|
|
|
|
parts = drone_id.split('_')
|
|
|
|
if len(parts) < 2:
|
|
|
|
if len(parts) < 2:
|
|
|
|
_log("ERROR", f"[ARM] 無效的 drone_id 格式: {drone_id}")
|
|
|
|
print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
sysid = int(parts[-1])
|
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
|
|
|
|
|
|
action_name = "解鎖" if arm else "上鎖"
|
|
|
|
action_name = "解鎖" if arm else "上鎖"
|
|
|
|
_log("INFO", f"[ARM] {drone_id} -> {action_name}")
|
|
|
|
print(f"\n📢 [ARM] {drone_id} → {action_name}")
|
|
|
|
|
|
|
|
|
|
|
|
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
|
|
|
|
if not self.command_long_client:
|
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
|
print(f"❌ [ARM] CommandLongClient 未初始化")
|
|
|
|
if not client:
|
|
|
|
|
|
|
|
_log("ERROR", "[ARM] CommandLongClient 無法初始化")
|
|
|
|
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
|
result = await client.arm_disarm_async(
|
|
|
|
result = await self.command_long_client.arm_disarm_async(
|
|
|
|
target_sysid=sysid,
|
|
|
|
target_sysid=sysid,
|
|
|
|
arm=arm,
|
|
|
|
arm=arm,
|
|
|
|
target_compid=0,
|
|
|
|
target_compid=0,
|
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
|
timeout_sec=2.0,
|
|
|
|
)
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
if result and result.success:
|
|
|
|
if result and result.success:
|
|
|
|
_log("INFO", f"[ARM] {drone_id} {action_name}成功")
|
|
|
|
print(f"✅ [ARM] {action_name}成功")
|
|
|
|
return True
|
|
|
|
return True
|
|
|
|
else:
|
|
|
|
else:
|
|
|
|
_log("ERROR", f"[ARM] {drone_id} {action_name}失敗 (message={result.message if result else 'None'})")
|
|
|
|
print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
except Exception as e:
|
|
|
|
except Exception as e:
|
|
|
|
_log("ERROR", f"[ARM] 例外錯誤: {e}")
|
|
|
|
print(f"❌ [ARM] 例外錯誤: {e}")
|
|
|
|
traceback.print_exc()
|
|
|
|
traceback.print_exc()
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
@ -1001,34 +685,32 @@ class DroneMonitor(Node):
|
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
|
parts = drone_id.split('_')
|
|
|
|
parts = drone_id.split('_')
|
|
|
|
if len(parts) < 2:
|
|
|
|
if len(parts) < 2:
|
|
|
|
_log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}")
|
|
|
|
print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
sysid = int(parts[-1])
|
|
|
|
sysid = int(parts[-1])
|
|
|
|
|
|
|
|
|
|
|
|
_log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)")
|
|
|
|
print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
|
|
|
|
|
|
|
|
|
|
|
|
# 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
|
|
|
|
if not self.command_long_client:
|
|
|
|
client = self.get_or_create_client(drone_id)
|
|
|
|
print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
|
|
|
|
if not client:
|
|
|
|
|
|
|
|
_log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化")
|
|
|
|
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
|
# 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
|
|
|
|
result = await client.takeoff_async(
|
|
|
|
result = await self.command_long_client.takeoff_async(
|
|
|
|
target_sysid=sysid,
|
|
|
|
target_sysid=sysid,
|
|
|
|
altitude_m=float(altitude),
|
|
|
|
altitude_m=float(altitude),
|
|
|
|
target_compid=0,
|
|
|
|
target_compid=0,
|
|
|
|
timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性
|
|
|
|
timeout_sec=2.0,
|
|
|
|
)
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
if result and result.success:
|
|
|
|
if result and result.success:
|
|
|
|
_log("INFO", f"[TAKEOFF] {drone_id} 起飛成功")
|
|
|
|
print(f"✅ [TAKEOFF] 起飛成功")
|
|
|
|
return True
|
|
|
|
return True
|
|
|
|
else:
|
|
|
|
else:
|
|
|
|
_log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
|
|
|
|
print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
except Exception as e:
|
|
|
|
except Exception as e:
|
|
|
|
_log("ERROR", f"[TAKEOFF] 例外錯誤: {e}")
|
|
|
|
print(f"❌ [TAKEOFF] 例外錯誤: {e}")
|
|
|
|
traceback.print_exc()
|
|
|
|
traceback.print_exc()
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
@ -1060,42 +742,17 @@ class DroneMonitor(Node):
|
|
|
|
return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
|
|
|
|
return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)
|
|
|
|
|
|
|
|
|
|
|
|
# callbacks
|
|
|
|
# callbacks
|
|
|
|
def attitude_callback(self, sys_id, msg):
|
|
|
|
def attitude_callback(self, drone_id, msg):
|
|
|
|
"""處理姿態 topic，支援 AttitudeRaw 與 IMU 四元數格式。"""
|
|
|
|
if hasattr(msg, 'orientation'):
|
|
|
|
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)
|
|
|
|
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
|
|
|
|
data = {
|
|
|
|
self.latest_data[(drone_id, 'attitude')] = {
|
|
|
|
'roll': roll,
|
|
|
|
'roll': roll,
|
|
|
|
'pitch': pitch,
|
|
|
|
'pitch': pitch,
|
|
|
|
'yaw': yaw,
|
|
|
|
'yaw': yaw,
|
|
|
|
'rates': (
|
|
|
|
'rates': (msg.angular_velocity.x,
|
|
|
|
msg.angular_velocity.x,
|
|
|
|
|
|
|
|
msg.angular_velocity.y,
|
|
|
|
msg.angular_velocity.y,
|
|
|
|
msg.angular_velocity.z,
|
|
|
|
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):
|
|
|
|
def battery_callback(self, sys_id, msg):
|
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
|
# 使用映射獲取實際的 drone_id
|
|
|
|
@ -1148,7 +805,6 @@ class DroneMonitor(Node):
|
|
|
|
sys_num = sys_id.replace('sys', '')
|
|
|
|
sys_num = sys_id.replace('sys', '')
|
|
|
|
actual_drone_id = f's{assigned_socket_id}_{sys_num}'
|
|
|
|
actual_drone_id = f's{assigned_socket_id}_{sys_num}'
|
|
|
|
self.sys_to_actual_id[sys_id] = actual_drone_id
|
|
|
|
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, {
|
|
|
|
self.signals.update_signal.emit('connection_type', actual_drone_id, {
|
|
|
|
@ -1229,55 +885,6 @@ class DroneMonitor(Node):
|
|
|
|
'climb': msg.climb
|
|
|
|
'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
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 儲存高度信息
|
|
|
|
|
|
|
|
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):
|
|
|
|
def loss_rate_callback(self, drone_id, msg):
|
|
|
|
self.latest_data[(drone_id, 'loss_rate')] = {
|
|
|
|
self.latest_data[(drone_id, 'loss_rate')] = {
|
|
|
|
'loss_rate': msg.data
|
|
|
|
'loss_rate': msg.data
|
|
|
|
@ -1289,10 +896,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)")
|
|
|
|
print(f"Started serial connection on {port} at {baudrate} baud")
|
|
|
|
return receiver
|
|
|
|
return receiver
|