diff --git a/.gitignore b/.gitignore index 5905970..ab42e47 100644 --- a/.gitignore +++ b/.gitignore @@ -25,3 +25,5 @@ Makefile **/*.class **/*.pyc **/*.pyo +logs/ +src/logs/ diff --git a/src/GUI/comm_panel.py b/src/GUI/comm_panel.py index bd12794..3a41226 100644 --- a/src/GUI/comm_panel.py +++ b/src/GUI/comm_panel.py @@ -306,8 +306,7 @@ class CommPanel(QWidget): # 發送信號通知主窗口 self.udp_connection_added.emit(ip, port) - # 清空輸入框 - self.udp_ip_input.clear() + # 只清空 port 輸入框，保留 IP self.udp_port_input.clear() def _handle_add_ws(self): diff --git a/src/GUI/communication.py b/src/GUI/communication.py index b97828b..9e82fbc 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -27,8 +27,7 @@ class UDPMavlinkReceiver(threading.Thread): self.signals = signals self.connection_name = connection_name self.monitor = monitor # 保存 monitor 引用 - # UDP 使用原始 socket_id = 8 - self.socket_id = monitor.get_or_assign_socket_id('8') if monitor else 0 + self.socket_id = monitor.get_next_socket_id() if monitor else 0 self.running = False self.sock = None @@ -67,6 +66,10 @@ class UDPMavlinkReceiver(threading.Thread): drone_id = f"s{self.socket_id}_{system_id}" if msg_type == "HEARTBEAT": + # 先發送連接類型資訊 + self.signals.update_signal.emit('connection_type', drone_id, { + 'type': 'UDP' + }) mode = mavutil.mode_string_v10(msg) armed = bool(msg.base_mode & 128) self.signals.update_signal.emit('state', drone_id, { @@ -144,8 +147,7 @@ class SerialMavlinkReceiver(threading.Thread): self.signals = signals self.connection_name = connection_name self.monitor = monitor # 保存 monitor 引用 - # Serial 使用原始 socket_id = 5 - self.socket_id = monitor.get_or_assign_socket_id('5') if monitor else 0 + self.socket_id = monitor.get_next_socket_id() if monitor else 0 self.running = False self.mav = None @@ -195,6 +197,10 @@ class SerialMavlinkReceiver(threading.Thread): drone_id = f"s{self.socket_id}_{system_id}" if msg_type == "HEARTBEAT": + # 先發送連接類型資訊 + self.signals.update_signal.emit('connection_type', drone_id, { + 'type': 'Serial' + }) mode = mavutil.mode_string_v10(msg) armed = bool(msg.base_mode & 128) self.signals.update_signal.emit('state', drone_id, { @@ -336,6 +342,10 @@ class WebSocketMavlinkReceiver(threading.Thread): # 模式 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'], }) @@ -427,6 +437,13 @@ class DroneMonitor(Node): # 主题检测定时器 self.create_timer(1.0, self.scan_topics) + def get_next_socket_id(self): + """获取下一个可用的 socket_id（从 0 开始连续分配）""" + with self.socket_id_lock: + current_id = self.socket_id_counter + self.socket_id_counter += 1 + return current_id + def get_or_assign_socket_id(self, original_socket_id): """根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配） 同一個原始 socket_id 會得到同一個分配的 ID @@ -437,7 +454,6 @@ class DroneMonitor(Node): if original_socket_id not in self.socket_id_mapping: # 分配新的 socket_id self.socket_id_mapping[original_socket_id] = self.socket_id_counter - print(f"🆕 Socket ID 映射: 原始 {original_socket_id} -> 分配 {self.socket_id_counter}") self.socket_id_counter += 1 return self.socket_id_mapping[original_socket_id] @@ -455,11 +471,11 @@ class DroneMonitor(Node): self.drone_topics.setdefault(sys_id, set()).add(topic_type) for sys_id in found_drones: - # 為每個 sys_id 分配 socket_id（如果還沒有分配） - # 注意：如果後續 summary 提供了 socket_id，會使用 summary 的映射覆蓋 + # 为每个 sys_id 分配 socket_id（如果还没有分配） + # 注意：如果后续 summary 提供了 socket_id，会使用 summary 的映射覆盖 if sys_id not in self.sys_to_socket_id: - # 暫時所有 ROS2 topic 共享同一個原始 socket_id，等 summary 提供實際的 socket_id - self.sys_to_socket_id[sys_id] = self.get_or_assign_socket_id('0') + # 暂时所有 ROS2 topic 共享同一个 socket_id = 0 + self.sys_to_socket_id[sys_id] = 0 if not hasattr(self, f'drone_{sys_id}_subs'): self.setup_drone(sys_id) @@ -646,6 +662,11 @@ class DroneMonitor(Node): actual_drone_id = f's{assigned_socket_id}_{sys_num}' self.sys_to_actual_id[sys_id] = actual_drone_id + # 先發送連接類型資訊 + self.signals.update_signal.emit('connection_type', actual_drone_id, { + 'type': 'ROS2' + }) + self.latest_data[(actual_drone_id, 'state')] = { 'mode': mode, 'armed': data.get('armed', False), diff --git a/src/GUI/drone_panel.py b/src/GUI/drone_panel.py index a5a7bf5..adf142f 100644 --- a/src/GUI/drone_panel.py +++ b/src/GUI/drone_panel.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 -from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, - QPushButton, QSizePolicy, QCheckBox) +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox) from PyQt6.QtCore import pyqtSignal +from PyQt6.QtGui import QPainter, QPen, QColor, QFont, QPolygonF +from PyQt6.QtCore import QPointF, Qt +import math class DronePanel(QWidget): """單個無人機面板類別""" @@ -16,7 +18,19 @@ class DronePanel(QWidget): def __init__(self, drone_id, parent=None): super().__init__(parent) self.drone_id = drone_id - self.display_id = 's' + drone_id.split('_')[1] + + # 提取資訊 (格式: s{socket_seq}_{system_id}, 如 s0_11, s1_12) + parts = drone_id.split('_') + if len(parts) >= 2: + self.socket_seq = parts[0][1:] # socket 序號 (移除 's' 前綴) + self.system_id = parts[1] # system ID + self.display_id = f"ID:{self.system_id}" # 顯示為 ID:11, ID:12 + else: + self.socket_seq = "?" + self.system_id = "?" + self.display_id = drone_id + + self.attitude_indicator = None self._init_ui() def _init_ui(self): @@ -43,12 +57,12 @@ class DronePanel(QWidget): # 左側資訊區域 info_widget = self._create_info_section() - # 右側控制按鈕區域 - control_widget = self._create_control_section() + # 右側態度指示器 + attitude_widget = self._create_attitude_indicator() - # 將 info 和 control 加入內容容器 - content_layout.addWidget(info_widget) - content_layout.addWidget(control_widget) + # 將 info 和 attitude 加入內容容器 + content_layout.addWidget(info_widget, 1) + content_layout.addWidget(attitude_widget, 0) # 將內容容器加入主佈局 main_layout.addWidget(content_widget) @@ -68,7 +82,22 @@ class DronePanel(QWidget): # 勾選框 self.checkbox = QCheckBox() self.checkbox.setObjectName(f"{self.drone_id}_checkbox") - self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }") + self.checkbox.setStyleSheet(""" + QCheckBox { + color: #DDD; + } + QCheckBox::indicator { + width: 16px; + height: 16px; + border: 2px solid #888888; + border-radius: 3px; + background: transparent; + } + QCheckBox::indicator:checked { + background-color: #7FFFD4; + border: 2px solid #888888; + } + """) self.checkbox.stateChanged.connect( lambda state: self.selection_changed.emit(self.drone_id, state) ) @@ -92,13 +121,13 @@ class DronePanel(QWidget): status_row = self._create_status_row() info_layout.addWidget(status_row) - # 第二行：電池 + # 第二行：電池（拆成百分比與電壓兩欄） battery_row = self._create_battery_row() info_layout.addWidget(battery_row) - # 第三行：位置 + 高度 - position_row = self._create_position_row() - info_layout.addWidget(position_row) + # 第三行：高度 + altitude_row = self._create_altitude_row() + info_layout.addWidget(altitude_row) # 第四行：航向 + 速度 nav_row = self._create_nav_row() @@ -106,6 +135,12 @@ class DronePanel(QWidget): return info_widget + def _create_attitude_indicator(self): + """創建態度指示器（ADI 人工地平儀）""" + self.attitude_indicator = AttitudeIndicator(self.drone_id) + self.attitude_indicator.setFixedSize(90, 100) + return self.attitude_indicator + def _create_status_row(self): """創建狀態行""" status_row = QWidget() @@ -130,132 +165,120 @@ class DronePanel(QWidget): return status_row + def _create_connection_row(self): + """創建連接資訊行 (Socket Seq + 連接方式)""" + connection_row = QWidget() + connection_layout = QHBoxLayout(connection_row) + connection_layout.setContentsMargins(0, 0, 0, 0) + + connection_title = QLabel("Socket：") + connection_title.setStyleSheet("color: #888; min-width: 50px;") + + # 根據解析的 drone_id 資訊設定初始值 + self.socket_seq_label = QLabel(self.socket_seq) + self.socket_seq_label.setObjectName(f"{self.drone_id}_socket_seq") + self.socket_seq_label.setStyleSheet("color: #DDD;") + + connection_sep = QLabel(" - ") + connection_sep.setStyleSheet("color: #DDD;") + + # 設定連接方式顯示 + connection_type_map = { + 'r': 'ROS2', + 'u': 'UDP', + 's': 'Serial', + 'w': 'WS' + } + connection_type = connection_type_map.get(self.type_prefix, 'Unknown') + + self.connection_type_label = QLabel(connection_type) + self.connection_type_label.setObjectName(f"{self.drone_id}_connection_type") + self.connection_type_label.setStyleSheet("color: #DDD;") + + connection_layout.addWidget(connection_title) + connection_layout.addWidget(self.socket_seq_label) + connection_layout.addWidget(connection_sep) + connection_layout.addWidget(self.connection_type_label) + connection_layout.addStretch() + + return connection_row + def _create_battery_row(self): """創建電池行""" battery_row = QWidget() battery_layout = QHBoxLayout(battery_row) battery_layout.setContentsMargins(0, 0, 0, 0) - + # 顯示百分比 battery_title = QLabel("電池：") battery_title.setStyleSheet("color: #888; min-width: 50px;") - - self.battery_label = QLabel("--") - self.battery_label.setObjectName(f"{self.drone_id}_battery") - self.battery_label.setStyleSheet("color: #DDD;") - + + self.battery_pct_label = QLabel("--") + self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct") + self.battery_pct_label.setStyleSheet("color: #DDD;") + + # 分隔符 + separator1 = QLabel(" - ") + separator1.setStyleSheet("color: #DDD;") + + # 顯示電壓 + self.battery_vol_label = QLabel("--") + self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol") + self.battery_vol_label.setStyleSheet("color: #DDD;") + + # 分隔符 + separator2 = QLabel(" - ") + separator2.setStyleSheet("color: #DDD;") + + # 顯示電池節數 (S count) + self.battery_cells_label = QLabel("--") + self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells") + self.battery_cells_label.setStyleSheet("color: #DDD;") + battery_layout.addWidget(battery_title) - battery_layout.addWidget(self.battery_label) + battery_layout.addWidget(self.battery_pct_label) + battery_layout.addWidget(separator1) + battery_layout.addWidget(self.battery_vol_label) + battery_layout.addWidget(separator2) + battery_layout.addWidget(self.battery_cells_label) battery_layout.addStretch() return battery_row - def _create_position_row(self): - """創建位置行""" - position_row = QWidget() - position_layout = QHBoxLayout(position_row) - position_layout.setContentsMargins(0, 0, 0, 0) - - position_title = QLabel("位置：") - position_title.setStyleSheet("color: #888; min-width: 50px;") - - self.local_label = QLabel("--") - self.local_label.setObjectName(f"{self.drone_id}_local") - self.local_label.setStyleSheet("color: #DDD;") + def _create_altitude_row(self): + """創建高度和速度行""" + altitude_row = QWidget() + altitude_layout = QHBoxLayout(altitude_row) + altitude_layout.setContentsMargins(0, 0, 0, 0) altitude_title = QLabel("高度：") - altitude_title.setStyleSheet("color: #888; margin-left: 10px;") + altitude_title.setStyleSheet("color: #888; min-width: 50px;") self.altitude_label = QLabel("--") self.altitude_label.setObjectName(f"{self.drone_id}_altitude") self.altitude_label.setStyleSheet("color: #DDD;") - position_layout.addWidget(position_title) - position_layout.addWidget(self.local_label) - position_layout.addWidget(altitude_title) - position_layout.addWidget(self.altitude_label) - position_layout.addStretch() - - return position_row - - def _create_nav_row(self): - """創建導航行""" - nav_row = QWidget() - nav_layout = QHBoxLayout(nav_row) - nav_layout.setContentsMargins(0, 0, 0, 0) - - heading_title = QLabel("航向：") - heading_title.setStyleSheet("color: #888; min-width: 50px;") - - self.heading_label = QLabel("--") - self.heading_label.setObjectName(f"{self.drone_id}_heading") - self.heading_label.setStyleSheet("color: #DDD;") - speed_title = QLabel("速度：") speed_title.setStyleSheet("color: #888; margin-left: 10px;") - self.groundspeed_label = QLabel("--") - self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed") - self.groundspeed_label.setStyleSheet("color: #DDD;") + self.speed_label = QLabel("--") + self.speed_label.setObjectName(f"{self.drone_id}_speed") + self.speed_label.setStyleSheet("color: #DDD;") - nav_layout.addWidget(heading_title) - nav_layout.addWidget(self.heading_label) - nav_layout.addWidget(speed_title) - nav_layout.addWidget(self.groundspeed_label) - nav_layout.addStretch() + altitude_layout.addWidget(altitude_title) + altitude_layout.addWidget(self.altitude_label) + altitude_layout.addWidget(speed_title) + altitude_layout.addWidget(self.speed_label) + altitude_layout.addStretch() - return nav_row + return altitude_row - def _create_control_section(self): - """創建控制按鈕區域""" - control_widget = QWidget() - control_layout = QVBoxLayout(control_widget) - control_layout.setContentsMargins(0, 0, 0, 0) - control_layout.setSpacing(6) - - control_widget.setFixedWidth(80) - - btn_style = """ - QPushButton { - background-color: #444; - color: #DDD; - border: none; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """ - # 模式切換按鈕 - mode_btn = QPushButton("切換模式") - mode_btn.setStyleSheet(btn_style) - mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) - mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id)) - - # 解鎖按鈕 - arm_btn = QPushButton("解鎖") - arm_btn.setStyleSheet(btn_style) - arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) - arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id)) - - # 起飛按鈕 - takeoff_btn = QPushButton("起飛") - takeoff_btn.setStyleSheet(btn_style) - takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) - takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id)) - - # Setpoint 按鈕 - setpoint_btn = QPushButton("Setpoint") - setpoint_btn.setStyleSheet(btn_style) - setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) - setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id)) - - control_layout.addWidget(mode_btn) - control_layout.addWidget(arm_btn) - control_layout.addWidget(takeoff_btn) - control_layout.addWidget(setpoint_btn) - - return control_widget + def _create_position_row(self): + """位置行已移除（位置座標不再顯示於面板）。""" + return QWidget() + + def _create_nav_row(self): + """創建導航行（已移除，不再顯示）""" + return QWidget() def update_field(self, field, text, color=None): """更新指定欄位的值""" @@ -265,6 +288,27 @@ class DronePanel(QWidget): if color: label.setStyleSheet(f"color: {color};") + def set_connection_info(self, socket_seq, connection_type): + """設定連接資訊（Socket Seq 和連接方式） + connection_type: 'UDP' | 'Serial' | 'WS' + """ + self.socket_seq_label.setText(str(socket_seq)) + + # 顯示友善的連接方式 + type_display = { + 'UDP': 'UDP', + 'Serial': 'Serial', + 'WS': 'WS' + }.get(connection_type, connection_type) + + self.connection_type_label.setText(type_display) + + + def update_attitude(self, heading, roll, pitch): + """更新態度指示器""" + if self.attitude_indicator: + self.attitude_indicator.update_attitude(heading, roll, pitch) + def get_checkbox(self): """獲取勾選框""" return self.checkbox @@ -281,10 +325,11 @@ class SocketGroupPanel(QWidget): # 定義信號 group_selection_changed = pyqtSignal(str, int) # socket_id, state - def __init__(self, socket_id, color='#AAAAAA', parent=None): + def __init__(self, socket_id, color='#AAAAAA', socket_type=None, parent=None): super().__init__(parent) self.socket_id = socket_id self.color = color + self.socket_type = socket_type self._init_ui() def _init_ui(self): @@ -330,8 +375,12 @@ class SocketGroupPanel(QWidget): ) # Socket 分組標題 - title_label = QLabel(f"Socket {self.socket_id}") - title_label.setStyleSheet(f""" + if self.socket_type: + title_text = f"{self.socket_type} {self.socket_id}" + else: + title_text = f"Socket {self.socket_id}" + self.title_label = QLabel(title_text) + self.title_label.setStyleSheet(f""" font-weight: bold; font-size: 16px; color: {self.color}; @@ -341,7 +390,7 @@ class SocketGroupPanel(QWidget): """) title_layout.addWidget(self.group_checkbox) - title_layout.addWidget(title_label) + title_layout.addWidget(self.title_label) title_layout.addStretch() layout.addWidget(title_row) @@ -373,6 +422,165 @@ class SocketGroupPanel(QWidget): """設置分組勾選狀態""" self.group_checkbox.setChecked(checked) + def set_socket_type(self, conn_type): + """設置 socket 類型並更新標題""" + self.title_label.setText(f"{conn_type} {self.socket_id}") + def set_check_state(self, state): """設置分組勾選狀態（支持半選）""" - self.group_checkbox.setCheckState(state) \ No newline at end of file + self.group_checkbox.setCheckState(state) + + +class AttitudeIndicator(QWidget): + """ + 人工地平儀 (ADI) — 仿 Mission Planner 風格 + 上半部顯示 roll/pitch 人工地平儀，下方細條顯示航向 + """ + + def __init__(self, drone_id, parent=None): + super().__init__(parent) + self.drone_id = drone_id + self.heading = 0.0 # 航向 yaw (0–360) + self.roll = 0.0 # 滾轉 (deg, left- negative) + self.pitch = 0.0 # 俯仰 (deg, nose-up positive) + self.setStyleSheet("background-color: transparent;") + + def update_attitude(self, heading, roll, pitch): + self.heading = heading % 360 + self.roll = roll + self.pitch = pitch + self.update() + + # ------------------------------------------------------------------ helpers + def _adi_rect(self): + """Returns the square rect used for the ADI ball.""" + w, h = self.width(), self.height() + side = min(w, h - 14) # leave 14 px at bottom for heading strip + x = (w - side) / 2 + y = 0 + return x, y, side, side + + # ------------------------------------------------------------------ paint + def paintEvent(self, event): + p = QPainter(self) + p.setRenderHint(QPainter.RenderHint.Antialiasing) + self._draw_adi(p) + self._draw_heading_strip(p) + + # ---- artificial horizon ------------------------------------------------ + def _draw_adi(self, p): + from PyQt6.QtGui import QPainterPath + x0, y0, side, _ = self._adi_rect() + cx = x0 + side / 2 + cy = y0 + side / 2 + r = side / 2 - 1 + + # clip to circle + clip_path = QPainterPath() + clip_path.addEllipse(QPointF(cx, cy), r, r) + p.setClipPath(clip_path) + + # pixels-per-degree for pitch (10 deg ≈ side/5) + ppd = side / 50.0 + + # ---- rotate + translate canvas for roll & pitch + p.save() + p.translate(cx, cy) + p.rotate(self.roll) + pitch_offset = self.pitch * ppd + + # sky (above horizon) + sky_color = QColor(30, 100, 180) + p.fillRect(int(-r*2), int(-r*2 + pitch_offset), int(r*4), int(r*4), sky_color) + + # ground (below horizon) + ground_color = QColor(140, 90, 40) + p.fillRect(int(-r*2), int(pitch_offset), int(r*4), int(r*4), ground_color) + + # horizon line + p.setPen(QPen(QColor("#FFFFFF"), 2)) + p.drawLine(int(-r), int(pitch_offset), int(r), int(pitch_offset)) + + # pitch ladder (every 10°, ±30°) + p.setPen(QPen(QColor(255, 255, 255, 180), 1)) + p.setFont(QFont("Arial", 6)) + for deg in range(-30, 31, 10): + if deg == 0: + continue + yy = int(pitch_offset - deg * ppd) + half = int(r * (0.35 if deg % 20 == 0 else 0.22)) + p.drawLine(-half, yy, half, yy) + + p.restore() + p.setClipping(False) + + # ---- roll arc & tick marks (outside clip, fixed frame) ---- + p.save() + p.translate(cx, cy) + + arc_r = r - 2 + p.setPen(QPen(QColor("#FFFFFF"), 1)) + # draw arc from -60° to +60° (Qt arc: 0=3o'clock, CCW, 16ths of deg) + p.drawArc(int(-arc_r), int(-arc_r), int(2*arc_r), int(2*arc_r), + (90 - 60) * 16, 120 * 16) + + # tick marks at 0, ±10, ±20, ±30, ±45, ±60 + for deg in [0, 10, 20, 30, 45, 60, -10, -20, -30, -45, -60]: + rad = math.radians(deg - 90) + tick = 6 if deg % 30 == 0 else 4 + x1 = arc_r * math.cos(rad) + y1 = arc_r * math.sin(rad) + x2 = (arc_r - tick) * math.cos(rad) + y2 = (arc_r - tick) * math.sin(rad) + p.drawLine(QPointF(x1, y1), QPointF(x2, y2)) + + # roll pointer triangle (rotates with roll) + p.rotate(self.roll) + ptr_r = arc_r - 1 + tri = QPolygonF([ + QPointF(0, -ptr_r), + QPointF(-5, -ptr_r + 9), + QPointF(5, -ptr_r + 9), + ]) + p.setBrush(QColor("#FFFFFF")) + p.setPen(Qt.PenStyle.NoPen) + p.drawPolygon(tri) + p.restore() + + # ---- fixed aircraft symbol ---- + p.save() + p.translate(cx, cy) + p.setPen(QPen(QColor("#FFD700"), 2)) + # left wing + p.drawLine(int(-r*0.5), 0, int(-r*0.15), 0) + p.drawLine(int(-r*0.15), 0, int(-r*0.15), int(r*0.12)) + # right wing + p.drawLine(int(r*0.15), 0, int(r*0.5), 0) + p.drawLine(int(r*0.15), 0, int(r*0.15), int(r*0.12)) + # centre dot + p.setBrush(QColor("#FFD700")) + p.drawEllipse(QPointF(0, 0), 2.5, 2.5) + p.restore() + + # ---- outer ring ---- + p.setPen(QPen(QColor("#888888"), 1)) + p.setBrush(Qt.BrushStyle.NoBrush) + p.drawEllipse(QPointF(cx, cy), r, r) + + # ---- heading strip at bottom ------------------------------------------ + def _draw_heading_strip(self, p): + w = self.width() + x0, y0, side, _ = self._adi_rect() + strip_y = y0 + side + strip_h = self.height() - strip_y + if strip_h < 4: + return + + # background + p.fillRect(0, int(strip_y), w, strip_h, QColor(30, 30, 30)) + + # heading text centred (bigger) + p.setPen(QPen(QColor("#FFFFFF"))) + p.setFont(QFont("Arial", 10, QFont.Weight.Bold)) + hdg_str = f"{int(self.heading)}°" + p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str) \ No newline at end of file diff --git a/src/GUI/gui.py b/src/GUI/gui.py index d73b08d..015167f 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -22,9 +22,11 @@ from mission_planner import FormationPlanner # ================================================================================ class ControlStationUI(QMainWindow): + VERSION = '1.0.0' + def __init__(self): super().__init__() - self.setWindowTitle('GCS') + self.setWindowTitle(f'GCS v{self.VERSION}') self.resize(1400, 900) # 初始化ROS2 @@ -44,6 +46,7 @@ class ControlStationUI(QMainWindow): # 初始化UI self.drones = {} self.socket_groups = {} + self.socket_types = {} self.socket_colors = { '0': '#00BFFF', # 天藍色 (DeepSkyBlue) @@ -143,37 +146,40 @@ class ControlStationUI(QMainWindow): """) batch_control_layout.addWidget(batch_title) - # 上方按鈕區域 - upper_buttons = QHBoxLayout() + # 第一行：全選按鈕 + first_row = QHBoxLayout() select_all_btn = QPushButton("全選") select_all_btn.clicked.connect(self.handle_select_all) - arm_all_btn = QPushButton("解鎖") - arm_all_btn.clicked.connect(self.handle_arm_selected) - 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(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 12px; - border-radius: 4px; - min-width: 80px; - } - QPushButton:hover { background-color: #555; } - """) - upper_buttons.addWidget(btn) - upper_buttons.addStretch() - - # 模式切換區域 + select_all_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + first_row.addWidget(select_all_btn) + first_row.addStretch() + + # 第二行：模式切換 mode_layout = QHBoxLayout() 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.addItems([ + "GUIDED", "AUTO", "LAND", "LOITER", + "STABILIZE", "ACRO", "ALT_HOLD", "RTL", + "CIRCLE", "DRIFT", "SPORT", "FLIP", + "AUTOTUNE", "POSHOLD", "BRAKE", "THROW", + "AVOID_ADSB", "GUIDED_NOGPS", "SMART_RTL", + "FLOWHOLD", "FOLLOW", "ZIGZAG", "SYSTEMID", + "AUTOROTATE", "AUTO_RTL" + ]) self.mode_combo.setCurrentIndex(1) self.mode_combo.setStyleSheet(""" QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} @@ -197,36 +203,42 @@ class ControlStationUI(QMainWindow): mode_layout.addWidget(batch_mode_btn) mode_layout.addStretch() - # 座標輸入區域 - coord_widget = QWidget() - coord_layout = QHBoxLayout(coord_widget) + # 第三行：解鎖按鈕 + third_row = QHBoxLayout() + arm_all_btn = QPushButton("解鎖") + arm_all_btn.clicked.connect(self.handle_arm_selected) + arm_all_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + third_row.addWidget(arm_all_btn) + third_row.addStretch() + + # 第四行：高度輸入和起飛按鈕 + fourth_row = QHBoxLayout() - self.x_input = QLineEdit() - self.y_input = QLineEdit() self.z_input = QLineEdit() + self.z_input.setFixedWidth(60) + self.z_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 3px; + } + """) - for input_field in [self.x_input, self.y_input, self.z_input]: - input_field.setFixedWidth(60) - input_field.setStyleSheet(""" - QLineEdit { - background-color: #333; - color: #DDD; - border: 1px solid #555; - border-radius: 4px; - padding: 3px; - } - """) - - coord_layout.addWidget(QLabel("X:", styleSheet="color: #DDD;")) - coord_layout.addWidget(self.x_input) - coord_layout.addWidget(QLabel("Y:", styleSheet="color: #DDD;")) - coord_layout.addWidget(self.y_input) - coord_layout.addWidget(QLabel("Z:", styleSheet="color: #DDD;")) - coord_layout.addWidget(self.z_input) - - setpoint_btn = QPushButton("Setpoint") - setpoint_btn.clicked.connect(self.handle_setpoint_selected) - setpoint_btn.setStyleSheet(""" + takeoff_all_btn = QPushButton("起飛") + takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) + takeoff_all_btn.setStyleSheet(""" QPushButton { background-color: #444; color: #DDD; @@ -237,16 +249,17 @@ class ControlStationUI(QMainWindow): } QPushButton:hover { background-color: #555; } """) - - lower_control = QHBoxLayout() - lower_control.addWidget(coord_widget) - lower_control.addWidget(setpoint_btn) - lower_control.addStretch() + + fourth_row.addWidget(QLabel("高度:", styleSheet="color: #DDD;")) + fourth_row.addWidget(self.z_input) + fourth_row.addWidget(takeoff_all_btn) + fourth_row.addStretch() # 組裝批次控制 layout - batch_control_layout.addLayout(upper_buttons) - batch_control_layout.addLayout(mode_layout) - batch_control_layout.addLayout(lower_control) + batch_control_layout.addLayout(first_row) + batch_control_layout.addLayout(mode_layout) + batch_control_layout.addLayout(third_row) + batch_control_layout.addLayout(fourth_row) # 將批次控制 layout 添加到右側容器 right_layout.addLayout(batch_control_layout) @@ -255,6 +268,8 @@ class ControlStationUI(QMainWindow): right_layout.addWidget(self.drone_map.get_widget()) self.drone_map.get_gps_signal().connect(self.handle_map_click) self.drone_map.get_drone_clicked_signal().connect(self.handle_drone_clicked) + self.drone_map.get_clear_all_drone_selection_signal().connect(self.handle_clear_all_drone_selection) + self.drone_map.get_toggle_select_all_drones_signal().connect(self.handle_toggle_select_all_drones) # Add widgets to splitter @@ -479,7 +494,9 @@ class ControlStationUI(QMainWindow): def create_socket_group_panel(self, socket_id): """創建 socket 分組面板""" color = self.socket_colors.get(socket_id, self.socket_colors['default']) - panel = SocketGroupPanel(socket_id, color) + # 如果已知socket類型，傳遞給panel + socket_type = self.socket_types.get(socket_id, None) + panel = SocketGroupPanel(socket_id, color, socket_type) panel.group_selection_changed.connect(self.handle_group_selection) return panel @@ -541,6 +558,22 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) def update_ui(self, msg_type, drone_id, data): + # 優先處理 connection_type，即使 drone 還不存在 + if msg_type == 'connection_type': + # 獲取連接類型和socket ID + conn_type = data.get('type', 'Unknown') + # 從 drone_id 提取 socket_id (格式: s{socket}_{sys}) + parts = drone_id.split('_') + if len(parts) >= 2 and parts[0].startswith('s'): + socket_id = parts[0][1:] # 移除 's' 前綴 + # 只在第一次收到時更新 + if socket_id not in self.socket_types: + self.socket_types[socket_id] = conn_type + # 如果 socket group 已存在，更新標題 + if socket_id in self.socket_groups: + self.socket_groups[socket_id].set_socket_type(conn_type) + return + # 檢查是否為新無人機，若是則加入無人機面板 if drone_id not in self.drones: self.add_drone(drone_id) @@ -595,27 +628,22 @@ class ControlStationUI(QMainWindow): percentage = data.get('percentage', percentage) - # 顯示總電壓、電池節數、單節電壓和百分比 - text = f"{percentage:.0f}%" + # 顯示百分比與電壓（分開欄位） + pct_text = f"{percentage:.0f}%" vol = f"{voltage:.2f}V" - - self.update_field(panel, drone_id, 'battery', text, voltage_color) + cells_text = f"{cells}S" + + self.update_field(panel, drone_id, 'battery_pct', pct_text, voltage_color) + self.update_field(panel, drone_id, 'battery_vol', vol) + self.update_field(panel, drone_id, 'battery_cells', cells_text) + # 維持 overview table 的 battery 欄位為電壓 self.update_overview_table(drone_id, 'battery', vol) elif msg_type == 'gps': + # 仍然儲存 GPS 到內部監控，但不在面板上顯示經緯度欄位 lat, lon = data.get('lat', 0), data.get('lon', 0) self.drone_positions[drone_id] = (lat, lon) - lon_text = f"{lon:.6f}°" - lat_text = f"{lat:.6f}°" - self.update_field(panel, drone_id, 'longitude', lon_text) - self.update_field(panel, drone_id, 'latitude', lat_text) - self.update_overview_table(drone_id, 'longitude', lon_text) - self.update_overview_table(drone_id, 'latitude', lat_text) - - # ================================================================================ - # 【新增】同時儲存到 monitor.drone_gps（處理 UDP/WebSocket 的 GPS 資料） - # ================================================================================ - alt = data.get('alt', 0) # UDP/WebSocket 可能沒有 alt + alt = data.get('alt', 0) if not hasattr(self.monitor, 'drone_gps'): self.monitor.drone_gps = {} self.monitor.drone_gps[drone_id] = { @@ -623,10 +651,12 @@ class ControlStationUI(QMainWindow): 'lon': lon, 'alt': alt } - # ================================================================================ + # 更新 overview table 的經緯度 + self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°") + self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°") - # 更新地圖上的無人機位置 - heading = self.drone_headings.get(drone_id, 0) # 如果沒有航向，使用 0 + # 更新地圖上的無人機位置（地圖仍需經緯度） + heading = self.drone_headings.get(drone_id, 0) self.drone_map.update_drone_position(drone_id, lat, lon, heading) elif msg_type == 'altitude': @@ -636,9 +666,19 @@ class ControlStationUI(QMainWindow): self.update_overview_table(drone_id, 'altitude', text) elif msg_type == 'local_pose': - text = f"{data['x']:.1f}, {data['y']:.1f}" - self.update_field(panel, drone_id, 'local', text) - self.update_overview_table(drone_id, 'local', text) + # 更新 local 座標並顯示在 overview table + x = data.get('x', 0) + y = data.get('y', 0) + z = data.get('z', 0) + if not hasattr(self.monitor, 'drone_local'): + self.monitor.drone_local = {} + self.monitor.drone_local[drone_id] = { + 'x': x, + 'y': y + } + # 更新 overview table 的位置欄位 (只顯示 x, y) + local_text = f"{x:.1f}, {y:.1f}" + self.update_overview_table(drone_id, 'local', local_text) elif msg_type == 'hud': heading = data.get('heading') @@ -677,6 +717,7 @@ class ControlStationUI(QMainWindow): self.update_field(panel, drone_id, 'heading', heading_text) self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) + self.update_field(panel, drone_id, 'speed', groundspeed_text) self.update_overview_table(drone_id, 'heading', heading_text) self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) self.update_overview_table(drone_id, 'airspeed', airspeed_text) @@ -684,6 +725,14 @@ class ControlStationUI(QMainWindow): self.update_overview_table(drone_id, 'hud_alt', hud_alt_text) self.update_overview_table(drone_id, 'climb', climb_text) + # 更新態度指示器的航向（如果有roll/pitch數據，下面的attitude訊息會更新） + if panel and hasattr(panel, 'attitude_indicator') and panel.attitude_indicator: + if not hasattr(panel, '_last_roll'): + panel._last_roll = 0 + if not hasattr(panel, '_last_pitch'): + panel._last_pitch = 0 + panel.attitude_indicator.update_attitude(heading, panel._last_roll, panel._last_pitch) + # 如果有位置資訊，也更新地圖上的航向 if drone_id in self.drone_positions: lat, lon = self.drone_positions[drone_id] @@ -715,6 +764,16 @@ class ControlStationUI(QMainWindow): self.update_overview_table(drone_id, 'roll', roll_text) self.update_overview_table(drone_id, 'pitch', pitch_text) self.update_overview_table(drone_id, 'yaw', yaw_text) + + # 儲存roll/pitch供attitude指示器使用，並使用現存的航向 + if panel: + panel._last_roll = roll + panel._last_pitch = pitch + + # 更新態度指示器（使用現存航向或預設0） + if panel and hasattr(panel, 'update_attitude'): + heading = self.drone_headings.get(drone_id, 0) + panel.update_attitude(heading, roll, pitch) # 新增處理分組勾選的方法 def handle_group_selection(self, socket_id, state): @@ -1048,6 +1107,55 @@ class ControlStationUI(QMainWindow): # 切換勾選狀態 checkbox.setChecked(not checkbox.isChecked()) + def handle_clear_all_drone_selection(self): + """清除所有無人機的勾選狀態""" + print("清除所有無人機選擇") + + for drone_id in self.drones.keys(): + checkbox = self.drones[drone_id].get_checkbox() + if checkbox: + checkbox.blockSignals(True) + checkbox.setChecked(False) + checkbox.blockSignals(False) + + # 清除選中集合 + self.monitor.selected_drones.clear() + + # 更新所有分組的勾選框狀態 + for socket_id in self.socket_groups.keys(): + self.update_group_checkbox_state(socket_id) + + def handle_toggle_select_all_drones(self): + """切換全選/取消全選所有無人機""" + # 檢查是否已經全選 + all_selected = all(self.drones[drone_id].get_checkbox().isChecked() + for drone_id in self.drones.keys()) + + if all_selected: + # 已全選，則取消全選 + print("取消全選所有無人機") + for drone_id in self.drones.keys(): + checkbox = self.drones[drone_id].get_checkbox() + if checkbox: + checkbox.blockSignals(True) + checkbox.setChecked(False) + checkbox.blockSignals(False) + self.monitor.selected_drones.clear() + else: + # 未全選，則全選 + print("全選所有無人機") + for drone_id in self.drones.keys(): + checkbox = self.drones[drone_id].get_checkbox() + if checkbox: + checkbox.blockSignals(True) + checkbox.setChecked(True) + checkbox.blockSignals(False) + self.monitor.selected_drones.add(drone_id) + + # 更新所有分組的勾選框狀態 + for socket_id in self.socket_groups.keys(): + self.update_group_checkbox_state(socket_id) + def show_planned_waypoints(self): """ 顯示規劃的航點（在終端輸出） diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py index c77c1fb..bd6d5c0 100644 --- a/src/GUI/map_layout.py +++ b/src/GUI/map_layout.py @@ -43,6 +43,14 @@ class DroneMap: top: 10px; right: 10px; z-index: 1000; + background-color: rgba(255, 255, 255, 0.95); + padding: 10px; + border-radius: 6px; + box-shadow: 0 2px 8px rgba(0,0,0,0.3); + display: flex; + flex-direction: column; + gap: 5px; + min-width: 150px; } .map-controls-bottom { position: absolute; @@ -52,7 +60,7 @@ class DroneMap: } .control-button { padding: 8px 12px; - background-color: #F44336; + background-color: #EF5350; color: white; border: none; border-radius: 4px; @@ -62,21 +70,7 @@ class DroneMap: box-shadow: 0 2px 5px rgba(0,0,0,0.2); } .control-button:hover { - background-color: #D32F2F; - } - .map-toggle-button { - padding: 8px 12px; - background-color: #2E7D32; - color: white; - border: none; - border-radius: 4px; - cursor: pointer; - font-size: 13px; - font-weight: bold; - box-shadow: 0 2px 5px rgba(0,0,0,0.2); - } - .map-toggle-button:hover { - background-color: #1B5E20; + background-color: #E53935; } .mission-info-panel { position: absolute; @@ -128,8 +122,24 @@ class DroneMap: margin-bottom: 10px; padding-bottom: 10px; border-bottom: 1px solid #ddd; + } .selection-button-blue { + width: 100%; + padding: 8px; + background-color: #64B5F6; + color: white; + border: none; + border-radius: 4px; + cursor: pointer; + font-size: 13px; + font-weight: bold; + transition: background-color 0.2s; } - .selection-button { + .selection-button-blue:hover { + background-color: #42A5F5; + } + .selection-button-blue.active { + background-color: #1976D2; + } .selection-button-blue { width: 100%; padding: 8px; background-color: #64B5F6; @@ -141,27 +151,43 @@ class DroneMap: font-weight: bold; transition: background-color 0.2s; } - .selection-button:hover { + .selection-button-blue:hover { background-color: #42A5F5; } - .selection-button.active { + .selection-button-blue.active { background-color: #1976D2; } + .selection-button { + width: 100%; + padding: 8px; + background-color: #F06A61; + color: white; + border: none; + border-radius: 4px; + cursor: pointer; + font-size: 13px; + font-weight: bold; + transition: background-color 0.2s; + } + .selection-button:hover { + background-color: #E53935; + } + .selection-button.active { + background-color: #D32F2F; + }
- -
-
- + + +
- - - + +
中心點: @@ -172,6 +198,7 @@ class DroneMap: 未設定
+