diff --git a/src/unitdev01/unitdev01/communication.py b/src/unitdev01/unitdev01/communication.py new file mode 100644 index 0000000..9584a62 --- /dev/null +++ b/src/unitdev01/unitdev01/communication.py @@ -0,0 +1,511 @@ +from rclpy.node import Node +from PyQt6.QtCore import QObject, pyqtSignal +import math +import re +import threading +from threading import Lock +import asyncio +import websockets +import json +import socket +from pymavlink import mavutil +from geometry_msgs.msg import Point, Vector3 +from sensor_msgs.msg import BatteryState, NavSatFix, Imu +from std_msgs.msg import Float64 +from mavros_msgs.msg import State, VfrHud +from mavros_msgs.srv import CommandBool, CommandTOL + +class DroneSignals(QObject): + update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) + +class UDPMavlinkReceiver(threading.Thread): + """UDP MAVLink 接收器""" + def __init__(self, ip, port, signals, connection_name): + super().__init__(daemon=True) + self.ip = ip + self.port = port + self.signals = signals + self.connection_name = connection_name + self.running = False + self.sock = None + + def run(self): + """執行 UDP 接收循環""" + self.running = True + try: + print(f"UDP MAVLink receiver started on {self.ip}:{self.port}") + + # 創建 MAVLink 連接 + mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}') + while self.running: + try: + msg = mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except socket.timeout: + continue + except Exception as e: + print(f"Error receiving MAVLink message: {e}") + + except Exception as e: + print(f"UDP receiver error: {e}") + finally: + if self.sock: + self.sock.close() + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class WebSocketMavlinkReceiver(threading.Thread): + """WebSocket MAVLink 接收器""" + def __init__(self, url, signals, connection_name): + super().__init__(daemon=True) + self.url = url + self.signals = signals + self.connection_name = connection_name + self.running = False + self.max_retries = 5 + self.base_delay = 1.0 + + def run(self): + """執行 WebSocket 接收循環""" + self.running = True + asyncio.set_event_loop(asyncio.new_event_loop()) + asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + + async def ws_client_loop(self): + """WebSocket 連接的主循環""" + retry_count = 0 + + print(f"Starting WebSocket client for {self.connection_name} at {self.url}") + + while self.running and retry_count < self.max_retries: + try: + async with websockets.connect(self.url) as websocket: + print(f"WebSocket {self.connection_name} connected to {self.url}") + retry_count = 0 # 重置重試計數 + + async for message in websocket: + if not self.running: + break + + try: + data = json.loads(message) + data['_connection_source'] = self.connection_name + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket {self.connection_name} JSON decode error: {e}") + except Exception as e: + print(f"WebSocket {self.connection_name} message processing error: {e}") + + except websockets.exceptions.ConnectionClosedError: + print(f"WebSocket {self.connection_name} connection closed") + if self.running: + retry_count += 1 + if retry_count < self.max_retries: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"Reconnecting in {delay}s...") + await asyncio.sleep(delay) + else: + break + except Exception as e: + retry_count += 1 + if retry_count < self.max_retries and self.running: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})") + await asyncio.sleep(delay) + else: + break + + print(f"WebSocket client {self.connection_name} stopped") + + def process_websocket_message(self, data): + """處理 WebSocket 訊息""" + try: + drone_id = data.get('system_id') + drone_id = f"s9_{drone_id}" if drone_id else None + if not drone_id: + return + + # 模式 + if 'mode' in data: + self.signals.update_signal.emit('state', drone_id, { + 'mode': data['mode'], + }) + + # 電池 + if 'battery' in data: + self.signals.update_signal.emit('battery', drone_id, { + 'percentage': data['battery'] + }) + + # 位置 + if 'position' in data: + pos = data['position'] + self.signals.update_signal.emit('gps', drone_id, { + 'lat': pos.get('lat', 0), + 'lon': pos.get('lon', 0) + }) + + # Local position - 設定 x, y 為 0.0 + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': 0.0, + 'y': 0.0, + 'z': 0.0 + }) + + # Altitude - 設定為 0.0 + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': 0.0 + }) + + # 航向 + if 'heading' in data: + self.signals.update_signal.emit('hud', drone_id, { + 'heading': data['heading'], + 'groundspeed': 0.0 + }) + + except Exception as e: + print(f"WebSocket message processing error: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class DroneMonitor(Node): + def __init__(self): + super().__init__('drone_monitor') + self.signals = DroneSignals() + self.drone_topics = {} + self.lock = Lock() + + self.arm_clients = {} + self.takeoff_clients = {} + self.setpoint_pubs = {} + self.selected_drones = set() + self.latest_data = {} + + # 定義需要過濾的模式 + self.filtered_modes = ['Mode(0x000000c0)'] + + # WebSocket 接收器列表 + self.ws_receivers = [] + + # 主题检测定时器 + self.create_timer(1.0, self.scan_topics) + + def scan_topics(self): + topics = self.get_topic_names_and_types() + drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') + + found_drones = set() + for topic_name, _ in topics: + if match := drone_pattern.match(topic_name): + drone_id, topic_type = match.groups() + found_drones.add(drone_id) + with self.lock: + self.drone_topics.setdefault(drone_id, set()).add(topic_type) + + for drone_id in found_drones: + if not hasattr(self, f'drone_{drone_id}_subs'): + self.setup_drone(drone_id) + + def setup_drone(self, drone_id): + base_topic = f'/MavLinkBus/{drone_id}' + + # Add service clients + self.arm_clients[drone_id] = self.create_client( + CommandBool, + f'{base_topic}/cmd/arming' + ) + self.takeoff_clients[drone_id] = self.create_client( + CommandTOL, + f'{base_topic}/cmd/takeoff' + ) + + # Add setpoint publisher + self.setpoint_pubs[drone_id] = self.create_publisher( + Point, + f'{base_topic}/setpoint_position/local', + 10 + ) + + subs = { + 'attitude': self.create_subscription( + Imu, + f'{base_topic}/attitude', + lambda msg, did=drone_id: self.attitude_callback(did, msg), + 10 + ), + 'battery': self.create_subscription( + BatteryState, + f'{base_topic}/battery', + lambda msg, did=drone_id: self.battery_callback(did, msg), + 10 + ), + 'global': self.create_subscription( + NavSatFix, + f'{base_topic}/global_position/global', + lambda msg, did=drone_id: self.gps_callback(did, msg), + 10 + ), + 'rel_alt': self.create_subscription( + Float64, + f'{base_topic}/global_position/rel_alt', + lambda msg, did=drone_id: self.altitude_callback(did, msg), + 10 + ), + 'local_pose': self.create_subscription( + Point, + f'{base_topic}/local_position/pose', + lambda msg, did=drone_id: self.local_pose_callback(did, msg), + 10 + ), + 'local_vel': self.create_subscription( + Vector3, + f'{base_topic}/local_position/velocity', + lambda msg, did=drone_id: self.local_vel_callback(did, msg), + 10 + ), + 'loss_rate': self.create_subscription( + Float64, + f'{base_topic}/packet_loss_rate', + lambda msg, did=drone_id: self.loss_rate_callback(did, msg), + 10 + ), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), + 10 + ), + 'ping': self.create_subscription( + Float64, + f'{base_topic}/ping', + lambda msg, did=drone_id: self.ping_callback(did, msg), + 10 + ), + 'vfr_hud': self.create_subscription( + VfrHud, + f'{base_topic}/vfr_hud', + lambda msg, did=drone_id: self.hud_callback(did, msg), + 10 + ) + } + + setattr(self, f'drone_{drone_id}_subs', subs) + + async def arm_drone(self, drone_id, arm): + if drone_id not in self.arm_clients: + return False + + client = self.arm_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandBool.Request() + request.value = arm + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Arm service call failed: {e}') + return False + + async def takeoff_drone(self, drone_id, altitude=10.0): + if drone_id not in self.takeoff_clients: + return False + + client = self.takeoff_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandTOL.Request() + request.altitude = altitude + request.min_pitch = 0.0 + request.yaw = 0.0 + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Takeoff service call failed: {e}') + return False + + def send_setpoint(self, drone_id, x, y, z): + """Send setpoint position command""" + if drone_id not in self.setpoint_pubs: + return False + + msg = Point() + msg.x = float(x) + msg.y = float(y) + msg.z = float(z) + + self.setpoint_pubs[drone_id].publish(msg) + return True + + def quaternion_to_euler(self, q): + sinr_cosp = 2 * (q.w * q.x + q.y * q.z) + cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) + roll = math.atan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (q.w * q.y - q.z * q.x) + pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp) + + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y**2 + q.z**2) + yaw = math.atan2(siny_cosp, cosy_cosp) + + return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) + + # callbacks + def attitude_callback(self, drone_id, msg): + if hasattr(msg, 'orientation'): + roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) + self.latest_data[(drone_id, 'attitude')] = { + 'roll': roll, + 'pitch': pitch, + 'yaw': yaw, + 'rates': (msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z) + } + + def battery_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'battery')] = { + 'voltage': msg.voltage + } + + def state_callback(self, drone_id, msg): + mode = msg.mode + if mode in self.filtered_modes: + return + self.latest_data[(drone_id, 'state')] = { + 'mode': msg.mode, + 'armed': msg.armed + } + + def gps_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'gps')] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + + def local_vel_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'velocity')] = { + 'vx': msg.x, + 'vy': msg.y, + 'vz': msg.z + } + + def altitude_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'altitude')] = { + 'altitude': msg.data + } + + def local_pose_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'local_pose')] = { + 'x': msg.x, + 'y': msg.y, + 'z': msg.z + } + + def hud_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'hud')] = { + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.altitude, + 'climb': msg.climb + } + + def loss_rate_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'loss_rate')] = { + 'loss_rate': msg.data + } + + def ping_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'ping')] = { + 'ping': msg.data + } \ No newline at end of file diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index a227321..5eac6a3 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -1,405 +1,16 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, QHeaderView, QPushButton, QCheckBox, QLineEdit) -from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal +from PyQt6.QtCore import Qt, QTimer from PyQt6.QtWebEngineWidgets import QWebEngineView -import math -import re 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 Float64 -from mavros_msgs.msg import State, VfrHud -from mavros_msgs.srv import CommandBool, CommandTOL, SetMode import asyncio -from functools import partial -import threading -import websockets -import json -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.arm_clients = {} - self.takeoff_clients = {} - self.setpoint_pubs = {} - self.selected_drones = set() - self.latest_data = {} - - # 定義需要過濾的模式 - self.filtered_modes = ['Mode(0x000000c0)'] - - # 定義多個 WebSocket 連接配置 - self.websocket_connections = [ - { - 'name': 'local_1', - 'url': 'ws://192.168.137.1:5163', - 'enabled': True - }, - { - 'name': 'local_2', - 'url': 'ws://0.0.0.0:8756', - 'enabled': True # 新增的端口 - }, - { - 'name': 'remote_8756', - 'url': 'ws://192.168.50.48:8756', - 'enabled': False # 可選擇啟用 - } - ] - - # 啟動多個 WebSocket client 執行緒 - for connection in self.websocket_connections: - if connection['enabled']: - threading.Thread( - target=self.start_ws_client, - args=(connection,), - daemon=True, - name=f"WebSocket-{connection['name']}" - ).start() - - # 主题检测定时器 - self.create_timer(1.0, self.scan_topics) - - def start_ws_client(self, connection_config): - """啟動單個 WebSocket 客戶端""" - asyncio.set_event_loop(asyncio.new_event_loop()) - asyncio.get_event_loop().run_until_complete( - self.ws_client_loop(connection_config) - ) - - async def ws_client_loop(self, connection_config): - """單個 WebSocket 連接的主循環""" - retry_count = 0 - max_retries = 5 - base_delay = 1.0 - connection_name = connection_config['name'] - url = connection_config['url'] - - print(f"Starting WebSocket client for {connection_name} at {url}") - - while retry_count < max_retries: - try: - async with websockets.connect(url) as websocket: - print(f"WebSocket {connection_name} connected to {url}") - retry_count = 0 # 重置重試計數 - - async for message in websocket: - try: - data = json.loads(message) - # 添加連接來源信息 - data['_connection_source'] = connection_name - self.process_websocket_message(data) - except json.JSONDecodeError as e: - print(f"WebSocket {connection_name} JSON decode error: {e}") - except Exception as e: - print(f"WebSocket {connection_name} message processing error: {e}") - - except websockets.exceptions.ConnectionClosedError: - print(f"WebSocket {connection_name} connection closed") - break - except Exception as e: - retry_count += 1 - delay = base_delay * (2 ** min(retry_count, 4)) # 指數退避 - print(f"WebSocket {connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})") - await asyncio.sleep(delay) - - print(f"WebSocket client {connection_name} stopped after maximum retries") - - def process_websocket_message(self, data): - try: - drone_id = data.get('system_id') - drone_id = f"s9_{drone_id}" if drone_id else None - if not drone_id: - return - - # 模式 - if 'mode' in data: - self.signals.update_signal.emit('state', drone_id, { - 'mode': data['mode'], - }) - - # 電池 - if 'battery' in data: - # 這裡假設 battery 是百分比 - 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) - }) - - self.signals.update_signal.emit('local_pose', drone_id, { - 'x': round(pos.get('lat', 0), 6), - 'y': round(pos.get('lon', 0), 6), - }) - - # 航向 - if 'heading' in data: - self.signals.update_signal.emit('hud', drone_id, { - 'heading': data['heading'], - }) - - except Exception as e: - print(f"WebSocket message processing error: {e}") - - def scan_topics(self): - topics = self.get_topic_names_and_types() - drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') - - found_drones = set() - for topic_name, _ in topics: - if match := drone_pattern.match(topic_name): - drone_id, topic_type = match.groups() - found_drones.add(drone_id) - with self.lock: - self.drone_topics.setdefault(drone_id, set()).add(topic_type) - - for drone_id in found_drones: - if not hasattr(self, f'drone_{drone_id}_subs'): - self.setup_drone(drone_id) - - def setup_drone(self, drone_id): - base_topic = f'/MavLinkBus/{drone_id}' - - # Add service clients - self.arm_clients[drone_id] = self.create_client( - CommandBool, - f'{base_topic}/cmd/arming' - ) - self.takeoff_clients[drone_id] = self.create_client( - CommandTOL, - f'{base_topic}/cmd/takeoff' - ) - - # Add setpoint publisher - self.setpoint_pubs[drone_id] = self.create_publisher( - Point, - f'{base_topic}/setpoint_position/local', - 10 - ) - - subs = { - 'attitude': self.create_subscription( - Imu, - f'{base_topic}/attitude', - lambda msg, did=drone_id: self.attitude_callback(did, msg), - 10 - ), - 'battery': self.create_subscription( - BatteryState, - f'{base_topic}/battery', - lambda msg, did=drone_id: self.battery_callback(did, msg), - 10 - ), - 'global': self.create_subscription( - NavSatFix, - f'{base_topic}/global_position/global', - lambda msg, did=drone_id: self.gps_callback(did, msg), - 10 - ), - 'rel_alt': self.create_subscription( - Float64, - f'{base_topic}/global_position/rel_alt', - lambda msg, did=drone_id: self.altitude_callback(did, msg), - 10 - ), - 'local_pose': self.create_subscription( - Point, - f'{base_topic}/local_position/pose', - lambda msg, did=drone_id: self.local_pose_callback(did, msg), - 10 - ), - 'local_vel': self.create_subscription( - Vector3, - f'{base_topic}/local_position/velocity', - lambda msg, did=drone_id: self.local_vel_callback(did, msg), - 10 - ), - 'loss_rate': self.create_subscription( - Float64, - f'{base_topic}/packet_loss_rate', - lambda msg, did=drone_id: self.loss_rate_callback(did, msg), - 10 - ), - 'state': self.create_subscription( - State, - f'{base_topic}/state', - lambda msg, did=drone_id: self.state_callback(did, msg), - 10 - ), - 'ping': self.create_subscription( - Float64, - f'{base_topic}/ping', - lambda msg, did=drone_id: self.ping_callback(did, msg), - 10 - ), - 'vfr_hud': self.create_subscription( - VfrHud, - f'{base_topic}/vfr_hud', - lambda msg, did=drone_id: self.hud_callback(did, msg), - 10 - ) - } - - setattr(self, f'drone_{drone_id}_subs', subs) - - async def arm_drone(self, drone_id, arm): - if drone_id not in self.arm_clients: - return False - - client = self.arm_clients[drone_id] - if not client.wait_for_service(timeout_sec=1.0): - return False - - request = CommandBool.Request() - request.value = arm - - future = client.call_async(request) - try: - response = await future - return response.success - except Exception as e: - self.get_logger().error(f'Arm service call failed: {e}') - return False - - async def takeoff_drone(self, drone_id, altitude=10.0): - if drone_id not in self.takeoff_clients: - return False - - client = self.takeoff_clients[drone_id] - if not client.wait_for_service(timeout_sec=1.0): - return False - - request = CommandTOL.Request() - request.altitude = altitude - request.min_pitch = 0.0 - request.yaw = 0.0 - - future = client.call_async(request) - try: - response = await future - return response.success - except Exception as e: - self.get_logger().error(f'Takeoff service call failed: {e}') - return False - - def send_setpoint(self, drone_id, x, y, z): - """Send setpoint position command""" - if drone_id not in self.setpoint_pubs: - return False - - msg = Point() - msg.x = float(x) - msg.y = float(y) - msg.z = float(z) - - self.setpoint_pubs[drone_id].publish(msg) - return True - - def quaternion_to_euler(self, q): - sinr_cosp = 2 * (q.w * q.x + q.y * q.z) - cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) - roll = math.atan2(sinr_cosp, cosr_cosp) - - sinp = 2 * (q.w * q.y - q.z * q.x) - pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp) - - siny_cosp = 2 * (q.w * q.z + q.x * q.y) - cosy_cosp = 1 - 2 * (q.y**2 + q.z**2) - yaw = math.atan2(siny_cosp, cosy_cosp) - - return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) - - # callbacks - def attitude_callback(self, drone_id, msg): - if hasattr(msg, 'orientation'): - roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) - self.latest_data[(drone_id, 'attitude')] = { - 'roll': roll, - 'pitch': pitch, - 'yaw': yaw, - 'rates': (msg.angular_velocity.x, - msg.angular_velocity.y, - msg.angular_velocity.z) - } - - def battery_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'battery')] = { - 'voltage': msg.voltage - } - - def state_callback(self, drone_id, msg): - mode = msg.mode - if mode in self.filtered_modes: - return - self.latest_data[(drone_id, 'state')] = { - 'mode': msg.mode, - 'armed': msg.armed - } - - def gps_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'gps')] = { - 'lat': msg.latitude, - 'lon': msg.longitude, - 'alt': msg.altitude - } - - def local_vel_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'velocity')] = { - 'vx': msg.x, - 'vy': msg.y, - 'vz': msg.z - } - - def altitude_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'altitude')] = { - 'altitude': msg.data - } - - def local_pose_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'local_pose')] = { - 'x': msg.x, - 'y': msg.y, - 'z': msg.z - } - - def hud_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'hud')] = { - 'airspeed': msg.airspeed, - 'groundspeed': msg.groundspeed, - 'heading': msg.heading, - 'throttle': msg.throttle, - 'alt': msg.altitude, - 'climb': msg.climb - } - - def loss_rate_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'loss_rate')] = { - 'loss_rate': msg.data - } - - def ping_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'ping')] = { - 'ping': msg.data - } +# 從 communication.py 導入分離的類別 +from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver class ControlStationUI(QMainWindow): def __init__(self): @@ -423,7 +34,7 @@ class ControlStationUI(QMainWindow): # 初始化UI self.drones = {} - self.socket_groups = {} # 新增：用於儲存 socket 分組 + self.socket_groups = {} self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向", "Roll", "Pitch", "Yaw", "丟包", "延遲"] self.info_type_map = { @@ -442,27 +53,29 @@ class ControlStationUI(QMainWindow): "ping": 12 } self.socket_colors = { - '0': '#00BFFF', # DeepSkyBlue - '1': '#FFD700', # Gold - '2': '#FF69B4', # HotPink - '9': '#7CFC00', # LawnGreen + '0': '#00BFFF', + '1': '#FFD700', + '2': "#FF6969", + '8': '#7CFC00', + '9': '#FF8C00', 'default': '#AAAAAA' } self.drone_positions = {} self.drone_headings = {} self.map_loaded = False - # 添加地圖更新節流定時器 self.map_update_timer = QTimer() self.map_update_timer.timeout.connect(self.update_map_positions) - self.map_update_timer.start(200) # 每 200ms 更新一次地圖 - # 添加待更新的無人機位置緩存 + self.map_update_timer.start(200) self.pending_map_updates = {} - self.init_ui() + # 初始化連接列表 + self.udp_receivers = [] + self.udp_connections = [] + self.ws_connections = [] + self.init_ui() def init_ui(self): - # 只呼叫一次 main_splitter = QSplitter(Qt.Orientation.Horizontal) @@ -482,17 +95,193 @@ class ControlStationUI(QMainWindow): scroll.setWidgetResizable(True) self.left_tab.addTab(scroll, "無人載具") - # 底部控制按鈕區域 - bottom_control = QWidget() - bottom_layout = QVBoxLayout(bottom_control) + # — 分頁 2：Overview Table + self.overview_table = QTableWidget() + self.overview_table.setColumnCount(1) + self.overview_table.setRowCount(len(self.info_types)) + self.overview_table.setHorizontalHeaderLabels(["資訊"]) + header = self.overview_table.horizontalHeader() + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.overview_table.verticalHeader().setVisible(False) + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(i, 0, item) + + self.left_tab.addTab(self.overview_table, "總覽") + + # — 分頁 3：通訊設定 + self.comm_tab = QWidget() + comm_layout = QVBoxLayout(self.comm_tab) + comm_layout.setContentsMargins(10, 10, 10, 10) + comm_layout.setSpacing(10) + + # ========== UDP MAVLink 區域 ========== + udp_title = QLabel("UDP MAVLink") + udp_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + comm_layout.addWidget(udp_title) + + # UDP 連接列表容器 + self.udp_list_widget = QWidget() + self.udp_list_layout = QVBoxLayout(self.udp_list_widget) + self.udp_list_layout.setContentsMargins(0, 0, 0, 0) + self.udp_list_layout.setSpacing(5) + comm_layout.addWidget(self.udp_list_widget) + + # UDP 添加新連接區域 + add_udp_widget = QWidget() + add_udp_layout = QHBoxLayout(add_udp_widget) + add_udp_layout.setContentsMargins(0, 0, 0, 0) + + self.udp_ip_input = QLineEdit() + self.udp_ip_input.setText("127.0.0.1") + self.udp_ip_input.setPlaceholderText("IP") + self.udp_ip_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + self.udp_port_input = QLineEdit() + self.udp_port_input.setText("14540") + self.udp_port_input.setPlaceholderText("Port") + self.udp_port_input.setFixedWidth(80) + self.udp_port_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_udp_btn = QPushButton("添加") + add_udp_btn.clicked.connect(self.handle_add_udp_connection) + add_udp_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_ip_input) + add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_port_input) + add_udp_layout.addWidget(add_udp_btn) + + comm_layout.addWidget(add_udp_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + comm_layout.addWidget(separator) + + # ========== WebSocket 區域 ========== + ws_title = QLabel("WebSocket") + ws_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + comm_layout.addWidget(ws_title) + + # WebSocket 連接列表容器 + self.ws_list_widget = QWidget() + self.ws_list_layout = QVBoxLayout(self.ws_list_widget) + self.ws_list_layout.setContentsMargins(0, 0, 0, 0) + self.ws_list_layout.setSpacing(5) + comm_layout.addWidget(self.ws_list_widget) + + # WebSocket 添加新連接區域 + add_ws_widget = QWidget() + add_ws_layout = QHBoxLayout(add_ws_widget) + add_ws_layout.setContentsMargins(0, 0, 0, 0) + + self.ws_url_input = QLineEdit() + self.ws_url_input.setPlaceholderText("host") + self.ws_url_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_ws_btn = QPushButton("添加") + add_ws_btn.clicked.connect(self.handle_add_ws_connection) + add_ws_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) + add_ws_layout.addWidget(self.ws_url_input) + add_ws_layout.addWidget(add_ws_btn) + + comm_layout.addWidget(add_ws_widget) + comm_layout.addStretch() + + self.left_tab.addTab(self.comm_tab, "通訊") + + # 右侧容器 + right_container = QWidget() + right_layout = QVBoxLayout(right_container) + right_layout.setContentsMargins(10, 10, 10, 10) + right_layout.setSpacing(10) + # ========== 批次控制區域（直接使用 layout） ========== + batch_control_layout = QHBoxLayout() + + # 添加批次操作標題 + batch_title = QLabel("批次操作") + batch_title.setStyleSheet(""" + color: #DDD; + font-size: 16px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + batch_control_layout.addWidget(batch_title) + # 上方按鈕區域 upper_buttons = QHBoxLayout() select_all_btn = QPushButton("全選") select_all_btn.clicked.connect(self.handle_select_all) - arm_all_btn = QPushButton("批次解鎖") + arm_all_btn = QPushButton("解鎖") arm_all_btn.clicked.connect(self.handle_arm_selected) - takeoff_all_btn = QPushButton("批次起飛") + takeoff_all_btn = QPushButton("起飛") takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]: btn.setStyleSheet(""" @@ -500,7 +289,7 @@ class ControlStationUI(QMainWindow): background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } @@ -509,29 +298,27 @@ class ControlStationUI(QMainWindow): upper_buttons.addWidget(btn) upper_buttons.addStretch() - # --- 模式切換區域 --- + # 模式切換區域 mode_layout = QHBoxLayout() - mode_layout.setContentsMargins(0, 0, 0, 0) - mode_layout.setSpacing(8) mode_label = QLabel("模式：") mode_label.setStyleSheet("color: #DDD; min-width: 40px;") from PyQt6.QtWidgets import QComboBox self.mode_combo = QComboBox() self.mode_combo.addItems(["AUTO", "GUIDED", "LOITER", "LAND"]) - self.mode_combo.setCurrentIndex(1) # 預設 GUIDED + self.mode_combo.setCurrentIndex(1) self.mode_combo.setStyleSheet(""" QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} """) - batch_mode_btn = QPushButton("批次切換模式") + batch_mode_btn = QPushButton("切換") batch_mode_btn.clicked.connect(self.handle_batch_mode_change) batch_mode_btn.setStyleSheet(""" QPushButton { background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } @@ -542,10 +329,9 @@ class ControlStationUI(QMainWindow): mode_layout.addWidget(batch_mode_btn) mode_layout.addStretch() - # Add coordinate inputs + # 座標輸入區域 coord_widget = QWidget() coord_layout = QHBoxLayout(coord_widget) - coord_layout.setContentsMargins(0, 0, 0, 0) self.x_input = QLineEdit() self.y_input = QLineEdit() @@ -563,14 +349,13 @@ class ControlStationUI(QMainWindow): } """) - coord_layout.addWidget(QLabel("X:")) + coord_layout.addWidget(QLabel("X:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.x_input) - coord_layout.addWidget(QLabel("Y:")) + coord_layout.addWidget(QLabel("Y:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.y_input) - coord_layout.addWidget(QLabel("Z:")) + coord_layout.addWidget(QLabel("Z:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.z_input) - # Add buttons to control layout setpoint_btn = QPushButton("Setpoint") setpoint_btn.clicked.connect(self.handle_setpoint_selected) setpoint_btn.setStyleSheet(""" @@ -578,44 +363,27 @@ class ControlStationUI(QMainWindow): background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } QPushButton:hover { background-color: #555; } """) - # 下方座標輸入區域 lower_control = QHBoxLayout() lower_control.addWidget(coord_widget) lower_control.addWidget(setpoint_btn) lower_control.addStretch() - - # 加入底部控制區 - bottom_layout.addLayout(upper_buttons) - bottom_layout.addLayout(mode_layout) - bottom_layout.addLayout(lower_control) - - # — 分頁 2：Overview Table - self.overview_table = QTableWidget() - # 一開始只有一欄 - self.overview_table.setColumnCount(1) - self.overview_table.setRowCount(len(self.info_types)) - self.overview_table.setHorizontalHeaderLabels(["資訊"]) - header = self.overview_table.horizontalHeader() - # 只有第一欄自動收縮到內容寬度，其它欄不動 - header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) - self.overview_table.verticalHeader().setVisible(False) - for i, txt in enumerate(self.info_types): - item = QTableWidgetItem(txt) - item.setFlags(Qt.ItemFlag.ItemIsEnabled) - item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) - self.overview_table.setItem(i, 0, item) - - self.left_tab.addTab(self.overview_table, "總覽") + # 組裝批次控制 layout + batch_control_layout.addLayout(upper_buttons) + batch_control_layout.addLayout(mode_layout) + batch_control_layout.addLayout(lower_control) - # 右侧地图区域 + # 將批次控制 layout 添加到右側容器 + right_layout.addLayout(batch_control_layout) + + # 添加地圖 self.map_view = QWebEngineView() inline_html = ''' @@ -671,8 +439,9 @@ class ControlStationUI(QMainWindow): function getColorBySocketId(id) { if (id.startsWith("s0_")) return "#00BFFF"; if (id.startsWith("s1_")) return "#FFD700"; - if (id.startsWith("s2_")) return "#FF69B4"; - if (id.startsWith("s9_")) return "#7CFC00"; + if (id.startsWith("s2_")) return "#FF6969"; + if (id.startsWith("s8_")) return "#7CFC00"; + if (id.startsWith("s9_")) return "#FF8C00"; return "#AAAAAA"; } @@ -803,23 +572,15 @@ class ControlStationUI(QMainWindow): ''' self.map_view.setHtml(inline_html) self.map_view.loadFinished.connect(self.on_map_loaded) - - # Create left container and its layout - left_container = QWidget() - left_layout = QVBoxLayout(left_container) - left_layout.setContentsMargins(0, 0, 0, 0) - - # Add tab widget and bottom control to left container - left_layout.addWidget(self.left_tab) - left_layout.addWidget(bottom_control) + right_layout.addWidget(self.map_view) + # Add widgets to splitter - main_splitter.addWidget(left_container) - main_splitter.addWidget(self.map_view) + main_splitter.addWidget(self.left_tab) + main_splitter.addWidget(right_container) main_splitter.setSizes([400, 1000]) self.setCentralWidget(main_splitter) - def on_map_loaded(self, ok: bool): if ok: self.map_loaded = True @@ -1079,6 +840,339 @@ class ControlStationUI(QMainWindow): main_layout.addWidget(control_widget) return panel + + def create_udp_connection_panel(self, conn): + """創建 UDP 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.toggle_udp_connection(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.remove_udp_connection(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def create_ws_connection_panel(self, conn): + """創建 WebSocket 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['url']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.toggle_ws_connection(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.remove_ws_connection(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def handle_add_ws_connection(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.statusBar().showMessage("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴（如果用戶沒有輸入協議前綴） + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + # 檢查是否包含 host:port 格式 + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.statusBar().showMessage("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.statusBar().showMessage("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.statusBar().showMessage("連接已存在", 3000) + return + + # 創建新連接 + new_conn = { + 'name': f'WS {len(self.ws_connections) + 1}', + 'url': url, + 'enabled': True + } + + # 啟動接收器 + receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.ws_connections.append(new_conn) + + # 添加到 UI + conn_panel = self.create_ws_connection_panel(new_conn) + self.ws_list_layout.addWidget(conn_panel) + + # 清空輸入框 + self.ws_url_input.clear() + + self.statusBar().showMessage(f"已添加 WebSocket 連接: {url}", 3000) + print(f"WebSocket receiver added and started: {url}") + + def toggle_ws_connection(self, conn, btn, status_label): + """切換 WebSocket 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000) + else: + # 啟動連接 + receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) + + def remove_ws_connection(self, conn, panel): + """移除 WebSocket 連接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.monitor.ws_receivers: + self.monitor.ws_receivers.remove(conn['receiver']) + + # 從列表移除 + if conn in self.ws_connections: + self.ws_connections.remove(conn) + + # 從 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 WebSocket 連接: {conn['url']}", 3000) + + def handle_add_udp_connection(self): + """處理添加 UDP 連接""" + ip = self.udp_ip_input.text().strip() + port_text = self.udp_port_input.text().strip() + + if not ip or not port_text: + self.statusBar().showMessage("請輸入 IP 和 Port", 3000) + return + + try: + port = int(port_text) + if port < 1 or port > 65535: + raise ValueError("Port 超出範圍") + except ValueError: + self.statusBar().showMessage("Port 必須是 1-65535 的數字", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.udp_connections: + if conn['ip'] == ip and conn['port'] == port: + self.statusBar().showMessage("連接已存在", 3000) + return + + # 創建新連接 + new_conn = { + 'name': f'UDP {len(self.udp_connections) + 1}', + 'ip': ip, + 'port': port, + 'enabled': True + } + + # 啟動接收器 + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.udp_connections.append(new_conn) + + # 添加到 UI + conn_panel = self.create_udp_connection_panel(new_conn) + self.udp_list_layout.addWidget(conn_panel) + + # 清空輸入框 + self.udp_ip_input.clear() + self.udp_port_input.clear() + + self.statusBar().showMessage(f"已添加 UDP 連接: {ip}:{port}", 3000) + print(f"UDP MAVLink receiver added and started on {ip}:{port}") + + def toggle_udp_connection(self, conn, btn, status_label): + """切換 UDP 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + else: + # 啟動連接 + receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + + def remove_udp_connection(self, conn, panel): + """移除 UDP 連接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.udp_receivers: + self.udp_receivers.remove(conn['receiver']) + + # 從列表移除 + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + # 從 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 UDP 連接: {conn['ip']}:{conn['port']}", 3000) def create_socket_group_panel(self, socket_id): """創建 socket 分組面板""" @@ -1593,6 +1687,14 @@ class ControlStationUI(QMainWindow): print(f"ROS spin error: {e}") def closeEvent(self, event): + # 停止 UDP 接收器 + for receiver in self.udp_receivers: + receiver.stop() + + # 停止 WebSocket 接收器 + for receiver in self.monitor.ws_receivers: + receiver.stop() + self.monitor.destroy_node() self.executor.shutdown() rclpy.shutdown()