forked from chiyu1468/AirTrapMine
Merge branch 'ken' into chiyu
commit
4d82b92a11
@ -0,0 +1,444 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout,
|
||||||
|
QWidget, QLabel, QSplitter, QScrollArea,
|
||||||
|
QSizePolicy, QApplication)
|
||||||
|
from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal, QUrl
|
||||||
|
from PyQt6.QtGui import QColor
|
||||||
|
from PyQt6.QtWebEngineWidgets import QWebEngineView
|
||||||
|
import math
|
||||||
|
import re
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
from threading import Lock
|
||||||
|
from geometry_msgs.msg import Point, Vector3
|
||||||
|
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
|
||||||
|
from std_msgs.msg import String, Float64
|
||||||
|
from mavros_msgs.msg import VfrHud
|
||||||
|
|
||||||
|
class DroneSignals(QObject):
|
||||||
|
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
|
||||||
|
|
||||||
|
class DroneMonitor(Node):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('drone_monitor')
|
||||||
|
self.signals = DroneSignals()
|
||||||
|
self.drone_topics = {}
|
||||||
|
self.lock = Lock()
|
||||||
|
|
||||||
|
# 主题检测定时器
|
||||||
|
self.create_timer(1.0, self.scan_topics)
|
||||||
|
|
||||||
|
def scan_topics(self):
|
||||||
|
topics = self.get_topic_names_and_types()
|
||||||
|
drone_pattern = re.compile(r'/MavLinkBus/(s\d+)/(\w+)')
|
||||||
|
|
||||||
|
found_drones = set()
|
||||||
|
for topic_name, _ in topics:
|
||||||
|
if match := drone_pattern.match(topic_name):
|
||||||
|
drone_id, topic_type = match.groups()
|
||||||
|
found_drones.add(drone_id)
|
||||||
|
with self.lock:
|
||||||
|
self.drone_topics.setdefault(drone_id, set()).add(topic_type)
|
||||||
|
|
||||||
|
for drone_id in found_drones:
|
||||||
|
if not hasattr(self, f'drone_{drone_id}_subs'):
|
||||||
|
self.setup_drone_subs(drone_id)
|
||||||
|
|
||||||
|
def setup_drone_subs(self, drone_id):
|
||||||
|
base_topic = f'/MavLinkBus/{drone_id}'
|
||||||
|
|
||||||
|
subs = {
|
||||||
|
'attitude': self.create_subscription(
|
||||||
|
Imu,
|
||||||
|
f'{base_topic}/attitude',
|
||||||
|
lambda msg, did=drone_id: self.attitude_callback(did, msg),
|
||||||
|
10
|
||||||
|
),
|
||||||
|
'battery': self.create_subscription(
|
||||||
|
BatteryState,
|
||||||
|
f'{base_topic}/battery',
|
||||||
|
lambda msg, did=drone_id: self.battery_callback(did, msg),
|
||||||
|
10
|
||||||
|
),
|
||||||
|
'flightMode': self.create_subscription(
|
||||||
|
String,
|
||||||
|
f'{base_topic}/flightMode',
|
||||||
|
lambda msg, did=drone_id: self.mode_callback(did, msg),
|
||||||
|
10
|
||||||
|
),
|
||||||
|
'global': self.create_subscription(
|
||||||
|
NavSatFix,
|
||||||
|
f'{base_topic}/global_position/global',
|
||||||
|
lambda msg, did=drone_id: self.gps_callback(did, msg),
|
||||||
|
10
|
||||||
|
),
|
||||||
|
'rel_alt': self.create_subscription(
|
||||||
|
Float64,
|
||||||
|
f'{base_topic}/global_position/rel_alt',
|
||||||
|
lambda msg, did=drone_id: self.altitude_callback(did, msg),
|
||||||
|
10
|
||||||
|
),
|
||||||
|
'local_pose': self.create_subscription(
|
||||||
|
Point,
|
||||||
|
f'{base_topic}/local_position/pose',
|
||||||
|
lambda msg, did=drone_id: self.local_pose_callback(did, msg),
|
||||||
|
10
|
||||||
|
),
|
||||||
|
'local_vel': self.create_subscription(
|
||||||
|
Vector3,
|
||||||
|
f'{base_topic}/local_position/velocity',
|
||||||
|
lambda msg, did=drone_id: self.local_vel_callback(did, msg),
|
||||||
|
10
|
||||||
|
),
|
||||||
|
'vfr_hud': self.create_subscription(
|
||||||
|
VfrHud,
|
||||||
|
f'{base_topic}/vfr_hud',
|
||||||
|
lambda msg, did=drone_id: self.hud_callback(did, msg),
|
||||||
|
10
|
||||||
|
)
|
||||||
|
}
|
||||||
|
|
||||||
|
setattr(self, f'drone_{drone_id}_subs', subs)
|
||||||
|
self.signals.update_signal.emit('new_drone', drone_id, None)
|
||||||
|
|
||||||
|
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)
|
||||||
|
|
||||||
|
# 回调函数
|
||||||
|
def attitude_callback(self, drone_id, msg):
|
||||||
|
if hasattr(msg, 'orientation'):
|
||||||
|
roll, pitch, yaw = self.quaternion_to_euler(msg.orientation)
|
||||||
|
self.signals.update_signal.emit('attitude', drone_id, {
|
||||||
|
'roll': roll,
|
||||||
|
'pitch': pitch,
|
||||||
|
'yaw': yaw,
|
||||||
|
'rates': (msg.angular_velocity.x,
|
||||||
|
msg.angular_velocity.y,
|
||||||
|
msg.angular_velocity.z)
|
||||||
|
})
|
||||||
|
|
||||||
|
def battery_callback(self, drone_id, msg):
|
||||||
|
self.signals.update_signal.emit('battery', drone_id, {
|
||||||
|
'voltage': msg.voltage
|
||||||
|
})
|
||||||
|
|
||||||
|
def mode_callback(self, drone_id, msg):
|
||||||
|
self.signals.update_signal.emit('mode', drone_id, msg.data)
|
||||||
|
|
||||||
|
def gps_callback(self, drone_id, msg):
|
||||||
|
self.signals.update_signal.emit('gps', drone_id, {
|
||||||
|
'lat': msg.latitude,
|
||||||
|
'lon': msg.longitude
|
||||||
|
,'alt': msg.altitude
|
||||||
|
})
|
||||||
|
|
||||||
|
def local_vel_callback(self, drone_id, msg):
|
||||||
|
self.signals.update_signal.emit('velocity', drone_id, {
|
||||||
|
'vx': msg.x,
|
||||||
|
'vy': msg.y,
|
||||||
|
'vz': msg.z
|
||||||
|
})
|
||||||
|
|
||||||
|
def altitude_callback(self, drone_id, msg):
|
||||||
|
self.signals.update_signal.emit('altitude', drone_id, {
|
||||||
|
'altitude': msg.data
|
||||||
|
})
|
||||||
|
|
||||||
|
def local_pose_callback(self, drone_id, msg):
|
||||||
|
self.signals.update_signal.emit('local_pose', drone_id, {
|
||||||
|
'x': msg.x,
|
||||||
|
'y': msg.y,
|
||||||
|
'z': msg.z
|
||||||
|
})
|
||||||
|
|
||||||
|
def hud_callback(self, drone_id, msg):
|
||||||
|
self.signals.update_signal.emit('hud', drone_id, {
|
||||||
|
'airspeed': msg.airspeed,
|
||||||
|
'groundspeed': msg.groundspeed,
|
||||||
|
'heading': msg.heading,
|
||||||
|
'throttle': msg.throttle,
|
||||||
|
'alt': msg.altitude,
|
||||||
|
'climb': msg.climb
|
||||||
|
})
|
||||||
|
|
||||||
|
class ControlStationUI(QMainWindow):
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__()
|
||||||
|
self.setWindowTitle('GCS')
|
||||||
|
self.resize(1400, 900)
|
||||||
|
self.drones = {}
|
||||||
|
|
||||||
|
# 初始化ROS2
|
||||||
|
rclpy.init()
|
||||||
|
self.monitor = DroneMonitor()
|
||||||
|
self.monitor.signals.update_signal.connect(self.update_ui)
|
||||||
|
|
||||||
|
# ROS执行器配置
|
||||||
|
self.executor = rclpy.executors.SingleThreadedExecutor()
|
||||||
|
self.executor.add_node(self.monitor)
|
||||||
|
|
||||||
|
# 初始化UI
|
||||||
|
self.drone_positions = {}
|
||||||
|
self.map_loaded = False
|
||||||
|
self.init_ui()
|
||||||
|
|
||||||
|
# 定时处理ROS事件
|
||||||
|
self.timer = QTimer()
|
||||||
|
self.timer.timeout.connect(self.spin_ros)
|
||||||
|
self.timer.start(50)
|
||||||
|
|
||||||
|
def init_ui(self):
|
||||||
|
main_splitter = QSplitter(Qt.Orientation.Horizontal)
|
||||||
|
|
||||||
|
# 左侧信息面板
|
||||||
|
left_panel = QWidget()
|
||||||
|
left_layout = QVBoxLayout(left_panel)
|
||||||
|
left_layout.setContentsMargins(5, 5, 5, 5)
|
||||||
|
|
||||||
|
# 信息滚动区域
|
||||||
|
scroll_area = QScrollArea()
|
||||||
|
scroll_area.setWidgetResizable(True)
|
||||||
|
self.info_container = QWidget()
|
||||||
|
self.info_layout = QVBoxLayout(self.info_container)
|
||||||
|
self.info_layout.setAlignment(Qt.AlignmentFlag.AlignTop)
|
||||||
|
scroll_area.setWidget(self.info_container)
|
||||||
|
|
||||||
|
left_layout.addWidget(scroll_area)
|
||||||
|
|
||||||
|
# 右侧地图区域
|
||||||
|
self.map_view = QWebEngineView()
|
||||||
|
|
||||||
|
inline_html = '''
|
||||||
|
<!DOCTYPE html>
|
||||||
|
<html>
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8" />
|
||||||
|
<link rel="stylesheet" href="https://unpkg.com/leaflet/dist/leaflet.css" />
|
||||||
|
<script src="https://unpkg.com/leaflet/dist/leaflet.js"></script>
|
||||||
|
<script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
|
||||||
|
<style>
|
||||||
|
html, body, #map { height: 100%; margin: 0; }
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<div id="map"></div>
|
||||||
|
<script>
|
||||||
|
var map = L.map('map').setView([0, 0], 2);
|
||||||
|
L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
|
||||||
|
maxZoom: 19
|
||||||
|
}).addTo(map);
|
||||||
|
|
||||||
|
var arrowIcon = L.icon({
|
||||||
|
iconUrl: 'https://cdn-icons-png.flaticon.com/512/399/399308.png', // 朝上的箭頭
|
||||||
|
iconSize: [40, 40],
|
||||||
|
iconAnchor: [20, 20]
|
||||||
|
});
|
||||||
|
|
||||||
|
var markers = {};
|
||||||
|
|
||||||
|
function updateDrone(lat, lon, id, heading) {
|
||||||
|
if (markers[id]) {
|
||||||
|
markers[id].setLatLng([lat, lon]);
|
||||||
|
markers[id].setRotationAngle(heading);
|
||||||
|
} else {
|
||||||
|
markers[id] = L.marker([lat, lon], {
|
||||||
|
icon: arrowIcon,
|
||||||
|
rotationAngle: heading,
|
||||||
|
rotationOrigin: 'center'
|
||||||
|
}).addTo(map).bindPopup(id);
|
||||||
|
}
|
||||||
|
map.setView([lat, lon], 16);
|
||||||
|
}
|
||||||
|
</script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
|
'''
|
||||||
|
self.map_view.setHtml(inline_html)
|
||||||
|
self.map_view.loadFinished.connect(self.on_map_loaded)
|
||||||
|
|
||||||
|
main_splitter.addWidget(left_panel)
|
||||||
|
main_splitter.addWidget(self.map_view)
|
||||||
|
main_splitter.setSizes([400, 1000])
|
||||||
|
|
||||||
|
self.setCentralWidget(main_splitter)
|
||||||
|
|
||||||
|
def on_map_loaded(self, ok: bool):
|
||||||
|
if ok:
|
||||||
|
self.map_loaded = True
|
||||||
|
else:
|
||||||
|
print("⚠️ 地图页加载失败")
|
||||||
|
|
||||||
|
def create_drone_panel(self, drone_id):
|
||||||
|
panel = QWidget()
|
||||||
|
panel.setObjectName(f"panel_{drone_id}")
|
||||||
|
panel.setStyleSheet("""
|
||||||
|
QWidget#panel_%s {
|
||||||
|
background-color: #2A2A2A;
|
||||||
|
border-radius: 8px;
|
||||||
|
margin: 6px;
|
||||||
|
padding: 10px;
|
||||||
|
border: 1px solid #444;
|
||||||
|
}
|
||||||
|
QLabel {
|
||||||
|
color: #DDD;
|
||||||
|
font-size: 12px;
|
||||||
|
padding: 2px;
|
||||||
|
}
|
||||||
|
""" % drone_id)
|
||||||
|
|
||||||
|
layout = QVBoxLayout(panel)
|
||||||
|
layout.setContentsMargins(8, 8, 8, 8)
|
||||||
|
layout.setSpacing(4)
|
||||||
|
|
||||||
|
# 顶部标题栏
|
||||||
|
header = QWidget()
|
||||||
|
header_layout = QHBoxLayout(header)
|
||||||
|
header_layout.setContentsMargins(0, 0, 0, 0)
|
||||||
|
|
||||||
|
# ID显示
|
||||||
|
id_label = QLabel(drone_id)
|
||||||
|
id_label.setStyleSheet("""
|
||||||
|
font-weight: bold;
|
||||||
|
font-size: 14px;
|
||||||
|
color: #7FFFD4;
|
||||||
|
min-width: 80px;
|
||||||
|
""")
|
||||||
|
|
||||||
|
# 状态指示灯
|
||||||
|
status_light = QLabel("●")
|
||||||
|
status_light.setStyleSheet("color: #00FF00; font-size: 16px;")
|
||||||
|
|
||||||
|
header_layout.addWidget(id_label)
|
||||||
|
header_layout.addWidget(status_light)
|
||||||
|
header_layout.addStretch()
|
||||||
|
|
||||||
|
layout.addWidget(header)
|
||||||
|
|
||||||
|
# 数据字段（带标签）
|
||||||
|
self.create_data_row(layout, "模式:", f"{drone_id}_mode", "--")
|
||||||
|
self.create_data_row(layout, "電壓:", f"{drone_id}_battery", "--")
|
||||||
|
self.create_data_row(layout, "GPS:", f"{drone_id}_gps", "--")
|
||||||
|
self.create_data_row(layout, "高度:", f"{drone_id}_altitude", "--")
|
||||||
|
self.create_data_row(layout, "Local:", f"{drone_id}_local", "--")
|
||||||
|
self.create_data_row(layout, "HUD:", f"{drone_id}_hud", "--")
|
||||||
|
|
||||||
|
return panel
|
||||||
|
|
||||||
|
def create_data_row(self, layout, title, object_name, default):
|
||||||
|
row = QWidget()
|
||||||
|
hbox = QHBoxLayout(row)
|
||||||
|
hbox.setContentsMargins(0, 0, 0, 0)
|
||||||
|
|
||||||
|
# 标题标签
|
||||||
|
title_label = QLabel(title)
|
||||||
|
title_label.setStyleSheet("color: #888; min-width: 80px;")
|
||||||
|
title_label.setSizePolicy(QSizePolicy.Policy.Fixed, QSizePolicy.Policy.Preferred)
|
||||||
|
|
||||||
|
# 数据标签
|
||||||
|
value_label = QLabel(default)
|
||||||
|
value_label.setObjectName(object_name)
|
||||||
|
value_label.setWordWrap(True)
|
||||||
|
value_label.setStyleSheet("margin-left: 10px;")
|
||||||
|
|
||||||
|
hbox.addWidget(title_label)
|
||||||
|
hbox.addWidget(value_label)
|
||||||
|
layout.addWidget(row)
|
||||||
|
|
||||||
|
def update_ui(self, msg_type, drone_id, data):
|
||||||
|
if msg_type == 'new_drone':
|
||||||
|
self.add_drone(drone_id)
|
||||||
|
return
|
||||||
|
|
||||||
|
if not (panel := self.drones.get(drone_id)):
|
||||||
|
return
|
||||||
|
|
||||||
|
if msg_type == 'mode':
|
||||||
|
self.update_field(panel, drone_id, 'mode', f"{data}",
|
||||||
|
'#FF5555' if '返航' in data else '#55FF55')
|
||||||
|
|
||||||
|
elif msg_type == 'battery':
|
||||||
|
voltage = data.get('voltage', 0)
|
||||||
|
self.update_field(panel, drone_id, 'battery',
|
||||||
|
f"{voltage:.1f} V",
|
||||||
|
'#FF6464' if voltage < 12 else '#FFFFFF')
|
||||||
|
|
||||||
|
elif msg_type == 'gps':
|
||||||
|
lat, lon = data['lat'], data['lon']
|
||||||
|
self.drone_positions[drone_id] = (lat, lon)
|
||||||
|
text = (f"緯度: {lat:.6f}°\n"
|
||||||
|
f"經度: {lon:.6f}°")
|
||||||
|
self.update_field(panel, drone_id, 'gps', text)
|
||||||
|
|
||||||
|
elif msg_type == 'altitude':
|
||||||
|
text = (f"{data['altitude']:.1f} m")
|
||||||
|
self.update_field(panel, drone_id, 'altitude', text)
|
||||||
|
|
||||||
|
elif msg_type == 'local_pose':
|
||||||
|
text = (f"({data['x']:.1f}, {data['y']:.1f}, {data['z']:.1f})")
|
||||||
|
self.update_field(panel, drone_id, 'local', text)
|
||||||
|
|
||||||
|
elif msg_type == 'hud':
|
||||||
|
heading = data['heading']
|
||||||
|
text = (f"地速: {data['groundspeed']:.1f} m/s\n"
|
||||||
|
f"航向: {heading:.1f}°")
|
||||||
|
self.update_field(panel, drone_id, 'hud', text)
|
||||||
|
|
||||||
|
if self.map_loaded and drone_id in self.drone_positions:
|
||||||
|
lat, lon = self.drone_positions[drone_id]
|
||||||
|
js = f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});"
|
||||||
|
self.map_view.page().runJavaScript(js)
|
||||||
|
'''
|
||||||
|
elif msg_type == 'velocity':
|
||||||
|
text = (f"VX: {data['vx']:.1f} m/s\n"
|
||||||
|
f"VY: {data['vy']:.1f} m/s\n"
|
||||||
|
f"VZ: {data['vz']:.1f} m/s")
|
||||||
|
self.update_field(panel, drone_id, 'velocity', text)
|
||||||
|
|
||||||
|
elif msg_type == 'attitude':
|
||||||
|
text = (f"Roll: {data['roll']:.1f}°\n"
|
||||||
|
f"Pitch: {data['pitch']:.1f}°\n"
|
||||||
|
f"Yaw: {data['yaw']:.1f}°")
|
||||||
|
self.update_field(panel, drone_id, 'attitude', text)
|
||||||
|
'''
|
||||||
|
def update_field(self, panel, drone_id, field, text, color=None):
|
||||||
|
if label := panel.findChild(QLabel, f"{drone_id}_{field}"):
|
||||||
|
label.setText(text)
|
||||||
|
if color:
|
||||||
|
label.setStyleSheet(f"color: {color};")
|
||||||
|
|
||||||
|
def add_drone(self, drone_id):
|
||||||
|
if drone_id not in self.drones:
|
||||||
|
panel = self.create_drone_panel(drone_id)
|
||||||
|
self.info_layout.addWidget(panel)
|
||||||
|
self.drones[drone_id] = panel
|
||||||
|
|
||||||
|
def spin_ros(self):
|
||||||
|
try:
|
||||||
|
self.executor.spin_once(timeout_sec=0)
|
||||||
|
except Exception as e:
|
||||||
|
print(f"ROS spin error: {e}")
|
||||||
|
|
||||||
|
def closeEvent(self, event):
|
||||||
|
self.monitor.destroy_node()
|
||||||
|
self.executor.shutdown()
|
||||||
|
rclpy.shutdown()
|
||||||
|
event.accept()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
app = QApplication(sys.argv)
|
||||||
|
station = ControlStationUI()
|
||||||
|
station.show()
|
||||||
|
app.exec(app.exec())
|
||||||
Loading…
Reference in New Issue