Replace 2D robot diagram with 3D wireframe visualization
Major UI overhaul replacing the 2D top-down robot diagram with a full 3D wireframe visualization using PyQtGraph OpenGL. 3D Robot Visualization: - Cylindrical body with configurable segment density - 3D extruded bumper arcs (left/right halves with collision colors) - Cylindrical drive wheels with hub, spokes, and rolling animation - Lidar turret with inner spinning element when enabled - Roller brush with spiral patterns rotating on X axis - 3-arm side brush with bristle details rotating on Z axis - Vacuum fan impeller with blade animation - Cliff sensors and button indicators - IMU-driven orientation (roll/pitch/yaw transforms all components) - Camera presets: Isometric, Top, Front, Side views Battery Status Overlay: - Real-time battery percentage with color coding - Voltage display (e.g., "14.5V") - Animated charging indicator with color cycling - Battery bar with gradient animation when charging Control Panel Improvements: - Reorganized layout: Motion Control at top, then Cleaning, Sensors, etc. - Collapsible groups for each section - Compact 3-column actuator row (Vacuum, Side Brush, Main Brush) - Emergency Stop button with Reset command - All sections use consistent dark theme styling Files Added: - robot_3d_geometry.py: Wireframe generation (cylinders, arcs, wheels, etc.) - robot_3d_view.py: Main 3D visualization widget with overlays - collapsible_group.py: Reusable expandable/collapsible container - compact_actuator.py: Mini actuator controls for horizontal layout Files Removed: - robot_diagram.py: Old 2D QPainter-based visualization - imu_3d_view.py: Separate IMU cube (now integrated into main 3D view)
This commit is contained in:
codetiger
Binary file not shown.
|
Before Width: | Height: | Size: 189 KiB After Width: | Height: | Size: 793 KiB |
+29
-34
@@ -1,7 +1,8 @@
|
||||
"""
|
||||
Main window for Drishti robot visualization application.
|
||||
|
||||
Single full-screen widget showing robot diagram with sensor overlays.
|
||||
Full-screen 3D wireframe robot visualization with real-time sensor overlays
|
||||
and IMU-based orientation display.
|
||||
"""
|
||||
|
||||
from PyQt5.QtCore import Qt, pyqtSlot, QEvent
|
||||
@@ -10,9 +11,8 @@ from PyQt5.QtGui import QFont, QKeyEvent
|
||||
import logging
|
||||
|
||||
from ui.threads.telemetry_thread import TelemetryThread
|
||||
from ui.widgets.robot_diagram import RobotDiagram
|
||||
from ui.widgets.robot_3d_view import Robot3DView
|
||||
from ui.widgets.control_panel import ControlPanel
|
||||
from ui.widgets.imu_3d_view import IMU3DView
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
@@ -59,15 +59,9 @@ class MainWindow(QMainWindow):
|
||||
content_layout.setContentsMargins(0, 0, 0, 0)
|
||||
content_layout.setSpacing(0)
|
||||
|
||||
# Robot diagram (left side, takes remaining space)
|
||||
self.robot_diagram = RobotDiagram()
|
||||
content_layout.addWidget(self.robot_diagram, 1) # Stretch factor 1 (expands)
|
||||
|
||||
# IMU 3D view overlay (top-left corner of robot diagram)
|
||||
self.imu_3d_view = IMU3DView()
|
||||
self.imu_3d_view.setParent(self.robot_diagram)
|
||||
self.imu_3d_view.setGeometry(10, 10, 200, 220)
|
||||
self.imu_3d_view.raise_() # Ensure it's on top
|
||||
# 3D robot view (left side, takes remaining space)
|
||||
self.robot_3d_view = Robot3DView()
|
||||
content_layout.addWidget(self.robot_3d_view, 1) # Stretch factor 1 (expands)
|
||||
|
||||
# Control panel (right side, fixed width) - dark theme
|
||||
self.control_panel = ControlPanel()
|
||||
@@ -129,33 +123,22 @@ class MainWindow(QMainWindow):
|
||||
@pyqtSlot(dict)
|
||||
def _on_sensor_data(self, data: dict):
|
||||
"""Handle sensor data from telemetry thread."""
|
||||
# Update robot diagram with new sensor values
|
||||
self.robot_diagram.update_sensors(data)
|
||||
# Update 3D robot view with new sensor values
|
||||
self.robot_3d_view.update_sensors(data)
|
||||
|
||||
# Update control panel with sensor data
|
||||
self.control_panel.update_sensors(data)
|
||||
|
||||
# Forward IMU data to 3D view
|
||||
# Forward IMU data to 3D view for orientation
|
||||
imu_keys = ['gyro_x', 'gyro_y', 'gyro_z', 'tilt_x', 'tilt_y', 'tilt_z']
|
||||
if all(k in data for k in imu_keys):
|
||||
# Debug: log IMU values periodically
|
||||
if hasattr(self, '_imu_debug_counter'):
|
||||
self._imu_debug_counter += 1
|
||||
else:
|
||||
self._imu_debug_counter = 0
|
||||
if self._imu_debug_counter % 500 == 0:
|
||||
logger.info(f"IMU data: gyro=({data['gyro_x']}, {data['gyro_y']}, {data['gyro_z']}), "
|
||||
f"tilt=({data['tilt_x']}, {data['tilt_y']}, {data['tilt_z']})")
|
||||
|
||||
self.imu_3d_view.update_orientation(
|
||||
self.robot_3d_view.update_orientation(
|
||||
data['gyro_x'], data['gyro_y'], data['gyro_z'],
|
||||
data['tilt_x'], data['tilt_y'], data['tilt_z']
|
||||
)
|
||||
else:
|
||||
# Debug: log which keys are missing
|
||||
if hasattr(self, '_imu_missing_logged'):
|
||||
pass
|
||||
else:
|
||||
# Debug: log which keys are missing (once)
|
||||
if not hasattr(self, '_imu_missing_logged'):
|
||||
self._imu_missing_logged = True
|
||||
missing = [k for k in imu_keys if k not in data]
|
||||
logger.warning(f"IMU keys missing from data: {missing}. Available keys: {list(data.keys())}")
|
||||
@@ -181,9 +164,11 @@ class MainWindow(QMainWindow):
|
||||
if component_id == 'lidar':
|
||||
if action_type == 'Enable':
|
||||
self.lidar_enabled = True
|
||||
self.robot_3d_view.set_lidar_enabled(True)
|
||||
self.status_bar.showMessage("Lidar ENABLED", 2000)
|
||||
elif action_type == 'Disable':
|
||||
self.lidar_enabled = False
|
||||
self.robot_3d_view.set_lidar_enabled(False)
|
||||
self.status_bar.showMessage("Lidar DISABLED", 2000)
|
||||
return
|
||||
|
||||
@@ -213,9 +198,9 @@ class MainWindow(QMainWindow):
|
||||
elif component_id == 'main_brush':
|
||||
self.main_brush_enabled = speed > 0
|
||||
|
||||
# Map component_id back to actuator for robot diagram
|
||||
# Map component_id back to actuator for 3D view
|
||||
actuator = component_id if component_id != 'main_brush' else 'brush'
|
||||
self.robot_diagram.update_actuator(actuator, speed)
|
||||
self.robot_3d_view.update_actuator(actuator, speed)
|
||||
self.status_bar.showMessage(f"{component_id}: {speed}%", 2000)
|
||||
return
|
||||
|
||||
@@ -252,6 +237,7 @@ class MainWindow(QMainWindow):
|
||||
# Toggle lidar
|
||||
self.lidar_enabled = not self.lidar_enabled
|
||||
self.control_panel.set_lidar_enabled(self.lidar_enabled)
|
||||
self.robot_3d_view.set_lidar_enabled(self.lidar_enabled)
|
||||
if self.lidar_enabled:
|
||||
self._send_command({
|
||||
"type": "ComponentControl",
|
||||
@@ -285,6 +271,7 @@ class MainWindow(QMainWindow):
|
||||
"action": {"type": "Disable"}
|
||||
})
|
||||
self.control_panel.set_actuator_state("side_brush", self.side_brush_enabled)
|
||||
self.robot_3d_view.update_actuator("side_brush", speed)
|
||||
state = "ON" if self.side_brush_enabled else "OFF"
|
||||
self.status_bar.showMessage(f"Side Brush {state}", 2000)
|
||||
logger.info(f"Side brush {state}")
|
||||
@@ -305,6 +292,7 @@ class MainWindow(QMainWindow):
|
||||
"action": {"type": "Disable"}
|
||||
})
|
||||
self.control_panel.set_actuator_state("brush", self.main_brush_enabled)
|
||||
self.robot_3d_view.update_actuator("brush", speed)
|
||||
state = "ON" if self.main_brush_enabled else "OFF"
|
||||
self.status_bar.showMessage(f"Main Brush {state}", 2000)
|
||||
logger.info(f"Main brush {state}")
|
||||
@@ -325,6 +313,7 @@ class MainWindow(QMainWindow):
|
||||
"action": {"type": "Disable"}
|
||||
})
|
||||
self.control_panel.set_actuator_state("vacuum", self.vacuum_enabled)
|
||||
self.robot_3d_view.update_actuator("vacuum", speed)
|
||||
state = "ON" if self.vacuum_enabled else "OFF"
|
||||
self.status_bar.showMessage(f"Vacuum {state}", 2000)
|
||||
logger.info(f"Vacuum {state}")
|
||||
@@ -372,11 +361,17 @@ class MainWindow(QMainWindow):
|
||||
self.angular_velocity = -self.angular_step
|
||||
self._send_velocity()
|
||||
elif event.key() == Qt.Key_Space:
|
||||
# Emergency stop
|
||||
# Emergency stop - send Reset command to immediately halt
|
||||
self.linear_velocity = 0.0
|
||||
self.angular_velocity = 0.0
|
||||
self._send_velocity()
|
||||
self.status_bar.showMessage("STOP", 2000)
|
||||
self._send_command({
|
||||
"type": "ComponentControl",
|
||||
"id": "drive",
|
||||
"action": {"type": "Reset"}
|
||||
})
|
||||
self.control_panel.set_velocity(0.0, 0.0)
|
||||
self.status_bar.showMessage("E-STOP", 2000)
|
||||
logger.info("Emergency stop triggered")
|
||||
else:
|
||||
super().keyPressEvent(event)
|
||||
|
||||
|
||||
@@ -1,24 +1,22 @@
|
||||
"""
|
||||
IMU Processor - Orientation estimation from tilt vector and gyroscope.
|
||||
IMU Processor - Orientation estimation from gyroscope integration.
|
||||
|
||||
The tilt vector from GD32 is already low-pass filtered, so we use it
|
||||
directly for roll/pitch. Gyroscope is integrated for yaw (which will drift).
|
||||
Integrates all three gyroscope axes for real-time responsive rotation.
|
||||
Note: Will drift over time without absolute reference (accelerometer/magnetometer).
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
|
||||
class IMUProcessor:
|
||||
"""
|
||||
IMU orientation processor.
|
||||
IMU orientation processor using pure gyroscope integration.
|
||||
|
||||
Uses the pre-filtered tilt vector directly for roll/pitch angles.
|
||||
Integrates gyroscope for yaw (no absolute reference without magnetometer).
|
||||
Integrates gyroscope for all axes (roll, pitch, yaw) at 500Hz.
|
||||
Provides smooth real-time orientation but will drift over time.
|
||||
"""
|
||||
|
||||
# Scale factor for raw gyro I16 values to degrees/sec
|
||||
# Typical MPU6050: 131 LSB/(deg/s) at ±250 deg/s range
|
||||
GYRO_SCALE = 1.0 / 131.0 # degrees/sec per LSB
|
||||
# Scale factor: raw gyro I16 values to degrees/sec
|
||||
# Empirically tuned to match physical rotation on CRL-200S
|
||||
GYRO_SCALE = 1.0 / 10.0
|
||||
|
||||
def __init__(self, sample_rate_hz: float = 500.0):
|
||||
"""
|
||||
@@ -34,7 +32,7 @@ class IMUProcessor:
|
||||
self.pitch = 0.0 # Y-axis rotation (tilt forward/back)
|
||||
self.yaw = 0.0 # Z-axis rotation (heading)
|
||||
|
||||
# Decimation for UI updates (500Hz -> 100Hz for smoother response)
|
||||
# Decimation for UI updates (500Hz -> 100Hz for label updates)
|
||||
self._counter = 0
|
||||
self._decimation = 5 # Update every 5th sample = 100Hz
|
||||
|
||||
@@ -45,31 +43,24 @@ class IMUProcessor:
|
||||
|
||||
Args:
|
||||
gyro_x, gyro_y, gyro_z: Raw gyroscope values (i16)
|
||||
tilt_x, tilt_y, tilt_z: Low-pass filtered gravity vector (i16)
|
||||
tilt_x, tilt_y, tilt_z: Filtered tilt values (unused, for API compat)
|
||||
|
||||
Returns:
|
||||
Tuple of (roll, pitch, yaw) in degrees
|
||||
"""
|
||||
# Calculate magnitude of tilt vector
|
||||
magnitude = math.sqrt(tilt_x**2 + tilt_y**2 + tilt_z**2)
|
||||
if magnitude < 100: # Near zero, sensor error
|
||||
return (self.roll, self.pitch, self.yaw)
|
||||
|
||||
# Roll and Pitch directly from tilt vector (already filtered by GD32)
|
||||
# Roll: rotation around X-axis (left/right tilt)
|
||||
self.roll = math.degrees(
|
||||
math.atan2(tilt_y, math.sqrt(tilt_x**2 + tilt_z**2))
|
||||
)
|
||||
|
||||
# Pitch: rotation around Y-axis (forward/back tilt)
|
||||
self.pitch = math.degrees(
|
||||
math.atan2(-tilt_x, math.sqrt(tilt_y**2 + tilt_z**2))
|
||||
)
|
||||
|
||||
# Yaw: integrate gyroscope Z (will drift without magnetometer)
|
||||
# Convert raw gyro to degrees/sec and integrate
|
||||
# Axis mapping tuned for CRL-200S physical orientation
|
||||
gx = gyro_x * self.GYRO_SCALE
|
||||
gy = gyro_y * self.GYRO_SCALE
|
||||
gz = gyro_z * self.GYRO_SCALE
|
||||
self.yaw += gz * self.dt
|
||||
|
||||
self.roll += gz * self.dt # Z gyro -> roll
|
||||
self.pitch += gy * self.dt # Y gyro -> pitch
|
||||
self.yaw += gx * self.dt # X gyro -> yaw
|
||||
|
||||
# Normalize to -180 to +180 range
|
||||
self.roll = ((self.roll + 180) % 360) - 180
|
||||
self.pitch = ((self.pitch + 180) % 360) - 180
|
||||
self.yaw = ((self.yaw + 180) % 360) - 180
|
||||
|
||||
return (self.roll, self.pitch, self.yaw)
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
"""Widgets for Drishti UI."""
|
||||
|
||||
from .robot_diagram import RobotDiagram
|
||||
from .robot_3d_view import Robot3DView
|
||||
from .control_panel import ControlPanel
|
||||
|
||||
__all__ = ['RobotDiagram']
|
||||
__all__ = ['Robot3DView', 'ControlPanel']
|
||||
|
||||
@@ -0,0 +1,125 @@
|
||||
"""
|
||||
Collapsible group widget for organizing control panel sections.
|
||||
"""
|
||||
|
||||
from PyQt5.QtCore import Qt, pyqtSignal
|
||||
from PyQt5.QtWidgets import QWidget, QVBoxLayout, QHBoxLayout, QLabel, QFrame
|
||||
|
||||
|
||||
class CollapsibleGroup(QWidget):
|
||||
"""
|
||||
A collapsible container widget with a clickable header.
|
||||
|
||||
Click the header to expand/collapse the content area.
|
||||
"""
|
||||
|
||||
toggled = pyqtSignal(bool) # Emits True when expanded, False when collapsed
|
||||
|
||||
def __init__(self, title: str, parent=None, expanded: bool = True):
|
||||
super().__init__(parent)
|
||||
self._expanded = expanded
|
||||
self._title = title
|
||||
|
||||
self._setup_ui()
|
||||
self._update_arrow()
|
||||
|
||||
def _setup_ui(self):
|
||||
"""Set up the widget layout."""
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(0, 0, 0, 0)
|
||||
layout.setSpacing(0)
|
||||
|
||||
# Header (clickable)
|
||||
self._header = QFrame()
|
||||
self._header.setStyleSheet("""
|
||||
QFrame {
|
||||
background-color: #3a3a3a;
|
||||
border-radius: 4px;
|
||||
padding: 0px;
|
||||
}
|
||||
QFrame:hover {
|
||||
background-color: #454545;
|
||||
}
|
||||
""")
|
||||
self._header.setCursor(Qt.PointingHandCursor)
|
||||
self._header.setFixedHeight(28)
|
||||
self._header.mousePressEvent = self._on_header_click
|
||||
|
||||
header_layout = QHBoxLayout(self._header)
|
||||
header_layout.setContentsMargins(10, 4, 10, 4)
|
||||
header_layout.setSpacing(8)
|
||||
|
||||
# Arrow indicator
|
||||
self._arrow_label = QLabel()
|
||||
self._arrow_label.setStyleSheet("color: #aaa; font-size: 10px; background: transparent;")
|
||||
self._arrow_label.setFixedWidth(12)
|
||||
header_layout.addWidget(self._arrow_label)
|
||||
|
||||
# Title
|
||||
self._title_label = QLabel(self._title)
|
||||
self._title_label.setStyleSheet("""
|
||||
color: #e0e0e0;
|
||||
font-weight: bold;
|
||||
font-size: 11px;
|
||||
background: transparent;
|
||||
""")
|
||||
header_layout.addWidget(self._title_label)
|
||||
header_layout.addStretch()
|
||||
|
||||
layout.addWidget(self._header)
|
||||
|
||||
# Content area
|
||||
self._content = QWidget()
|
||||
self._content_layout = QVBoxLayout(self._content)
|
||||
self._content_layout.setContentsMargins(0, 6, 0, 6)
|
||||
self._content_layout.setSpacing(4)
|
||||
layout.addWidget(self._content)
|
||||
|
||||
# Set initial state
|
||||
self._content.setVisible(self._expanded)
|
||||
|
||||
def _on_header_click(self, event):
|
||||
"""Handle header click to toggle expansion."""
|
||||
self.toggle()
|
||||
|
||||
def _update_arrow(self):
|
||||
"""Update the arrow indicator based on expansion state."""
|
||||
self._arrow_label.setText("▼" if self._expanded else "▶")
|
||||
|
||||
def toggle(self):
|
||||
"""Toggle the expanded/collapsed state."""
|
||||
self._expanded = not self._expanded
|
||||
self._content.setVisible(self._expanded)
|
||||
self._update_arrow()
|
||||
self.toggled.emit(self._expanded)
|
||||
|
||||
def expand(self):
|
||||
"""Expand the content area."""
|
||||
if not self._expanded:
|
||||
self.toggle()
|
||||
|
||||
def collapse(self):
|
||||
"""Collapse the content area."""
|
||||
if self._expanded:
|
||||
self.toggle()
|
||||
|
||||
def is_expanded(self) -> bool:
|
||||
"""Return whether the content is expanded."""
|
||||
return self._expanded
|
||||
|
||||
def set_title(self, title: str):
|
||||
"""Set the group title."""
|
||||
self._title = title
|
||||
self._title_label.setText(title)
|
||||
|
||||
def add_widget(self, widget: QWidget):
|
||||
"""Add a widget to the content area."""
|
||||
self._content_layout.addWidget(widget)
|
||||
|
||||
def add_layout(self, layout):
|
||||
"""Add a layout to the content area."""
|
||||
self._content_layout.addLayout(layout)
|
||||
|
||||
def content_layout(self):
|
||||
"""Return the content layout for direct manipulation."""
|
||||
return self._content_layout
|
||||
@@ -0,0 +1,218 @@
|
||||
"""
|
||||
Compact actuator controls for horizontal layout.
|
||||
"""
|
||||
|
||||
from PyQt5.QtCore import Qt, pyqtSignal
|
||||
from PyQt5.QtWidgets import (
|
||||
QWidget, QVBoxLayout, QHBoxLayout, QLabel, QPushButton, QSlider
|
||||
)
|
||||
|
||||
|
||||
class MiniActuator(QWidget):
|
||||
"""
|
||||
Compact single actuator control with toggle + slider.
|
||||
|
||||
Stacked vertically: Label, Toggle button, Speed slider
|
||||
Width: ~70px to fit 3 in a row
|
||||
"""
|
||||
|
||||
command_changed = pyqtSignal(str, int) # actuator_id, speed (0-100)
|
||||
|
||||
def __init__(self, actuator_id: str, label: str, parent=None):
|
||||
super().__init__(parent)
|
||||
self.actuator_id = actuator_id
|
||||
self._enabled = False
|
||||
self._speed = 0
|
||||
|
||||
self._setup_ui(label)
|
||||
|
||||
def _setup_ui(self, label: str):
|
||||
"""Set up the widget layout."""
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(2, 2, 2, 2)
|
||||
layout.setSpacing(4)
|
||||
layout.setAlignment(Qt.AlignCenter)
|
||||
|
||||
# Label
|
||||
self._label = QLabel(label)
|
||||
self._label.setStyleSheet("""
|
||||
color: #aaa;
|
||||
font-size: 9px;
|
||||
background: transparent;
|
||||
""")
|
||||
self._label.setAlignment(Qt.AlignCenter)
|
||||
layout.addWidget(self._label)
|
||||
|
||||
# Toggle button
|
||||
self._toggle_btn = QPushButton("OFF")
|
||||
self._toggle_btn.setFixedSize(50, 22)
|
||||
self._toggle_btn.setCheckable(True)
|
||||
self._toggle_btn.clicked.connect(self._on_toggle)
|
||||
self._update_toggle_style()
|
||||
layout.addWidget(self._toggle_btn, alignment=Qt.AlignCenter)
|
||||
|
||||
# Speed slider (horizontal)
|
||||
self._slider = QSlider(Qt.Horizontal)
|
||||
self._slider.setRange(0, 4) # 0%, 25%, 50%, 75%, 100%
|
||||
self._slider.setValue(0)
|
||||
self._slider.setFixedWidth(60)
|
||||
self._slider.setFixedHeight(16)
|
||||
self._slider.setStyleSheet("""
|
||||
QSlider::groove:horizontal {
|
||||
background: #444;
|
||||
height: 6px;
|
||||
border-radius: 3px;
|
||||
}
|
||||
QSlider::handle:horizontal {
|
||||
background: #888;
|
||||
width: 12px;
|
||||
margin: -4px 0;
|
||||
border-radius: 6px;
|
||||
}
|
||||
QSlider::handle:horizontal:hover {
|
||||
background: #aaa;
|
||||
}
|
||||
QSlider::sub-page:horizontal {
|
||||
background: #4CAF50;
|
||||
border-radius: 3px;
|
||||
}
|
||||
""")
|
||||
self._slider.valueChanged.connect(self._on_slider_changed)
|
||||
layout.addWidget(self._slider, alignment=Qt.AlignCenter)
|
||||
|
||||
# Speed label
|
||||
self._speed_label = QLabel("0%")
|
||||
self._speed_label.setStyleSheet("""
|
||||
color: #888;
|
||||
font-size: 8px;
|
||||
background: transparent;
|
||||
""")
|
||||
self._speed_label.setAlignment(Qt.AlignCenter)
|
||||
layout.addWidget(self._speed_label)
|
||||
|
||||
def _update_toggle_style(self):
|
||||
"""Update toggle button appearance based on state."""
|
||||
if self._enabled:
|
||||
self._toggle_btn.setText("ON")
|
||||
self._toggle_btn.setStyleSheet("""
|
||||
QPushButton {
|
||||
background-color: #4CAF50;
|
||||
color: white;
|
||||
border: none;
|
||||
border-radius: 3px;
|
||||
font-size: 10px;
|
||||
font-weight: bold;
|
||||
}
|
||||
QPushButton:hover { background-color: #66BB6A; }
|
||||
""")
|
||||
else:
|
||||
self._toggle_btn.setText("OFF")
|
||||
self._toggle_btn.setStyleSheet("""
|
||||
QPushButton {
|
||||
background-color: #555;
|
||||
color: #ccc;
|
||||
border: none;
|
||||
border-radius: 3px;
|
||||
font-size: 10px;
|
||||
}
|
||||
QPushButton:hover { background-color: #666; }
|
||||
""")
|
||||
|
||||
def _on_toggle(self, checked: bool):
|
||||
"""Handle toggle button click."""
|
||||
self._enabled = checked
|
||||
self._update_toggle_style()
|
||||
|
||||
if self._enabled:
|
||||
# Turn on at current slider position or 100% if at 0
|
||||
if self._slider.value() == 0:
|
||||
self._slider.setValue(4) # 100%
|
||||
self._speed = self._slider.value() * 25
|
||||
else:
|
||||
self._speed = 0
|
||||
|
||||
self._speed_label.setText(f"{self._speed}%")
|
||||
self.command_changed.emit(self.actuator_id, self._speed)
|
||||
|
||||
def _on_slider_changed(self, value: int):
|
||||
"""Handle slider value change."""
|
||||
self._speed = value * 25
|
||||
self._speed_label.setText(f"{self._speed}%")
|
||||
|
||||
# Auto-enable when slider moves above 0
|
||||
if value > 0 and not self._enabled:
|
||||
self._enabled = True
|
||||
self._toggle_btn.setChecked(True)
|
||||
self._update_toggle_style()
|
||||
# Auto-disable when slider moves to 0
|
||||
elif value == 0 and self._enabled:
|
||||
self._enabled = False
|
||||
self._toggle_btn.setChecked(False)
|
||||
self._update_toggle_style()
|
||||
|
||||
self.command_changed.emit(self.actuator_id, self._speed)
|
||||
|
||||
def set_state(self, enabled: bool, speed: int = 100):
|
||||
"""Set the actuator state programmatically."""
|
||||
self._enabled = enabled
|
||||
self._speed = speed if enabled else 0
|
||||
self._toggle_btn.setChecked(enabled)
|
||||
self._slider.setValue(self._speed // 25)
|
||||
self._speed_label.setText(f"{self._speed}%")
|
||||
self._update_toggle_style()
|
||||
|
||||
def is_enabled(self) -> bool:
|
||||
"""Return whether the actuator is enabled."""
|
||||
return self._enabled
|
||||
|
||||
def get_speed(self) -> int:
|
||||
"""Return the current speed (0-100)."""
|
||||
return self._speed
|
||||
|
||||
|
||||
class CompactActuatorRow(QWidget):
|
||||
"""
|
||||
Horizontal row of 3 compact actuator controls.
|
||||
|
||||
Displays Vacuum, Side Brush, and Main Brush in a single row.
|
||||
"""
|
||||
|
||||
command_changed = pyqtSignal(str, int) # actuator_id, speed
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self._setup_ui()
|
||||
|
||||
def _setup_ui(self):
|
||||
"""Set up the widget layout."""
|
||||
layout = QHBoxLayout(self)
|
||||
layout.setContentsMargins(4, 4, 4, 4)
|
||||
layout.setSpacing(8)
|
||||
|
||||
# Vacuum
|
||||
self.vacuum = MiniActuator("vacuum", "Vacuum")
|
||||
self.vacuum.command_changed.connect(self._forward_command)
|
||||
layout.addWidget(self.vacuum)
|
||||
|
||||
# Side Brush
|
||||
self.side_brush = MiniActuator("side_brush", "Side")
|
||||
self.side_brush.command_changed.connect(self._forward_command)
|
||||
layout.addWidget(self.side_brush)
|
||||
|
||||
# Main Brush
|
||||
self.main_brush = MiniActuator("main_brush", "Main")
|
||||
self.main_brush.command_changed.connect(self._forward_command)
|
||||
layout.addWidget(self.main_brush)
|
||||
|
||||
def _forward_command(self, actuator_id: str, speed: int):
|
||||
"""Forward command from child actuators."""
|
||||
self.command_changed.emit(actuator_id, speed)
|
||||
|
||||
def set_actuator_state(self, actuator_id: str, enabled: bool, speed: int = 100):
|
||||
"""Set state of a specific actuator."""
|
||||
if actuator_id == "vacuum":
|
||||
self.vacuum.set_state(enabled, speed)
|
||||
elif actuator_id == "side_brush":
|
||||
self.side_brush.set_state(enabled, speed)
|
||||
elif actuator_id == "main_brush" or actuator_id == "brush":
|
||||
self.main_brush.set_state(enabled, speed)
|
||||
+491
-827
File diff suppressed because it is too large
Load Diff
@@ -1,227 +0,0 @@
|
||||
"""
|
||||
IMU 3D Visualization Widget
|
||||
|
||||
Displays a 3D rotating robot model showing real-time orientation
|
||||
based on IMU sensor data (gyro + accelerometer + tilt).
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from PyQt5.QtWidgets import QWidget, QVBoxLayout, QHBoxLayout, QLabel
|
||||
from PyQt5.QtGui import QFont
|
||||
from PyQt5.QtCore import Qt
|
||||
|
||||
import pyqtgraph.opengl as gl
|
||||
|
||||
from ..processors.imu_processor import IMUProcessor
|
||||
|
||||
|
||||
class IMU3DView(QWidget):
|
||||
"""
|
||||
3D robot orientation visualization using OpenGL.
|
||||
|
||||
Shows a cylinder representing the robot body that rotates
|
||||
according to IMU-derived Euler angles.
|
||||
"""
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self.imu_processor = IMUProcessor()
|
||||
self._setup_ui()
|
||||
self._create_scene()
|
||||
|
||||
def _setup_ui(self):
|
||||
"""Set up the widget layout."""
|
||||
self.setStyleSheet("background-color: #2d2d2d; border-radius: 5px;")
|
||||
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(5, 5, 5, 5)
|
||||
layout.setSpacing(2)
|
||||
|
||||
# Title
|
||||
title = QLabel("IMU Orientation")
|
||||
title.setFont(QFont("Arial", 9, QFont.Bold))
|
||||
title.setStyleSheet("color: #e0e0e0; background: transparent;")
|
||||
title.setAlignment(Qt.AlignCenter)
|
||||
layout.addWidget(title)
|
||||
|
||||
# OpenGL widget
|
||||
self.gl_widget = gl.GLViewWidget()
|
||||
self.gl_widget.setCameraPosition(distance=150, elevation=30, azimuth=45)
|
||||
self.gl_widget.setBackgroundColor('#2d2d2d')
|
||||
layout.addWidget(self.gl_widget, 1)
|
||||
|
||||
# Euler angle display
|
||||
angle_layout = QHBoxLayout()
|
||||
angle_layout.setSpacing(5)
|
||||
|
||||
label_style = "color: #aaa; background: transparent; font-size: 9px;"
|
||||
|
||||
self.roll_label = QLabel("R: 0.0")
|
||||
self.roll_label.setStyleSheet(label_style)
|
||||
angle_layout.addWidget(self.roll_label)
|
||||
|
||||
self.pitch_label = QLabel("P: 0.0")
|
||||
self.pitch_label.setStyleSheet(label_style)
|
||||
angle_layout.addWidget(self.pitch_label)
|
||||
|
||||
self.yaw_label = QLabel("Y: 0.0")
|
||||
self.yaw_label.setStyleSheet(label_style)
|
||||
angle_layout.addWidget(self.yaw_label)
|
||||
|
||||
layout.addLayout(angle_layout)
|
||||
|
||||
def _create_scene(self):
|
||||
"""Create the 3D scene with robot mesh and reference elements."""
|
||||
# Floor grid
|
||||
grid = gl.GLGridItem()
|
||||
grid.setSize(100, 100)
|
||||
grid.setSpacing(10, 10)
|
||||
grid.translate(0, 0, -20)
|
||||
self.gl_widget.addItem(grid)
|
||||
|
||||
# XYZ axes for reference
|
||||
axis = gl.GLAxisItem()
|
||||
axis.setSize(40, 40, 40)
|
||||
self.gl_widget.addItem(axis)
|
||||
|
||||
# Robot mesh (flat cylinder/disc)
|
||||
self.robot_mesh = self._create_robot_mesh()
|
||||
self.gl_widget.addItem(self.robot_mesh)
|
||||
|
||||
# Forward direction indicator (red arrow)
|
||||
self.forward_arrow = self._create_forward_arrow()
|
||||
self.gl_widget.addItem(self.forward_arrow)
|
||||
|
||||
def _create_robot_mesh(self):
|
||||
"""Create a flat cylinder mesh representing the robot."""
|
||||
segments = 32
|
||||
radius = 35.0
|
||||
height = 8.0
|
||||
|
||||
# Generate vertices for top and bottom circles
|
||||
angles = np.linspace(0, 2 * np.pi, segments, endpoint=False)
|
||||
|
||||
# Top circle vertices
|
||||
top_x = radius * np.cos(angles)
|
||||
top_y = radius * np.sin(angles)
|
||||
top_z = np.full(segments, height / 2)
|
||||
top_verts = np.column_stack([top_x, top_y, top_z])
|
||||
|
||||
# Bottom circle vertices
|
||||
bottom_x = radius * np.cos(angles)
|
||||
bottom_y = radius * np.sin(angles)
|
||||
bottom_z = np.full(segments, -height / 2)
|
||||
bottom_verts = np.column_stack([bottom_x, bottom_y, bottom_z])
|
||||
|
||||
# Center vertices for caps
|
||||
top_center = np.array([[0, 0, height / 2]])
|
||||
bottom_center = np.array([[0, 0, -height / 2]])
|
||||
|
||||
# Combine all vertices
|
||||
# Layout: top circle (0..31), bottom circle (32..63), top center (64), bottom center (65)
|
||||
verts = np.vstack([top_verts, bottom_verts, top_center, bottom_center])
|
||||
|
||||
# Generate faces
|
||||
faces = []
|
||||
|
||||
# Top cap (triangles from center to edge)
|
||||
top_center_idx = segments * 2
|
||||
for i in range(segments):
|
||||
next_i = (i + 1) % segments
|
||||
faces.append([top_center_idx, i, next_i])
|
||||
|
||||
# Bottom cap (triangles from center to edge, reversed winding)
|
||||
bottom_center_idx = segments * 2 + 1
|
||||
for i in range(segments):
|
||||
next_i = (i + 1) % segments
|
||||
faces.append([bottom_center_idx, segments + next_i, segments + i])
|
||||
|
||||
# Side faces (quads as two triangles)
|
||||
for i in range(segments):
|
||||
next_i = (i + 1) % segments
|
||||
# Top-left triangle
|
||||
faces.append([i, segments + i, next_i])
|
||||
# Bottom-right triangle
|
||||
faces.append([next_i, segments + i, segments + next_i])
|
||||
|
||||
faces = np.array(faces)
|
||||
|
||||
# Create mesh item
|
||||
mesh = gl.GLMeshItem(
|
||||
vertexes=verts,
|
||||
faces=faces,
|
||||
color=(0.9, 0.9, 0.9, 1.0),
|
||||
shader='shaded',
|
||||
smooth=True
|
||||
)
|
||||
|
||||
return mesh
|
||||
|
||||
def _create_forward_arrow(self):
|
||||
"""Create a red triangle indicating forward direction."""
|
||||
# Triangle pointing in +Y direction (forward)
|
||||
verts = np.array([
|
||||
[0, 30, 5], # Tip (forward)
|
||||
[-8, 15, 5], # Left base
|
||||
[8, 15, 5], # Right base
|
||||
])
|
||||
|
||||
faces = np.array([[0, 1, 2]])
|
||||
|
||||
arrow = gl.GLMeshItem(
|
||||
vertexes=verts,
|
||||
faces=faces,
|
||||
color=(0.9, 0.2, 0.2, 1.0),
|
||||
shader='shaded'
|
||||
)
|
||||
|
||||
return arrow
|
||||
|
||||
def update_orientation(self, gyro_x: int, gyro_y: int, gyro_z: int,
|
||||
tilt_x: int, tilt_y: int, tilt_z: int):
|
||||
"""
|
||||
Update the 3D visualization with new IMU data.
|
||||
|
||||
Called at 500Hz from the telemetry thread. The IMU processor
|
||||
handles decimation to reduce UI update rate to ~50Hz.
|
||||
|
||||
Args:
|
||||
gyro_x, gyro_y, gyro_z: Raw gyroscope values (i16)
|
||||
tilt_x, tilt_y, tilt_z: Low-pass filtered gravity vector (i16)
|
||||
"""
|
||||
# Process IMU data through complementary filter
|
||||
roll, pitch, yaw = self.imu_processor.process(
|
||||
gyro_x, gyro_y, gyro_z,
|
||||
tilt_x, tilt_y, tilt_z
|
||||
)
|
||||
|
||||
# Check if UI should be updated (decimation)
|
||||
if not self.imu_processor.should_update_ui():
|
||||
return
|
||||
|
||||
# Update angle labels
|
||||
self.roll_label.setText(f"R:{roll:+5.1f}")
|
||||
self.pitch_label.setText(f"P:{pitch:+5.1f}")
|
||||
self.yaw_label.setText(f"Y:{yaw:+5.1f}")
|
||||
|
||||
# Apply rotation to robot mesh
|
||||
# Reset transform and apply new rotation
|
||||
self.robot_mesh.resetTransform()
|
||||
self.robot_mesh.rotate(yaw, 0, 0, 1) # Yaw around Z
|
||||
self.robot_mesh.rotate(pitch, 1, 0, 0) # Pitch around X
|
||||
self.robot_mesh.rotate(roll, 0, 1, 0) # Roll around Y
|
||||
|
||||
# Apply same rotation to forward arrow
|
||||
self.forward_arrow.resetTransform()
|
||||
self.forward_arrow.rotate(yaw, 0, 0, 1)
|
||||
self.forward_arrow.rotate(pitch, 1, 0, 0)
|
||||
self.forward_arrow.rotate(roll, 0, 1, 0)
|
||||
|
||||
def reset_orientation(self):
|
||||
"""Reset orientation to zero."""
|
||||
self.imu_processor.reset()
|
||||
self.robot_mesh.resetTransform()
|
||||
self.forward_arrow.resetTransform()
|
||||
self.roll_label.setText("R: 0.0")
|
||||
self.pitch_label.setText("P: 0.0")
|
||||
self.yaw_label.setText("Y: 0.0")
|
||||
@@ -0,0 +1,595 @@
|
||||
"""
|
||||
Wireframe geometry generation for 3D robot visualization.
|
||||
|
||||
All functions return Nx3 numpy arrays of line segment endpoints
|
||||
for use with pyqtgraph.opengl.GLLinePlotItem in 'lines' mode.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
|
||||
def create_cylinder_wireframe(radius: float, height: float, segments: int = 48,
|
||||
vertical_lines: int = 12, horizontal_rings: int = 1) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe edges for a cylinder with denser geometry.
|
||||
|
||||
Args:
|
||||
radius: Cylinder radius
|
||||
height: Cylinder height
|
||||
segments: Number of segments around circumference
|
||||
vertical_lines: Number of vertical edge lines
|
||||
horizontal_rings: Number of intermediate horizontal rings (0 = top/bottom only)
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints (pairs of vertices)
|
||||
"""
|
||||
angles = np.linspace(0, 2 * np.pi, segments, endpoint=False)
|
||||
points = []
|
||||
|
||||
# Create all horizontal rings (including top and bottom)
|
||||
z_positions = np.linspace(-height / 2, height / 2, horizontal_rings + 2)
|
||||
|
||||
for z in z_positions:
|
||||
for i in range(segments):
|
||||
x1 = radius * np.cos(angles[i])
|
||||
y1 = radius * np.sin(angles[i])
|
||||
x2 = radius * np.cos(angles[(i + 1) % segments])
|
||||
y2 = radius * np.sin(angles[(i + 1) % segments])
|
||||
points.extend([[x1, y1, z], [x2, y2, z]])
|
||||
|
||||
# Vertical edges (evenly spaced)
|
||||
step = max(1, segments // vertical_lines)
|
||||
for i in range(0, segments, step):
|
||||
x = radius * np.cos(angles[i])
|
||||
y = radius * np.sin(angles[i])
|
||||
points.extend([[x, y, height / 2], [x, y, -height / 2]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
|
||||
|
||||
def create_arc_wireframe(radius: float, start_angle: float, end_angle: float,
|
||||
thickness: float = 0.0, segments: int = 32,
|
||||
height: float = 0.0, height_segments: int = 0) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe edges for an arc (portion of a circle) with optional 3D extrusion.
|
||||
|
||||
Args:
|
||||
radius: Arc radius
|
||||
start_angle: Start angle in degrees
|
||||
end_angle: End angle in degrees
|
||||
thickness: Optional radial thickness (creates inner and outer arcs)
|
||||
segments: Number of segments along the arc
|
||||
height: Optional height for 3D arc (extrusion)
|
||||
height_segments: Number of vertical divisions for 3D arc
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
start_rad = math.radians(start_angle)
|
||||
end_rad = math.radians(end_angle)
|
||||
angles = np.linspace(start_rad, end_rad, segments)
|
||||
points = []
|
||||
|
||||
# Determine Z positions
|
||||
if height > 0 and height_segments > 0:
|
||||
z_positions = np.linspace(-height / 2, height / 2, height_segments + 1)
|
||||
else:
|
||||
z_positions = [0]
|
||||
|
||||
# Outer arc at each height
|
||||
for z in z_positions:
|
||||
for i in range(len(angles) - 1):
|
||||
x1 = radius * np.cos(angles[i])
|
||||
y1 = radius * np.sin(angles[i])
|
||||
x2 = radius * np.cos(angles[i + 1])
|
||||
y2 = radius * np.sin(angles[i + 1])
|
||||
points.extend([[x1, y1, z], [x2, y2, z]])
|
||||
|
||||
# Inner arc (if thickness specified)
|
||||
if thickness > 0:
|
||||
inner_radius = radius - thickness
|
||||
for z in z_positions:
|
||||
for i in range(len(angles) - 1):
|
||||
x1 = inner_radius * np.cos(angles[i])
|
||||
y1 = inner_radius * np.sin(angles[i])
|
||||
x2 = inner_radius * np.cos(angles[i + 1])
|
||||
y2 = inner_radius * np.sin(angles[i + 1])
|
||||
points.extend([[x1, y1, z], [x2, y2, z]])
|
||||
|
||||
# End caps (connect inner to outer) at each height
|
||||
for z in z_positions:
|
||||
for angle in [angles[0], angles[-1]]:
|
||||
x_outer = radius * np.cos(angle)
|
||||
y_outer = radius * np.sin(angle)
|
||||
x_inner = inner_radius * np.cos(angle)
|
||||
y_inner = inner_radius * np.sin(angle)
|
||||
points.extend([[x_outer, y_outer, z], [x_inner, y_inner, z]])
|
||||
|
||||
# Vertical lines connecting heights (for 3D arcs)
|
||||
if height > 0 and len(z_positions) > 1:
|
||||
# Add vertical lines at regular intervals along outer arc
|
||||
vert_step = max(1, len(angles) // 8)
|
||||
for i in range(0, len(angles), vert_step):
|
||||
x = radius * np.cos(angles[i])
|
||||
y = radius * np.sin(angles[i])
|
||||
points.extend([[x, y, z_positions[0]], [x, y, z_positions[-1]]])
|
||||
|
||||
# Add vertical lines along inner arc too
|
||||
if thickness > 0:
|
||||
for i in range(0, len(angles), vert_step):
|
||||
x = inner_radius * np.cos(angles[i])
|
||||
y = inner_radius * np.sin(angles[i])
|
||||
points.extend([[x, y, z_positions[0]], [x, y, z_positions[-1]]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
|
||||
|
||||
def create_box_wireframe(width: float, length: float, height: float) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe edges for a rectangular box.
|
||||
|
||||
Args:
|
||||
width: Box width (X dimension)
|
||||
length: Box length (Y dimension)
|
||||
height: Box height (Z dimension)
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
w, l, h = width / 2, length / 2, height / 2
|
||||
points = []
|
||||
|
||||
# Bottom rectangle
|
||||
corners_bottom = [(-w, -l, -h), (w, -l, -h), (w, l, -h), (-w, l, -h)]
|
||||
for i in range(4):
|
||||
points.extend([corners_bottom[i], corners_bottom[(i + 1) % 4]])
|
||||
|
||||
# Top rectangle
|
||||
corners_top = [(-w, -l, h), (w, -l, h), (w, l, h), (-w, l, h)]
|
||||
for i in range(4):
|
||||
points.extend([corners_top[i], corners_top[(i + 1) % 4]])
|
||||
|
||||
# Vertical edges
|
||||
for i in range(4):
|
||||
points.extend([corners_bottom[i], corners_top[i]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
|
||||
|
||||
def create_circle_wireframe(radius: float, segments: int = 32, z: float = 0) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe edges for a circle.
|
||||
|
||||
Args:
|
||||
radius: Circle radius
|
||||
segments: Number of segments (default 32 for smooth appearance)
|
||||
z: Z position of the circle
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
angles = np.linspace(0, 2 * np.pi, segments, endpoint=False)
|
||||
points = []
|
||||
|
||||
for i in range(segments):
|
||||
x1 = radius * np.cos(angles[i])
|
||||
y1 = radius * np.sin(angles[i])
|
||||
x2 = radius * np.cos(angles[(i + 1) % segments])
|
||||
y2 = radius * np.sin(angles[(i + 1) % segments])
|
||||
points.extend([[x1, y1, z], [x2, y2, z]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
|
||||
|
||||
def create_3arm_brush_wireframe(radius: float, hub_radius: float = 8.0) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe for a 3-arm side brush with denser geometry.
|
||||
|
||||
Args:
|
||||
radius: Length of brush arms
|
||||
hub_radius: Radius of center hub
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
points = []
|
||||
|
||||
# Center hub (small circle) - denser
|
||||
hub_points = create_circle_wireframe(hub_radius, segments=24)
|
||||
points.extend(hub_points.tolist())
|
||||
|
||||
# Inner hub ring for visual depth
|
||||
inner_hub = create_circle_wireframe(hub_radius * 0.5, segments=16)
|
||||
points.extend(inner_hub.tolist())
|
||||
|
||||
# 3 main arms at 120 degree intervals with bristle details
|
||||
for i in range(3):
|
||||
angle = math.radians(i * 120)
|
||||
x_tip = radius * math.cos(angle)
|
||||
y_tip = radius * math.sin(angle)
|
||||
|
||||
# Main arm spine
|
||||
points.extend([[0, 0, 0], [x_tip, y_tip, 0]])
|
||||
|
||||
# Bristle lines along the arm (perpendicular to arm direction)
|
||||
num_bristles = 5
|
||||
for j in range(1, num_bristles + 1):
|
||||
t = j / (num_bristles + 1) # Position along arm
|
||||
arm_x = t * x_tip
|
||||
arm_y = t * y_tip
|
||||
|
||||
# Perpendicular direction
|
||||
perp_angle = angle + math.pi / 2
|
||||
bristle_len = radius * 0.15 * (1 - t * 0.5) # Bristles get shorter toward tip
|
||||
bx = bristle_len * math.cos(perp_angle)
|
||||
by = bristle_len * math.sin(perp_angle)
|
||||
|
||||
# Bristles on both sides
|
||||
points.extend([[arm_x - bx, arm_y - by, 0], [arm_x + bx, arm_y + by, 0]])
|
||||
|
||||
# Arm tip arc (brush end)
|
||||
tip_arc_radius = radius * 0.1
|
||||
for k in range(4):
|
||||
a1 = angle - 0.5 + k * 0.33
|
||||
a2 = angle - 0.5 + (k + 1) * 0.33
|
||||
x1 = x_tip + tip_arc_radius * math.cos(a1)
|
||||
y1 = y_tip + tip_arc_radius * math.sin(a1)
|
||||
x2 = x_tip + tip_arc_radius * math.cos(a2)
|
||||
y2 = y_tip + tip_arc_radius * math.sin(a2)
|
||||
points.extend([[x1, y1, 0], [x2, y2, 0]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
|
||||
|
||||
def create_roller_brush_wireframe(width: float, radius: float, segments: int = 32,
|
||||
bristle_lines: int = 12) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe for a roller brush (main brush) with denser geometry.
|
||||
|
||||
The roller is oriented along the X axis (rotates around X).
|
||||
|
||||
Args:
|
||||
width: Brush width (length along X axis)
|
||||
radius: Brush radius
|
||||
segments: Number of segments around circumference
|
||||
bristle_lines: Number of axial bristle/spiral lines
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
points = []
|
||||
half_width = width / 2
|
||||
angles = np.linspace(0, 2 * np.pi, segments, endpoint=False)
|
||||
|
||||
# End circles (in Y-Z plane at X = ±half_width)
|
||||
for i in range(segments):
|
||||
y1 = radius * np.cos(angles[i])
|
||||
z1 = radius * np.sin(angles[i])
|
||||
y2 = radius * np.cos(angles[(i + 1) % segments])
|
||||
z2 = radius * np.sin(angles[(i + 1) % segments])
|
||||
# Left end cap
|
||||
points.extend([[-half_width, y1, z1], [-half_width, y2, z2]])
|
||||
# Right end cap
|
||||
points.extend([[half_width, y1, z1], [half_width, y2, z2]])
|
||||
|
||||
# Middle ring for visual reference
|
||||
for i in range(segments):
|
||||
y1 = radius * np.cos(angles[i])
|
||||
z1 = radius * np.sin(angles[i])
|
||||
y2 = radius * np.cos(angles[(i + 1) % segments])
|
||||
z2 = radius * np.sin(angles[(i + 1) % segments])
|
||||
points.extend([[0, y1, z1], [0, y2, z2]])
|
||||
|
||||
# Straight axial bristle lines
|
||||
bristle_angles = np.linspace(0, 2 * np.pi, bristle_lines, endpoint=False)
|
||||
for angle in bristle_angles:
|
||||
y = radius * np.cos(angle)
|
||||
z = radius * np.sin(angle)
|
||||
points.extend([[-half_width, y, z], [half_width, y, z]])
|
||||
|
||||
# Spiral pattern lines for visual interest (makes rotation visible)
|
||||
num_spirals = 4
|
||||
spiral_segments = 16
|
||||
for s in range(num_spirals):
|
||||
base_angle = s * (2 * np.pi / num_spirals)
|
||||
for i in range(spiral_segments):
|
||||
t1 = i / spiral_segments
|
||||
t2 = (i + 1) / spiral_segments
|
||||
x1 = -half_width + t1 * width
|
||||
x2 = -half_width + t2 * width
|
||||
angle1 = base_angle + t1 * np.pi # Half rotation along length
|
||||
angle2 = base_angle + t2 * np.pi
|
||||
y1 = radius * np.cos(angle1)
|
||||
z1 = radius * np.sin(angle1)
|
||||
y2 = radius * np.cos(angle2)
|
||||
z2 = radius * np.sin(angle2)
|
||||
points.extend([[x1, y1, z1], [x2, y2, z2]])
|
||||
|
||||
# Inner hub circles (smaller radius) for visual depth
|
||||
hub_radius = radius * 0.3
|
||||
for i in range(segments // 2): # Fewer segments for inner
|
||||
y1 = hub_radius * np.cos(angles[i * 2])
|
||||
z1 = hub_radius * np.sin(angles[i * 2])
|
||||
y2 = hub_radius * np.cos(angles[(i * 2 + 2) % segments])
|
||||
z2 = hub_radius * np.sin(angles[(i * 2 + 2) % segments])
|
||||
points.extend([[-half_width, y1, z1], [-half_width, y2, z2]])
|
||||
points.extend([[half_width, y1, z1], [half_width, y2, z2]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
|
||||
|
||||
def create_fan_wireframe(radius: float, blades: int = 8, hub_radius: float = 8.0) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe for a fan/impeller with denser geometry.
|
||||
|
||||
Args:
|
||||
radius: Fan radius
|
||||
blades: Number of blades
|
||||
hub_radius: Radius of center hub
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
points = []
|
||||
|
||||
# Center hub - denser circle
|
||||
hub_points = create_circle_wireframe(hub_radius, segments=24)
|
||||
points.extend(hub_points.tolist())
|
||||
|
||||
# Inner hub for depth
|
||||
inner_hub = create_circle_wireframe(hub_radius * 0.5, segments=16)
|
||||
points.extend(inner_hub.tolist())
|
||||
|
||||
# Outer rim circle
|
||||
outer_rim = create_circle_wireframe(radius * 0.95, segments=48)
|
||||
points.extend(outer_rim.tolist())
|
||||
|
||||
# Blades (curved, more detailed)
|
||||
blade_angle_span = 0.4 # radians - width of blade at base
|
||||
for i in range(blades):
|
||||
base_angle = math.radians(i * (360 / blades))
|
||||
|
||||
# Blade leading edge
|
||||
x_base_lead = hub_radius * math.cos(base_angle - blade_angle_span / 2)
|
||||
y_base_lead = hub_radius * math.sin(base_angle - blade_angle_span / 2)
|
||||
|
||||
# Blade trailing edge
|
||||
x_base_trail = hub_radius * math.cos(base_angle + blade_angle_span / 2)
|
||||
y_base_trail = hub_radius * math.sin(base_angle + blade_angle_span / 2)
|
||||
|
||||
# Blade tip (curved forward)
|
||||
tip_angle = base_angle + 0.15 # Curved blade
|
||||
x_tip = radius * 0.9 * math.cos(tip_angle)
|
||||
y_tip = radius * 0.9 * math.sin(tip_angle)
|
||||
|
||||
# Draw blade outline
|
||||
points.extend([[x_base_lead, y_base_lead, 0], [x_tip, y_tip, 0]])
|
||||
points.extend([[x_tip, y_tip, 0], [x_base_trail, y_base_trail, 0]])
|
||||
points.extend([[x_base_lead, y_base_lead, 0], [x_base_trail, y_base_trail, 0]])
|
||||
|
||||
# Add internal blade line for detail
|
||||
mid_radius = (hub_radius + radius * 0.9) / 2
|
||||
x_mid = mid_radius * math.cos(base_angle)
|
||||
y_mid = mid_radius * math.sin(base_angle)
|
||||
points.extend([[x_base_lead, y_base_lead, 0], [x_mid, y_mid, 0]])
|
||||
points.extend([[x_mid, y_mid, 0], [x_tip, y_tip, 0]])
|
||||
|
||||
# Spoke lines from hub to outer rim (between blades)
|
||||
for i in range(blades):
|
||||
angle = math.radians(i * (360 / blades) + (180 / blades)) # Between blades
|
||||
x_hub = hub_radius * math.cos(angle)
|
||||
y_hub = hub_radius * math.sin(angle)
|
||||
x_outer = radius * 0.95 * math.cos(angle)
|
||||
y_outer = radius * 0.95 * math.sin(angle)
|
||||
points.extend([[x_hub, y_hub, 0], [x_outer, y_outer, 0]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
|
||||
|
||||
def create_wheel_wireframe(width: float, radius: float,
|
||||
tread_lines: int = 8) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe for a drive wheel as a cylinder.
|
||||
|
||||
The wheel is a cylinder oriented with its axis along X.
|
||||
Rotation happens around the X axis (rolling forward/backward).
|
||||
|
||||
Args:
|
||||
width: Wheel width/thickness (X dimension)
|
||||
radius: Wheel radius (Y-Z plane)
|
||||
tread_lines: Number of tread pattern lines around circumference
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
points = []
|
||||
half_width = width / 2
|
||||
segments = 32 # Smooth circle
|
||||
|
||||
angles = np.linspace(0, 2 * np.pi, segments, endpoint=False)
|
||||
|
||||
# Left face circle (at -half_width on X axis)
|
||||
for i in range(segments):
|
||||
y1 = radius * np.cos(angles[i])
|
||||
z1 = radius * np.sin(angles[i])
|
||||
y2 = radius * np.cos(angles[(i + 1) % segments])
|
||||
z2 = radius * np.sin(angles[(i + 1) % segments])
|
||||
points.extend([[-half_width, y1, z1], [-half_width, y2, z2]])
|
||||
|
||||
# Right face circle (at +half_width on X axis)
|
||||
for i in range(segments):
|
||||
y1 = radius * np.cos(angles[i])
|
||||
z1 = radius * np.sin(angles[i])
|
||||
y2 = radius * np.cos(angles[(i + 1) % segments])
|
||||
z2 = radius * np.sin(angles[(i + 1) % segments])
|
||||
points.extend([[half_width, y1, z1], [half_width, y2, z2]])
|
||||
|
||||
# Connecting lines between faces (tread lines)
|
||||
tread_step = max(1, segments // tread_lines)
|
||||
for i in range(0, segments, tread_step):
|
||||
y = radius * np.cos(angles[i])
|
||||
z = radius * np.sin(angles[i])
|
||||
points.extend([[-half_width, y, z], [half_width, y, z]])
|
||||
|
||||
# Hub circles (smaller inner circles for visual detail)
|
||||
hub_radius = radius * 0.35
|
||||
for i in range(segments):
|
||||
y1 = hub_radius * np.cos(angles[i])
|
||||
z1 = hub_radius * np.sin(angles[i])
|
||||
y2 = hub_radius * np.cos(angles[(i + 1) % segments])
|
||||
z2 = hub_radius * np.sin(angles[(i + 1) % segments])
|
||||
points.extend([[-half_width, y1, z1], [-half_width, y2, z2]])
|
||||
points.extend([[half_width, y1, z1], [half_width, y2, z2]])
|
||||
|
||||
# Spoke lines from hub to rim (on both faces) - makes rotation visible
|
||||
spoke_count = 5
|
||||
spoke_step = segments // spoke_count
|
||||
for i in range(0, segments, spoke_step):
|
||||
y_hub = hub_radius * np.cos(angles[i])
|
||||
z_hub = hub_radius * np.sin(angles[i])
|
||||
y_rim = radius * np.cos(angles[i])
|
||||
z_rim = radius * np.sin(angles[i])
|
||||
points.extend([[-half_width, y_hub, z_hub], [-half_width, y_rim, z_rim]])
|
||||
points.extend([[half_width, y_hub, z_hub], [half_width, y_rim, z_rim]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
|
||||
|
||||
def rotate_wheel_x(points: np.ndarray, angle_deg: float) -> np.ndarray:
|
||||
"""
|
||||
Rotate wheel geometry around X axis (for wheel rolling motion).
|
||||
|
||||
This rotates in the Y-Z plane, which makes the wheel appear to roll.
|
||||
|
||||
Args:
|
||||
points: Nx3 array of points
|
||||
angle_deg: Rotation angle in degrees
|
||||
|
||||
Returns:
|
||||
Rotated points array
|
||||
"""
|
||||
angle = math.radians(angle_deg)
|
||||
cos_a = math.cos(angle)
|
||||
sin_a = math.sin(angle)
|
||||
|
||||
rotated = points.copy()
|
||||
# Rotate Y and Z coordinates around X axis
|
||||
rotated[:, 1] = points[:, 1] * cos_a - points[:, 2] * sin_a
|
||||
rotated[:, 2] = points[:, 1] * sin_a + points[:, 2] * cos_a
|
||||
return rotated
|
||||
|
||||
|
||||
def translate_geometry(points: np.ndarray, dx: float, dy: float, dz: float) -> np.ndarray:
|
||||
"""Translate geometry by given offset."""
|
||||
offset = np.array([dx, dy, dz], dtype=np.float32)
|
||||
return points + offset
|
||||
|
||||
|
||||
def rotate_geometry_z(points: np.ndarray, angle_deg: float) -> np.ndarray:
|
||||
"""Rotate geometry around Z axis."""
|
||||
angle = math.radians(angle_deg)
|
||||
cos_a = math.cos(angle)
|
||||
sin_a = math.sin(angle)
|
||||
|
||||
rotated = points.copy()
|
||||
rotated[:, 0] = points[:, 0] * cos_a - points[:, 1] * sin_a
|
||||
rotated[:, 1] = points[:, 0] * sin_a + points[:, 1] * cos_a
|
||||
return rotated
|
||||
|
||||
|
||||
def rotate_geometry_x(points: np.ndarray, angle_deg: float) -> np.ndarray:
|
||||
"""Rotate geometry around X axis."""
|
||||
angle = math.radians(angle_deg)
|
||||
cos_a = math.cos(angle)
|
||||
sin_a = math.sin(angle)
|
||||
|
||||
rotated = points.copy()
|
||||
rotated[:, 1] = points[:, 1] * cos_a - points[:, 2] * sin_a
|
||||
rotated[:, 2] = points[:, 1] * sin_a + points[:, 2] * cos_a
|
||||
return rotated
|
||||
|
||||
|
||||
def rotate_geometry_y(points: np.ndarray, angle_deg: float) -> np.ndarray:
|
||||
"""Rotate geometry around Y axis (for wheel rotation)."""
|
||||
angle = math.radians(angle_deg)
|
||||
cos_a = math.cos(angle)
|
||||
sin_a = math.sin(angle)
|
||||
|
||||
rotated = points.copy()
|
||||
rotated[:, 0] = points[:, 0] * cos_a + points[:, 2] * sin_a
|
||||
rotated[:, 2] = -points[:, 0] * sin_a + points[:, 2] * cos_a
|
||||
return rotated
|
||||
|
||||
|
||||
def create_lidar_turret_wireframe(radius: float, height: float,
|
||||
segments: int = 32, vertical_lines: int = 8) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe for the lidar turret outer housing.
|
||||
|
||||
Args:
|
||||
radius: Turret radius
|
||||
height: Turret height
|
||||
segments: Number of segments around circumference
|
||||
vertical_lines: Number of vertical lines
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
return create_cylinder_wireframe(radius, height, segments, vertical_lines)
|
||||
|
||||
|
||||
def create_lidar_spinner_wireframe(radius: float, height: float,
|
||||
segments: int = 16, blades: int = 4) -> np.ndarray:
|
||||
"""
|
||||
Create wireframe for the lidar inner spinning element.
|
||||
|
||||
Shows a smaller cylinder with visible internal structure that rotates.
|
||||
|
||||
Args:
|
||||
radius: Spinner radius (smaller than turret)
|
||||
height: Spinner height
|
||||
segments: Number of segments around circumference
|
||||
blades: Number of blade/spoke lines
|
||||
|
||||
Returns:
|
||||
Nx3 array of line segment endpoints
|
||||
"""
|
||||
points = []
|
||||
half_height = height / 2
|
||||
|
||||
angles = np.linspace(0, 2 * np.pi, segments, endpoint=False)
|
||||
|
||||
# Top circle
|
||||
for i in range(segments):
|
||||
x1 = radius * np.cos(angles[i])
|
||||
y1 = radius * np.sin(angles[i])
|
||||
x2 = radius * np.cos(angles[(i + 1) % segments])
|
||||
y2 = radius * np.sin(angles[(i + 1) % segments])
|
||||
points.extend([[x1, y1, half_height], [x2, y2, half_height]])
|
||||
|
||||
# Bottom circle
|
||||
for i in range(segments):
|
||||
x1 = radius * np.cos(angles[i])
|
||||
y1 = radius * np.sin(angles[i])
|
||||
x2 = radius * np.cos(angles[(i + 1) % segments])
|
||||
y2 = radius * np.sin(angles[(i + 1) % segments])
|
||||
points.extend([[x1, y1, -half_height], [x2, y2, -half_height]])
|
||||
|
||||
# Vertical lines
|
||||
step = max(1, segments // blades)
|
||||
for i in range(0, segments, step):
|
||||
x = radius * np.cos(angles[i])
|
||||
y = radius * np.sin(angles[i])
|
||||
points.extend([[x, y, half_height], [x, y, -half_height]])
|
||||
|
||||
# Internal cross/blade structure (makes rotation visible)
|
||||
for i in range(blades):
|
||||
angle = math.radians(i * (360 / blades))
|
||||
x_outer = radius * 0.9 * math.cos(angle)
|
||||
y_outer = radius * 0.9 * math.sin(angle)
|
||||
# Blade from center to edge at multiple heights
|
||||
for z in [-half_height * 0.5, 0, half_height * 0.5]:
|
||||
points.extend([[0, 0, z], [x_outer, y_outer, z]])
|
||||
|
||||
return np.array(points, dtype=np.float32)
|
||||
@@ -0,0 +1,998 @@
|
||||
"""
|
||||
Full-screen 3D wireframe robot visualization widget.
|
||||
|
||||
Replaces the 2D top-view with a see-through 3D wireframe showing
|
||||
animated internal components with IMU orientation overlay.
|
||||
"""
|
||||
|
||||
from PyQt5.QtWidgets import QWidget, QVBoxLayout, QHBoxLayout, QLabel, QPushButton, QProgressBar
|
||||
from PyQt5.QtCore import Qt, QTimer
|
||||
from PyQt5.QtGui import QPainter, QColor, QLinearGradient
|
||||
|
||||
import pyqtgraph.opengl as gl
|
||||
|
||||
from .robot_3d_geometry import (
|
||||
create_cylinder_wireframe,
|
||||
create_arc_wireframe,
|
||||
create_circle_wireframe,
|
||||
create_3arm_brush_wireframe,
|
||||
create_roller_brush_wireframe,
|
||||
create_fan_wireframe,
|
||||
create_wheel_wireframe,
|
||||
create_lidar_turret_wireframe,
|
||||
create_lidar_spinner_wireframe,
|
||||
translate_geometry,
|
||||
rotate_geometry_z,
|
||||
rotate_geometry_x,
|
||||
rotate_geometry_y,
|
||||
rotate_wheel_x,
|
||||
)
|
||||
from ..processors.imu_processor import IMUProcessor
|
||||
|
||||
|
||||
# Color palette (RGBA, 0.0-1.0)
|
||||
COLORS = {
|
||||
'body': (0.5, 0.5, 0.5, 0.6),
|
||||
'body_edge': (0.4, 0.4, 0.4, 1.0),
|
||||
'bumper_ok': (0.5, 0.5, 0.5, 1.0),
|
||||
'bumper_triggered': (1.0, 0.15, 0.15, 1.0),
|
||||
'lidar': (0.3, 0.3, 0.3, 1.0),
|
||||
'lidar_spinner': (0.5, 0.7, 0.9, 1.0),
|
||||
'lidar_spinner_active': (0.3, 0.9, 1.0, 1.0),
|
||||
'wheel': (0.25, 0.25, 0.25, 1.0),
|
||||
'wheel_moving': (0.3, 0.75, 0.3, 1.0),
|
||||
'brush_off': (0.6, 0.4, 0.2, 0.8),
|
||||
'brush_spinning': (1.0, 0.5, 0.2, 1.0),
|
||||
'side_brush': (0.7, 0.7, 0.7, 1.0),
|
||||
'cliff_ok': (0.2, 0.85, 0.2, 1.0),
|
||||
'cliff_triggered': (1.0, 0.15, 0.15, 1.0),
|
||||
'button_off': (0.4, 0.4, 0.4, 1.0),
|
||||
'button_on': (0.2, 0.85, 0.2, 1.0),
|
||||
'fan': (0.5, 0.5, 0.6, 0.8),
|
||||
'fan_spinning': (0.4, 0.6, 0.9, 1.0),
|
||||
'grid': (0.25, 0.25, 0.25, 0.5),
|
||||
'axis_x': (1.0, 0.2, 0.2, 1.0),
|
||||
'axis_y': (0.2, 1.0, 0.2, 1.0),
|
||||
'axis_z': (0.2, 0.2, 1.0, 1.0),
|
||||
}
|
||||
|
||||
# Camera presets
|
||||
CAMERA_PRESETS = {
|
||||
'iso': {'distance': 450, 'elevation': 30, 'azimuth': 45},
|
||||
'top': {'distance': 500, 'elevation': 89, 'azimuth': 0},
|
||||
'front': {'distance': 450, 'elevation': 15, 'azimuth': 90},
|
||||
'side': {'distance': 450, 'elevation': 15, 'azimuth': 0},
|
||||
}
|
||||
|
||||
|
||||
class BatteryBar(QWidget):
|
||||
"""Custom battery bar widget with animated charging effect."""
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self.setFixedHeight(10)
|
||||
self._level = 0 # 0-100
|
||||
self._charging = False
|
||||
self._animation_phase = 0
|
||||
|
||||
def set_level(self, level: int):
|
||||
"""Set battery level (0-100)."""
|
||||
self._level = max(0, min(100, level))
|
||||
self.update()
|
||||
|
||||
def set_charging(self, charging: bool):
|
||||
"""Set charging state."""
|
||||
self._charging = charging
|
||||
self.update()
|
||||
|
||||
def advance_animation(self):
|
||||
"""Advance charging animation phase."""
|
||||
self._animation_phase = (self._animation_phase + 1) % 4
|
||||
if self._charging:
|
||||
self.update()
|
||||
|
||||
def paintEvent(self, event):
|
||||
"""Paint the battery bar."""
|
||||
painter = QPainter(self)
|
||||
painter.setRenderHint(QPainter.Antialiasing)
|
||||
|
||||
w = self.width()
|
||||
h = self.height()
|
||||
|
||||
# Background (empty battery)
|
||||
painter.setBrush(QColor(60, 60, 60))
|
||||
painter.setPen(QColor(80, 80, 80))
|
||||
painter.drawRoundedRect(0, 0, w, h, 3, 3)
|
||||
|
||||
# Calculate fill width
|
||||
fill_width = int((w - 4) * self._level / 100)
|
||||
|
||||
if fill_width > 0:
|
||||
# Determine color based on level
|
||||
if self._level > 50:
|
||||
color = QColor(76, 175, 80) # Green
|
||||
elif self._level > 20:
|
||||
color = QColor(255, 193, 7) # Yellow/Orange
|
||||
else:
|
||||
color = QColor(244, 67, 54) # Red
|
||||
|
||||
# If charging, create animated gradient
|
||||
if self._charging:
|
||||
gradient = QLinearGradient(2, 0, 2 + fill_width, 0)
|
||||
# Animate by shifting gradient stops
|
||||
offset = self._animation_phase * 0.25
|
||||
gradient.setColorAt(0, color.darker(120))
|
||||
gradient.setColorAt((0.5 + offset) % 1.0, color.lighter(130))
|
||||
gradient.setColorAt(1, color.darker(120))
|
||||
painter.setBrush(gradient)
|
||||
else:
|
||||
painter.setBrush(color)
|
||||
|
||||
painter.setPen(Qt.NoPen)
|
||||
painter.drawRoundedRect(2, 2, fill_width, h - 4, 2, 2)
|
||||
|
||||
painter.end()
|
||||
|
||||
|
||||
class Robot3DView(QWidget):
|
||||
"""
|
||||
Full-screen 3D wireframe robot visualization.
|
||||
|
||||
Coordinate system (robot frame):
|
||||
- Origin at robot center
|
||||
- +X points right
|
||||
- +Y points forward (front of robot)
|
||||
- +Z points up
|
||||
- All dimensions in mm
|
||||
"""
|
||||
|
||||
# Robot physical dimensions (mm) - CRL-200S
|
||||
ROBOT_DIAMETER = 350.0
|
||||
ROBOT_RADIUS = ROBOT_DIAMETER / 2
|
||||
ROBOT_HEIGHT = 50.0
|
||||
|
||||
# Bumper arc
|
||||
BUMPER_ARC_ANGLE = 220 # degrees, centered on front
|
||||
BUMPER_THICKNESS = 6.0
|
||||
|
||||
# Lidar turret
|
||||
LIDAR_RADIUS = 35.0
|
||||
LIDAR_HEIGHT = 20.0
|
||||
LIDAR_Y_OFFSET = -100.0 # rear
|
||||
LIDAR_SPINNER_RADIUS = 25.0 # Inner spinning element
|
||||
|
||||
# Drive wheels (cylinder: radius = height/2, width = thickness)
|
||||
WHEEL_WIDTH = 18.0 # Thickness of wheel (was 25, now 75% = ~18)
|
||||
WHEEL_RADIUS = 40.0 # Wheel radius (was height/2 = 20, now 2x = 40)
|
||||
WHEEL_X_OFFSET = 130.0
|
||||
WHEEL_Z_OFFSET = -10.0 # Adjusted for larger wheel
|
||||
|
||||
# Main brush roller
|
||||
MAIN_BRUSH_WIDTH = 140.0
|
||||
MAIN_BRUSH_RADIUS = 15.0
|
||||
MAIN_BRUSH_Y_OFFSET = -30.0
|
||||
MAIN_BRUSH_Z_OFFSET = -20.0
|
||||
|
||||
# Side brush
|
||||
SIDE_BRUSH_RADIUS = 35.0
|
||||
SIDE_BRUSH_X_OFFSET = -100.0
|
||||
SIDE_BRUSH_Y_OFFSET = 80.0
|
||||
SIDE_BRUSH_Z_OFFSET = -20.0
|
||||
|
||||
# Cliff sensors
|
||||
CLIFF_SENSOR_RADIUS = 8.0
|
||||
CLIFF_POSITIONS = {
|
||||
'left_front': (-70.0, 140.0),
|
||||
'right_front': (70.0, 140.0),
|
||||
'left_side': (-130.0, 60.0),
|
||||
'right_side': (130.0, 60.0),
|
||||
}
|
||||
CLIFF_Z_OFFSET = -25.0
|
||||
|
||||
# Buttons
|
||||
BUTTON_RADIUS = 10.0
|
||||
BUTTON_Y_OFFSET = 100.0
|
||||
BUTTON_SPACING = 25.0
|
||||
BUTTON_Z_OFFSET = 25.0
|
||||
|
||||
# Vacuum fan
|
||||
FAN_RADIUS = 30.0
|
||||
FAN_Z_OFFSET = 0.0
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self.imu_processor = IMUProcessor()
|
||||
|
||||
# Sensor state
|
||||
self.sensor_data = {
|
||||
'is_charging': False,
|
||||
'battery_level': 0,
|
||||
'wheel_left': 0,
|
||||
'wheel_right': 0,
|
||||
'bumper_left': False,
|
||||
'bumper_right': False,
|
||||
'cliff_left_front': False,
|
||||
'cliff_right_front': False,
|
||||
'cliff_left_side': False,
|
||||
'cliff_right_side': False,
|
||||
'start_button': 0,
|
||||
'dock_button': 0,
|
||||
}
|
||||
|
||||
# Actuator speeds (0-100%)
|
||||
self.actuator_speeds = {
|
||||
'vacuum': 0.0,
|
||||
'main_brush': 0.0,
|
||||
'side_brush': 0.0,
|
||||
}
|
||||
|
||||
# Animation state
|
||||
self.wheel_left_angle = 0.0
|
||||
self.wheel_right_angle = 0.0
|
||||
self.side_brush_angle = 0.0
|
||||
self.main_brush_angle = 0.0
|
||||
self.fan_angle = 0.0
|
||||
self.lidar_spinner_angle = 0.0
|
||||
self.last_wheel_left = 0
|
||||
self.last_wheel_right = 0
|
||||
self.lidar_enabled = False
|
||||
|
||||
# Battery state
|
||||
self.battery_level = 0
|
||||
self.battery_voltage = 0.0
|
||||
self.is_charging = False
|
||||
self._charging_animation_phase = 0
|
||||
|
||||
# Sensor state cache for change detection
|
||||
self._cache = {}
|
||||
|
||||
# GL items storage
|
||||
self.gl_items = {}
|
||||
|
||||
# Base geometry templates (for rotation)
|
||||
self._wheel_left_base = None
|
||||
self._wheel_right_base = None
|
||||
self._side_brush_base = None
|
||||
self._main_brush_base = None
|
||||
self._fan_base = None
|
||||
self._lidar_spinner_base = None
|
||||
|
||||
self._setup_ui()
|
||||
self._create_scene()
|
||||
self._start_animation()
|
||||
|
||||
# Cache items list for fast IMU transform iteration
|
||||
self._gl_items_list = list(self.gl_items.values())
|
||||
|
||||
def _setup_ui(self):
|
||||
"""Set up the widget layout."""
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(0, 0, 0, 0)
|
||||
layout.setSpacing(0)
|
||||
|
||||
# OpenGL widget (full size)
|
||||
self.gl_widget = gl.GLViewWidget()
|
||||
self.gl_widget.setCameraPosition(**CAMERA_PRESETS['iso'])
|
||||
self.gl_widget.setBackgroundColor('#1a1a1a')
|
||||
layout.addWidget(self.gl_widget, 1)
|
||||
|
||||
# View controls overlay (top-left)
|
||||
self._create_view_overlay()
|
||||
|
||||
# IMU display overlay (bottom-left)
|
||||
self._create_imu_overlay()
|
||||
|
||||
# Battery status overlay (bottom-right)
|
||||
self._create_battery_overlay()
|
||||
|
||||
def _create_view_overlay(self):
|
||||
"""Create floating view control buttons."""
|
||||
self.view_overlay = QWidget(self.gl_widget)
|
||||
self.view_overlay.setStyleSheet("""
|
||||
background-color: rgba(45, 45, 45, 180);
|
||||
border-radius: 8px;
|
||||
""")
|
||||
self.view_overlay.setFixedSize(80, 160)
|
||||
self.view_overlay.move(10, 10)
|
||||
|
||||
layout = QVBoxLayout(self.view_overlay)
|
||||
layout.setContentsMargins(8, 8, 8, 8)
|
||||
layout.setSpacing(5)
|
||||
|
||||
title = QLabel("View")
|
||||
title.setStyleSheet("color: #ddd; font-weight: bold; background: transparent;")
|
||||
title.setAlignment(Qt.AlignCenter)
|
||||
layout.addWidget(title)
|
||||
|
||||
btn_style = """
|
||||
QPushButton {
|
||||
background: #555;
|
||||
color: white;
|
||||
border: none;
|
||||
padding: 5px;
|
||||
border-radius: 3px;
|
||||
}
|
||||
QPushButton:hover { background: #666; }
|
||||
QPushButton:pressed { background: #444; }
|
||||
"""
|
||||
|
||||
for name in ['Iso', 'Top', 'Front', 'Side']:
|
||||
btn = QPushButton(name)
|
||||
btn.setStyleSheet(btn_style)
|
||||
btn.clicked.connect(lambda _, n=name.lower(): self._set_camera_preset(n))
|
||||
layout.addWidget(btn)
|
||||
|
||||
layout.addStretch()
|
||||
|
||||
def _create_imu_overlay(self):
|
||||
"""Create IMU angle display overlay."""
|
||||
self.imu_overlay = QWidget(self.gl_widget)
|
||||
self.imu_overlay.setStyleSheet("""
|
||||
background-color: rgba(45, 45, 45, 180);
|
||||
border-radius: 8px;
|
||||
""")
|
||||
self.imu_overlay.setFixedSize(150, 70)
|
||||
|
||||
layout = QVBoxLayout(self.imu_overlay)
|
||||
layout.setContentsMargins(8, 8, 8, 8)
|
||||
layout.setSpacing(2)
|
||||
|
||||
title = QLabel("IMU Orientation")
|
||||
title.setStyleSheet("color: #ddd; font-weight: bold; font-size: 10px; background: transparent;")
|
||||
title.setAlignment(Qt.AlignCenter)
|
||||
layout.addWidget(title)
|
||||
|
||||
angle_layout = QHBoxLayout()
|
||||
angle_layout.setSpacing(5)
|
||||
|
||||
label_style = "color: #aaa; background: transparent; font-size: 10px;"
|
||||
|
||||
self.roll_label = QLabel("R: 0.0")
|
||||
self.roll_label.setStyleSheet(label_style)
|
||||
angle_layout.addWidget(self.roll_label)
|
||||
|
||||
self.pitch_label = QLabel("P: 0.0")
|
||||
self.pitch_label.setStyleSheet(label_style)
|
||||
angle_layout.addWidget(self.pitch_label)
|
||||
|
||||
self.yaw_label = QLabel("Y: 0.0")
|
||||
self.yaw_label.setStyleSheet(label_style)
|
||||
angle_layout.addWidget(self.yaw_label)
|
||||
|
||||
layout.addLayout(angle_layout)
|
||||
|
||||
def _create_battery_overlay(self):
|
||||
"""Create battery status display overlay with charging animation."""
|
||||
self.battery_overlay = QWidget(self.gl_widget)
|
||||
self.battery_overlay.setStyleSheet("""
|
||||
background-color: rgba(45, 45, 45, 180);
|
||||
border-radius: 8px;
|
||||
""")
|
||||
self.battery_overlay.setFixedSize(130, 85)
|
||||
|
||||
layout = QVBoxLayout(self.battery_overlay)
|
||||
layout.setContentsMargins(8, 8, 8, 8)
|
||||
layout.setSpacing(3)
|
||||
|
||||
# Title with charging indicator
|
||||
title_layout = QHBoxLayout()
|
||||
title_layout.setSpacing(4)
|
||||
|
||||
title = QLabel("Battery")
|
||||
title.setStyleSheet("color: #ddd; font-weight: bold; font-size: 10px; background: transparent;")
|
||||
title_layout.addWidget(title)
|
||||
|
||||
self.charging_indicator = QLabel("")
|
||||
self.charging_indicator.setStyleSheet("color: #4CAF50; font-size: 14px; background: transparent;")
|
||||
title_layout.addWidget(self.charging_indicator)
|
||||
title_layout.addStretch()
|
||||
|
||||
layout.addLayout(title_layout)
|
||||
|
||||
# Battery percentage and voltage in same row
|
||||
info_layout = QHBoxLayout()
|
||||
info_layout.setSpacing(8)
|
||||
|
||||
self.battery_percent_label = QLabel("0%")
|
||||
self.battery_percent_label.setStyleSheet("color: #aaa; font-size: 18px; font-weight: bold; background: transparent;")
|
||||
info_layout.addWidget(self.battery_percent_label)
|
||||
|
||||
self.battery_voltage_label = QLabel("0.0V")
|
||||
self.battery_voltage_label.setStyleSheet("color: #888; font-size: 11px; background: transparent;")
|
||||
self.battery_voltage_label.setAlignment(Qt.AlignRight | Qt.AlignVCenter)
|
||||
info_layout.addWidget(self.battery_voltage_label)
|
||||
|
||||
layout.addLayout(info_layout)
|
||||
|
||||
# Battery bar container
|
||||
bar_container = QWidget()
|
||||
bar_container.setFixedHeight(14)
|
||||
bar_container.setStyleSheet("background: transparent;")
|
||||
bar_layout = QHBoxLayout(bar_container)
|
||||
bar_layout.setContentsMargins(0, 0, 0, 0)
|
||||
bar_layout.setSpacing(0)
|
||||
|
||||
# Battery bar (custom painted)
|
||||
self.battery_bar = BatteryBar()
|
||||
bar_layout.addWidget(self.battery_bar)
|
||||
|
||||
layout.addWidget(bar_container)
|
||||
|
||||
def resizeEvent(self, event):
|
||||
"""Handle resize to position overlays."""
|
||||
super().resizeEvent(event)
|
||||
# Position IMU overlay at bottom-left
|
||||
if hasattr(self, 'imu_overlay'):
|
||||
self.imu_overlay.move(10, self.height() - self.imu_overlay.height() - 10)
|
||||
# Position battery overlay at bottom-right
|
||||
if hasattr(self, 'battery_overlay'):
|
||||
self.battery_overlay.move(
|
||||
self.width() - self.battery_overlay.width() - 10,
|
||||
self.height() - self.battery_overlay.height() - 10
|
||||
)
|
||||
|
||||
def _set_camera_preset(self, preset: str):
|
||||
"""Apply a camera preset."""
|
||||
if preset in CAMERA_PRESETS:
|
||||
self.gl_widget.setCameraPosition(**CAMERA_PRESETS[preset])
|
||||
|
||||
def _create_scene(self):
|
||||
"""Create the 3D scene with all robot components."""
|
||||
# Floor grid
|
||||
grid = gl.GLGridItem()
|
||||
grid.setSize(400, 400)
|
||||
grid.setSpacing(50, 50)
|
||||
grid.translate(0, 0, -30)
|
||||
self.gl_widget.addItem(grid)
|
||||
|
||||
# XYZ axes
|
||||
axis = gl.GLAxisItem()
|
||||
axis.setSize(80, 80, 80)
|
||||
self.gl_widget.addItem(axis)
|
||||
|
||||
# Create all robot components
|
||||
self._create_body()
|
||||
self._create_bumper()
|
||||
self._create_lidar()
|
||||
self._create_wheels()
|
||||
self._create_main_brush()
|
||||
self._create_side_brush()
|
||||
self._create_cliff_sensors()
|
||||
self._create_buttons()
|
||||
self._create_fan()
|
||||
|
||||
def _create_body(self):
|
||||
"""Create robot body wireframe with denser geometry."""
|
||||
body_geom = create_cylinder_wireframe(
|
||||
self.ROBOT_RADIUS, self.ROBOT_HEIGHT,
|
||||
segments=64, vertical_lines=16, horizontal_rings=2
|
||||
)
|
||||
self.gl_items['body'] = gl.GLLinePlotItem(
|
||||
pos=body_geom,
|
||||
color=COLORS['body_edge'],
|
||||
width=1.5,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['body'])
|
||||
|
||||
def _create_bumper(self):
|
||||
"""Create front bumper arc (left and right halves) with 3D extrusion."""
|
||||
# Bumper spans from (90 - half_angle) to (90 + half_angle) degrees
|
||||
# where 90 degrees is +Y direction (front)
|
||||
half_angle = self.BUMPER_ARC_ANGLE / 2
|
||||
bumper_radius = self.ROBOT_RADIUS - self.BUMPER_THICKNESS / 2
|
||||
bumper_height = 15.0 # 3D extrusion height
|
||||
|
||||
# Right half: 90 - half_angle to 90
|
||||
right_geom = create_arc_wireframe(
|
||||
bumper_radius, 90 - half_angle, 90,
|
||||
thickness=self.BUMPER_THICKNESS, segments=24,
|
||||
height=bumper_height, height_segments=2
|
||||
)
|
||||
right_geom = translate_geometry(right_geom, 0, 0, self.ROBOT_HEIGHT / 2 + 2)
|
||||
self.gl_items['bumper_right'] = gl.GLLinePlotItem(
|
||||
pos=right_geom,
|
||||
color=COLORS['bumper_ok'],
|
||||
width=2.0,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['bumper_right'])
|
||||
|
||||
# Left half: 90 to 90 + half_angle
|
||||
left_geom = create_arc_wireframe(
|
||||
bumper_radius, 90, 90 + half_angle,
|
||||
thickness=self.BUMPER_THICKNESS, segments=24,
|
||||
height=bumper_height, height_segments=2
|
||||
)
|
||||
left_geom = translate_geometry(left_geom, 0, 0, self.ROBOT_HEIGHT / 2 + 2)
|
||||
self.gl_items['bumper_left'] = gl.GLLinePlotItem(
|
||||
pos=left_geom,
|
||||
color=COLORS['bumper_ok'],
|
||||
width=2.0,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['bumper_left'])
|
||||
|
||||
def _create_lidar(self):
|
||||
"""Create lidar turret with outer housing and inner spinning element."""
|
||||
lidar_z = self.ROBOT_HEIGHT / 2 + self.LIDAR_HEIGHT / 2
|
||||
|
||||
# Outer housing (static)
|
||||
lidar_geom = create_lidar_turret_wireframe(
|
||||
self.LIDAR_RADIUS, self.LIDAR_HEIGHT,
|
||||
segments=32, vertical_lines=8
|
||||
)
|
||||
lidar_geom = translate_geometry(
|
||||
lidar_geom, 0, self.LIDAR_Y_OFFSET, lidar_z
|
||||
)
|
||||
self.gl_items['lidar'] = gl.GLLinePlotItem(
|
||||
pos=lidar_geom,
|
||||
color=COLORS['lidar'],
|
||||
width=1.5,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['lidar'])
|
||||
|
||||
# Inner spinner (rotates when lidar is active)
|
||||
spinner_geom = create_lidar_spinner_wireframe(
|
||||
self.LIDAR_SPINNER_RADIUS, self.LIDAR_HEIGHT * 0.8,
|
||||
segments=16, blades=4
|
||||
)
|
||||
self._lidar_spinner_base = spinner_geom.copy()
|
||||
spinner_geom = translate_geometry(
|
||||
spinner_geom, 0, self.LIDAR_Y_OFFSET, lidar_z
|
||||
)
|
||||
self.gl_items['lidar_spinner'] = gl.GLLinePlotItem(
|
||||
pos=spinner_geom,
|
||||
color=COLORS['lidar_spinner'],
|
||||
width=1.5,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['lidar_spinner'])
|
||||
|
||||
def _create_wheels(self):
|
||||
"""Create drive wheels as cylinders."""
|
||||
# Left wheel - cylinder with axis along X
|
||||
left_geom = create_wheel_wireframe(self.WHEEL_WIDTH, self.WHEEL_RADIUS)
|
||||
self._wheel_left_base = left_geom.copy()
|
||||
left_geom = translate_geometry(
|
||||
left_geom, -self.WHEEL_X_OFFSET, 0, self.WHEEL_Z_OFFSET
|
||||
)
|
||||
self.gl_items['wheel_left'] = gl.GLLinePlotItem(
|
||||
pos=left_geom,
|
||||
color=COLORS['wheel'],
|
||||
width=1.5,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['wheel_left'])
|
||||
|
||||
# Right wheel
|
||||
right_geom = create_wheel_wireframe(self.WHEEL_WIDTH, self.WHEEL_RADIUS)
|
||||
self._wheel_right_base = right_geom.copy()
|
||||
right_geom = translate_geometry(
|
||||
right_geom, self.WHEEL_X_OFFSET, 0, self.WHEEL_Z_OFFSET
|
||||
)
|
||||
self.gl_items['wheel_right'] = gl.GLLinePlotItem(
|
||||
pos=right_geom,
|
||||
color=COLORS['wheel'],
|
||||
width=1.5,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['wheel_right'])
|
||||
|
||||
def _create_main_brush(self):
|
||||
"""Create main brush roller."""
|
||||
brush_geom = create_roller_brush_wireframe(
|
||||
self.MAIN_BRUSH_WIDTH, self.MAIN_BRUSH_RADIUS
|
||||
)
|
||||
self._main_brush_base = brush_geom.copy()
|
||||
brush_geom = translate_geometry(
|
||||
brush_geom, 0, self.MAIN_BRUSH_Y_OFFSET, self.MAIN_BRUSH_Z_OFFSET
|
||||
)
|
||||
self.gl_items['main_brush'] = gl.GLLinePlotItem(
|
||||
pos=brush_geom,
|
||||
color=COLORS['brush_off'],
|
||||
width=1.5,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['main_brush'])
|
||||
|
||||
def _create_side_brush(self):
|
||||
"""Create 3-arm side brush."""
|
||||
brush_geom = create_3arm_brush_wireframe(self.SIDE_BRUSH_RADIUS)
|
||||
self._side_brush_base = brush_geom.copy()
|
||||
brush_geom = translate_geometry(
|
||||
brush_geom, self.SIDE_BRUSH_X_OFFSET,
|
||||
self.SIDE_BRUSH_Y_OFFSET, self.SIDE_BRUSH_Z_OFFSET
|
||||
)
|
||||
self.gl_items['side_brush'] = gl.GLLinePlotItem(
|
||||
pos=brush_geom,
|
||||
color=COLORS['side_brush'],
|
||||
width=2.0,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['side_brush'])
|
||||
|
||||
def _create_cliff_sensors(self):
|
||||
"""Create cliff sensor indicators."""
|
||||
for name, (x, y) in self.CLIFF_POSITIONS.items():
|
||||
sensor_geom = create_circle_wireframe(self.CLIFF_SENSOR_RADIUS, segments=16)
|
||||
sensor_geom = translate_geometry(sensor_geom, x, y, self.CLIFF_Z_OFFSET)
|
||||
self.gl_items[f'cliff_{name}'] = gl.GLLinePlotItem(
|
||||
pos=sensor_geom,
|
||||
color=COLORS['cliff_ok'],
|
||||
width=2.0,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items[f'cliff_{name}'])
|
||||
|
||||
def _create_buttons(self):
|
||||
"""Create start and dock button indicators."""
|
||||
# Dock button (top)
|
||||
dock_geom = create_circle_wireframe(self.BUTTON_RADIUS, segments=16)
|
||||
dock_geom = translate_geometry(
|
||||
dock_geom, 0, self.BUTTON_Y_OFFSET - self.BUTTON_SPACING / 2,
|
||||
self.BUTTON_Z_OFFSET
|
||||
)
|
||||
self.gl_items['button_dock'] = gl.GLLinePlotItem(
|
||||
pos=dock_geom,
|
||||
color=COLORS['button_off'],
|
||||
width=2.0,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['button_dock'])
|
||||
|
||||
# Start button (bottom)
|
||||
start_geom = create_circle_wireframe(self.BUTTON_RADIUS, segments=16)
|
||||
start_geom = translate_geometry(
|
||||
start_geom, 0, self.BUTTON_Y_OFFSET + self.BUTTON_SPACING / 2,
|
||||
self.BUTTON_Z_OFFSET
|
||||
)
|
||||
self.gl_items['button_start'] = gl.GLLinePlotItem(
|
||||
pos=start_geom,
|
||||
color=COLORS['button_off'],
|
||||
width=2.0,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['button_start'])
|
||||
|
||||
def _create_fan(self):
|
||||
"""Create vacuum fan/impeller."""
|
||||
fan_geom = create_fan_wireframe(self.FAN_RADIUS, blades=6)
|
||||
self._fan_base = fan_geom.copy()
|
||||
fan_geom = translate_geometry(fan_geom, 0, 0, self.FAN_Z_OFFSET)
|
||||
self.gl_items['fan'] = gl.GLLinePlotItem(
|
||||
pos=fan_geom,
|
||||
color=COLORS['fan'],
|
||||
width=1.5,
|
||||
mode='lines',
|
||||
antialias=True
|
||||
)
|
||||
self.gl_widget.addItem(self.gl_items['fan'])
|
||||
|
||||
def _start_animation(self):
|
||||
"""Start animation timer."""
|
||||
self.animation_timer = QTimer(self)
|
||||
self.animation_timer.timeout.connect(self._update_animation)
|
||||
self.animation_timer.start(16) # ~60 FPS
|
||||
|
||||
def _update_animation(self):
|
||||
"""Update brush/wheel/lidar animations. IMU is updated separately."""
|
||||
# Update wheel rotation from encoder deltas
|
||||
wheel_left = self.sensor_data.get('wheel_left', 0)
|
||||
wheel_right = self.sensor_data.get('wheel_right', 0)
|
||||
|
||||
wheel_moving = False
|
||||
if wheel_left != self.last_wheel_left:
|
||||
delta = wheel_left - self.last_wheel_left
|
||||
# Handle wrap-around
|
||||
if abs(delta) < 30000:
|
||||
self.wheel_left_angle += delta * 0.5 # Degrees per tick
|
||||
wheel_moving = True
|
||||
self.last_wheel_left = wheel_left
|
||||
|
||||
if wheel_right != self.last_wheel_right:
|
||||
delta = wheel_right - self.last_wheel_right
|
||||
if abs(delta) < 30000:
|
||||
self.wheel_right_angle += delta * 0.5
|
||||
wheel_moving = True
|
||||
self.last_wheel_right = wheel_right
|
||||
|
||||
# Update wheel colors based on movement
|
||||
if wheel_moving:
|
||||
if 'wheel_left' in self.gl_items:
|
||||
self.gl_items['wheel_left'].setData(color=COLORS['wheel_moving'])
|
||||
if 'wheel_right' in self.gl_items:
|
||||
self.gl_items['wheel_right'].setData(color=COLORS['wheel_moving'])
|
||||
else:
|
||||
if 'wheel_left' in self.gl_items:
|
||||
self.gl_items['wheel_left'].setData(color=COLORS['wheel'])
|
||||
if 'wheel_right' in self.gl_items:
|
||||
self.gl_items['wheel_right'].setData(color=COLORS['wheel'])
|
||||
|
||||
# Update brush/fan rotation from actuator speeds
|
||||
side_speed = self.actuator_speeds.get('side_brush', 0)
|
||||
main_speed = self.actuator_speeds.get('main_brush', 0)
|
||||
vacuum_speed = self.actuator_speeds.get('vacuum', 0)
|
||||
|
||||
if side_speed > 0:
|
||||
self.side_brush_angle += 7.5 * (side_speed / 100.0)
|
||||
if main_speed > 0:
|
||||
self.main_brush_angle += 4.5 * (main_speed / 100.0)
|
||||
if vacuum_speed > 0:
|
||||
self.fan_angle += 12.0 * (vacuum_speed / 100.0)
|
||||
|
||||
# Update lidar spinner rotation when enabled
|
||||
if self.lidar_enabled:
|
||||
self.lidar_spinner_angle += 8.0 # Constant spin rate
|
||||
|
||||
# Update battery charging animation
|
||||
if self.is_charging:
|
||||
self._charging_animation_phase += 1
|
||||
if self._charging_animation_phase % 10 == 0: # Every ~160ms
|
||||
if hasattr(self, 'battery_bar'):
|
||||
self.battery_bar.advance_animation()
|
||||
# Update charging indicator color animation
|
||||
self.update_battery(self.battery_level, self.battery_voltage, self.is_charging)
|
||||
|
||||
# Update animated geometry (brushes/fan/wheels/lidar)
|
||||
self._update_animated_geometry()
|
||||
|
||||
def _update_animated_geometry(self):
|
||||
"""Update animated geometry (brushes/fan/wheels/lidar)."""
|
||||
# Side brush rotation (Z axis)
|
||||
if self._side_brush_base is not None:
|
||||
rotated = rotate_geometry_z(self._side_brush_base, self.side_brush_angle)
|
||||
translated = translate_geometry(
|
||||
rotated, self.SIDE_BRUSH_X_OFFSET,
|
||||
self.SIDE_BRUSH_Y_OFFSET, self.SIDE_BRUSH_Z_OFFSET
|
||||
)
|
||||
self.gl_items['side_brush'].setData(pos=translated)
|
||||
|
||||
# Main brush rotation (X axis - roller)
|
||||
if self._main_brush_base is not None:
|
||||
rotated = rotate_geometry_x(self._main_brush_base, self.main_brush_angle)
|
||||
translated = translate_geometry(
|
||||
rotated, 0, self.MAIN_BRUSH_Y_OFFSET, self.MAIN_BRUSH_Z_OFFSET
|
||||
)
|
||||
self.gl_items['main_brush'].setData(pos=translated)
|
||||
|
||||
# Fan rotation (Z axis)
|
||||
if self._fan_base is not None:
|
||||
rotated = rotate_geometry_z(self._fan_base, self.fan_angle)
|
||||
translated = translate_geometry(rotated, 0, 0, self.FAN_Z_OFFSET)
|
||||
self.gl_items['fan'].setData(pos=translated)
|
||||
|
||||
# Left wheel rotation (around X axis - rolling motion)
|
||||
if self._wheel_left_base is not None:
|
||||
rotated = rotate_wheel_x(self._wheel_left_base, self.wheel_left_angle)
|
||||
translated = translate_geometry(
|
||||
rotated, -self.WHEEL_X_OFFSET, 0, self.WHEEL_Z_OFFSET
|
||||
)
|
||||
self.gl_items['wheel_left'].setData(pos=translated)
|
||||
|
||||
# Right wheel rotation (around X axis - rolling motion)
|
||||
if self._wheel_right_base is not None:
|
||||
rotated = rotate_wheel_x(self._wheel_right_base, self.wheel_right_angle)
|
||||
translated = translate_geometry(
|
||||
rotated, self.WHEEL_X_OFFSET, 0, self.WHEEL_Z_OFFSET
|
||||
)
|
||||
self.gl_items['wheel_right'].setData(pos=translated)
|
||||
|
||||
# Lidar spinner rotation (Z axis)
|
||||
if self._lidar_spinner_base is not None:
|
||||
lidar_z = self.ROBOT_HEIGHT / 2 + self.LIDAR_HEIGHT / 2
|
||||
rotated = rotate_geometry_z(self._lidar_spinner_base, self.lidar_spinner_angle)
|
||||
translated = translate_geometry(
|
||||
rotated, 0, self.LIDAR_Y_OFFSET, lidar_z
|
||||
)
|
||||
self.gl_items['lidar_spinner'].setData(pos=translated)
|
||||
|
||||
def _apply_imu_transform(self, roll: float, pitch: float, yaw: float):
|
||||
"""Apply IMU orientation to all robot components."""
|
||||
for item in self._gl_items_list:
|
||||
item.resetTransform()
|
||||
item.rotate(yaw, 0, 0, 1) # Yaw around Z
|
||||
item.rotate(pitch, 1, 0, 0) # Pitch around X
|
||||
item.rotate(roll, 0, 1, 0) # Roll around Y
|
||||
|
||||
def update_sensors(self, data: dict):
|
||||
"""Update sensor data from telemetry."""
|
||||
self.sensor_data.update(data)
|
||||
|
||||
# Update bumper colors
|
||||
bumper_left = data.get('bumper_left', False)
|
||||
if bumper_left != self._cache.get('bumper_left'):
|
||||
color = COLORS['bumper_triggered'] if bumper_left else COLORS['bumper_ok']
|
||||
self.gl_items['bumper_left'].setData(color=color)
|
||||
self._cache['bumper_left'] = bumper_left
|
||||
|
||||
bumper_right = data.get('bumper_right', False)
|
||||
if bumper_right != self._cache.get('bumper_right'):
|
||||
color = COLORS['bumper_triggered'] if bumper_right else COLORS['bumper_ok']
|
||||
self.gl_items['bumper_right'].setData(color=color)
|
||||
self._cache['bumper_right'] = bumper_right
|
||||
|
||||
# Update cliff sensor colors
|
||||
cliff_map = {
|
||||
'cliff_left_front': 'cliff_left_front',
|
||||
'cliff_right_front': 'cliff_right_front',
|
||||
'cliff_left_side': 'cliff_left_side',
|
||||
'cliff_right_side': 'cliff_right_side',
|
||||
}
|
||||
for sensor_key, item_key in cliff_map.items():
|
||||
triggered = data.get(sensor_key, False)
|
||||
if triggered != self._cache.get(sensor_key):
|
||||
color = COLORS['cliff_triggered'] if triggered else COLORS['cliff_ok']
|
||||
self.gl_items[item_key].setData(color=color)
|
||||
self._cache[sensor_key] = triggered
|
||||
|
||||
# Update button colors
|
||||
start_pressed = data.get('start_button', 0) > 100
|
||||
if start_pressed != self._cache.get('start_pressed'):
|
||||
color = COLORS['button_on'] if start_pressed else COLORS['button_off']
|
||||
self.gl_items['button_start'].setData(color=color)
|
||||
self._cache['start_pressed'] = start_pressed
|
||||
|
||||
dock_pressed = data.get('dock_button', 0) > 100
|
||||
if dock_pressed != self._cache.get('dock_pressed'):
|
||||
color = COLORS['button_on'] if dock_pressed else COLORS['button_off']
|
||||
self.gl_items['button_dock'].setData(color=color)
|
||||
self._cache['dock_pressed'] = dock_pressed
|
||||
|
||||
# Update battery status
|
||||
battery_level = data.get('battery_level', None)
|
||||
battery_voltage = data.get('battery_voltage', None)
|
||||
is_charging = data.get('is_charging', None)
|
||||
if battery_level is not None or is_charging is not None or battery_voltage is not None:
|
||||
self.update_battery(
|
||||
battery_level if battery_level is not None else self.battery_level,
|
||||
battery_voltage if battery_voltage is not None else self.battery_voltage,
|
||||
is_charging if is_charging is not None else self.is_charging
|
||||
)
|
||||
|
||||
def update_battery(self, level: int, voltage: float, charging: bool):
|
||||
"""Update battery display.
|
||||
|
||||
Args:
|
||||
level: Battery level 0-100
|
||||
voltage: Battery voltage in volts
|
||||
charging: True if charging
|
||||
"""
|
||||
self.battery_level = level
|
||||
self.battery_voltage = voltage
|
||||
self.is_charging = charging
|
||||
|
||||
# Update battery bar
|
||||
if hasattr(self, 'battery_bar'):
|
||||
self.battery_bar.set_level(level)
|
||||
self.battery_bar.set_charging(charging)
|
||||
|
||||
# Update percentage label
|
||||
if hasattr(self, 'battery_percent_label'):
|
||||
self.battery_percent_label.setText(f"{level}%")
|
||||
# Color based on level
|
||||
if level > 50:
|
||||
color = "#4CAF50" # Green
|
||||
elif level > 20:
|
||||
color = "#FFC107" # Yellow/Orange
|
||||
else:
|
||||
color = "#F44336" # Red
|
||||
self.battery_percent_label.setStyleSheet(
|
||||
f"color: {color}; font-size: 18px; font-weight: bold; background: transparent;"
|
||||
)
|
||||
|
||||
# Update voltage label
|
||||
if hasattr(self, 'battery_voltage_label'):
|
||||
self.battery_voltage_label.setText(f"{voltage:.1f}V")
|
||||
|
||||
# Update charging indicator with animation
|
||||
if hasattr(self, 'charging_indicator'):
|
||||
if charging:
|
||||
# Animate the charging indicator
|
||||
phase = self._charging_animation_phase % 4
|
||||
if phase == 0:
|
||||
self.charging_indicator.setText("⚡")
|
||||
self.charging_indicator.setStyleSheet("color: #4CAF50; font-size: 14px; background: transparent;")
|
||||
elif phase == 1:
|
||||
self.charging_indicator.setText("⚡")
|
||||
self.charging_indicator.setStyleSheet("color: #8BC34A; font-size: 14px; background: transparent;")
|
||||
elif phase == 2:
|
||||
self.charging_indicator.setText("⚡")
|
||||
self.charging_indicator.setStyleSheet("color: #CDDC39; font-size: 14px; background: transparent;")
|
||||
else:
|
||||
self.charging_indicator.setText("⚡")
|
||||
self.charging_indicator.setStyleSheet("color: #FFEB3B; font-size: 14px; background: transparent;")
|
||||
else:
|
||||
self.charging_indicator.setText("")
|
||||
|
||||
def update_actuator(self, actuator_id: str, speed: float):
|
||||
"""Update actuator speed for animation.
|
||||
|
||||
Args:
|
||||
actuator_id: 'vacuum', 'brush', or 'side_brush'
|
||||
speed: Speed in percent (0-100)
|
||||
"""
|
||||
if actuator_id == 'brush':
|
||||
self.actuator_speeds['main_brush'] = speed
|
||||
color = COLORS['brush_spinning'] if speed > 0 else COLORS['brush_off']
|
||||
self.gl_items['main_brush'].setData(color=color)
|
||||
elif actuator_id == 'side_brush':
|
||||
self.actuator_speeds['side_brush'] = speed
|
||||
color = COLORS['brush_spinning'] if speed > 0 else COLORS['side_brush']
|
||||
self.gl_items['side_brush'].setData(color=color)
|
||||
elif actuator_id == 'vacuum':
|
||||
self.actuator_speeds['vacuum'] = speed
|
||||
color = COLORS['fan_spinning'] if speed > 0 else COLORS['fan']
|
||||
self.gl_items['fan'].setData(color=color)
|
||||
|
||||
def set_lidar_enabled(self, enabled: bool):
|
||||
"""Set lidar enabled state for spinner animation.
|
||||
|
||||
Args:
|
||||
enabled: True to spin the lidar, False to stop
|
||||
"""
|
||||
self.lidar_enabled = enabled
|
||||
if 'lidar_spinner' in self.gl_items:
|
||||
color = COLORS['lidar_spinner_active'] if enabled else COLORS['lidar_spinner']
|
||||
self.gl_items['lidar_spinner'].setData(color=color)
|
||||
|
||||
def update_orientation(self, gyro_x: int, gyro_y: int, gyro_z: int,
|
||||
tilt_x: int, tilt_y: int, tilt_z: int):
|
||||
"""
|
||||
Update orientation from IMU data.
|
||||
|
||||
Called at 500Hz from telemetry thread. Updates every call for
|
||||
smooth real-time response (labels decimated separately).
|
||||
"""
|
||||
roll, pitch, yaw = self.imu_processor.process(
|
||||
gyro_x, gyro_y, gyro_z,
|
||||
tilt_x, tilt_y, tilt_z
|
||||
)
|
||||
|
||||
# Always apply IMU transform for smooth updates
|
||||
self._apply_imu_transform(roll, pitch, yaw)
|
||||
|
||||
# Update labels at decimated rate to reduce string operations
|
||||
if self.imu_processor.should_update_ui():
|
||||
self.roll_label.setText(f"R:{roll:+5.1f}")
|
||||
self.pitch_label.setText(f"P:{pitch:+5.1f}")
|
||||
self.yaw_label.setText(f"Y:{yaw:+5.1f}")
|
||||
|
||||
def reset_orientation(self):
|
||||
"""Reset IMU orientation to zero."""
|
||||
self.imu_processor.reset()
|
||||
self.roll_label.setText("R: 0.0")
|
||||
self.pitch_label.setText("P: 0.0")
|
||||
self.yaw_label.setText("Y: 0.0")
|
||||
# Reset all transforms
|
||||
self._apply_imu_transform(0.0, 0.0, 0.0)
|
||||
|
||||
def keyPressEvent(self, event):
|
||||
"""Handle keyboard shortcuts for view control."""
|
||||
key = event.key()
|
||||
|
||||
if key == Qt.Key_1:
|
||||
self._set_camera_preset('iso')
|
||||
elif key == Qt.Key_2:
|
||||
self._set_camera_preset('top')
|
||||
elif key == Qt.Key_3:
|
||||
self._set_camera_preset('front')
|
||||
elif key == Qt.Key_4:
|
||||
self._set_camera_preset('side')
|
||||
elif key == Qt.Key_R:
|
||||
self.reset_orientation()
|
||||
else:
|
||||
super().keyPressEvent(event)
|
||||
@@ -1,608 +0,0 @@
|
||||
"""
|
||||
Physically-correct robot diagram widget for CRL-200S vacuum robot.
|
||||
All dimensions based on actual robot measurements.
|
||||
"""
|
||||
|
||||
from PyQt5.QtWidgets import QWidget
|
||||
from PyQt5.QtCore import Qt, QRectF, QPointF, QTimer
|
||||
from PyQt5.QtGui import QPainter, QPen, QBrush, QColor, QFont, QPainterPath, QRadialGradient
|
||||
|
||||
import math
|
||||
|
||||
|
||||
class RobotDiagram(QWidget):
|
||||
"""
|
||||
Full-screen widget showing top-down view of robot with sensor overlays.
|
||||
|
||||
Coordinate system:
|
||||
- Origin at robot center
|
||||
- +X points right
|
||||
- +Y points forward (up on screen)
|
||||
- All dimensions in mm, scaled to pixels
|
||||
"""
|
||||
|
||||
# Robot physical dimensions (mm) - CRL-200S / Xiaomi style
|
||||
ROBOT_DIAMETER = 350.0
|
||||
ROBOT_RADIUS = ROBOT_DIAMETER / 2
|
||||
|
||||
# Bumper - thin front arc (like the real robot)
|
||||
BUMPER_ARC_ANGLE = 220 # degrees, centered on front
|
||||
BUMPER_WIDTH = 6 # mm thickness (thin like real robot)
|
||||
|
||||
# Lidar turret - at rear/top of robot
|
||||
LIDAR_RADIUS = 35.0
|
||||
LIDAR_Y_OFFSET = -100.0 # toward rear (negative Y = top of screen)
|
||||
|
||||
# Drive wheels - positioned on sides
|
||||
WHEEL_WIDTH = 25.0
|
||||
WHEEL_LENGTH = 50.0
|
||||
WHEEL_X_OFFSET = 130.0 # distance from center
|
||||
WHEEL_Y_OFFSET = 0.0 # centered vertically
|
||||
|
||||
# Main brush roller
|
||||
MAIN_BRUSH_WIDTH = 140.0
|
||||
MAIN_BRUSH_HEIGHT = 30.0
|
||||
MAIN_BRUSH_Y_OFFSET = -30.0 # toward rear
|
||||
|
||||
# Side brush (3-arm) - front-left
|
||||
SIDE_BRUSH_RADIUS = 35.0
|
||||
SIDE_BRUSH_X_OFFSET = -100.0 # left side
|
||||
SIDE_BRUSH_Y_OFFSET = 80.0 # toward front (positive Y = bottom of screen)
|
||||
|
||||
# Cliff sensors - all in front half of robot (positive Y = front/bottom of screen)
|
||||
CLIFF_SENSOR_RADIUS = 8.0
|
||||
CLIFF_POSITIONS = {
|
||||
'left_front': (-70.0, 140.0), # front-left corner
|
||||
'right_front': (70.0, 140.0), # front-right corner
|
||||
'left_side': (-130.0, 60.0), # mid-left side
|
||||
'right_side': (130.0, 60.0), # mid-right side
|
||||
}
|
||||
|
||||
# Buttons - vertical pill-shaped panel at front of robot (like real robot)
|
||||
BUTTON_PANEL_WIDTH = 20.0
|
||||
BUTTON_PANEL_HEIGHT = 50.0
|
||||
BUTTON_Y_OFFSET = 100.0 # toward front (positive Y = bottom of screen)
|
||||
|
||||
# Caster wheel - at front
|
||||
CASTER_RADIUS = 12.0
|
||||
CASTER_Y_OFFSET = 110.0 # front (positive Y = bottom of screen)
|
||||
|
||||
# Dustbox - at rear
|
||||
DUSTBOX_WIDTH = 120.0
|
||||
DUSTBOX_HEIGHT = 60.0
|
||||
DUSTBOX_Y_OFFSET = -80.0 # rear (negative Y = top of screen)
|
||||
|
||||
# Colors
|
||||
COLOR_BODY = QColor(240, 240, 240)
|
||||
COLOR_BODY_BORDER = QColor(180, 180, 180)
|
||||
COLOR_BUMPER_OK = QColor(100, 100, 100)
|
||||
COLOR_BUMPER_TRIGGERED = QColor(255, 60, 60)
|
||||
COLOR_LIDAR = QColor(60, 60, 60)
|
||||
COLOR_WHEEL = QColor(50, 50, 50)
|
||||
COLOR_WHEEL_ACTIVE = QColor(80, 200, 80)
|
||||
COLOR_BRUSH_MAIN = QColor(255, 120, 60)
|
||||
COLOR_BRUSH_SIDE = QColor(200, 200, 200)
|
||||
COLOR_CLIFF_OK = QColor(60, 200, 60)
|
||||
COLOR_CLIFF_ALERT = QColor(255, 60, 60)
|
||||
COLOR_BUTTON_OFF = QColor(80, 80, 80)
|
||||
COLOR_BUTTON_ON = QColor(60, 200, 60)
|
||||
COLOR_CHARGING = QColor(255, 200, 60)
|
||||
COLOR_DUSTBOX_OK = QColor(180, 180, 180)
|
||||
COLOR_DUSTBOX_MISSING = QColor(255, 100, 100, 100)
|
||||
COLOR_TEXT = QColor(40, 40, 40)
|
||||
COLOR_BACKGROUND = QColor(245, 245, 245)
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self.setMinimumSize(400, 400)
|
||||
|
||||
# Sensor state
|
||||
self.sensor_data = {
|
||||
'is_charging': False,
|
||||
'is_battery_connected': True,
|
||||
'battery_level': 0,
|
||||
'battery_voltage': 0.0,
|
||||
'wheel_left': 0,
|
||||
'wheel_right': 0,
|
||||
'bumper_left': False,
|
||||
'bumper_right': False,
|
||||
'cliff_left_side': False,
|
||||
'cliff_left_front': False,
|
||||
'cliff_right_front': False,
|
||||
'cliff_right_side': False,
|
||||
'dustbox_attached': True,
|
||||
'start_button': 0,
|
||||
'dock_button': 0,
|
||||
}
|
||||
|
||||
# Actuator speeds (0-100%)
|
||||
self.actuator_speeds = {
|
||||
'vacuum': 0.0,
|
||||
'main_brush': 0.0,
|
||||
'side_brush': 0.0,
|
||||
}
|
||||
|
||||
# Animation state
|
||||
self.wheel_left_angle = 0.0
|
||||
self.wheel_right_angle = 0.0
|
||||
self.side_brush_angle = 0.0
|
||||
self.main_brush_angle = 0.0
|
||||
self.last_wheel_left = 0
|
||||
self.last_wheel_right = 0
|
||||
self.charging_animation_phase = 0.0 # 0.0 to 1.0 for charging animation
|
||||
self.charging_debounce_count = 0 # Debounce charging state changes
|
||||
|
||||
# Animation timer
|
||||
self.animation_timer = QTimer(self)
|
||||
self.animation_timer.timeout.connect(self._update_animation)
|
||||
self.animation_timer.start(33) # ~30 FPS
|
||||
|
||||
def update_sensors(self, data: dict):
|
||||
"""Update sensor data from telemetry."""
|
||||
# Debounce charging state at sensor update rate (500Hz)
|
||||
if 'is_charging' in data:
|
||||
raw_charging = data.get('is_charging', False)
|
||||
if raw_charging:
|
||||
self.charging_debounce_count = min(self.charging_debounce_count + 1, 100)
|
||||
else:
|
||||
self.charging_debounce_count = max(self.charging_debounce_count - 1, 0)
|
||||
|
||||
self.sensor_data.update(data)
|
||||
self.update()
|
||||
|
||||
def update_actuator(self, actuator_id: str, speed: float):
|
||||
"""Update actuator speed for animation.
|
||||
|
||||
Args:
|
||||
actuator_id: 'vacuum', 'brush', or 'side_brush'
|
||||
speed: Speed in percent (0-100)
|
||||
"""
|
||||
if actuator_id == 'brush':
|
||||
self.actuator_speeds['main_brush'] = speed
|
||||
elif actuator_id == 'side_brush':
|
||||
self.actuator_speeds['side_brush'] = speed
|
||||
elif actuator_id == 'vacuum':
|
||||
self.actuator_speeds['vacuum'] = speed
|
||||
|
||||
def _update_animation(self):
|
||||
"""Update animation angles based on sensor changes."""
|
||||
# Wheel animation based on encoder changes
|
||||
wheel_left = self.sensor_data.get('wheel_left', 0)
|
||||
wheel_right = self.sensor_data.get('wheel_right', 0)
|
||||
|
||||
if wheel_left != self.last_wheel_left:
|
||||
delta = wheel_left - self.last_wheel_left
|
||||
self.wheel_left_angle += delta * 0.1
|
||||
self.last_wheel_left = wheel_left
|
||||
|
||||
if wheel_right != self.last_wheel_right:
|
||||
delta = wheel_right - self.last_wheel_right
|
||||
self.wheel_right_angle += delta * 0.1
|
||||
self.last_wheel_right = wheel_right
|
||||
|
||||
# Brush rotation based on actuator speeds
|
||||
# Scale rotation speed by actuator percentage (0-100%)
|
||||
side_brush_speed = self.actuator_speeds.get('side_brush', 0.0)
|
||||
main_brush_speed = self.actuator_speeds.get('main_brush', 0.0)
|
||||
|
||||
if side_brush_speed > 0:
|
||||
self.side_brush_angle += 15.0 * (side_brush_speed / 100.0)
|
||||
if main_brush_speed > 0:
|
||||
self.main_brush_angle += 9.0 * (main_brush_speed / 100.0)
|
||||
|
||||
# Charging animation (oscillates 0 to 1 and back)
|
||||
# Consider charging if debounce count is above threshold (needs ~50ms of consistent charging)
|
||||
is_charging_stable = self.charging_debounce_count >= 25
|
||||
|
||||
if is_charging_stable:
|
||||
self.charging_animation_phase += 0.03
|
||||
if self.charging_animation_phase > 2.0:
|
||||
self.charging_animation_phase = 0.0
|
||||
else:
|
||||
self.charging_animation_phase = 0.0
|
||||
|
||||
self.update()
|
||||
|
||||
def paintEvent(self, event):
|
||||
"""Render the robot diagram."""
|
||||
painter = QPainter(self)
|
||||
painter.setRenderHint(QPainter.Antialiasing)
|
||||
|
||||
# Calculate scale to fit widget
|
||||
widget_size = min(self.width(), self.height()) - 40
|
||||
scale = widget_size / self.ROBOT_DIAMETER
|
||||
|
||||
# Center transform
|
||||
# +Y points down (forward/front of robot at bottom of screen)
|
||||
painter.translate(self.width() / 2, self.height() / 2)
|
||||
painter.scale(scale, scale)
|
||||
|
||||
# Draw components back to front (in robot coordinates)
|
||||
self._draw_body(painter)
|
||||
self._draw_main_brush(painter)
|
||||
self._draw_wheels(painter)
|
||||
self._draw_side_brush(painter)
|
||||
self._draw_cliff_sensors(painter)
|
||||
self._draw_bumper(painter)
|
||||
self._draw_lidar(painter)
|
||||
self._draw_buttons(painter)
|
||||
|
||||
# Draw overlays in screen coordinates
|
||||
self._draw_labels(painter, scale)
|
||||
self._draw_battery_indicator(painter)
|
||||
|
||||
def _draw_body(self, painter):
|
||||
"""Draw robot circular body with clean white design."""
|
||||
# Main body - white with subtle gradient
|
||||
gradient = QRadialGradient(0, -50, self.ROBOT_RADIUS * 1.2)
|
||||
gradient.setColorAt(0, QColor(255, 255, 255))
|
||||
gradient.setColorAt(1, QColor(245, 245, 245))
|
||||
|
||||
painter.setPen(QPen(self.COLOR_BODY_BORDER, 1.5))
|
||||
painter.setBrush(QBrush(gradient))
|
||||
painter.drawEllipse(QPointF(0, 0), self.ROBOT_RADIUS, self.ROBOT_RADIUS)
|
||||
|
||||
def _draw_bumper(self, painter):
|
||||
"""Draw front bumper arc with left/right sections using simple arc stroke."""
|
||||
bumper_left = self.sensor_data.get('bumper_left', False)
|
||||
bumper_right = self.sensor_data.get('bumper_right', False)
|
||||
|
||||
# Bumper arc parameters
|
||||
radius = self.ROBOT_RADIUS - self.BUMPER_WIDTH / 2 # Center of bumper thickness
|
||||
half_angle = self.BUMPER_ARC_ANGLE / 2
|
||||
|
||||
# Create rect for arc drawing
|
||||
rect = QRectF(-radius, -radius, radius * 2, radius * 2)
|
||||
|
||||
# Draw as thick stroked arcs (much simpler and cleaner)
|
||||
painter.setBrush(Qt.NoBrush)
|
||||
|
||||
# Right half: from (270 - half_angle) to 270, draw as arc
|
||||
color_right = self.COLOR_BUMPER_TRIGGERED if bumper_right else self.COLOR_BUMPER_OK
|
||||
pen_right = QPen(color_right, self.BUMPER_WIDTH)
|
||||
pen_right.setCapStyle(Qt.FlatCap)
|
||||
painter.setPen(pen_right)
|
||||
# Qt drawArc uses 1/16th of a degree, and positive angles go counter-clockwise
|
||||
start_angle_right = int((270 - half_angle) * 16)
|
||||
span_angle_right = int(half_angle * 16)
|
||||
painter.drawArc(rect, start_angle_right, span_angle_right)
|
||||
|
||||
# Left half: from 270 to (270 + half_angle)
|
||||
color_left = self.COLOR_BUMPER_TRIGGERED if bumper_left else self.COLOR_BUMPER_OK
|
||||
pen_left = QPen(color_left, self.BUMPER_WIDTH)
|
||||
pen_left.setCapStyle(Qt.FlatCap)
|
||||
painter.setPen(pen_left)
|
||||
start_angle_left = int(270 * 16)
|
||||
span_angle_left = int(half_angle * 16)
|
||||
painter.drawArc(rect, start_angle_left, span_angle_left)
|
||||
|
||||
def _draw_lidar(self, painter):
|
||||
"""Draw lidar turret."""
|
||||
# Outer ring
|
||||
painter.setPen(QPen(QColor(80, 80, 80), 2))
|
||||
painter.setBrush(QBrush(self.COLOR_LIDAR))
|
||||
painter.drawEllipse(QPointF(0, self.LIDAR_Y_OFFSET),
|
||||
self.LIDAR_RADIUS, self.LIDAR_RADIUS)
|
||||
|
||||
# Inner circle (slightly lighter)
|
||||
painter.setPen(Qt.NoPen)
|
||||
painter.setBrush(QBrush(QColor(75, 75, 75)))
|
||||
painter.drawEllipse(QPointF(0, self.LIDAR_Y_OFFSET),
|
||||
self.LIDAR_RADIUS * 0.6, self.LIDAR_RADIUS * 0.6)
|
||||
|
||||
def _draw_wheels(self, painter):
|
||||
"""Draw drive wheels with animation."""
|
||||
for side in ['left', 'right']:
|
||||
x = -self.WHEEL_X_OFFSET if side == 'left' else self.WHEEL_X_OFFSET
|
||||
y = self.WHEEL_Y_OFFSET
|
||||
|
||||
# Check if wheel is moving
|
||||
encoder_key = f'wheel_{side}'
|
||||
is_moving = abs(self.sensor_data.get(encoder_key, 0) -
|
||||
(self.last_wheel_left if side == 'left' else self.last_wheel_right)) > 0
|
||||
|
||||
color = self.COLOR_WHEEL_ACTIVE if is_moving else self.COLOR_WHEEL
|
||||
|
||||
painter.save()
|
||||
painter.translate(x, y)
|
||||
|
||||
# Wheel body
|
||||
painter.setPen(QPen(QColor(30, 30, 30), 1))
|
||||
painter.setBrush(QBrush(color))
|
||||
rect = QRectF(-self.WHEEL_WIDTH/2, -self.WHEEL_LENGTH/2,
|
||||
self.WHEEL_WIDTH, self.WHEEL_LENGTH)
|
||||
painter.drawRoundedRect(rect, 3, 3)
|
||||
|
||||
# Tread lines
|
||||
angle = self.wheel_left_angle if side == 'left' else self.wheel_right_angle
|
||||
painter.setPen(QPen(QColor(20, 20, 20), 1))
|
||||
for i in range(5):
|
||||
offset = ((angle + i * 10) % 50) - 25
|
||||
painter.drawLine(QPointF(-self.WHEEL_WIDTH/2 + 2, offset),
|
||||
QPointF(self.WHEEL_WIDTH/2 - 2, offset))
|
||||
|
||||
painter.restore()
|
||||
|
||||
def _draw_main_brush(self, painter):
|
||||
"""Draw main brush roller."""
|
||||
painter.save()
|
||||
painter.translate(0, self.MAIN_BRUSH_Y_OFFSET)
|
||||
|
||||
# Brush housing
|
||||
painter.setPen(QPen(QColor(80, 80, 80), 1))
|
||||
painter.setBrush(QBrush(self.COLOR_BRUSH_MAIN))
|
||||
rect = QRectF(-self.MAIN_BRUSH_WIDTH/2, -self.MAIN_BRUSH_HEIGHT/2,
|
||||
self.MAIN_BRUSH_WIDTH, self.MAIN_BRUSH_HEIGHT)
|
||||
painter.drawRoundedRect(rect, 5, 5)
|
||||
|
||||
# Brush pattern (rotating lines)
|
||||
painter.setPen(QPen(QColor(200, 80, 40), 2))
|
||||
for i in range(7):
|
||||
x = -self.MAIN_BRUSH_WIDTH/2 + 10 + i * 20
|
||||
offset = math.sin(math.radians(self.main_brush_angle + i * 30)) * 3
|
||||
painter.drawLine(QPointF(x, -self.MAIN_BRUSH_HEIGHT/2 + 5 + offset),
|
||||
QPointF(x, self.MAIN_BRUSH_HEIGHT/2 - 5 + offset))
|
||||
|
||||
painter.restore()
|
||||
|
||||
def _draw_side_brush(self, painter):
|
||||
"""Draw 3-arm side brush."""
|
||||
painter.save()
|
||||
painter.translate(self.SIDE_BRUSH_X_OFFSET, self.SIDE_BRUSH_Y_OFFSET)
|
||||
painter.rotate(self.side_brush_angle)
|
||||
|
||||
# Center hub
|
||||
painter.setPen(QPen(QColor(60, 60, 60), 1))
|
||||
painter.setBrush(QBrush(QColor(80, 80, 80)))
|
||||
painter.drawEllipse(QPointF(0, 0), 8, 8)
|
||||
|
||||
# 3 arms
|
||||
painter.setPen(QPen(self.COLOR_BRUSH_SIDE, 3))
|
||||
for i in range(3):
|
||||
angle = math.radians(i * 120)
|
||||
painter.drawLine(QPointF(0, 0),
|
||||
QPointF(self.SIDE_BRUSH_RADIUS * math.cos(angle),
|
||||
self.SIDE_BRUSH_RADIUS * math.sin(angle)))
|
||||
|
||||
painter.restore()
|
||||
|
||||
def _draw_caster(self, painter):
|
||||
"""Draw front caster wheel."""
|
||||
painter.setPen(QPen(QColor(60, 60, 60), 1))
|
||||
painter.setBrush(QBrush(QColor(100, 100, 100)))
|
||||
painter.drawEllipse(QPointF(0, self.CASTER_Y_OFFSET),
|
||||
self.CASTER_RADIUS, self.CASTER_RADIUS)
|
||||
|
||||
def _draw_cliff_sensors(self, painter):
|
||||
"""Draw 4 cliff sensors with status."""
|
||||
sensor_map = {
|
||||
'left_front': 'cliff_left_front',
|
||||
'right_front': 'cliff_right_front',
|
||||
'left_side': 'cliff_left_side',
|
||||
'right_side': 'cliff_right_side',
|
||||
}
|
||||
|
||||
for pos_name, (x, y) in self.CLIFF_POSITIONS.items():
|
||||
sensor_key = sensor_map[pos_name]
|
||||
triggered = self.sensor_data.get(sensor_key, False)
|
||||
|
||||
color = self.COLOR_CLIFF_ALERT if triggered else self.COLOR_CLIFF_OK
|
||||
|
||||
painter.setPen(QPen(QColor(40, 40, 40), 1))
|
||||
painter.setBrush(QBrush(color))
|
||||
painter.drawEllipse(QPointF(x, y),
|
||||
self.CLIFF_SENSOR_RADIUS, self.CLIFF_SENSOR_RADIUS)
|
||||
|
||||
def _draw_dustbox(self, painter):
|
||||
"""Draw dustbox compartment."""
|
||||
attached = self.sensor_data.get('dustbox_attached', True)
|
||||
|
||||
color = self.COLOR_DUSTBOX_OK if attached else self.COLOR_DUSTBOX_MISSING
|
||||
|
||||
painter.setPen(QPen(QColor(120, 120, 120), 1))
|
||||
painter.setBrush(QBrush(color))
|
||||
rect = QRectF(-self.DUSTBOX_WIDTH/2, self.DUSTBOX_Y_OFFSET - self.DUSTBOX_HEIGHT/2,
|
||||
self.DUSTBOX_WIDTH, self.DUSTBOX_HEIGHT)
|
||||
painter.drawRoundedRect(rect, 5, 5)
|
||||
|
||||
if not attached:
|
||||
# Draw X mark
|
||||
painter.setPen(QPen(QColor(200, 60, 60), 2))
|
||||
painter.drawLine(QPointF(-20, self.DUSTBOX_Y_OFFSET - 15),
|
||||
QPointF(20, self.DUSTBOX_Y_OFFSET + 15))
|
||||
painter.drawLine(QPointF(-20, self.DUSTBOX_Y_OFFSET + 15),
|
||||
QPointF(20, self.DUSTBOX_Y_OFFSET - 15))
|
||||
|
||||
def _draw_buttons(self, painter):
|
||||
"""Draw start and dock buttons in vertical pill-shaped panel (like real robot)."""
|
||||
start_value = self.sensor_data.get('start_button', 0)
|
||||
dock_value = self.sensor_data.get('dock_button', 0)
|
||||
|
||||
x = 0
|
||||
y = self.BUTTON_Y_OFFSET
|
||||
w = self.BUTTON_PANEL_WIDTH
|
||||
h = self.BUTTON_PANEL_HEIGHT
|
||||
|
||||
start_pressed = start_value > 100
|
||||
dock_pressed = dock_value > 100
|
||||
|
||||
# Draw pill-shaped panel background (vertical orientation like real robot)
|
||||
painter.setPen(QPen(QColor(200, 200, 200), 1))
|
||||
painter.setBrush(QBrush(QColor(250, 250, 250)))
|
||||
panel_rect = QRectF(x - w/2, y - h/2, w, h)
|
||||
painter.drawRoundedRect(panel_rect, w/2, w/2)
|
||||
|
||||
# Dock button (top - home/charging icon)
|
||||
dock_y = y - h/4 - 2
|
||||
|
||||
# Draw glow effect when dock button is pressed
|
||||
if dock_pressed:
|
||||
glow = QRadialGradient(x, dock_y, 12)
|
||||
glow.setColorAt(0, QColor(60, 200, 60, 200))
|
||||
glow.setColorAt(0.5, QColor(60, 200, 60, 100))
|
||||
glow.setColorAt(1, QColor(60, 200, 60, 0))
|
||||
painter.setBrush(QBrush(glow))
|
||||
painter.setPen(Qt.NoPen)
|
||||
painter.drawEllipse(QPointF(x, dock_y), 12, 12)
|
||||
|
||||
color = self.COLOR_BUTTON_ON if dock_pressed else self.COLOR_BUTTON_OFF
|
||||
line_width = 2.0 if dock_pressed else 1.5
|
||||
|
||||
# Draw home/dock icon (house shape with base)
|
||||
painter.setPen(QPen(color, line_width, Qt.SolidLine, Qt.RoundCap, Qt.RoundJoin))
|
||||
painter.setBrush(Qt.NoBrush)
|
||||
# Roof
|
||||
painter.drawLine(QPointF(x - 5, dock_y), QPointF(x, dock_y - 5))
|
||||
painter.drawLine(QPointF(x, dock_y - 5), QPointF(x + 5, dock_y))
|
||||
# Walls
|
||||
painter.drawLine(QPointF(x - 4, dock_y), QPointF(x - 4, dock_y + 4))
|
||||
painter.drawLine(QPointF(x + 4, dock_y), QPointF(x + 4, dock_y + 4))
|
||||
# Base
|
||||
painter.drawLine(QPointF(x - 4, dock_y + 4), QPointF(x + 4, dock_y + 4))
|
||||
|
||||
# Start button (bottom - power symbol)
|
||||
start_y = y + h/4 + 2
|
||||
|
||||
# Draw glow effect when start button is pressed
|
||||
if start_pressed:
|
||||
glow = QRadialGradient(x, start_y, 12)
|
||||
glow.setColorAt(0, QColor(60, 200, 60, 200))
|
||||
glow.setColorAt(0.5, QColor(60, 200, 60, 100))
|
||||
glow.setColorAt(1, QColor(60, 200, 60, 0))
|
||||
painter.setBrush(QBrush(glow))
|
||||
painter.setPen(Qt.NoPen)
|
||||
painter.drawEllipse(QPointF(x, start_y), 12, 12)
|
||||
|
||||
color = self.COLOR_BUTTON_ON if start_pressed else self.COLOR_BUTTON_OFF
|
||||
line_width = 2.0 if start_pressed else 1.5
|
||||
|
||||
# Draw power icon (circle with vertical line at top)
|
||||
painter.setPen(QPen(color, line_width, Qt.SolidLine, Qt.RoundCap))
|
||||
painter.setBrush(Qt.NoBrush)
|
||||
# Arc (open at top)
|
||||
painter.drawArc(QRectF(x - 5, start_y - 5, 10, 10), 50 * 16, 260 * 16)
|
||||
# Vertical line
|
||||
painter.drawLine(QPointF(x, start_y - 6), QPointF(x, start_y - 1))
|
||||
|
||||
def _draw_battery_indicator(self, painter):
|
||||
"""Draw battery/charging indicator in top-right corner of widget."""
|
||||
# Reset transform to screen coordinates
|
||||
painter.resetTransform()
|
||||
|
||||
# Use debounced charging state for stable display
|
||||
is_charging = self.charging_debounce_count >= 25
|
||||
battery_level = self.sensor_data.get('battery_level', 0)
|
||||
battery_voltage = self.sensor_data.get('battery_voltage', 0.0)
|
||||
|
||||
# Position at top-right corner of widget
|
||||
x = self.width() - 60
|
||||
y = 25
|
||||
w, h = 36, 18
|
||||
|
||||
# Battery outline
|
||||
painter.setPen(QPen(QColor(100, 100, 100), 1.5))
|
||||
painter.setBrush(QBrush(QColor(240, 240, 240)))
|
||||
painter.drawRoundedRect(QRectF(x - w/2, y - h/2, w, h), 2, 2)
|
||||
|
||||
# Battery terminal (positive end)
|
||||
painter.setPen(Qt.NoPen)
|
||||
painter.setBrush(QBrush(QColor(100, 100, 100)))
|
||||
painter.drawRect(QRectF(x + w/2, y - 4, 3, 8))
|
||||
|
||||
# Battery fill based on level
|
||||
fill_x = x - w/2 + 2
|
||||
max_fill_width = w - 4
|
||||
|
||||
if is_charging:
|
||||
# Animated fill: oscillates from current level to 100%
|
||||
# Phase 0-1: expand from battery_level to 100%
|
||||
# Phase 1-2: contract from 100% back to battery_level
|
||||
if self.charging_animation_phase <= 1.0:
|
||||
anim_level = battery_level + (100 - battery_level) * self.charging_animation_phase
|
||||
else:
|
||||
anim_level = 100 - (100 - battery_level) * (self.charging_animation_phase - 1.0)
|
||||
fill_width = max_fill_width * (anim_level / 100.0)
|
||||
fill_color = self.COLOR_CHARGING # Orange/yellow for charging
|
||||
else:
|
||||
fill_width = max_fill_width * (battery_level / 100.0)
|
||||
# Color based on level
|
||||
if battery_level > 50:
|
||||
fill_color = QColor(76, 175, 80) # Green
|
||||
elif battery_level > 20:
|
||||
fill_color = QColor(255, 193, 7) # Yellow/Amber
|
||||
else:
|
||||
fill_color = QColor(244, 67, 54) # Red
|
||||
|
||||
painter.setBrush(QBrush(fill_color))
|
||||
painter.drawRoundedRect(QRectF(fill_x, y - h/2 + 2, fill_width, h - 4), 1, 1)
|
||||
|
||||
# Charging bolt overlay (on top of fill)
|
||||
if is_charging:
|
||||
painter.setPen(QPen(QColor(80, 80, 80), 1))
|
||||
painter.setBrush(QBrush(QColor(255, 220, 0))) # Yellow bolt
|
||||
# Filled lightning bolt centered in battery
|
||||
bolt = QPainterPath()
|
||||
bolt.moveTo(x + 2, y - 5)
|
||||
bolt.lineTo(x - 1, y - 1)
|
||||
bolt.lineTo(x + 1, y - 1)
|
||||
bolt.lineTo(x - 2, y + 5)
|
||||
bolt.lineTo(x + 1, y + 1)
|
||||
bolt.lineTo(x - 1, y + 1)
|
||||
bolt.closeSubpath()
|
||||
painter.drawPath(bolt)
|
||||
|
||||
# Battery percentage and voltage text below
|
||||
painter.setPen(QColor(80, 80, 80))
|
||||
font = QFont('Arial', 8)
|
||||
painter.setFont(font)
|
||||
text = f"{battery_level}% ({battery_voltage:.1f}V)"
|
||||
painter.drawText(QRectF(x - 40, y + h/2 + 2, 80, 15),
|
||||
Qt.AlignCenter, text)
|
||||
|
||||
def _draw_labels(self, painter, scale):
|
||||
"""Draw text labels for sensor values."""
|
||||
# Reset transform for text (don't flip Y)
|
||||
painter.resetTransform()
|
||||
painter.translate(self.width() / 2, self.height() / 2)
|
||||
|
||||
font = QFont('Arial', 8)
|
||||
painter.setFont(font)
|
||||
painter.setPen(self.COLOR_TEXT)
|
||||
|
||||
# Wheel encoder values
|
||||
left_ticks = self.sensor_data.get('wheel_left', 0)
|
||||
right_ticks = self.sensor_data.get('wheel_right', 0)
|
||||
|
||||
# Scale positions back to pixel space
|
||||
# Wheels are at Y=0 in robot coords, which is center in screen coords
|
||||
# Place labels below wheels (positive Y in screen = down)
|
||||
left_x = -self.WHEEL_X_OFFSET * scale
|
||||
right_x = self.WHEEL_X_OFFSET * scale
|
||||
wheel_y = (self.WHEEL_LENGTH / 2 + 10) * scale # Below wheel bottom
|
||||
|
||||
painter.drawText(QRectF(left_x - 30, wheel_y, 60, 20),
|
||||
Qt.AlignCenter, f"L:{left_ticks}")
|
||||
painter.drawText(QRectF(right_x - 30, wheel_y, 60, 20),
|
||||
Qt.AlignCenter, f"R:{right_ticks}")
|
||||
|
||||
# Status text at bottom
|
||||
status_parts = []
|
||||
if not self.sensor_data.get('dustbox_attached', True):
|
||||
status_parts.append("NO DUSTBOX")
|
||||
if self.sensor_data.get('bumper_left', False) or self.sensor_data.get('bumper_right', False):
|
||||
status_parts.append("BUMPER")
|
||||
|
||||
any_cliff = any([
|
||||
self.sensor_data.get('cliff_left_front', False),
|
||||
self.sensor_data.get('cliff_right_front', False),
|
||||
self.sensor_data.get('cliff_left_side', False),
|
||||
self.sensor_data.get('cliff_right_side', False),
|
||||
])
|
||||
if any_cliff:
|
||||
status_parts.append("CLIFF!")
|
||||
|
||||
if status_parts:
|
||||
status_text = " | ".join(status_parts)
|
||||
painter.setPen(QColor(200, 60, 60))
|
||||
painter.drawText(QRectF(-100, self.height()/2 - 30, 200, 20),
|
||||
Qt.AlignCenter, status_text)
|
||||
Reference in New Issue
Block a user