import time import sys import os from PyQt6.QtWidgets import ( QApplication, QWidget, QVBoxLayout, QHBoxLayout, QPushButton, QLabel, QFrame, QGridLayout, QSpacerItem, QSizePolicy ) from PyQt6.QtGui import QPixmap, QImage, QFont from PyQt6.QtCore import Qt, QTimer import numpy as np import cv2 from lidar_processor import LidarProcessor # Import LIDAR processing def hex_to_bgr(hex_color): """Convert HEX to OpenCV BGR""" hex_color = hex_color.lstrip("#") r, g, b = tuple(int(hex_color[i:i+2], 16) for i in (0, 2, 4)) return (b, g, r) class RobotGUI(QWidget): def __init__(self, lidar_processor): super().__init__() self.lidar_processor = lidar_processor self.robot_path = [] self.setWindowTitle("Robot Control GUI") self.setGeometry(100, 100, 800, 480) self.showFullScreen() # Colors self.bg_color = hex_to_bgr("#2E3440") self.wall_color = hex_to_bgr("#FFD700") # Layout layout = QVBoxLayout() self.setLayout(layout) # Top Bar status_bar = QHBoxLayout() self.icon_label = QLabel("🤖") self.icon_label.setFont(QFont("Noto Color Emoji", 20)) self.battery_label = QLabel("Battery: 100%") self.speed_label = QLabel("Speed: 0 m/s") self.power_label = QLabel("Power: 0W") for widget in [self.icon_label, self.battery_label, self.speed_label, self.power_label]: status_bar.addWidget(widget) layout.addLayout(status_bar) # Main Content main_content = QHBoxLayout() layout.addLayout(main_content) # Buttons button_container = QVBoxLayout() button_grid = QGridLayout() self.start_btn = QPushButton("Start") self.stop_btn = QPushButton("Stop") self.feature_btn = QPushButton("Feature") self.drift_btn = QPushButton("Drift") button_size = 150 for btn in [self.start_btn, self.stop_btn, self.feature_btn, self.drift_btn]: btn.setObjectName("controlButton") btn.setMinimumSize(button_size, button_size) button_grid.addWidget(self.start_btn, 0, 0) button_grid.addWidget(self.stop_btn, 0, 1) button_grid.addWidget(self.feature_btn, 1, 0) button_grid.addWidget(self.drift_btn, 1, 1) button_container.addSpacerItem(QSpacerItem(20, 40, QSizePolicy.Policy.Minimum, QSizePolicy.Policy.Expanding)) button_container.addLayout(button_grid) main_content.addLayout(button_container) # Map Display self.map_label = QLabel() self.map_label.setObjectName("mapLabel") self.map_label.setFrameStyle(QFrame.Shape.Box) self.map_label.setAlignment(Qt.AlignmentFlag.AlignCenter) self.map_label.setMinimumSize(350, 350) main_content.addWidget(self.map_label) # Timer self.timer = QTimer(self) self.timer.timeout.connect(self.update_map) self.timer.start(100) self.show() # Load stylesheet self.load_stylesheet() def load_stylesheet(self): try: qss_path = os.path.join(os.path.dirname(__file__), "styles.qss") with open(qss_path, "r") as f: self.setStyleSheet(f.read()) except FileNotFoundError: print("Warning: styles.qss not found. Running without stylesheet.") def update_map(self): map_img, pose = self.lidar_processor.get_data() height, width = map_img.shape # --- Colors --- nord_bgr = (64, 52, 46) # Nord Background (#2E3440) yellow_bgr = (0, 215, 255) # Cyberpunk Yellow (#FFD700) white_bgr = (255, 255, 255) green_bgr = (0, 255, 128) # Cyberpunk Green (custom pick) # --- Start with yellow background --- color_map = np.full((height, width, 3), yellow_bgr, dtype=np.uint8) # --- Threshold walls from map --- _, wall_mask = cv2.threshold(map_img, 100, 255, cv2.THRESH_BINARY) # --- Apply Nord Background where wall mask is present color_map[wall_mask == 255] = nord_bgr # --- Convert robot pose to pixel coordinates --- MM_TO_PIXELS = 100 CENTER = height // 2 x_pix = int(pose[0] * MM_TO_PIXELS + CENTER) y_pix = int(pose[1] * MM_TO_PIXELS + CENTER) # Determine movement speed (basic) if len(self.robot_path) > 1: dx = self.robot_path[-1][0] - self.robot_path[-2][0] dy = self.robot_path[-1][1] - self.robot_path[-2][1] movement = np.hypot(dx, dy) else: movement = 0 # Breathing pulse (only if moving) base_radius = 6 pulse_amplitude = 2 # how big the pulse gets pulse_speed = 2.0 # Hz if movement > 1: # Animate based on time t = time.time() pulse = int(pulse_amplitude * (0.5 + 0.5 * np.sin(2 * np.pi * pulse_speed * t))) dot_radius = base_radius + pulse else: dot_radius = base_radius # --- Draw robot as white dot --- cv2.circle(color_map, (x_pix, y_pix), dot_radius, white_bgr, -1) # --- Trail logic --- self.robot_path.append((x_pix, y_pix)) if len(self.robot_path) > 50: self.robot_path.pop(0) # --- Draw fading green trail --- for i in range(1, len(self.robot_path)): alpha = int(255 * (i / len(self.robot_path))) # Fade effect trail_color = ( int(green_bgr[0] * (alpha / 255)), int(green_bgr[1] * (alpha / 255)), int(green_bgr[2] * (alpha / 255)) ) cv2.line(color_map, self.robot_path[i - 1], self.robot_path[i], trail_color, 1) # --- Convert to RGB for Qt display --- rgb_image = cv2.cvtColor(color_map, cv2.COLOR_BGR2RGB) qimg = QImage(rgb_image.data, width, height, width * 3, QImage.Format.Format_RGB888) self.map_label.setPixmap(QPixmap.fromImage(qimg)) def close_application(self): print("🔌 Stopping LIDAR and closing GUI...") self.timer.stop() self.lidar_processor.stop() self.close() sys.exit() def closeEvent(self, event): self.close_application() if __name__ == "__main__": app = QApplication(sys.argv) lidar_processor = LidarProcessor() window = RobotGUI(lidar_processor) sys.exit(app.exec())