Refactor drishti into dual-mode architecture (sensor/SLAM)
Split the visualization app into two distinct modes: - sensor mode: Direct SangamIO connection for raw sensor visualization - slam mode: DhruvaSLAM connection for SLAM/navigation visualization Added new SLAM mode components: - SlamMainWindow with SlamView, SlamControlPanel, NavigationPanel - SlamThread for DhruvaStream/Command protocol handling - Navigation panel with Ctrl+Click goal setting support Updated protobuf definitions for new DhruvaStream protocol including RobotStatus, CurrentMap, SensorStatus, NavigationStatus messages and command/response types for mapping control. Simplified sensor mode main_window.py by removing SLAM integration.
This commit is contained in:
codetiger
+113
-65
@@ -2,14 +2,15 @@
|
||||
"""
|
||||
Drishti UI - CRL-200S Robot Visualization
|
||||
|
||||
Full-screen robot diagram with sensor overlays.
|
||||
Two modes:
|
||||
- sensor: Connect to SangamIO for raw sensor visualization (robot view)
|
||||
- slam: Connect to DhruvaSLAM for SLAM visualization (map, trajectory, controls)
|
||||
"""
|
||||
|
||||
import sys
|
||||
import argparse
|
||||
import logging
|
||||
from PyQt5.QtWidgets import QApplication
|
||||
from ui.main_window import MainWindow
|
||||
|
||||
# Configure logging
|
||||
logging.basicConfig(
|
||||
@@ -20,82 +21,26 @@ logging.basicConfig(
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def main():
|
||||
"""Main entry point."""
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Drishti - CRL-200S Robot Visualization"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--robot",
|
||||
default="192.168.68.101",
|
||||
help="Robot hostname or IP address (default: 192.168.68.101)"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--port",
|
||||
type=int,
|
||||
default=5555,
|
||||
help="TCP port (default: 5555)"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--verbose", "-v",
|
||||
action="store_true",
|
||||
help="Enable verbose logging"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--log-raw-packets",
|
||||
action="store_true",
|
||||
help="Log raw GD32 status packets to file for debugging"
|
||||
)
|
||||
def run_sensor_mode(args):
|
||||
"""Run sensor mode - connects to SangamIO for raw sensor data."""
|
||||
from ui.main_window import MainWindow
|
||||
|
||||
# SLAM/Odometry arguments
|
||||
parser.add_argument(
|
||||
"--slam-host",
|
||||
default="localhost",
|
||||
help="dhruva-slam-node hostname (default: localhost)"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--slam-port",
|
||||
type=int,
|
||||
default=5557,
|
||||
help="dhruva-slam-node TCP port (default: 5557)"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-slam",
|
||||
action="store_true",
|
||||
help="Disable SLAM/odometry connection"
|
||||
)
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.verbose:
|
||||
logging.getLogger().setLevel(logging.DEBUG)
|
||||
|
||||
logger.info("Drishti - CRL-200S Robot Visualization")
|
||||
logger.info(f"Connecting to robot at {args.robot}:{args.port}")
|
||||
logger.info("Drishti - Sensor Mode")
|
||||
logger.info(f"Connecting to SangamIO at {args.robot}:{args.port}")
|
||||
if args.log_raw_packets:
|
||||
logger.info("Raw packet logging ENABLED")
|
||||
if not args.no_slam:
|
||||
logger.info(f"SLAM enabled: {args.slam_host}:{args.slam_port}")
|
||||
else:
|
||||
logger.info("SLAM disabled")
|
||||
|
||||
# Create Qt application
|
||||
app = QApplication(sys.argv)
|
||||
app.setStyle('Fusion')
|
||||
|
||||
# Create and show main window
|
||||
try:
|
||||
window = MainWindow(
|
||||
robot_ip=args.robot,
|
||||
port=args.port,
|
||||
log_raw_packets=args.log_raw_packets,
|
||||
slam_host=args.slam_host,
|
||||
slam_port=args.slam_port,
|
||||
no_slam=args.no_slam
|
||||
log_raw_packets=args.log_raw_packets
|
||||
)
|
||||
window.show()
|
||||
|
||||
logger.info("Application started")
|
||||
logger.info("Sensor mode started")
|
||||
sys.exit(app.exec_())
|
||||
|
||||
except KeyboardInterrupt:
|
||||
@@ -106,5 +51,108 @@ def main():
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def run_slam_mode(args):
|
||||
"""Run SLAM mode - connects to DhruvaSLAM for SLAM visualization."""
|
||||
from ui.slam_main_window import SlamMainWindow
|
||||
|
||||
logger.info("Drishti - SLAM Mode")
|
||||
logger.info(f"Connecting to DhruvaSLAM at {args.host}:{args.port}")
|
||||
if args.command_port:
|
||||
logger.info(f"Command port: {args.command_port}")
|
||||
|
||||
app = QApplication(sys.argv)
|
||||
app.setStyle('Fusion')
|
||||
|
||||
try:
|
||||
window = SlamMainWindow(
|
||||
slam_host=args.host,
|
||||
slam_port=args.port,
|
||||
command_port=args.command_port
|
||||
)
|
||||
window.show()
|
||||
logger.info("SLAM mode started")
|
||||
sys.exit(app.exec_())
|
||||
|
||||
except KeyboardInterrupt:
|
||||
logger.info("Interrupted by user")
|
||||
sys.exit(0)
|
||||
except Exception as e:
|
||||
logger.error(f"Fatal error: {e}", exc_info=True)
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def main():
|
||||
"""Main entry point."""
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Drishti - CRL-200S Robot Visualization"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--verbose", "-v",
|
||||
action="store_true",
|
||||
help="Enable verbose logging"
|
||||
)
|
||||
|
||||
subparsers = parser.add_subparsers(dest="mode", help="Operating mode")
|
||||
|
||||
# Sensor mode subcommand
|
||||
sensor_parser = subparsers.add_parser(
|
||||
"sensor",
|
||||
help="Sensor mode - connect to SangamIO for raw sensor visualization"
|
||||
)
|
||||
sensor_parser.add_argument(
|
||||
"--robot",
|
||||
default="192.168.68.101",
|
||||
help="SangamIO hostname or IP address (default: 192.168.68.101)"
|
||||
)
|
||||
sensor_parser.add_argument(
|
||||
"--port",
|
||||
type=int,
|
||||
default=5555,
|
||||
help="SangamIO TCP port (default: 5555)"
|
||||
)
|
||||
sensor_parser.add_argument(
|
||||
"--log-raw-packets",
|
||||
action="store_true",
|
||||
help="Log raw GD32 status packets to file for debugging"
|
||||
)
|
||||
|
||||
# SLAM mode subcommand
|
||||
slam_parser = subparsers.add_parser(
|
||||
"slam",
|
||||
help="SLAM mode - connect to DhruvaSLAM for map visualization"
|
||||
)
|
||||
slam_parser.add_argument(
|
||||
"--host",
|
||||
default="localhost",
|
||||
help="DhruvaSLAM hostname (default: localhost)"
|
||||
)
|
||||
slam_parser.add_argument(
|
||||
"--port",
|
||||
type=int,
|
||||
default=5557,
|
||||
help="DhruvaSLAM stream port (default: 5557)"
|
||||
)
|
||||
slam_parser.add_argument(
|
||||
"--command-port",
|
||||
type=int,
|
||||
default=5558,
|
||||
help="DhruvaSLAM command port (default: 5558)"
|
||||
)
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.verbose:
|
||||
logging.getLogger().setLevel(logging.DEBUG)
|
||||
|
||||
if args.mode == "sensor":
|
||||
run_sensor_mode(args)
|
||||
elif args.mode == "slam":
|
||||
run_slam_mode(args)
|
||||
else:
|
||||
# Default: show help
|
||||
parser.print_help()
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
+84
-26
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
+13
-163
@@ -1,41 +1,34 @@
|
||||
"""
|
||||
Main window for Drishti robot visualization application.
|
||||
|
||||
Full-screen 3D wireframe robot visualization with real-time sensor overlays,
|
||||
IMU-based orientation display, and SLAM map/trajectory visualization.
|
||||
Full-screen 3D wireframe robot visualization with real-time sensor overlays
|
||||
and IMU-based orientation display. Connects to SangamIO for raw sensor data.
|
||||
"""
|
||||
|
||||
from PyQt5.QtCore import Qt, pyqtSlot, QEvent
|
||||
from PyQt5.QtWidgets import (
|
||||
QMainWindow, QWidget, QVBoxLayout, QHBoxLayout, QLabel, QStatusBar, QTabWidget
|
||||
QMainWindow, QWidget, QVBoxLayout, QHBoxLayout, QLabel, QStatusBar
|
||||
)
|
||||
from PyQt5.QtGui import QFont, QKeyEvent
|
||||
import logging
|
||||
|
||||
from ui.threads.telemetry_thread import TelemetryThread
|
||||
from ui.threads.odometry_thread import OdometryThread
|
||||
from ui.widgets.robot_3d_view import Robot3DView
|
||||
from ui.widgets.slam_view import SlamView
|
||||
from ui.widgets.control_panel import ControlPanel
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class MainWindow(QMainWindow):
|
||||
"""Main application window with full-screen robot diagram."""
|
||||
"""Main application window for sensor mode with robot diagram."""
|
||||
|
||||
def __init__(self, robot_ip="192.168.68.101", port=5555, log_raw_packets=False,
|
||||
slam_host=None, slam_port=5557, no_slam=False):
|
||||
def __init__(self, robot_ip="192.168.68.101", port=5555, log_raw_packets=False):
|
||||
super().__init__()
|
||||
|
||||
self.robot_ip = robot_ip
|
||||
self.port = port
|
||||
self.log_raw_packets = log_raw_packets
|
||||
self.slam_host = slam_host or "localhost"
|
||||
self.slam_port = slam_port
|
||||
self.slam_enabled = not no_slam
|
||||
self.telemetry_thread = None
|
||||
self.odometry_thread = None
|
||||
self.lidar_enabled = False # Lidar starts disabled
|
||||
self.side_brush_enabled = False
|
||||
self.main_brush_enabled = False
|
||||
@@ -48,9 +41,9 @@ class MainWindow(QMainWindow):
|
||||
self.pressed_keys = set() # Track currently pressed arrow keys for combinations
|
||||
|
||||
# Initialize UI
|
||||
self.setWindowTitle(f"Drishti - CRL-200S Monitor ({robot_ip})")
|
||||
self.setWindowTitle(f"Drishti Sensor - CRL-200S ({robot_ip})")
|
||||
self.setGeometry(100, 100, 800, 800)
|
||||
self.setStyleSheet("background-color: #f5f5f5;")
|
||||
self.setStyleSheet("background-color: #1a1a1a;")
|
||||
|
||||
# Create central widget and main layout
|
||||
central_widget = QWidget()
|
||||
@@ -62,49 +55,14 @@ class MainWindow(QMainWindow):
|
||||
title_bar = self._create_title_bar()
|
||||
main_layout.addWidget(title_bar)
|
||||
|
||||
# Content area: tab widget (left) + control panel (right)
|
||||
# Content area: robot view (left) + control panel (right)
|
||||
content_layout = QHBoxLayout()
|
||||
content_layout.setContentsMargins(0, 0, 0, 0)
|
||||
content_layout.setSpacing(0)
|
||||
|
||||
# Tab widget for different views
|
||||
self.tab_widget = QTabWidget()
|
||||
self.tab_widget.setStyleSheet("""
|
||||
QTabWidget::pane {
|
||||
border: none;
|
||||
background-color: #1a1a1a;
|
||||
}
|
||||
QTabBar::tab {
|
||||
background-color: #333;
|
||||
color: #aaa;
|
||||
padding: 8px 20px;
|
||||
margin-right: 2px;
|
||||
border-top-left-radius: 4px;
|
||||
border-top-right-radius: 4px;
|
||||
}
|
||||
QTabBar::tab:selected {
|
||||
background-color: #1a1a1a;
|
||||
color: #fff;
|
||||
}
|
||||
QTabBar::tab:hover:!selected {
|
||||
background-color: #444;
|
||||
}
|
||||
""")
|
||||
|
||||
# 3D robot view (Tab 1)
|
||||
# 3D robot view
|
||||
self.robot_3d_view = Robot3DView()
|
||||
self.tab_widget.addTab(self.robot_3d_view, "Robot View")
|
||||
|
||||
# SLAM view (Tab 2)
|
||||
self.slam_view = SlamView()
|
||||
self.slam_view.velocity_command.connect(self._on_slam_velocity)
|
||||
self.slam_view.slam_command.connect(self._on_slam_command)
|
||||
self.tab_widget.addTab(self.slam_view, "SLAM")
|
||||
|
||||
# Handle tab changes to show/hide control panel
|
||||
self.tab_widget.currentChanged.connect(self._on_tab_changed)
|
||||
|
||||
content_layout.addWidget(self.tab_widget, 1) # Stretch factor 1 (expands)
|
||||
content_layout.addWidget(self.robot_3d_view, 1) # Stretch factor 1 (expands)
|
||||
|
||||
# Control panel (right side, fixed width) - dark theme
|
||||
self.control_panel = ControlPanel()
|
||||
@@ -116,7 +74,7 @@ class MainWindow(QMainWindow):
|
||||
|
||||
# Status bar
|
||||
self.status_bar = QStatusBar()
|
||||
self.status_bar.setStyleSheet("background-color: #e0e0e0;")
|
||||
self.status_bar.setStyleSheet("background-color: #252525; color: #aaa;")
|
||||
self.setStatusBar(self.status_bar)
|
||||
self.connection_status_label = QLabel("Connecting...")
|
||||
self.connection_status_label.setStyleSheet("color: #666; font-weight: bold;")
|
||||
@@ -129,10 +87,6 @@ class MainWindow(QMainWindow):
|
||||
# Connect to robot
|
||||
self._start_telemetry()
|
||||
|
||||
# Connect to SLAM daemon (optional)
|
||||
if self.slam_enabled:
|
||||
self._start_odometry()
|
||||
|
||||
def _create_title_bar(self):
|
||||
"""Create application title bar."""
|
||||
title_widget = QWidget()
|
||||
@@ -141,7 +95,7 @@ class MainWindow(QMainWindow):
|
||||
layout = QVBoxLayout(title_widget)
|
||||
layout.setContentsMargins(15, 5, 15, 5)
|
||||
|
||||
title_label = QLabel(f"Drishti - CRL-200S Monitor")
|
||||
title_label = QLabel("Drishti Sensor - CRL-200S")
|
||||
title_label.setFont(QFont("Arial", 14, QFont.Bold))
|
||||
title_label.setStyleSheet("color: white;")
|
||||
layout.addWidget(title_label)
|
||||
@@ -151,7 +105,7 @@ class MainWindow(QMainWindow):
|
||||
def _start_telemetry(self):
|
||||
"""Start telemetry thread to receive sensor data."""
|
||||
try:
|
||||
self.status_bar.showMessage("Connecting to robot...")
|
||||
self.status_bar.showMessage("Connecting to SangamIO...")
|
||||
|
||||
# Create and start telemetry thread
|
||||
self.telemetry_thread = TelemetryThread(
|
||||
@@ -167,108 +121,6 @@ class MainWindow(QMainWindow):
|
||||
self.connection_status_label.setStyleSheet("color: red; font-weight: bold;")
|
||||
self.status_bar.showMessage(f"Error: {e}")
|
||||
|
||||
def _start_odometry(self):
|
||||
"""Start odometry thread to receive pose data from dhruva-slam-node."""
|
||||
try:
|
||||
logger.info(f"Connecting to SLAM at {self.slam_host}:{self.slam_port}")
|
||||
|
||||
# Create and start odometry thread
|
||||
self.odometry_thread = OdometryThread(
|
||||
self.slam_host, self.slam_port
|
||||
)
|
||||
self.odometry_thread.pose_received.connect(self._on_pose_data)
|
||||
self.odometry_thread.diagnostics_received.connect(self._on_diagnostics_data)
|
||||
self.odometry_thread.connection_status.connect(self._on_slam_connection_status)
|
||||
|
||||
# Connect SLAM signals to SlamView
|
||||
self.odometry_thread.slam_status_received.connect(self.slam_view.update_status)
|
||||
self.odometry_thread.slam_map_received.connect(self.slam_view.update_map)
|
||||
self.odometry_thread.slam_scan_received.connect(self.slam_view.update_scan)
|
||||
self.odometry_thread.slam_diagnostics_received.connect(self.slam_view.update_diagnostics)
|
||||
|
||||
self.odometry_thread.start()
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Failed to start odometry: {e}")
|
||||
self.status_bar.showMessage(f"SLAM Error: {e}", 3000)
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_pose_data(self, pose: dict):
|
||||
"""Handle pose data from odometry thread."""
|
||||
x = pose.get('x', 0.0)
|
||||
y = pose.get('y', 0.0)
|
||||
theta = pose.get('theta', 0.0)
|
||||
# Update SLAM view with pose for trajectory
|
||||
self.slam_view.update_pose(x, y, theta)
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_diagnostics_data(self, diagnostics: dict):
|
||||
"""Handle diagnostics data from odometry thread."""
|
||||
# Diagnostics are no longer shown in SLAM view
|
||||
pass
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_slam_velocity(self, velocity: dict):
|
||||
"""Handle velocity command from SLAM view (go-to-point controller)."""
|
||||
linear = velocity.get('linear', 0.0)
|
||||
angular = velocity.get('angular', 0.0)
|
||||
command = {
|
||||
"type": "ComponentControl",
|
||||
"id": "drive",
|
||||
"action": {
|
||||
"type": "Configure",
|
||||
"config": {
|
||||
"linear": {"F32": linear},
|
||||
"angular": {"F32": angular}
|
||||
}
|
||||
}
|
||||
}
|
||||
self._send_command(command)
|
||||
self.control_panel.set_velocity(linear, angular)
|
||||
logger.debug(f"SLAM velocity: linear={linear:.2f}, angular={angular:.2f}")
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_slam_command(self, command: dict):
|
||||
"""Handle SLAM command from SlamView (mode change, reset, etc.)."""
|
||||
# SLAM commands go to dhruva-slam-node, not SangamIO
|
||||
# TODO: Implement sending commands to dhruva-slam-node when supported
|
||||
cmd_type = command.get('command', '')
|
||||
if cmd_type == 'set_mode':
|
||||
mode = command.get('mode', 'Idle')
|
||||
logger.info(f"SLAM mode change requested: {mode}")
|
||||
self.status_bar.showMessage(f"SLAM mode: {mode}", 2000)
|
||||
elif cmd_type == 'reset':
|
||||
logger.info("SLAM reset requested")
|
||||
self.status_bar.showMessage("SLAM reset requested", 2000)
|
||||
elif cmd_type == 'reset_pose':
|
||||
x = command.get('x', 0)
|
||||
y = command.get('y', 0)
|
||||
theta = command.get('theta', 0)
|
||||
logger.info(f"SLAM pose reset requested: ({x:.2f}, {y:.2f}, {theta:.2f})")
|
||||
self.status_bar.showMessage("SLAM pose reset requested", 2000)
|
||||
elif cmd_type == 'save_map':
|
||||
path = command.get('path', '')
|
||||
logger.info(f"SLAM save map requested: {path}")
|
||||
self.status_bar.showMessage(f"SLAM save map: {path}", 2000)
|
||||
|
||||
@pyqtSlot(bool, str)
|
||||
def _on_slam_connection_status(self, connected: bool, message: str):
|
||||
"""Handle SLAM connection status changes."""
|
||||
if connected:
|
||||
logger.info(f"SLAM connected: {message}")
|
||||
else:
|
||||
logger.warning(f"SLAM disconnected: {message}")
|
||||
|
||||
@pyqtSlot(int)
|
||||
def _on_tab_changed(self, index: int):
|
||||
"""Handle tab change to show/hide control panel."""
|
||||
# Tab 0 = Robot View (show control panel)
|
||||
# Tab 1 = SLAM (hide control panel - it has its own status panel)
|
||||
if index == 0:
|
||||
self.control_panel.show()
|
||||
else:
|
||||
self.control_panel.hide()
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_sensor_data(self, data: dict):
|
||||
"""Handle sensor data from telemetry thread."""
|
||||
@@ -591,6 +443,4 @@ class MainWindow(QMainWindow):
|
||||
"""Handle window close event."""
|
||||
if self.telemetry_thread:
|
||||
self.telemetry_thread.stop()
|
||||
if self.odometry_thread:
|
||||
self.odometry_thread.stop()
|
||||
event.accept()
|
||||
|
||||
@@ -0,0 +1,381 @@
|
||||
"""
|
||||
SLAM main window for Drishti visualization application.
|
||||
|
||||
Connects to DhruvaSLAM daemon for:
|
||||
- Real-time map visualization
|
||||
- Robot pose and trajectory
|
||||
- Lidar scan overlay
|
||||
- Mapping control (start/stop/save)
|
||||
- Map management (list/enable/rename/delete)
|
||||
"""
|
||||
|
||||
import math
|
||||
from PyQt5.QtCore import Qt, pyqtSlot
|
||||
from PyQt5.QtWidgets import (
|
||||
QMainWindow, QWidget, QVBoxLayout, QHBoxLayout, QLabel, QStatusBar
|
||||
)
|
||||
from PyQt5.QtGui import QFont
|
||||
import logging
|
||||
|
||||
from ui.threads.slam_thread import SlamThread
|
||||
from ui.widgets.slam_view import SlamView
|
||||
from ui.widgets.slam_control_panel import SlamControlPanel
|
||||
from ui.widgets.navigation_panel import NavigationPanel
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class SlamMainWindow(QMainWindow):
|
||||
"""Main application window for SLAM mode."""
|
||||
|
||||
def __init__(self, slam_host="localhost", slam_port=5557, command_port=5558):
|
||||
super().__init__()
|
||||
|
||||
self.slam_host = slam_host
|
||||
self.slam_port = slam_port
|
||||
self.command_port = command_port
|
||||
self.slam_thread = None
|
||||
|
||||
# Initialize UI
|
||||
self.setWindowTitle(f"Drishti SLAM - {slam_host}:{slam_port}")
|
||||
self.setGeometry(100, 100, 1200, 800)
|
||||
self.setStyleSheet("background-color: #1a1a1a;")
|
||||
|
||||
# Create central widget and main layout
|
||||
central_widget = QWidget()
|
||||
self.setCentralWidget(central_widget)
|
||||
main_layout = QVBoxLayout(central_widget)
|
||||
main_layout.setContentsMargins(0, 0, 0, 0)
|
||||
|
||||
# Title bar
|
||||
title_bar = self._create_title_bar()
|
||||
main_layout.addWidget(title_bar)
|
||||
|
||||
# Content area: SLAM view (left) + control panel (right)
|
||||
content_layout = QHBoxLayout()
|
||||
content_layout.setContentsMargins(0, 0, 0, 0)
|
||||
content_layout.setSpacing(0)
|
||||
|
||||
# SLAM view
|
||||
self.slam_view = SlamView()
|
||||
content_layout.addWidget(self.slam_view, 1) # Stretch factor 1 (expands)
|
||||
|
||||
# Right side panel with control panel and navigation panel
|
||||
right_panel = QWidget()
|
||||
right_panel.setStyleSheet("background-color: #2d2d2d;")
|
||||
right_panel.setFixedWidth(280)
|
||||
right_layout = QVBoxLayout(right_panel)
|
||||
right_layout.setContentsMargins(0, 0, 0, 0)
|
||||
right_layout.setSpacing(0)
|
||||
|
||||
# Control panel (top of right side)
|
||||
self.control_panel = SlamControlPanel()
|
||||
self.control_panel.setStyleSheet("background-color: #2d2d2d;")
|
||||
self._connect_control_panel_signals()
|
||||
right_layout.addWidget(self.control_panel, 1)
|
||||
|
||||
# Navigation panel (bottom of right side)
|
||||
self.navigation_panel = NavigationPanel()
|
||||
self.navigation_panel.setStyleSheet("background-color: #2d2d2d;")
|
||||
self._connect_navigation_signals()
|
||||
right_layout.addWidget(self.navigation_panel, 0)
|
||||
|
||||
content_layout.addWidget(right_panel, 0) # Stretch factor 0 (fixed)
|
||||
|
||||
main_layout.addLayout(content_layout, 1)
|
||||
|
||||
# Status bar
|
||||
self.status_bar = QStatusBar()
|
||||
self.status_bar.setStyleSheet("background-color: #252525; color: #aaa;")
|
||||
self.setStatusBar(self.status_bar)
|
||||
self.connection_status_label = QLabel("Connecting...")
|
||||
self.connection_status_label.setStyleSheet("color: #666; font-weight: bold;")
|
||||
self.status_bar.addPermanentWidget(self.connection_status_label)
|
||||
|
||||
# Connect to SLAM daemon
|
||||
self._start_slam_connection()
|
||||
|
||||
def _create_title_bar(self):
|
||||
"""Create application title bar."""
|
||||
title_widget = QWidget()
|
||||
title_widget.setStyleSheet("background-color: #2d2d2d; padding: 8px;")
|
||||
title_widget.setFixedHeight(50)
|
||||
layout = QVBoxLayout(title_widget)
|
||||
layout.setContentsMargins(15, 5, 15, 5)
|
||||
|
||||
title_label = QLabel("Drishti SLAM Visualization")
|
||||
title_label.setFont(QFont("Arial", 14, QFont.Bold))
|
||||
title_label.setStyleSheet("color: white;")
|
||||
layout.addWidget(title_label)
|
||||
|
||||
return title_widget
|
||||
|
||||
def _connect_control_panel_signals(self):
|
||||
"""Connect control panel signals to SLAM commands."""
|
||||
self.control_panel.start_mapping_requested.connect(self._on_start_mapping)
|
||||
self.control_panel.stop_mapping_requested.connect(self._on_stop_mapping)
|
||||
self.control_panel.clear_map_requested.connect(self._on_clear_map)
|
||||
self.control_panel.emergency_stop_requested.connect(self._on_emergency_stop)
|
||||
self.control_panel.enable_map_requested.connect(self._on_enable_map)
|
||||
self.control_panel.rename_map_requested.connect(self._on_rename_map)
|
||||
self.control_panel.delete_map_requested.connect(self._on_delete_map)
|
||||
|
||||
def _connect_navigation_signals(self):
|
||||
"""Connect navigation-related signals."""
|
||||
# SlamView Ctrl+Click -> set navigation goal
|
||||
self.slam_view.goal_selected.connect(self._on_goal_selected)
|
||||
|
||||
# NavigationPanel cancel button -> cancel navigation
|
||||
self.navigation_panel.cancel_navigation.connect(self._on_cancel_navigation)
|
||||
|
||||
def _start_slam_connection(self):
|
||||
"""Start SLAM thread to receive data."""
|
||||
try:
|
||||
self.status_bar.showMessage("Connecting to DhruvaSLAM...")
|
||||
|
||||
# Create and start SLAM thread
|
||||
self.slam_thread = SlamThread(
|
||||
self.slam_host, self.slam_port, self.command_port
|
||||
)
|
||||
|
||||
# Connect data signals
|
||||
self.slam_thread.robot_status_received.connect(self._on_robot_status)
|
||||
self.slam_thread.sensor_status_received.connect(self._on_sensor_status)
|
||||
self.slam_thread.current_map_received.connect(self._on_current_map)
|
||||
self.slam_thread.map_list_received.connect(self._on_map_list)
|
||||
self.slam_thread.navigation_status_received.connect(self._on_navigation_status)
|
||||
|
||||
# Connect status signals
|
||||
self.slam_thread.connection_status.connect(self._on_connection_status)
|
||||
self.slam_thread.command_response_received.connect(self._on_command_response)
|
||||
|
||||
self.slam_thread.start()
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Failed to start SLAM connection: {e}")
|
||||
self.connection_status_label.setText("Connection Failed")
|
||||
self.connection_status_label.setStyleSheet("color: red; font-weight: bold;")
|
||||
self.status_bar.showMessage(f"Error: {e}")
|
||||
|
||||
# ==================== Data Handlers ====================
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_robot_status(self, data: dict):
|
||||
"""Handle robot status data."""
|
||||
# Update control panel
|
||||
self.control_panel.update_robot_status(data)
|
||||
|
||||
# Update SLAM view with pose
|
||||
pose = data.get('pose', {})
|
||||
x = pose.get('x', 0.0)
|
||||
y = pose.get('y', 0.0)
|
||||
theta = pose.get('theta', 0.0)
|
||||
self.slam_view.update_pose(x, y, theta)
|
||||
|
||||
# Update status bar
|
||||
state = data.get('state', 'Unknown')
|
||||
battery = data.get('battery_percent', 0.0)
|
||||
self.status_bar.showMessage(f"State: {state} | Battery: {battery:.0f}%")
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_sensor_status(self, data: dict):
|
||||
"""Handle sensor status data."""
|
||||
# Update SLAM view with lidar scan
|
||||
lidar = data.get('lidar')
|
||||
if lidar and lidar.get('ranges'):
|
||||
# Convert polar to cartesian points in robot frame
|
||||
ranges = lidar['ranges']
|
||||
angle_min = lidar.get('angle_min', -math.pi)
|
||||
angle_increment = lidar.get('angle_increment', 2 * math.pi / len(ranges))
|
||||
|
||||
# Convert polar to cartesian in robot frame
|
||||
# The angles from DhruvaSLAM have lidar offset applied but may have
|
||||
# different zero-reference than expected. Apply 90° rotation to align
|
||||
# with how the map was built.
|
||||
# Rotate by -π/2: x' = r*cos(θ - π/2) = r*sin(θ), y' = r*sin(θ - π/2) = -r*cos(θ)
|
||||
points = []
|
||||
for i, r in enumerate(ranges):
|
||||
if r > 0.01 and r < 30.0: # Valid range
|
||||
angle = angle_min + i * angle_increment
|
||||
x = r * math.sin(angle) # Rotated by -90°
|
||||
y = -r * math.cos(angle) # Rotated by -90°
|
||||
points.append([x, y])
|
||||
|
||||
self.slam_view.update_scan({'points': points})
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_current_map(self, data: dict):
|
||||
"""Handle current map data."""
|
||||
# Convert to format expected by SlamView.update_map()
|
||||
map_data = {
|
||||
'resolution': data.get('resolution', 0.05),
|
||||
'width': data.get('width', 0),
|
||||
'height': data.get('height', 0),
|
||||
'origin_x': data.get('origin_x', 0.0),
|
||||
'origin_y': data.get('origin_y', 0.0),
|
||||
'cells': data.get('cells', b''),
|
||||
}
|
||||
self.slam_view.update_map(map_data)
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_map_list(self, data: dict):
|
||||
"""Handle map list data."""
|
||||
self.control_panel.update_map_list(data)
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_navigation_status(self, data: dict):
|
||||
"""Handle navigation status data."""
|
||||
# Update SLAM view with path and goal visualization
|
||||
self.slam_view.update_navigation(data)
|
||||
|
||||
# Update navigation panel with status
|
||||
self.navigation_panel.update_navigation_status(data)
|
||||
|
||||
@pyqtSlot(bool, str)
|
||||
def _on_connection_status(self, connected: bool, message: str):
|
||||
"""Handle connection status changes."""
|
||||
self.control_panel.set_connection_status(connected, message)
|
||||
|
||||
if connected:
|
||||
self.connection_status_label.setText(f"Connected to {self.slam_host}")
|
||||
self.connection_status_label.setStyleSheet("color: green; font-weight: bold;")
|
||||
else:
|
||||
self.connection_status_label.setText("Disconnected")
|
||||
self.connection_status_label.setStyleSheet("color: red; font-weight: bold;")
|
||||
|
||||
self.status_bar.showMessage(message, 3000)
|
||||
logger.info(message)
|
||||
|
||||
@pyqtSlot(dict)
|
||||
def _on_command_response(self, response: dict):
|
||||
"""Handle command response."""
|
||||
success = response.get('success', False)
|
||||
error = response.get('error_message', '')
|
||||
|
||||
if success:
|
||||
data = response.get('data', {})
|
||||
if 'map_id' in data:
|
||||
self.status_bar.showMessage(f"Mapping started: {data['map_id']}", 3000)
|
||||
elif 'saved' in data:
|
||||
if data['saved']:
|
||||
self.status_bar.showMessage(f"Map saved: {data.get('area_m2', 0):.1f} m²", 3000)
|
||||
else:
|
||||
self.status_bar.showMessage("Map discarded", 3000)
|
||||
else:
|
||||
self.status_bar.showMessage("Command successful", 2000)
|
||||
else:
|
||||
self.status_bar.showMessage(f"Command failed: {error}", 5000)
|
||||
logger.error(f"Command failed: {error}")
|
||||
|
||||
# ==================== Command Handlers ====================
|
||||
|
||||
@pyqtSlot(str)
|
||||
def _on_start_mapping(self, map_name: str):
|
||||
"""Handle start mapping request."""
|
||||
if self.slam_thread:
|
||||
request_id = self.slam_thread.start_mapping(map_name)
|
||||
if request_id:
|
||||
self.status_bar.showMessage("Starting mapping...", 2000)
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send command", 3000)
|
||||
|
||||
@pyqtSlot(bool)
|
||||
def _on_stop_mapping(self, save: bool):
|
||||
"""Handle stop mapping request."""
|
||||
if self.slam_thread:
|
||||
request_id = self.slam_thread.stop_mapping(save)
|
||||
if request_id:
|
||||
action = "Saving" if save else "Discarding"
|
||||
self.status_bar.showMessage(f"{action} map...", 2000)
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send command", 3000)
|
||||
|
||||
@pyqtSlot()
|
||||
def _on_clear_map(self):
|
||||
"""Handle clear map request."""
|
||||
if self.slam_thread:
|
||||
request_id = self.slam_thread.clear_map()
|
||||
if request_id:
|
||||
self.status_bar.showMessage("Clearing map...", 2000)
|
||||
self.slam_view.clear_trajectory()
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send command", 3000)
|
||||
|
||||
@pyqtSlot()
|
||||
def _on_emergency_stop(self):
|
||||
"""Handle emergency stop request - immediate stop of all robot motion."""
|
||||
if self.slam_thread:
|
||||
request_id = self.slam_thread.emergency_stop()
|
||||
if request_id:
|
||||
self.status_bar.showMessage("EMERGENCY STOP activated!", 5000)
|
||||
logger.warning("Emergency stop activated by user")
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send emergency stop!", 3000)
|
||||
logger.error("Failed to send emergency stop command")
|
||||
|
||||
@pyqtSlot(str)
|
||||
def _on_enable_map(self, map_id: str):
|
||||
"""Handle enable map request."""
|
||||
if self.slam_thread:
|
||||
request_id = self.slam_thread.enable_map(map_id)
|
||||
if request_id:
|
||||
self.status_bar.showMessage(f"Enabling map: {map_id}", 2000)
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send command", 3000)
|
||||
|
||||
@pyqtSlot(str, str)
|
||||
def _on_rename_map(self, map_id: str, new_name: str):
|
||||
"""Handle rename map request."""
|
||||
if self.slam_thread:
|
||||
request_id = self.slam_thread.rename_map(map_id, new_name)
|
||||
if request_id:
|
||||
self.status_bar.showMessage(f"Renaming map to: {new_name}", 2000)
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send command", 3000)
|
||||
|
||||
@pyqtSlot(str)
|
||||
def _on_delete_map(self, map_id: str):
|
||||
"""Handle delete map request."""
|
||||
if self.slam_thread:
|
||||
request_id = self.slam_thread.delete_map(map_id)
|
||||
if request_id:
|
||||
self.status_bar.showMessage(f"Deleting map: {map_id}", 2000)
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send command", 3000)
|
||||
|
||||
# ==================== Navigation Handlers ====================
|
||||
|
||||
@pyqtSlot(float, float, float)
|
||||
def _on_goal_selected(self, x: float, y: float, theta: float):
|
||||
"""Handle goal selection from SLAM view (Ctrl+Click)."""
|
||||
if self.slam_thread:
|
||||
description = f"({x:.2f}, {y:.2f})"
|
||||
request_id = self.slam_thread.set_goal(x, y, theta, description)
|
||||
if request_id:
|
||||
self.status_bar.showMessage(f"Setting goal: {description}", 2000)
|
||||
logger.info(f"Goal set: x={x:.2f}, y={y:.2f}, theta={theta:.2f}")
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send goal command", 3000)
|
||||
else:
|
||||
self.status_bar.showMessage("Not connected to SLAM daemon", 3000)
|
||||
|
||||
@pyqtSlot()
|
||||
def _on_cancel_navigation(self):
|
||||
"""Handle cancel navigation request."""
|
||||
if self.slam_thread:
|
||||
request_id = self.slam_thread.cancel_goal()
|
||||
if request_id:
|
||||
self.status_bar.showMessage("Cancelling navigation...", 2000)
|
||||
logger.info("Navigation cancelled by user")
|
||||
# Clear navigation visualization
|
||||
self.slam_view.clear_navigation()
|
||||
else:
|
||||
self.status_bar.showMessage("Failed to send cancel command", 3000)
|
||||
else:
|
||||
self.status_bar.showMessage("Not connected to SLAM daemon", 3000)
|
||||
|
||||
def closeEvent(self, event):
|
||||
"""Handle window close event."""
|
||||
if self.slam_thread:
|
||||
self.slam_thread.stop()
|
||||
event.accept()
|
||||
@@ -0,0 +1,492 @@
|
||||
"""
|
||||
SLAM thread for receiving data from DhruvaSLAM daemon.
|
||||
|
||||
Uses the new DhruvaStream protocol (port 5557) for continuous data:
|
||||
- RobotStatus: pose, state, battery, mapping progress
|
||||
- SensorStatus: lidar, encoders, IMU
|
||||
- CurrentMap: occupancy grid
|
||||
- MapList: saved maps
|
||||
- NavigationStatus: path planning (future)
|
||||
|
||||
And DhruvaCommand/Response protocol (port 5558) for commands:
|
||||
- StartMapping, StopMapping, ClearMap
|
||||
- RenameMap, DeleteMap, EnableMap
|
||||
"""
|
||||
|
||||
import socket
|
||||
import struct
|
||||
import logging
|
||||
import uuid
|
||||
import os
|
||||
from PyQt5.QtCore import QThread, pyqtSignal
|
||||
|
||||
# Import protobuf definitions
|
||||
import sys
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', '..'))
|
||||
from proto import dhruva_pb2
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class SlamThread(QThread):
|
||||
"""Background thread for receiving SLAM data from DhruvaSLAM daemon."""
|
||||
|
||||
# Robot status signal (10Hz)
|
||||
# Payload: {pose: {x, y, theta}, state: str, active_map_id: str,
|
||||
# battery_percent, is_charging, is_docked,
|
||||
# distance_traveled_m, keyframe_count, loop_closures, map_area_m2,
|
||||
# localization_confidence}
|
||||
robot_status_received = pyqtSignal(dict)
|
||||
|
||||
# Sensor status signal (10Hz)
|
||||
# Payload: {lidar: {ranges, angle_min, angle_max, angle_increment},
|
||||
# left_encoder_ticks, right_encoder_ticks,
|
||||
# left_velocity_mps, right_velocity_mps,
|
||||
# gyro_z_radps, accel_x_mps2, accel_y_mps2,
|
||||
# raw_odometry: {x, y, theta}}
|
||||
sensor_status_received = pyqtSignal(dict)
|
||||
|
||||
# Current map signal (1Hz)
|
||||
# Payload: {map_id, name, resolution, width, height, origin_x, origin_y,
|
||||
# cells: bytes, explored_area_m2, obstacle_area_m2,
|
||||
# min_x, max_x, min_y, max_y}
|
||||
current_map_received = pyqtSignal(dict)
|
||||
|
||||
# Map list signal (on change)
|
||||
# Payload: {maps: [{map_id, name, created_at_us, updated_at_us, area_m2,
|
||||
# room_count, is_complete}, ...], active_map_id}
|
||||
map_list_received = pyqtSignal(dict)
|
||||
|
||||
# Navigation status signal (5Hz)
|
||||
# Payload: {state: str, path: [{x, y, theta}, ...], current_waypoint_index,
|
||||
# path_length_m, distance_remaining_m, estimated_time_remaining_s,
|
||||
# goal: {x, y, theta}, goal_description}
|
||||
navigation_status_received = pyqtSignal(dict)
|
||||
|
||||
# Connection status signal
|
||||
connection_status = pyqtSignal(bool, str)
|
||||
|
||||
# Command response signal
|
||||
# Payload: {request_id, success, error_message, data: {...}}
|
||||
command_response_received = pyqtSignal(dict)
|
||||
|
||||
def __init__(self, host: str, port: int = 5557, command_port: int = 5558, parent=None):
|
||||
super().__init__(parent)
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.command_port = command_port
|
||||
self._running = True
|
||||
self.stream_socket = None
|
||||
self.command_socket = None
|
||||
|
||||
def run(self):
|
||||
"""Main thread loop - connect and receive data."""
|
||||
logger.info(f"SLAM thread starting, connecting to {self.host}:{self.port}")
|
||||
|
||||
while self._running:
|
||||
try:
|
||||
# Connect to stream port
|
||||
self.stream_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.stream_socket.settimeout(5.0)
|
||||
self.stream_socket.connect((self.host, self.port))
|
||||
self.connection_status.emit(True, f"Connected to DhruvaSLAM at {self.host}:{self.port}")
|
||||
logger.info(f"Connected to DhruvaSLAM stream at {self.host}:{self.port}")
|
||||
|
||||
# Receive loop
|
||||
while self._running:
|
||||
try:
|
||||
message = self._read_stream_message()
|
||||
if message:
|
||||
self._process_stream_message(message)
|
||||
except socket.timeout:
|
||||
continue
|
||||
except Exception as e:
|
||||
if self._running:
|
||||
logger.error(f"Stream read error: {e}")
|
||||
self.connection_status.emit(False, f"SLAM read error: {e}")
|
||||
break
|
||||
|
||||
except Exception as e:
|
||||
if self._running:
|
||||
logger.warning(f"SLAM connection failed: {e}")
|
||||
self.connection_status.emit(False, f"SLAM connection failed: {e}")
|
||||
# Wait before retry
|
||||
self.msleep(3000)
|
||||
|
||||
finally:
|
||||
if self.stream_socket:
|
||||
try:
|
||||
self.stream_socket.close()
|
||||
except:
|
||||
pass
|
||||
self.stream_socket = None
|
||||
|
||||
logger.info("SLAM thread stopped")
|
||||
|
||||
def stop(self):
|
||||
"""Stop the thread."""
|
||||
logger.info("Stopping SLAM thread...")
|
||||
self._running = False
|
||||
if self.stream_socket:
|
||||
try:
|
||||
self.stream_socket.close()
|
||||
except:
|
||||
pass
|
||||
if self.command_socket:
|
||||
try:
|
||||
self.command_socket.close()
|
||||
except:
|
||||
pass
|
||||
self.wait()
|
||||
|
||||
def _read_stream_message(self) -> dhruva_pb2.DhruvaStream:
|
||||
"""Read a length-prefixed DhruvaStream message."""
|
||||
# Read 4-byte length prefix
|
||||
length_data = self._recv_exact(self.stream_socket, 4)
|
||||
if not length_data:
|
||||
return None
|
||||
|
||||
length = struct.unpack('>I', length_data)[0]
|
||||
|
||||
# Sanity check
|
||||
if length > 10 * 1024 * 1024: # 10MB max
|
||||
raise ValueError(f"Message too large: {length} bytes")
|
||||
|
||||
# Read message body
|
||||
data = self._recv_exact(self.stream_socket, length)
|
||||
if not data:
|
||||
return None
|
||||
|
||||
# Parse Protobuf
|
||||
try:
|
||||
msg = dhruva_pb2.DhruvaStream()
|
||||
msg.ParseFromString(data)
|
||||
return msg
|
||||
except Exception as e:
|
||||
logger.error(f"Parse error: {e}, data len={len(data)}, first 50 bytes: {data[:50].hex()}")
|
||||
raise
|
||||
|
||||
def _recv_exact(self, sock: socket.socket, n: int) -> bytes:
|
||||
"""Receive exactly n bytes."""
|
||||
data = b''
|
||||
while len(data) < n:
|
||||
chunk = sock.recv(n - len(data))
|
||||
if not chunk:
|
||||
return None
|
||||
data += chunk
|
||||
return data
|
||||
|
||||
def _process_stream_message(self, message: dhruva_pb2.DhruvaStream):
|
||||
"""Process a received DhruvaStream message and emit appropriate signal."""
|
||||
which = message.WhichOneof('data')
|
||||
|
||||
if which == 'robot_status':
|
||||
status = message.robot_status
|
||||
state_names = {
|
||||
dhruva_pb2.ROBOT_STATE_IDLE: 'Idle',
|
||||
dhruva_pb2.ROBOT_STATE_MAPPING: 'Mapping',
|
||||
dhruva_pb2.ROBOT_STATE_LOCALIZING: 'Localizing',
|
||||
dhruva_pb2.ROBOT_STATE_LOST: 'Lost',
|
||||
}
|
||||
payload = {
|
||||
'pose': {
|
||||
'x': status.pose.x if status.pose else 0.0,
|
||||
'y': status.pose.y if status.pose else 0.0,
|
||||
'theta': status.pose.theta if status.pose else 0.0,
|
||||
},
|
||||
'state': state_names.get(status.state, 'Unknown'),
|
||||
'active_map_id': status.active_map_id,
|
||||
'battery_percent': status.battery_percent,
|
||||
'is_charging': status.is_charging,
|
||||
'is_docked': status.is_docked,
|
||||
'distance_traveled_m': status.distance_traveled_m,
|
||||
'keyframe_count': status.keyframe_count,
|
||||
'loop_closures': status.loop_closures,
|
||||
'map_area_m2': status.map_area_m2,
|
||||
'localization_confidence': status.localization_confidence,
|
||||
'timestamp_us': message.timestamp_us,
|
||||
}
|
||||
self.robot_status_received.emit(payload)
|
||||
logger.debug(f"Robot status: state={payload['state']} pose=({status.pose.x:.2f}, {status.pose.y:.2f})")
|
||||
|
||||
elif which == 'sensor_status':
|
||||
sensor = message.sensor_status
|
||||
lidar_data = None
|
||||
if sensor.HasField('lidar'):
|
||||
lidar = sensor.lidar
|
||||
lidar_data = {
|
||||
'ranges': list(lidar.ranges),
|
||||
'intensities': list(lidar.intensities),
|
||||
'angle_min': lidar.angle_min,
|
||||
'angle_max': lidar.angle_max,
|
||||
'angle_increment': lidar.angle_increment,
|
||||
}
|
||||
|
||||
raw_odom = None
|
||||
if sensor.HasField('raw_odometry'):
|
||||
raw_odom = {
|
||||
'x': sensor.raw_odometry.x,
|
||||
'y': sensor.raw_odometry.y,
|
||||
'theta': sensor.raw_odometry.theta,
|
||||
}
|
||||
|
||||
payload = {
|
||||
'lidar': lidar_data,
|
||||
'left_encoder_ticks': sensor.left_encoder_ticks,
|
||||
'right_encoder_ticks': sensor.right_encoder_ticks,
|
||||
'left_velocity_mps': sensor.left_velocity_mps,
|
||||
'right_velocity_mps': sensor.right_velocity_mps,
|
||||
'gyro_z_radps': sensor.gyro_z_radps,
|
||||
'accel_x_mps2': sensor.accel_x_mps2,
|
||||
'accel_y_mps2': sensor.accel_y_mps2,
|
||||
'raw_odometry': raw_odom,
|
||||
'timestamp_us': message.timestamp_us,
|
||||
}
|
||||
self.sensor_status_received.emit(payload)
|
||||
if lidar_data:
|
||||
logger.debug(f"Sensor status: lidar={len(lidar_data['ranges'])} rays")
|
||||
|
||||
elif which == 'current_map':
|
||||
map_data = message.current_map
|
||||
payload = {
|
||||
'map_id': map_data.map_id,
|
||||
'name': map_data.name,
|
||||
'resolution': map_data.resolution,
|
||||
'width': map_data.width,
|
||||
'height': map_data.height,
|
||||
'origin_x': map_data.origin_x,
|
||||
'origin_y': map_data.origin_y,
|
||||
'cells': map_data.cells,
|
||||
'explored_area_m2': map_data.explored_area_m2,
|
||||
'obstacle_area_m2': map_data.obstacle_area_m2,
|
||||
'min_x': map_data.min_x,
|
||||
'max_x': map_data.max_x,
|
||||
'min_y': map_data.min_y,
|
||||
'max_y': map_data.max_y,
|
||||
'timestamp_us': message.timestamp_us,
|
||||
}
|
||||
self.current_map_received.emit(payload)
|
||||
logger.info(f"Current map received: {map_data.width}x{map_data.height} @ {map_data.resolution}m, cells={len(map_data.cells)} bytes")
|
||||
|
||||
elif which == 'map_list':
|
||||
map_list = message.map_list
|
||||
maps = []
|
||||
for m in map_list.maps:
|
||||
maps.append({
|
||||
'map_id': m.map_id,
|
||||
'name': m.name,
|
||||
'created_at_us': m.created_at_us,
|
||||
'updated_at_us': m.updated_at_us,
|
||||
'area_m2': m.area_m2,
|
||||
'room_count': m.room_count,
|
||||
'is_complete': m.is_complete,
|
||||
})
|
||||
payload = {
|
||||
'maps': maps,
|
||||
'active_map_id': map_list.active_map_id,
|
||||
'timestamp_us': message.timestamp_us,
|
||||
}
|
||||
self.map_list_received.emit(payload)
|
||||
logger.debug(f"Map list: {len(maps)} maps, active={map_list.active_map_id}")
|
||||
|
||||
elif which == 'navigation_status':
|
||||
nav = message.navigation_status
|
||||
state_names = {
|
||||
dhruva_pb2.NAV_STATE_IDLE: 'Idle',
|
||||
dhruva_pb2.NAV_STATE_PLANNING: 'Planning',
|
||||
dhruva_pb2.NAV_STATE_NAVIGATING: 'Navigating',
|
||||
dhruva_pb2.NAV_STATE_ROTATING_TO_HEADING: 'Rotating',
|
||||
dhruva_pb2.NAV_STATE_REACHED: 'Reached',
|
||||
dhruva_pb2.NAV_STATE_FAILED: 'Failed',
|
||||
dhruva_pb2.NAV_STATE_CANCELLED: 'Cancelled',
|
||||
}
|
||||
path = [{'x': p.x, 'y': p.y, 'theta': p.theta} for p in nav.path]
|
||||
goal = None
|
||||
if nav.HasField('goal'):
|
||||
goal = {'x': nav.goal.x, 'y': nav.goal.y, 'theta': nav.goal.theta}
|
||||
|
||||
payload = {
|
||||
'state': state_names.get(nav.state, 'Unknown'),
|
||||
'state_enum': nav.state,
|
||||
'path': path,
|
||||
'current_waypoint_index': nav.current_waypoint_index,
|
||||
'path_length_m': nav.path_length_m,
|
||||
'distance_remaining_m': nav.distance_remaining_m,
|
||||
'estimated_time_remaining_s': nav.estimated_time_remaining_s,
|
||||
'goal': goal,
|
||||
'goal_description': nav.goal_description,
|
||||
'target_id': nav.target_id,
|
||||
'targets_pending': nav.targets_pending,
|
||||
'targets_completed': nav.targets_completed,
|
||||
'status_message': nav.status_message,
|
||||
'failure_reason': nav.failure_reason,
|
||||
'timestamp_us': message.timestamp_us,
|
||||
}
|
||||
self.navigation_status_received.emit(payload)
|
||||
logger.debug(f"Nav status: state={payload['state']} waypoints={len(path)}")
|
||||
|
||||
# ==================== Command Methods ====================
|
||||
|
||||
def send_command(self, command: dhruva_pb2.DhruvaCommand) -> bool:
|
||||
"""Send a command to DhruvaSLAM and wait for response.
|
||||
|
||||
Returns True if command was sent successfully.
|
||||
Response will be emitted via command_response_received signal.
|
||||
"""
|
||||
try:
|
||||
# Connect to command port if needed
|
||||
if not self.command_socket:
|
||||
self.command_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.command_socket.settimeout(5.0)
|
||||
self.command_socket.connect((self.host, self.command_port))
|
||||
logger.info(f"Connected to DhruvaSLAM command port at {self.host}:{self.command_port}")
|
||||
|
||||
# Serialize and send
|
||||
payload = command.SerializeToString()
|
||||
length = struct.pack('>I', len(payload))
|
||||
self.command_socket.sendall(length + payload)
|
||||
|
||||
# Read response
|
||||
len_bytes = self._recv_exact(self.command_socket, 4)
|
||||
if not len_bytes:
|
||||
logger.error("No response from command port")
|
||||
return False
|
||||
|
||||
resp_len = struct.unpack('>I', len_bytes)[0]
|
||||
resp_data = self._recv_exact(self.command_socket, resp_len)
|
||||
if not resp_data:
|
||||
logger.error("Incomplete response from command port")
|
||||
return False
|
||||
|
||||
# Parse response
|
||||
response = dhruva_pb2.DhruvaResponse()
|
||||
response.ParseFromString(resp_data)
|
||||
|
||||
# Emit response
|
||||
resp_payload = {
|
||||
'request_id': response.request_id,
|
||||
'success': response.success,
|
||||
'error_message': response.error_message,
|
||||
}
|
||||
|
||||
which = response.WhichOneof('data')
|
||||
if which == 'mapping_started':
|
||||
resp_payload['data'] = {'map_id': response.mapping_started.map_id}
|
||||
elif which == 'mapping_stopped':
|
||||
resp_payload['data'] = {
|
||||
'saved': response.mapping_stopped.saved,
|
||||
'area_m2': response.mapping_stopped.area_m2,
|
||||
}
|
||||
elif which == 'goal_accepted':
|
||||
resp_payload['data'] = {'target_id': response.goal_accepted.target_id}
|
||||
elif which == 'goal_cancelled':
|
||||
resp_payload['data'] = {}
|
||||
|
||||
self.command_response_received.emit(resp_payload)
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Command send failed: {e}")
|
||||
# Reset command socket
|
||||
if self.command_socket:
|
||||
try:
|
||||
self.command_socket.close()
|
||||
except:
|
||||
pass
|
||||
self.command_socket = None
|
||||
return False
|
||||
|
||||
def start_mapping(self, map_name: str = "") -> str:
|
||||
"""Start mapping with optional map name. Returns request_id."""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.start_mapping.map_name = map_name
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
|
||||
def stop_mapping(self, save: bool = True) -> str:
|
||||
"""Stop mapping. Returns request_id."""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.stop_mapping.save = save
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
|
||||
def clear_map(self) -> str:
|
||||
"""Clear current map. Returns request_id."""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.clear_map.CopyFrom(dhruva_pb2.ClearMapCommand())
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
|
||||
def rename_map(self, map_id: str, new_name: str) -> str:
|
||||
"""Rename a saved map. Returns request_id."""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.rename_map.map_id = map_id
|
||||
cmd.rename_map.new_name = new_name
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
|
||||
def delete_map(self, map_id: str) -> str:
|
||||
"""Delete a saved map. Returns request_id."""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.delete_map.map_id = map_id
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
|
||||
def enable_map(self, map_id: str) -> str:
|
||||
"""Enable a map for localization. Returns request_id."""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.enable_map.map_id = map_id
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
|
||||
def emergency_stop(self) -> str:
|
||||
"""Emergency stop - disable lidar, motors, and all motion. Returns request_id."""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.emergency_stop.CopyFrom(dhruva_pb2.EmergencyStopCommand())
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
|
||||
def set_goal(self, x: float, y: float, theta: float = None, description: str = "") -> str:
|
||||
"""Set navigation goal. Returns request_id.
|
||||
|
||||
Args:
|
||||
x: Target X position in meters
|
||||
y: Target Y position in meters
|
||||
theta: Optional target heading in radians (None = planner chooses)
|
||||
description: Optional description for UI
|
||||
"""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.set_goal.x = x
|
||||
cmd.set_goal.y = y
|
||||
if theta is not None:
|
||||
cmd.set_goal.theta = theta
|
||||
cmd.set_goal.description = description if description else f"({x:.1f}, {y:.1f})"
|
||||
logger.info(f"Sending SetGoal: ({x:.2f}, {y:.2f}), theta={theta}, desc={cmd.set_goal.description}")
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
|
||||
def cancel_goal(self) -> str:
|
||||
"""Cancel active navigation. Returns request_id."""
|
||||
cmd = dhruva_pb2.DhruvaCommand()
|
||||
cmd.request_id = str(uuid.uuid4())
|
||||
cmd.cancel_goal.CopyFrom(dhruva_pb2.CancelGoalCommand())
|
||||
logger.info("Sending CancelGoal")
|
||||
if self.send_command(cmd):
|
||||
return cmd.request_id
|
||||
return None
|
||||
@@ -1,6 +1,17 @@
|
||||
"""
|
||||
Telemetry thread for receiving sensor data from SangamIO daemon.
|
||||
Supports Protobuf wire format with Message/SensorGroup structure.
|
||||
|
||||
Protocol Architecture:
|
||||
- **TCP (port 5555)**: Commands + registration for UDP streaming
|
||||
- **UDP (port 5556)**: Receives sensor data via unicast (fire-and-forget)
|
||||
|
||||
Connection Flow:
|
||||
1. Connect via TCP to SangamIO (registers client IP for UDP streaming)
|
||||
2. SangamIO starts sending UDP unicast packets to this client
|
||||
3. TCP connection kept alive for sending commands
|
||||
4. On TCP disconnect, UDP streaming stops
|
||||
|
||||
This design eliminates network flooding - only registered clients receive data.
|
||||
"""
|
||||
|
||||
import socket
|
||||
@@ -18,9 +29,17 @@ from proto import sangamio_pb2
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Default ports
|
||||
UDP_SENSOR_PORT = 5556 # UDP unicast for sensor data
|
||||
TCP_COMMAND_PORT = 5555 # TCP for commands + UDP registration
|
||||
|
||||
|
||||
class TelemetryThread(QThread):
|
||||
"""Background thread for receiving telemetry from SangamIO."""
|
||||
"""Background thread for receiving telemetry from SangamIO.
|
||||
|
||||
Connects via TCP first (to register for UDP streaming), then
|
||||
receives sensor data via UDP unicast. TCP is kept alive for commands.
|
||||
"""
|
||||
|
||||
# Signal emitted when new sensor data is received
|
||||
sensor_data_received = pyqtSignal(dict)
|
||||
@@ -28,12 +47,16 @@ class TelemetryThread(QThread):
|
||||
# Signal for connection status
|
||||
connection_status = pyqtSignal(bool, str)
|
||||
|
||||
def __init__(self, host: str, port: int = 5555, log_raw_packets: bool = False, parent=None):
|
||||
def __init__(self, host: str, port: int = TCP_COMMAND_PORT,
|
||||
udp_port: int = UDP_SENSOR_PORT,
|
||||
log_raw_packets: bool = False, parent=None):
|
||||
super().__init__(parent)
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.tcp_port = port # TCP port for commands
|
||||
self.udp_port = udp_port # UDP port for sensor broadcast
|
||||
self._running = True
|
||||
self.socket = None
|
||||
self.udp_socket = None # UDP socket for receiving sensors
|
||||
self.tcp_socket = None # TCP socket for sending commands
|
||||
self.log_raw_packets = log_raw_packets
|
||||
self.raw_packet_file = None
|
||||
self.sensor_data_file = None
|
||||
@@ -41,8 +64,10 @@ class TelemetryThread(QThread):
|
||||
self._sensor_log_count = 0
|
||||
|
||||
def run(self):
|
||||
"""Main thread loop - connect and receive data."""
|
||||
logger.info(f"Telemetry thread starting, connecting to {self.host}:{self.port}")
|
||||
"""Main thread loop - connect TCP first, then receive UDP sensor data."""
|
||||
logger.info(f"Telemetry thread starting")
|
||||
logger.info(f" TCP registration: {self.host}:{self.tcp_port}")
|
||||
logger.info(f" UDP sensor data: port {self.udp_port}")
|
||||
|
||||
# Open log files if enabled
|
||||
if self.log_raw_packets:
|
||||
@@ -56,51 +81,124 @@ class TelemetryThread(QThread):
|
||||
|
||||
while self._running:
|
||||
try:
|
||||
# Connect to SangamIO
|
||||
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.socket.settimeout(5.0)
|
||||
self.socket.connect((self.host, self.port))
|
||||
self.connection_status.emit(True, f"Connected to {self.host}:{self.port}")
|
||||
logger.info(f"Connected to {self.host}:{self.port} (wire format: Protobuf)")
|
||||
# Step 1: Connect TCP first (registers us for UDP streaming)
|
||||
self.connection_status.emit(False, f"Connecting to {self.host}:{self.tcp_port}...")
|
||||
if not self._connect_tcp():
|
||||
if self._running:
|
||||
self.connection_status.emit(False, f"TCP connection failed, retrying...")
|
||||
self.msleep(2000)
|
||||
continue
|
||||
|
||||
# Receive loop
|
||||
# Step 2: Create UDP socket for receiving sensor data
|
||||
self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self.udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
|
||||
# Bind to UDP port to receive unicast packets
|
||||
self.udp_socket.bind(('0.0.0.0', self.udp_port))
|
||||
self.udp_socket.settimeout(2.0) # Timeout for checking _running flag
|
||||
|
||||
self.connection_status.emit(True, f"Connected to {self.host} (UDP streaming active)")
|
||||
logger.info(f"UDP socket bound to port {self.udp_port} (receiving unicast from {self.host})")
|
||||
|
||||
# Step 3: Receive loop - UDP datagrams
|
||||
while self._running:
|
||||
try:
|
||||
message = self._read_message()
|
||||
if message:
|
||||
self._process_message(message)
|
||||
# UDP receives complete datagrams
|
||||
data, addr = self.udp_socket.recvfrom(65536) # Max UDP packet size
|
||||
if data:
|
||||
message = self._parse_udp_message(data)
|
||||
if message:
|
||||
self._process_message(message)
|
||||
except socket.timeout:
|
||||
# Check if TCP connection is still alive
|
||||
if not self._is_tcp_connected():
|
||||
logger.warning("TCP connection lost, reconnecting...")
|
||||
self.connection_status.emit(False, "TCP disconnected, reconnecting...")
|
||||
break
|
||||
continue
|
||||
except Exception as e:
|
||||
if self._running:
|
||||
logger.error(f"Read error: {e}")
|
||||
self.connection_status.emit(False, f"Read error: {e}")
|
||||
logger.error(f"UDP receive error: {e}")
|
||||
self.connection_status.emit(False, f"UDP error: {e}")
|
||||
break
|
||||
|
||||
except Exception as e:
|
||||
if self._running:
|
||||
logger.error(f"Connection failed: {e}")
|
||||
self.connection_status.emit(False, f"Connection failed: {e}")
|
||||
logger.error(f"Connection setup failed: {e}")
|
||||
self.connection_status.emit(False, f"Setup failed: {e}")
|
||||
# Wait before retry
|
||||
self.msleep(2000)
|
||||
|
||||
finally:
|
||||
if self.socket:
|
||||
# Clean up sockets
|
||||
if self.udp_socket:
|
||||
try:
|
||||
self.socket.close()
|
||||
self.udp_socket.close()
|
||||
except:
|
||||
pass
|
||||
self.socket = None
|
||||
self.udp_socket = None
|
||||
if self.tcp_socket:
|
||||
try:
|
||||
self.tcp_socket.close()
|
||||
except:
|
||||
pass
|
||||
self.tcp_socket = None
|
||||
|
||||
logger.info("Telemetry thread stopped")
|
||||
|
||||
def _connect_tcp(self) -> bool:
|
||||
"""Connect TCP socket to register for UDP streaming."""
|
||||
try:
|
||||
self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
self.tcp_socket.settimeout(5.0)
|
||||
self.tcp_socket.connect((self.host, self.tcp_port))
|
||||
logger.info(f"TCP connected to {self.host}:{self.tcp_port} (registered for UDP streaming)")
|
||||
return True
|
||||
except Exception as e:
|
||||
logger.error(f"TCP connection failed: {e}")
|
||||
if self.tcp_socket:
|
||||
try:
|
||||
self.tcp_socket.close()
|
||||
except:
|
||||
pass
|
||||
self.tcp_socket = None
|
||||
return False
|
||||
|
||||
def _is_tcp_connected(self) -> bool:
|
||||
"""Check if TCP socket is still connected."""
|
||||
if not self.tcp_socket:
|
||||
return False
|
||||
try:
|
||||
# Use non-blocking check - recv with MSG_PEEK
|
||||
self.tcp_socket.setblocking(False)
|
||||
try:
|
||||
data = self.tcp_socket.recv(1, socket.MSG_PEEK)
|
||||
# If recv returns empty, connection was closed
|
||||
if data == b'':
|
||||
return False
|
||||
except BlockingIOError:
|
||||
# No data available but connection is alive
|
||||
pass
|
||||
except ConnectionResetError:
|
||||
return False
|
||||
finally:
|
||||
self.tcp_socket.setblocking(True)
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
|
||||
def stop(self):
|
||||
"""Stop the thread."""
|
||||
logger.info("Stopping telemetry thread...")
|
||||
self._running = False
|
||||
if self.socket:
|
||||
if self.udp_socket:
|
||||
try:
|
||||
self.socket.close()
|
||||
self.udp_socket.close()
|
||||
except:
|
||||
pass
|
||||
if self.tcp_socket:
|
||||
try:
|
||||
self.tcp_socket.close()
|
||||
except:
|
||||
pass
|
||||
if self.raw_packet_file:
|
||||
@@ -117,8 +215,12 @@ class TelemetryThread(QThread):
|
||||
pass
|
||||
self.wait()
|
||||
|
||||
def _ensure_tcp_connected(self) -> bool:
|
||||
"""Check if TCP socket is connected (established in run())."""
|
||||
return self.tcp_socket is not None and self._is_tcp_connected()
|
||||
|
||||
def send_command(self, command: dict) -> bool:
|
||||
"""Send a command through the existing connection.
|
||||
"""Send a command via TCP connection.
|
||||
|
||||
Commands use the ComponentControl protocol:
|
||||
{
|
||||
@@ -127,8 +229,8 @@ class TelemetryThread(QThread):
|
||||
"action": {"type": "Enable" | "Disable" | "Reset" | "Configure", "config": {...}}
|
||||
}
|
||||
"""
|
||||
if not self.socket:
|
||||
logger.error("Cannot send command: not connected")
|
||||
if not self._ensure_tcp_connected():
|
||||
logger.error("Cannot send command: TCP not connected")
|
||||
return False
|
||||
|
||||
try:
|
||||
@@ -165,15 +267,16 @@ class TelemetryThread(QThread):
|
||||
self._set_sensor_value(sensor_val, value)
|
||||
action.config[key].CopyFrom(sensor_val)
|
||||
|
||||
# Serialize and send
|
||||
# Serialize and send via TCP
|
||||
data = msg.SerializeToString()
|
||||
length = struct.pack('>I', len(data))
|
||||
self.socket.sendall(length + data)
|
||||
logger.debug(f"Command sent: {command}")
|
||||
self.tcp_socket.sendall(length + data)
|
||||
logger.debug(f"Command sent via TCP: {command}")
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Failed to send command: {e}")
|
||||
# Connection error will be detected by run() loop on next timeout
|
||||
return False
|
||||
|
||||
def _set_sensor_value(self, sensor_val: sangamio_pb2.SensorValue, value):
|
||||
@@ -213,39 +316,35 @@ class TelemetryThread(QThread):
|
||||
elif isinstance(value, str):
|
||||
sensor_val.string_val = value
|
||||
|
||||
def _read_message(self) -> sangamio_pb2.Message:
|
||||
"""Read a length-prefixed Protobuf message."""
|
||||
# Read 4-byte length prefix
|
||||
length_data = self._recv_exact(4)
|
||||
if not length_data:
|
||||
def _parse_udp_message(self, data: bytes) -> sangamio_pb2.Message:
|
||||
"""Parse a UDP datagram containing a length-prefixed Protobuf message.
|
||||
|
||||
UDP datagrams are received atomically, so we get the complete message
|
||||
in one receive. The format is: [4-byte length][protobuf payload]
|
||||
"""
|
||||
if len(data) < 4:
|
||||
logger.warning(f"UDP packet too short: {len(data)} bytes")
|
||||
return None
|
||||
|
||||
length = struct.unpack('>I', length_data)[0]
|
||||
# Read 4-byte length prefix
|
||||
length = struct.unpack('>I', data[:4])[0]
|
||||
|
||||
# Validate length matches packet size
|
||||
expected_size = 4 + length
|
||||
if len(data) != expected_size:
|
||||
logger.warning(f"UDP packet size mismatch: expected {expected_size}, got {len(data)}")
|
||||
return None
|
||||
|
||||
# Sanity check
|
||||
if length > 1024 * 1024:
|
||||
raise ValueError(f"Message too large: {length} bytes")
|
||||
|
||||
# Read message body
|
||||
data = self._recv_exact(length)
|
||||
if not data:
|
||||
logger.warning(f"UDP message too large: {length} bytes")
|
||||
return None
|
||||
|
||||
# Parse Protobuf
|
||||
# Parse Protobuf payload (after 4-byte length prefix)
|
||||
msg = sangamio_pb2.Message()
|
||||
msg.ParseFromString(data)
|
||||
msg.ParseFromString(data[4:])
|
||||
return msg
|
||||
|
||||
def _recv_exact(self, n: int) -> bytes:
|
||||
"""Receive exactly n bytes."""
|
||||
data = b''
|
||||
while len(data) < n:
|
||||
chunk = self.socket.recv(n - len(data))
|
||||
if not chunk:
|
||||
return None
|
||||
data += chunk
|
||||
return data
|
||||
|
||||
def _process_message(self, message: sangamio_pb2.Message):
|
||||
"""Process a received message and emit signal."""
|
||||
topic = message.topic
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
from .robot_3d_view import Robot3DView
|
||||
from .control_panel import ControlPanel
|
||||
from .slam_view import SlamView
|
||||
from .diagnostics_panel import DiagnosticsPanel
|
||||
from .slam_control_panel import SlamControlPanel
|
||||
from .navigation_panel import NavigationPanel
|
||||
|
||||
__all__ = ['Robot3DView', 'ControlPanel', 'SlamView', 'DiagnosticsPanel']
|
||||
__all__ = ['Robot3DView', 'ControlPanel', 'SlamView', 'SlamControlPanel', 'NavigationPanel']
|
||||
|
||||
@@ -0,0 +1,314 @@
|
||||
"""
|
||||
Navigation status panel widget.
|
||||
|
||||
Displays:
|
||||
- Current navigation state
|
||||
- Target description
|
||||
- Progress (distance remaining, ETA)
|
||||
- Path length and waypoint count
|
||||
- Cancel button
|
||||
- Instructions for Ctrl+Click
|
||||
|
||||
Emits:
|
||||
- cancel_navigation: User clicked cancel button
|
||||
"""
|
||||
|
||||
import logging
|
||||
from PyQt5.QtWidgets import (
|
||||
QWidget, QVBoxLayout, QHBoxLayout, QLabel, QFrame, QPushButton,
|
||||
QProgressBar, QGroupBox
|
||||
)
|
||||
from PyQt5.QtCore import Qt, pyqtSignal
|
||||
from PyQt5.QtGui import QFont
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class NavigationPanel(QWidget):
|
||||
"""Navigation status and control panel."""
|
||||
|
||||
# Signal emitted when user clicks cancel
|
||||
cancel_navigation = pyqtSignal()
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
|
||||
self._current_state = 'Idle'
|
||||
self._setup_ui()
|
||||
|
||||
def _setup_ui(self):
|
||||
"""Setup the UI components."""
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(10, 10, 10, 10)
|
||||
layout.setSpacing(10)
|
||||
|
||||
# Title
|
||||
title = QLabel("Navigation")
|
||||
title.setFont(QFont("Arial", 12, QFont.Bold))
|
||||
title.setStyleSheet("color: white;")
|
||||
layout.addWidget(title)
|
||||
|
||||
# Instructions
|
||||
instructions = QLabel("Ctrl+Click on map to set goal")
|
||||
instructions.setStyleSheet("color: #888; font-size: 11px; font-style: italic;")
|
||||
layout.addWidget(instructions)
|
||||
|
||||
# Status frame
|
||||
status_frame = self._create_status_frame()
|
||||
layout.addWidget(status_frame)
|
||||
|
||||
# Progress frame
|
||||
progress_frame = self._create_progress_frame()
|
||||
layout.addWidget(progress_frame)
|
||||
|
||||
# Cancel button
|
||||
self._cancel_btn = QPushButton("Cancel Navigation")
|
||||
self._cancel_btn.setEnabled(False)
|
||||
self._cancel_btn.setStyleSheet("""
|
||||
QPushButton {
|
||||
background-color: #a04040;
|
||||
color: white;
|
||||
border: 1px solid #555;
|
||||
border-radius: 4px;
|
||||
padding: 8px;
|
||||
font-weight: bold;
|
||||
}
|
||||
QPushButton:hover { background-color: #c04040; }
|
||||
QPushButton:disabled {
|
||||
background-color: #444;
|
||||
color: #666;
|
||||
}
|
||||
""")
|
||||
self._cancel_btn.clicked.connect(self._on_cancel_clicked)
|
||||
layout.addWidget(self._cancel_btn)
|
||||
|
||||
layout.addStretch()
|
||||
|
||||
def _create_status_frame(self) -> QFrame:
|
||||
"""Create the status display frame."""
|
||||
frame = QGroupBox("Status")
|
||||
frame.setStyleSheet("""
|
||||
QGroupBox {
|
||||
color: #aaa;
|
||||
border: 1px solid #444;
|
||||
border-radius: 4px;
|
||||
margin-top: 10px;
|
||||
padding-top: 10px;
|
||||
}
|
||||
QGroupBox::title {
|
||||
subcontrol-origin: margin;
|
||||
left: 10px;
|
||||
padding: 0 5px;
|
||||
}
|
||||
QLabel { color: #ccc; }
|
||||
""")
|
||||
layout = QVBoxLayout(frame)
|
||||
layout.setSpacing(6)
|
||||
|
||||
# State row
|
||||
state_layout = QHBoxLayout()
|
||||
state_layout.addWidget(QLabel("State:"))
|
||||
self._state_label = QLabel("Idle")
|
||||
self._state_label.setStyleSheet("color: #888; font-weight: bold;")
|
||||
state_layout.addWidget(self._state_label)
|
||||
state_layout.addStretch()
|
||||
layout.addLayout(state_layout)
|
||||
|
||||
# Target row
|
||||
target_layout = QHBoxLayout()
|
||||
target_layout.addWidget(QLabel("Target:"))
|
||||
self._target_label = QLabel("-")
|
||||
self._target_label.setStyleSheet("color: #aaa;")
|
||||
self._target_label.setWordWrap(True)
|
||||
target_layout.addWidget(self._target_label, 1)
|
||||
layout.addLayout(target_layout)
|
||||
|
||||
# Status message row
|
||||
msg_layout = QHBoxLayout()
|
||||
msg_layout.addWidget(QLabel("Status:"))
|
||||
self._message_label = QLabel("-")
|
||||
self._message_label.setStyleSheet("color: #aaa;")
|
||||
self._message_label.setWordWrap(True)
|
||||
msg_layout.addWidget(self._message_label, 1)
|
||||
layout.addLayout(msg_layout)
|
||||
|
||||
return frame
|
||||
|
||||
def _create_progress_frame(self) -> QFrame:
|
||||
"""Create the progress display frame."""
|
||||
frame = QGroupBox("Progress")
|
||||
frame.setStyleSheet("""
|
||||
QGroupBox {
|
||||
color: #aaa;
|
||||
border: 1px solid #444;
|
||||
border-radius: 4px;
|
||||
margin-top: 10px;
|
||||
padding-top: 10px;
|
||||
}
|
||||
QGroupBox::title {
|
||||
subcontrol-origin: margin;
|
||||
left: 10px;
|
||||
padding: 0 5px;
|
||||
}
|
||||
QLabel { color: #ccc; }
|
||||
""")
|
||||
layout = QVBoxLayout(frame)
|
||||
layout.setSpacing(6)
|
||||
|
||||
# Progress bar
|
||||
self._progress_bar = QProgressBar()
|
||||
self._progress_bar.setRange(0, 100)
|
||||
self._progress_bar.setValue(0)
|
||||
self._progress_bar.setTextVisible(True)
|
||||
self._progress_bar.setFormat("%p%")
|
||||
self._progress_bar.setStyleSheet("""
|
||||
QProgressBar {
|
||||
border: 1px solid #444;
|
||||
border-radius: 3px;
|
||||
background-color: #333;
|
||||
text-align: center;
|
||||
color: white;
|
||||
}
|
||||
QProgressBar::chunk {
|
||||
background-color: #4a9;
|
||||
border-radius: 2px;
|
||||
}
|
||||
""")
|
||||
layout.addWidget(self._progress_bar)
|
||||
|
||||
# Distance row
|
||||
dist_layout = QHBoxLayout()
|
||||
dist_layout.addWidget(QLabel("Remaining:"))
|
||||
self._distance_label = QLabel("-")
|
||||
self._distance_label.setStyleSheet("color: #aaa;")
|
||||
dist_layout.addWidget(self._distance_label)
|
||||
dist_layout.addStretch()
|
||||
layout.addLayout(dist_layout)
|
||||
|
||||
# Path info row
|
||||
path_layout = QHBoxLayout()
|
||||
path_layout.addWidget(QLabel("Path:"))
|
||||
self._path_label = QLabel("-")
|
||||
self._path_label.setStyleSheet("color: #aaa;")
|
||||
path_layout.addWidget(self._path_label)
|
||||
path_layout.addStretch()
|
||||
layout.addLayout(path_layout)
|
||||
|
||||
# ETA row
|
||||
eta_layout = QHBoxLayout()
|
||||
eta_layout.addWidget(QLabel("ETA:"))
|
||||
self._eta_label = QLabel("-")
|
||||
self._eta_label.setStyleSheet("color: #aaa;")
|
||||
eta_layout.addWidget(self._eta_label)
|
||||
eta_layout.addStretch()
|
||||
layout.addLayout(eta_layout)
|
||||
|
||||
return frame
|
||||
|
||||
def _on_cancel_clicked(self):
|
||||
"""Handle cancel button click."""
|
||||
logger.info("Cancel navigation clicked")
|
||||
self.cancel_navigation.emit()
|
||||
|
||||
def update_navigation_status(self, nav_data: dict):
|
||||
"""Update the panel with navigation status data.
|
||||
|
||||
Args:
|
||||
nav_data: Dict with state, goal_description, path, distance_remaining_m,
|
||||
estimated_time_remaining_s, status_message, etc.
|
||||
"""
|
||||
state = nav_data.get('state', 'Idle')
|
||||
self._current_state = state
|
||||
|
||||
# Update state label with color coding
|
||||
state_colors = {
|
||||
'Idle': '#888',
|
||||
'Planning': '#ff9',
|
||||
'Navigating': '#4f4',
|
||||
'Rotating': '#4ff',
|
||||
'Reached': '#4f4',
|
||||
'Failed': '#f44',
|
||||
'Cancelled': '#888',
|
||||
}
|
||||
state_color = state_colors.get(state, '#aaa')
|
||||
self._state_label.setText(state)
|
||||
self._state_label.setStyleSheet(f"color: {state_color}; font-weight: bold;")
|
||||
|
||||
# Update target
|
||||
goal_desc = nav_data.get('goal_description', '')
|
||||
goal = nav_data.get('goal')
|
||||
if goal_desc:
|
||||
self._target_label.setText(goal_desc)
|
||||
elif goal:
|
||||
self._target_label.setText(f"({goal['x']:.1f}, {goal['y']:.1f})")
|
||||
else:
|
||||
self._target_label.setText("-")
|
||||
|
||||
# Update status message
|
||||
status_msg = nav_data.get('status_message', '')
|
||||
failure_reason = nav_data.get('failure_reason', '')
|
||||
if failure_reason and state == 'Failed':
|
||||
self._message_label.setText(failure_reason)
|
||||
self._message_label.setStyleSheet("color: #f88;")
|
||||
elif status_msg:
|
||||
self._message_label.setText(status_msg)
|
||||
self._message_label.setStyleSheet("color: #aaa;")
|
||||
else:
|
||||
self._message_label.setText("-")
|
||||
self._message_label.setStyleSheet("color: #aaa;")
|
||||
|
||||
# Update progress
|
||||
path_length = nav_data.get('path_length_m', 0.0)
|
||||
distance_remaining = nav_data.get('distance_remaining_m', 0.0)
|
||||
eta = nav_data.get('estimated_time_remaining_s', 0.0)
|
||||
path = nav_data.get('path', [])
|
||||
current_wp = nav_data.get('current_waypoint_index', 0)
|
||||
|
||||
# Calculate progress percentage
|
||||
if path_length > 0:
|
||||
progress = int((1.0 - distance_remaining / path_length) * 100)
|
||||
progress = max(0, min(100, progress))
|
||||
else:
|
||||
progress = 0
|
||||
|
||||
self._progress_bar.setValue(progress)
|
||||
|
||||
# Update distance
|
||||
if distance_remaining > 0:
|
||||
self._distance_label.setText(f"{distance_remaining:.2f} m")
|
||||
else:
|
||||
self._distance_label.setText("-")
|
||||
|
||||
# Update path info
|
||||
if path:
|
||||
self._path_label.setText(f"{len(path)} pts, {path_length:.1f}m total")
|
||||
else:
|
||||
self._path_label.setText("-")
|
||||
|
||||
# Update ETA
|
||||
if eta > 0:
|
||||
if eta < 60:
|
||||
self._eta_label.setText(f"{eta:.0f} sec")
|
||||
else:
|
||||
minutes = int(eta / 60)
|
||||
seconds = int(eta % 60)
|
||||
self._eta_label.setText(f"{minutes}:{seconds:02d}")
|
||||
else:
|
||||
self._eta_label.setText("-")
|
||||
|
||||
# Enable/disable cancel button
|
||||
active_states = ('Planning', 'Navigating', 'Rotating')
|
||||
self._cancel_btn.setEnabled(state in active_states)
|
||||
|
||||
def clear(self):
|
||||
"""Clear all display fields."""
|
||||
self._state_label.setText("Idle")
|
||||
self._state_label.setStyleSheet("color: #888; font-weight: bold;")
|
||||
self._target_label.setText("-")
|
||||
self._message_label.setText("-")
|
||||
self._message_label.setStyleSheet("color: #aaa;")
|
||||
self._progress_bar.setValue(0)
|
||||
self._distance_label.setText("-")
|
||||
self._path_label.setText("-")
|
||||
self._eta_label.setText("-")
|
||||
self._cancel_btn.setEnabled(False)
|
||||
@@ -0,0 +1,588 @@
|
||||
"""
|
||||
SLAM control panel widget for DhruvaSLAM.
|
||||
|
||||
Provides UI controls for:
|
||||
- Mapping control (start, stop, clear)
|
||||
- Map management (list, enable, rename, delete)
|
||||
- Status display (state, progress, localization confidence)
|
||||
"""
|
||||
|
||||
from PyQt5.QtWidgets import (
|
||||
QWidget, QVBoxLayout, QHBoxLayout, QLabel,
|
||||
QPushButton, QFrame, QProgressBar, QLineEdit,
|
||||
QScrollArea, QListWidget, QListWidgetItem, QInputDialog,
|
||||
QMessageBox
|
||||
)
|
||||
from PyQt5.QtCore import Qt, pyqtSignal
|
||||
from PyQt5.QtGui import QFont
|
||||
|
||||
from .collapsible_group import CollapsibleGroup
|
||||
|
||||
|
||||
class StatusDisplay(QWidget):
|
||||
"""Displays current SLAM status."""
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self._setup_ui()
|
||||
|
||||
def _setup_ui(self):
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(0, 0, 0, 0)
|
||||
layout.setSpacing(4)
|
||||
|
||||
# State row
|
||||
state_row = QHBoxLayout()
|
||||
state_label = QLabel("State:")
|
||||
state_label.setStyleSheet("color: #aaa;")
|
||||
state_label.setFixedWidth(80)
|
||||
state_row.addWidget(state_label)
|
||||
|
||||
self.state_value = QLabel("Idle")
|
||||
self.state_value.setStyleSheet("color: #4CAF50; font-weight: bold;")
|
||||
state_row.addWidget(self.state_value)
|
||||
state_row.addStretch()
|
||||
layout.addLayout(state_row)
|
||||
|
||||
# Active map row
|
||||
map_row = QHBoxLayout()
|
||||
map_label = QLabel("Active Map:")
|
||||
map_label.setStyleSheet("color: #aaa;")
|
||||
map_label.setFixedWidth(80)
|
||||
map_row.addWidget(map_label)
|
||||
|
||||
self.map_value = QLabel("None")
|
||||
self.map_value.setStyleSheet("color: #ccc;")
|
||||
map_row.addWidget(self.map_value)
|
||||
map_row.addStretch()
|
||||
layout.addLayout(map_row)
|
||||
|
||||
# Pose row
|
||||
pose_row = QHBoxLayout()
|
||||
pose_label = QLabel("Pose:")
|
||||
pose_label.setStyleSheet("color: #aaa;")
|
||||
pose_label.setFixedWidth(80)
|
||||
pose_row.addWidget(pose_label)
|
||||
|
||||
self.pose_value = QLabel("(0.00, 0.00, 0.0°)")
|
||||
self.pose_value.setStyleSheet("color: #ccc; font-family: monospace;")
|
||||
pose_row.addWidget(self.pose_value)
|
||||
pose_row.addStretch()
|
||||
layout.addLayout(pose_row)
|
||||
|
||||
# Confidence bar (for localization)
|
||||
conf_row = QHBoxLayout()
|
||||
conf_label = QLabel("Confidence:")
|
||||
conf_label.setStyleSheet("color: #aaa;")
|
||||
conf_label.setFixedWidth(80)
|
||||
conf_row.addWidget(conf_label)
|
||||
|
||||
self.confidence_bar = QProgressBar()
|
||||
self.confidence_bar.setRange(0, 100)
|
||||
self.confidence_bar.setValue(0)
|
||||
self.confidence_bar.setTextVisible(True)
|
||||
self.confidence_bar.setStyleSheet("""
|
||||
QProgressBar {
|
||||
background-color: #333;
|
||||
border: 1px solid #555;
|
||||
border-radius: 3px;
|
||||
text-align: center;
|
||||
color: white;
|
||||
}
|
||||
QProgressBar::chunk {
|
||||
background-color: #4CAF50;
|
||||
}
|
||||
""")
|
||||
conf_row.addWidget(self.confidence_bar)
|
||||
layout.addLayout(conf_row)
|
||||
|
||||
def update_status(self, data: dict):
|
||||
"""Update status display from robot_status data."""
|
||||
state = data.get('state', 'Unknown')
|
||||
self.state_value.setText(state)
|
||||
|
||||
# Color code state
|
||||
state_colors = {
|
||||
'Idle': '#777',
|
||||
'Mapping': '#4CAF50',
|
||||
'Localizing': '#2196F3',
|
||||
'Lost': '#f44336',
|
||||
}
|
||||
color = state_colors.get(state, '#ccc')
|
||||
self.state_value.setStyleSheet(f"color: {color}; font-weight: bold;")
|
||||
|
||||
# Active map
|
||||
map_id = data.get('active_map_id', '')
|
||||
self.map_value.setText(map_id if map_id else "None")
|
||||
|
||||
# Pose
|
||||
pose = data.get('pose', {})
|
||||
x = pose.get('x', 0.0)
|
||||
y = pose.get('y', 0.0)
|
||||
theta = pose.get('theta', 0.0)
|
||||
import math
|
||||
theta_deg = math.degrees(theta)
|
||||
self.pose_value.setText(f"({x:.2f}, {y:.2f}, {theta_deg:.1f}°)")
|
||||
|
||||
# Confidence
|
||||
confidence = data.get('localization_confidence', 0.0)
|
||||
self.confidence_bar.setValue(int(confidence * 100))
|
||||
|
||||
# Color code confidence bar
|
||||
if confidence > 0.7:
|
||||
self.confidence_bar.setStyleSheet("""
|
||||
QProgressBar { background-color: #333; border: 1px solid #555; border-radius: 3px; text-align: center; color: white; }
|
||||
QProgressBar::chunk { background-color: #4CAF50; }
|
||||
""")
|
||||
elif confidence > 0.3:
|
||||
self.confidence_bar.setStyleSheet("""
|
||||
QProgressBar { background-color: #333; border: 1px solid #555; border-radius: 3px; text-align: center; color: white; }
|
||||
QProgressBar::chunk { background-color: #FFC107; }
|
||||
""")
|
||||
else:
|
||||
self.confidence_bar.setStyleSheet("""
|
||||
QProgressBar { background-color: #333; border: 1px solid #555; border-radius: 3px; text-align: center; color: white; }
|
||||
QProgressBar::chunk { background-color: #f44336; }
|
||||
""")
|
||||
|
||||
|
||||
class MappingProgress(QWidget):
|
||||
"""Displays mapping progress."""
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self._setup_ui()
|
||||
|
||||
def _setup_ui(self):
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(0, 0, 0, 0)
|
||||
layout.setSpacing(4)
|
||||
|
||||
# Distance traveled
|
||||
dist_row = QHBoxLayout()
|
||||
dist_label = QLabel("Distance:")
|
||||
dist_label.setStyleSheet("color: #aaa;")
|
||||
dist_label.setFixedWidth(80)
|
||||
dist_row.addWidget(dist_label)
|
||||
|
||||
self.distance_value = QLabel("0.0 m")
|
||||
self.distance_value.setStyleSheet("color: #ccc;")
|
||||
dist_row.addWidget(self.distance_value)
|
||||
dist_row.addStretch()
|
||||
layout.addLayout(dist_row)
|
||||
|
||||
# Keyframes
|
||||
kf_row = QHBoxLayout()
|
||||
kf_label = QLabel("Keyframes:")
|
||||
kf_label.setStyleSheet("color: #aaa;")
|
||||
kf_label.setFixedWidth(80)
|
||||
kf_row.addWidget(kf_label)
|
||||
|
||||
self.keyframes_value = QLabel("0")
|
||||
self.keyframes_value.setStyleSheet("color: #ccc;")
|
||||
kf_row.addWidget(self.keyframes_value)
|
||||
kf_row.addStretch()
|
||||
layout.addLayout(kf_row)
|
||||
|
||||
# Loop closures
|
||||
lc_row = QHBoxLayout()
|
||||
lc_label = QLabel("Loops:")
|
||||
lc_label.setStyleSheet("color: #aaa;")
|
||||
lc_label.setFixedWidth(80)
|
||||
lc_row.addWidget(lc_label)
|
||||
|
||||
self.loops_value = QLabel("0")
|
||||
self.loops_value.setStyleSheet("color: #ccc;")
|
||||
lc_row.addWidget(self.loops_value)
|
||||
lc_row.addStretch()
|
||||
layout.addLayout(lc_row)
|
||||
|
||||
# Map area
|
||||
area_row = QHBoxLayout()
|
||||
area_label = QLabel("Map Area:")
|
||||
area_label.setStyleSheet("color: #aaa;")
|
||||
area_label.setFixedWidth(80)
|
||||
area_row.addWidget(area_label)
|
||||
|
||||
self.area_value = QLabel("0.0 m²")
|
||||
self.area_value.setStyleSheet("color: #ccc;")
|
||||
area_row.addWidget(self.area_value)
|
||||
area_row.addStretch()
|
||||
layout.addLayout(area_row)
|
||||
|
||||
def update_progress(self, data: dict):
|
||||
"""Update progress display from robot_status data."""
|
||||
distance = data.get('distance_traveled_m', 0.0)
|
||||
self.distance_value.setText(f"{distance:.1f} m")
|
||||
|
||||
keyframes = data.get('keyframe_count', 0)
|
||||
self.keyframes_value.setText(str(keyframes))
|
||||
|
||||
loops = data.get('loop_closures', 0)
|
||||
self.loops_value.setText(str(loops))
|
||||
|
||||
area = data.get('map_area_m2', 0.0)
|
||||
self.area_value.setText(f"{area:.1f} m²")
|
||||
|
||||
|
||||
class MappingControls(QWidget):
|
||||
"""Mapping start/stop/clear controls."""
|
||||
|
||||
start_mapping = pyqtSignal(str) # map_name
|
||||
stop_mapping = pyqtSignal(bool) # save
|
||||
clear_map = pyqtSignal()
|
||||
emergency_stop = pyqtSignal()
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self._is_mapping = False
|
||||
self._setup_ui()
|
||||
|
||||
def _setup_ui(self):
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(0, 0, 0, 0)
|
||||
layout.setSpacing(6)
|
||||
|
||||
btn_style = "background-color: #555; color: #e0e0e0; padding: 6px 12px; border-radius: 3px;"
|
||||
btn_start = "background-color: #4CAF50; color: white; padding: 6px 12px; border-radius: 3px; font-weight: bold;"
|
||||
btn_stop = "background-color: #FF9800; color: white; padding: 6px 12px; border-radius: 3px; font-weight: bold;"
|
||||
btn_danger = "background-color: #f44336; color: white; padding: 6px 12px; border-radius: 3px; font-weight: bold;"
|
||||
|
||||
# Map name input
|
||||
name_row = QHBoxLayout()
|
||||
name_label = QLabel("Name:")
|
||||
name_label.setStyleSheet("color: #aaa;")
|
||||
name_label.setFixedWidth(50)
|
||||
name_row.addWidget(name_label)
|
||||
|
||||
self.name_input = QLineEdit()
|
||||
self.name_input.setPlaceholderText("Optional map name")
|
||||
self.name_input.setStyleSheet(
|
||||
"QLineEdit { background-color: #3a3a3a; color: #e0e0e0; border: 1px solid #555; padding: 4px; }"
|
||||
)
|
||||
name_row.addWidget(self.name_input)
|
||||
layout.addLayout(name_row)
|
||||
|
||||
# Toggle Start/Stop button
|
||||
btn_row = QHBoxLayout()
|
||||
|
||||
self.toggle_btn = QPushButton("Start Mapping")
|
||||
self.toggle_btn.setStyleSheet(btn_start)
|
||||
self.toggle_btn.clicked.connect(self._on_toggle)
|
||||
btn_row.addWidget(self.toggle_btn)
|
||||
|
||||
layout.addLayout(btn_row)
|
||||
|
||||
# Clear Map and Emergency Stop buttons
|
||||
action_row = QHBoxLayout()
|
||||
|
||||
self.clear_btn = QPushButton("Clear Map")
|
||||
self.clear_btn.setStyleSheet(btn_style)
|
||||
self.clear_btn.clicked.connect(self._on_clear)
|
||||
self.clear_btn.setVisible(False) # Hidden until mapping is complete/stopped
|
||||
action_row.addWidget(self.clear_btn)
|
||||
|
||||
self.emergency_btn = QPushButton("EMERGENCY STOP")
|
||||
self.emergency_btn.setStyleSheet(btn_danger)
|
||||
self.emergency_btn.clicked.connect(self._on_emergency_stop)
|
||||
action_row.addWidget(self.emergency_btn)
|
||||
|
||||
layout.addLayout(action_row)
|
||||
|
||||
# Store styles for toggling
|
||||
self._btn_start_style = btn_start
|
||||
self._btn_stop_style = btn_stop
|
||||
|
||||
def _on_toggle(self):
|
||||
if self._is_mapping:
|
||||
# Currently mapping - stop and save
|
||||
self.stop_mapping.emit(True)
|
||||
else:
|
||||
# Not mapping - start
|
||||
name = self.name_input.text().strip()
|
||||
self.start_mapping.emit(name)
|
||||
|
||||
def _on_clear(self):
|
||||
reply = QMessageBox.question(
|
||||
self, "Clear Map",
|
||||
"Are you sure you want to clear the current map?",
|
||||
QMessageBox.Yes | QMessageBox.No, QMessageBox.No
|
||||
)
|
||||
if reply == QMessageBox.Yes:
|
||||
self.clear_map.emit()
|
||||
|
||||
def _on_emergency_stop(self):
|
||||
# No confirmation - emergency stop should be immediate
|
||||
self.emergency_stop.emit()
|
||||
|
||||
def set_mapping_state(self, is_mapping: bool):
|
||||
"""Update button states based on mapping state."""
|
||||
self._is_mapping = is_mapping
|
||||
|
||||
if is_mapping:
|
||||
self.toggle_btn.setText("Stop Mapping")
|
||||
self.toggle_btn.setStyleSheet(self._btn_stop_style)
|
||||
self.clear_btn.setVisible(False)
|
||||
else:
|
||||
self.toggle_btn.setText("Start Mapping")
|
||||
self.toggle_btn.setStyleSheet(self._btn_start_style)
|
||||
# Show clear button when not mapping (to clear a completed/stopped map)
|
||||
self.clear_btn.setVisible(True)
|
||||
|
||||
self.name_input.setEnabled(not is_mapping)
|
||||
|
||||
|
||||
class MapListWidget(QWidget):
|
||||
"""Widget to display and manage saved maps."""
|
||||
|
||||
enable_map = pyqtSignal(str) # map_id
|
||||
rename_map = pyqtSignal(str, str) # map_id, new_name
|
||||
delete_map = pyqtSignal(str) # map_id
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self._maps = []
|
||||
self._active_map_id = ""
|
||||
self._setup_ui()
|
||||
|
||||
def _setup_ui(self):
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(0, 0, 0, 0)
|
||||
layout.setSpacing(4)
|
||||
|
||||
# Map list
|
||||
self.list_widget = QListWidget()
|
||||
self.list_widget.setStyleSheet("""
|
||||
QListWidget {
|
||||
background-color: #2a2a2a;
|
||||
border: 1px solid #444;
|
||||
border-radius: 3px;
|
||||
}
|
||||
QListWidget::item {
|
||||
color: #ccc;
|
||||
padding: 6px;
|
||||
border-bottom: 1px solid #333;
|
||||
}
|
||||
QListWidget::item:selected {
|
||||
background-color: #3a3a3a;
|
||||
color: white;
|
||||
}
|
||||
QListWidget::item:hover {
|
||||
background-color: #333;
|
||||
}
|
||||
""")
|
||||
self.list_widget.setMinimumHeight(120)
|
||||
layout.addWidget(self.list_widget)
|
||||
|
||||
# Action buttons
|
||||
btn_row = QHBoxLayout()
|
||||
btn_style = "background-color: #555; color: #e0e0e0; padding: 4px 8px; border-radius: 3px; font-size: 11px;"
|
||||
|
||||
self.enable_btn = QPushButton("Enable")
|
||||
self.enable_btn.setStyleSheet(btn_style)
|
||||
self.enable_btn.clicked.connect(self._on_enable)
|
||||
btn_row.addWidget(self.enable_btn)
|
||||
|
||||
self.rename_btn = QPushButton("Rename")
|
||||
self.rename_btn.setStyleSheet(btn_style)
|
||||
self.rename_btn.clicked.connect(self._on_rename)
|
||||
btn_row.addWidget(self.rename_btn)
|
||||
|
||||
self.delete_btn = QPushButton("Delete")
|
||||
self.delete_btn.setStyleSheet("background-color: #8B4513; color: #e0e0e0; padding: 4px 8px; border-radius: 3px; font-size: 11px;")
|
||||
self.delete_btn.clicked.connect(self._on_delete)
|
||||
btn_row.addWidget(self.delete_btn)
|
||||
|
||||
layout.addLayout(btn_row)
|
||||
|
||||
def update_maps(self, data: dict):
|
||||
"""Update map list from map_list data."""
|
||||
self._maps = data.get('maps', [])
|
||||
self._active_map_id = data.get('active_map_id', '')
|
||||
|
||||
self.list_widget.clear()
|
||||
for m in self._maps:
|
||||
map_id = m.get('map_id', '')
|
||||
name = m.get('name', map_id)
|
||||
area = m.get('area_m2', 0.0)
|
||||
is_complete = m.get('is_complete', False)
|
||||
|
||||
# Format display text
|
||||
status = "✓" if is_complete else "…"
|
||||
active = " [ACTIVE]" if map_id == self._active_map_id else ""
|
||||
text = f"{status} {name} ({area:.1f} m²){active}"
|
||||
|
||||
item = QListWidgetItem(text)
|
||||
item.setData(Qt.UserRole, map_id)
|
||||
if map_id == self._active_map_id:
|
||||
item.setForeground(Qt.green)
|
||||
self.list_widget.addItem(item)
|
||||
|
||||
def _get_selected_map_id(self) -> str:
|
||||
"""Get currently selected map ID."""
|
||||
item = self.list_widget.currentItem()
|
||||
if item:
|
||||
return item.data(Qt.UserRole)
|
||||
return None
|
||||
|
||||
def _on_enable(self):
|
||||
map_id = self._get_selected_map_id()
|
||||
if map_id:
|
||||
self.enable_map.emit(map_id)
|
||||
|
||||
def _on_rename(self):
|
||||
map_id = self._get_selected_map_id()
|
||||
if not map_id:
|
||||
return
|
||||
|
||||
# Find current name
|
||||
current_name = map_id
|
||||
for m in self._maps:
|
||||
if m.get('map_id') == map_id:
|
||||
current_name = m.get('name', map_id)
|
||||
break
|
||||
|
||||
new_name, ok = QInputDialog.getText(
|
||||
self, "Rename Map",
|
||||
"Enter new name:",
|
||||
text=current_name
|
||||
)
|
||||
if ok and new_name.strip():
|
||||
self.rename_map.emit(map_id, new_name.strip())
|
||||
|
||||
def _on_delete(self):
|
||||
map_id = self._get_selected_map_id()
|
||||
if not map_id:
|
||||
return
|
||||
|
||||
reply = QMessageBox.question(
|
||||
self, "Delete Map",
|
||||
f"Are you sure you want to delete map '{map_id}'?",
|
||||
QMessageBox.Yes | QMessageBox.No, QMessageBox.No
|
||||
)
|
||||
if reply == QMessageBox.Yes:
|
||||
self.delete_map.emit(map_id)
|
||||
|
||||
|
||||
class SlamControlPanel(QWidget):
|
||||
"""SLAM control panel with status, mapping controls, and map management."""
|
||||
|
||||
# Signals for SLAM commands
|
||||
start_mapping_requested = pyqtSignal(str) # map_name
|
||||
stop_mapping_requested = pyqtSignal(bool) # save
|
||||
clear_map_requested = pyqtSignal()
|
||||
emergency_stop_requested = pyqtSignal()
|
||||
enable_map_requested = pyqtSignal(str) # map_id
|
||||
rename_map_requested = pyqtSignal(str, str) # map_id, new_name
|
||||
delete_map_requested = pyqtSignal(str) # map_id
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
self.setFixedWidth(280)
|
||||
self._setup_ui()
|
||||
|
||||
def _setup_ui(self):
|
||||
main_layout = QVBoxLayout(self)
|
||||
main_layout.setContentsMargins(0, 0, 0, 0)
|
||||
main_layout.setSpacing(0)
|
||||
|
||||
# Scroll area
|
||||
scroll = QScrollArea()
|
||||
scroll.setWidgetResizable(True)
|
||||
scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
|
||||
scroll.setVerticalScrollBarPolicy(Qt.ScrollBarAsNeeded)
|
||||
scroll.setStyleSheet("""
|
||||
QScrollArea { border: none; background-color: transparent; }
|
||||
QScrollBar:vertical { background-color: #3a3a3a; width: 8px; }
|
||||
QScrollBar::handle:vertical { background-color: #555; border-radius: 4px; min-height: 20px; }
|
||||
QScrollBar::add-line:vertical, QScrollBar::sub-line:vertical { height: 0; }
|
||||
""")
|
||||
|
||||
content = QWidget()
|
||||
content.setFixedWidth(272)
|
||||
layout = QVBoxLayout(content)
|
||||
layout.setContentsMargins(5, 5, 5, 5)
|
||||
layout.setSpacing(6)
|
||||
|
||||
# === Status Group ===
|
||||
self.status_group = CollapsibleGroup("SLAM Status")
|
||||
self.status_display = StatusDisplay()
|
||||
self.status_group.add_widget(self.status_display)
|
||||
layout.addWidget(self.status_group)
|
||||
|
||||
# === Mapping Progress Group ===
|
||||
self.progress_group = CollapsibleGroup("Mapping Progress")
|
||||
self.mapping_progress = MappingProgress()
|
||||
self.progress_group.add_widget(self.mapping_progress)
|
||||
layout.addWidget(self.progress_group)
|
||||
|
||||
# === Mapping Controls Group ===
|
||||
self.controls_group = CollapsibleGroup("Mapping Controls")
|
||||
self.mapping_controls = MappingControls()
|
||||
self.mapping_controls.start_mapping.connect(self.start_mapping_requested)
|
||||
self.mapping_controls.stop_mapping.connect(self.stop_mapping_requested)
|
||||
self.mapping_controls.clear_map.connect(self.clear_map_requested)
|
||||
self.mapping_controls.emergency_stop.connect(self.emergency_stop_requested)
|
||||
self.controls_group.add_widget(self.mapping_controls)
|
||||
layout.addWidget(self.controls_group)
|
||||
|
||||
# === Saved Maps Group ===
|
||||
self.maps_group = CollapsibleGroup("Saved Maps")
|
||||
self.map_list = MapListWidget()
|
||||
self.map_list.enable_map.connect(self.enable_map_requested)
|
||||
self.map_list.rename_map.connect(self.rename_map_requested)
|
||||
self.map_list.delete_map.connect(self.delete_map_requested)
|
||||
self.maps_group.add_widget(self.map_list)
|
||||
layout.addWidget(self.maps_group)
|
||||
|
||||
# === Connection Status ===
|
||||
self.connection_frame = QFrame()
|
||||
self.connection_frame.setStyleSheet("""
|
||||
QFrame {
|
||||
background-color: #252525;
|
||||
border-top: 1px solid #444;
|
||||
padding: 5px;
|
||||
}
|
||||
""")
|
||||
conn_layout = QHBoxLayout(self.connection_frame)
|
||||
conn_layout.setContentsMargins(5, 5, 5, 5)
|
||||
|
||||
self.connection_dot = QLabel()
|
||||
self.connection_dot.setFixedSize(10, 10)
|
||||
self.connection_dot.setStyleSheet("background-color: #666; border-radius: 5px;")
|
||||
conn_layout.addWidget(self.connection_dot)
|
||||
|
||||
self.connection_label = QLabel("Disconnected")
|
||||
self.connection_label.setStyleSheet("color: #777;")
|
||||
conn_layout.addWidget(self.connection_label)
|
||||
conn_layout.addStretch()
|
||||
|
||||
layout.addStretch()
|
||||
scroll.setWidget(content)
|
||||
main_layout.addWidget(scroll)
|
||||
main_layout.addWidget(self.connection_frame)
|
||||
|
||||
def update_robot_status(self, data: dict):
|
||||
"""Update from robot_status data."""
|
||||
self.status_display.update_status(data)
|
||||
self.mapping_progress.update_progress(data)
|
||||
|
||||
# Update mapping controls state
|
||||
state = data.get('state', 'Idle')
|
||||
is_mapping = state == 'Mapping'
|
||||
self.mapping_controls.set_mapping_state(is_mapping)
|
||||
|
||||
def update_map_list(self, data: dict):
|
||||
"""Update from map_list data."""
|
||||
self.map_list.update_maps(data)
|
||||
|
||||
def set_connection_status(self, connected: bool, message: str = ""):
|
||||
"""Update connection status display."""
|
||||
if connected:
|
||||
self.connection_dot.setStyleSheet("background-color: #4CAF50; border-radius: 5px;")
|
||||
self.connection_label.setText("Connected")
|
||||
self.connection_label.setStyleSheet("color: #4CAF50;")
|
||||
else:
|
||||
self.connection_dot.setStyleSheet("background-color: #f44336; border-radius: 5px;")
|
||||
self.connection_label.setText(message or "Disconnected")
|
||||
self.connection_label.setStyleSheet("color: #f44336;")
|
||||
+209
-484
@@ -1,30 +1,28 @@
|
||||
"""
|
||||
SLAM visualization widget.
|
||||
|
||||
Full occupancy grid map display with:
|
||||
Map-only display with:
|
||||
- Occupancy grid map background
|
||||
- Trajectory trail
|
||||
- Lidar scan overlay
|
||||
- Robot marker with heading
|
||||
- Click-to-move control
|
||||
- SLAM status panel
|
||||
- Auto-follow mode
|
||||
- Navigation path and goal visualization
|
||||
- Ctrl+Click to set navigation goal
|
||||
"""
|
||||
|
||||
import math
|
||||
import base64
|
||||
import logging
|
||||
import numpy as np
|
||||
from enum import Enum
|
||||
from collections import deque
|
||||
from PyQt5.QtWidgets import (
|
||||
QWidget, QVBoxLayout, QHBoxLayout, QLabel, QFrame,
|
||||
QPushButton, QComboBox, QProgressBar, QFileDialog, QMessageBox,
|
||||
QScrollArea, QSplitter
|
||||
QWidget, QVBoxLayout, QHBoxLayout, QLabel, QFrame, QPushButton
|
||||
)
|
||||
from .diagnostics_panel import DiagnosticsPanel
|
||||
from PyQt5.QtCore import Qt, QTimer, pyqtSignal
|
||||
from PyQt5.QtGui import QFont, QImage
|
||||
from PyQt5.QtCore import Qt, pyqtSignal
|
||||
from PyQt5.QtGui import QFont
|
||||
import pyqtgraph as pg
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# Color palette
|
||||
COLORS = {
|
||||
@@ -32,38 +30,36 @@ COLORS = {
|
||||
'robot': (255, 200, 0), # Yellow/gold
|
||||
'heading_arrow': (255, 100, 0), # Orange
|
||||
'lidar_points': (0, 255, 100), # Green
|
||||
'target_marker': (255, 50, 50), # Red
|
||||
'grid': (60, 60, 60), # Dark gray
|
||||
'origin': (100, 100, 100), # Gray
|
||||
'background': (30, 30, 30), # Dark background
|
||||
'text': (200, 200, 200), # Light gray text
|
||||
'map_free': (255, 255, 255), # White = free
|
||||
'map_occupied': (0, 0, 0), # Black = occupied
|
||||
'map_unknown': (128, 128, 128), # Gray = unknown
|
||||
'nav_path': (0, 200, 255), # Cyan/blue for nav path
|
||||
'nav_goal': (255, 0, 128), # Magenta for goal
|
||||
'nav_waypoint': (100, 150, 255), # Light blue for waypoints
|
||||
}
|
||||
|
||||
|
||||
class GoToState(Enum):
|
||||
"""Go-to-point state machine states."""
|
||||
IDLE = 0
|
||||
ROTATING = 1
|
||||
DRIVING = 2
|
||||
ARRIVED = 3
|
||||
# Pre-computed occupancy grid color lookup table for fast rendering
|
||||
# Dhruva encoding: 0 = free, 100 = occupied, 255 = unknown
|
||||
OCCUPANCY_COLOR_MAP = np.zeros((256, 3), dtype=np.uint8)
|
||||
OCCUPANCY_COLOR_MAP[0] = [200, 200, 200] # Free - light gray
|
||||
OCCUPANCY_COLOR_MAP[100] = [255, 255, 255] # Occupied - white
|
||||
OCCUPANCY_COLOR_MAP[255] = [40, 40, 40] # Unknown - dark gray
|
||||
# Fill intermediate values (shouldn't occur, but handle gracefully)
|
||||
for i in range(1, 100):
|
||||
OCCUPANCY_COLOR_MAP[i] = [200, 200, 200] # Treat as free
|
||||
for i in range(101, 255):
|
||||
OCCUPANCY_COLOR_MAP[i] = [255, 255, 255] # Treat as occupied
|
||||
|
||||
|
||||
class SlamView(QWidget):
|
||||
"""SLAM visualization with occupancy grid, lidar overlay, and click-to-move."""
|
||||
"""SLAM map visualization with occupancy grid, trajectory, and lidar overlay."""
|
||||
|
||||
# Signal emitted when velocity command is requested
|
||||
# Payload: {'linear': float, 'angular': float}
|
||||
velocity_command = pyqtSignal(dict)
|
||||
|
||||
# Signal emitted when SLAM command is requested
|
||||
# Payload: {'command': str, ...}
|
||||
slam_command = pyqtSignal(dict)
|
||||
# Signal emitted when user Ctrl+Clicks to set a goal
|
||||
# Parameters: x (float), y (float), theta (float)
|
||||
goal_selected = pyqtSignal(float, float, float)
|
||||
|
||||
# Maximum trajectory points
|
||||
MAX_TRAJECTORY_POINTS = 1000
|
||||
MAX_TRAJECTORY_POINTS = 2000
|
||||
|
||||
def __init__(self, parent=None):
|
||||
super().__init__(parent)
|
||||
@@ -77,59 +73,27 @@ class SlamView(QWidget):
|
||||
self._pose_y = 0.0
|
||||
self._pose_theta = 0.0
|
||||
|
||||
# Current lidar scan (global frame points)
|
||||
self._scan_points = []
|
||||
|
||||
# Map data
|
||||
self._map_image = None
|
||||
self._map_origin_x = 0.0
|
||||
self._map_origin_y = 0.0
|
||||
self._map_resolution = 0.05
|
||||
self._map_width = 0
|
||||
self._map_height = 0
|
||||
|
||||
# SLAM status
|
||||
self._slam_mode = "Idle"
|
||||
self._match_score = 0.0
|
||||
self._num_keyframes = 0
|
||||
self._num_submaps = 0
|
||||
self._is_lost = False
|
||||
|
||||
# Go-to-point state
|
||||
self._goto_state = GoToState.IDLE
|
||||
self._target_x = 0.0
|
||||
self._target_y = 0.0
|
||||
self._target_angle = 0.0
|
||||
|
||||
# Auto-follow mode
|
||||
self._auto_follow = True
|
||||
|
||||
# Navigation data
|
||||
self._nav_path = []
|
||||
self._nav_goal = None
|
||||
self._current_waypoint_idx = 0
|
||||
|
||||
# Setup UI
|
||||
self._setup_ui()
|
||||
|
||||
# Go-to-point control timer
|
||||
self._goto_timer = QTimer()
|
||||
self._goto_timer.setInterval(50) # 20Hz control loop
|
||||
self._goto_timer.timeout.connect(self._goto_tick)
|
||||
|
||||
def _setup_ui(self):
|
||||
"""Setup the UI components."""
|
||||
main_layout = QHBoxLayout(self)
|
||||
main_layout.setContentsMargins(0, 0, 0, 0)
|
||||
main_layout.setSpacing(0)
|
||||
|
||||
# Left: Map visualization
|
||||
map_widget = self._create_map_widget()
|
||||
main_layout.addWidget(map_widget, 1)
|
||||
|
||||
# Right: Side panel with status and diagnostics
|
||||
right_panel = self._create_right_panel()
|
||||
main_layout.addWidget(right_panel)
|
||||
|
||||
def _create_map_widget(self) -> QWidget:
|
||||
"""Create the map visualization widget."""
|
||||
widget = QWidget()
|
||||
layout = QVBoxLayout(widget)
|
||||
layout = QVBoxLayout(self)
|
||||
layout.setContentsMargins(0, 0, 0, 0)
|
||||
layout.setSpacing(0)
|
||||
|
||||
@@ -141,9 +105,6 @@ class SlamView(QWidget):
|
||||
self._plot_widget.setLabel('bottom', 'X (meters)')
|
||||
self._plot_widget.setLabel('left', 'Y (meters)')
|
||||
|
||||
# Enable mouse click for go-to-point
|
||||
self._plot_widget.scene().sigMouseClicked.connect(self._on_map_clicked)
|
||||
|
||||
# Setup plot items
|
||||
self._setup_plot_items()
|
||||
|
||||
@@ -153,15 +114,13 @@ class SlamView(QWidget):
|
||||
info_bar = self._create_info_bar()
|
||||
layout.addWidget(info_bar)
|
||||
|
||||
return widget
|
||||
|
||||
def _setup_plot_items(self):
|
||||
"""Setup plot items (map, trajectory, robot marker, etc.)."""
|
||||
# Map image item (background layer)
|
||||
self._map_item = pg.ImageItem()
|
||||
self._plot_widget.addItem(self._map_item)
|
||||
|
||||
# Origin marker
|
||||
# Origin marker (crosshair at 0,0)
|
||||
origin_pen = pg.mkPen(color=COLORS['origin'], width=1)
|
||||
self._origin_h = pg.PlotDataItem([-0.5, 0.5], [0, 0], pen=origin_pen)
|
||||
self._origin_v = pg.PlotDataItem([0, 0], [-0.5, 0.5], pen=origin_pen)
|
||||
@@ -176,45 +135,63 @@ class SlamView(QWidget):
|
||||
# Lidar scan points
|
||||
self._lidar_scatter = pg.ScatterPlotItem(
|
||||
[], [],
|
||||
size=4,
|
||||
size=3,
|
||||
pen=pg.mkPen(None),
|
||||
brush=pg.mkBrush(color=COLORS['lidar_points'])
|
||||
)
|
||||
self._plot_widget.addItem(self._lidar_scatter)
|
||||
|
||||
# Target marker (X shape)
|
||||
target_pen = pg.mkPen(color=COLORS['target_marker'], width=3)
|
||||
self._target_marker = pg.ScatterPlotItem(
|
||||
[], [],
|
||||
symbol='x',
|
||||
size=20,
|
||||
pen=target_pen
|
||||
)
|
||||
self._plot_widget.addItem(self._target_marker)
|
||||
|
||||
# Robot marker (triangle)
|
||||
# Robot marker (circle representing robot body)
|
||||
robot_brush = pg.mkBrush(color=COLORS['robot'])
|
||||
robot_pen = pg.mkPen(color=COLORS['robot'], width=2)
|
||||
self._robot_marker = pg.ScatterPlotItem(
|
||||
[0], [0],
|
||||
symbol='t',
|
||||
size=20,
|
||||
symbol='o', # Circle for robot body
|
||||
size=18,
|
||||
brush=robot_brush,
|
||||
pen=robot_pen
|
||||
)
|
||||
self._plot_widget.addItem(self._robot_marker)
|
||||
|
||||
# Heading arrow
|
||||
heading_pen = pg.mkPen(color=COLORS['heading_arrow'], width=3)
|
||||
# Heading line (shows direction robot is facing)
|
||||
# ROS REP-103: X=forward, Y=left, theta=CCW from X axis
|
||||
heading_pen = pg.mkPen(color=COLORS['heading_arrow'], width=4)
|
||||
self._heading_arrow = pg.PlotDataItem([0, 0.3], [0, 0], pen=heading_pen)
|
||||
self._plot_widget.addItem(self._heading_arrow)
|
||||
|
||||
# Navigation path line (dashed)
|
||||
nav_path_pen = pg.mkPen(color=COLORS['nav_path'], width=3, style=Qt.DashLine)
|
||||
self._nav_path_line = pg.PlotDataItem([], [], pen=nav_path_pen)
|
||||
self._plot_widget.addItem(self._nav_path_line)
|
||||
|
||||
# Navigation waypoints
|
||||
self._nav_waypoint_scatter = pg.ScatterPlotItem(
|
||||
[], [],
|
||||
size=8,
|
||||
pen=pg.mkPen(color=COLORS['nav_waypoint'], width=1),
|
||||
brush=pg.mkBrush(color=COLORS['nav_waypoint'])
|
||||
)
|
||||
self._plot_widget.addItem(self._nav_waypoint_scatter)
|
||||
|
||||
# Navigation goal marker (larger cross/plus)
|
||||
self._nav_goal_marker = pg.ScatterPlotItem(
|
||||
[], [],
|
||||
symbol='+',
|
||||
size=24,
|
||||
pen=pg.mkPen(color=COLORS['nav_goal'], width=3),
|
||||
brush=pg.mkBrush(None)
|
||||
)
|
||||
self._plot_widget.addItem(self._nav_goal_marker)
|
||||
|
||||
# Set initial view
|
||||
self._plot_widget.setXRange(-5, 5, padding=0.1)
|
||||
self._plot_widget.setYRange(-5, 5, padding=0.1)
|
||||
|
||||
# Connect mouse click handler
|
||||
self._plot_widget.scene().sigMouseClicked.connect(self._on_mouse_clicked)
|
||||
|
||||
def _create_info_bar(self) -> QFrame:
|
||||
"""Create the bottom info bar."""
|
||||
"""Create the bottom info bar with pose display and follow toggle."""
|
||||
frame = QFrame()
|
||||
frame.setStyleSheet("""
|
||||
QFrame {
|
||||
@@ -232,20 +209,20 @@ class SlamView(QWidget):
|
||||
layout.setContentsMargins(10, 5, 10, 5)
|
||||
|
||||
# Pose display
|
||||
self._pose_label = QLabel("X: 0.000 Y: 0.000 \u03b8: 0.0\u00b0")
|
||||
self._pose_label = QLabel("X: 0.000 Y: 0.000 θ: 0.0°")
|
||||
self._pose_label.setFont(QFont("Courier", 11))
|
||||
layout.addWidget(self._pose_label)
|
||||
|
||||
layout.addStretch()
|
||||
|
||||
# Go-to status
|
||||
self._goto_label = QLabel("")
|
||||
self._goto_label.setFont(QFont("Courier", 10))
|
||||
layout.addWidget(self._goto_label)
|
||||
# Map info
|
||||
self._map_info_label = QLabel("No map")
|
||||
self._map_info_label.setStyleSheet("color: #777;")
|
||||
layout.addWidget(self._map_info_label)
|
||||
|
||||
layout.addStretch()
|
||||
|
||||
# Control buttons
|
||||
# Follow toggle button
|
||||
self._follow_btn = QPushButton("Follow: ON")
|
||||
self._follow_btn.setCheckable(True)
|
||||
self._follow_btn.setChecked(True)
|
||||
@@ -264,186 +241,55 @@ class SlamView(QWidget):
|
||||
self._follow_btn.clicked.connect(self._toggle_follow)
|
||||
layout.addWidget(self._follow_btn)
|
||||
|
||||
return frame
|
||||
|
||||
def _create_status_panel(self) -> QFrame:
|
||||
"""Create the right-side status panel."""
|
||||
frame = QFrame()
|
||||
frame.setFixedWidth(200)
|
||||
frame.setStyleSheet("""
|
||||
QFrame {
|
||||
background-color: #202020;
|
||||
border-left: 1px solid #444;
|
||||
}
|
||||
QLabel {
|
||||
color: #ccc;
|
||||
font-size: 11px;
|
||||
}
|
||||
QComboBox, QPushButton {
|
||||
background-color: #333;
|
||||
# Clear trajectory button
|
||||
self._clear_btn = QPushButton("Clear Trail")
|
||||
self._clear_btn.setFixedWidth(80)
|
||||
self._clear_btn.setStyleSheet("""
|
||||
QPushButton {
|
||||
background-color: #444;
|
||||
color: white;
|
||||
border: 1px solid #555;
|
||||
border-radius: 3px;
|
||||
padding: 5px;
|
||||
}
|
||||
QPushButton:hover {
|
||||
background-color: #444;
|
||||
}
|
||||
QProgressBar {
|
||||
background-color: #333;
|
||||
border: 1px solid #555;
|
||||
border-radius: 3px;
|
||||
text-align: center;
|
||||
}
|
||||
QProgressBar::chunk {
|
||||
background-color: #4CAF50;
|
||||
padding: 3px;
|
||||
}
|
||||
QPushButton:hover { background-color: #555; }
|
||||
""")
|
||||
|
||||
layout = QVBoxLayout(frame)
|
||||
layout.setContentsMargins(10, 10, 10, 10)
|
||||
layout.setSpacing(10)
|
||||
|
||||
# Title
|
||||
title = QLabel("SLAM Status")
|
||||
title.setFont(QFont("Arial", 12, QFont.Bold))
|
||||
title.setStyleSheet("color: white;")
|
||||
layout.addWidget(title)
|
||||
|
||||
# Mode selector
|
||||
mode_layout = QHBoxLayout()
|
||||
mode_layout.addWidget(QLabel("Mode:"))
|
||||
self._mode_combo = QComboBox()
|
||||
self._mode_combo.addItems(["Mapping", "Localization", "Idle"])
|
||||
self._mode_combo.currentTextChanged.connect(self._on_mode_changed)
|
||||
mode_layout.addWidget(self._mode_combo)
|
||||
layout.addLayout(mode_layout)
|
||||
|
||||
# Match quality bar
|
||||
layout.addWidget(QLabel("Match Quality:"))
|
||||
self._match_bar = QProgressBar()
|
||||
self._match_bar.setRange(0, 100)
|
||||
self._match_bar.setValue(0)
|
||||
layout.addWidget(self._match_bar)
|
||||
|
||||
# Stats
|
||||
self._keyframes_label = QLabel("Keyframes: 0")
|
||||
layout.addWidget(self._keyframes_label)
|
||||
|
||||
self._submaps_label = QLabel("Submaps: 0")
|
||||
layout.addWidget(self._submaps_label)
|
||||
|
||||
# Lost warning
|
||||
self._lost_label = QLabel("\u26a0 LOST")
|
||||
self._lost_label.setStyleSheet("color: #ff4444; font-weight: bold;")
|
||||
self._lost_label.hide()
|
||||
layout.addWidget(self._lost_label)
|
||||
|
||||
layout.addSpacing(10)
|
||||
|
||||
# Position display
|
||||
pos_title = QLabel("Position")
|
||||
pos_title.setFont(QFont("Arial", 11, QFont.Bold))
|
||||
layout.addWidget(pos_title)
|
||||
|
||||
self._pos_x_label = QLabel("X: 0.000 m")
|
||||
layout.addWidget(self._pos_x_label)
|
||||
|
||||
self._pos_y_label = QLabel("Y: 0.000 m")
|
||||
layout.addWidget(self._pos_y_label)
|
||||
|
||||
self._pos_theta_label = QLabel("\u03b8: 0.0\u00b0")
|
||||
layout.addWidget(self._pos_theta_label)
|
||||
|
||||
layout.addSpacing(10)
|
||||
|
||||
# Control buttons
|
||||
btn_layout1 = QHBoxLayout()
|
||||
self._clear_btn = QPushButton("Clear Map")
|
||||
self._clear_btn.clicked.connect(self._on_clear_map)
|
||||
btn_layout1.addWidget(self._clear_btn)
|
||||
|
||||
self._save_btn = QPushButton("Save Map")
|
||||
self._save_btn.clicked.connect(self._on_save_map)
|
||||
btn_layout1.addWidget(self._save_btn)
|
||||
layout.addLayout(btn_layout1)
|
||||
|
||||
btn_layout2 = QHBoxLayout()
|
||||
self._reset_btn = QPushButton("Reset Pose")
|
||||
self._reset_btn.clicked.connect(self._on_reset_pose)
|
||||
btn_layout2.addWidget(self._reset_btn)
|
||||
|
||||
self._stop_btn = QPushButton("STOP")
|
||||
self._stop_btn.setStyleSheet("QPushButton { background-color: #a33; }")
|
||||
self._stop_btn.clicked.connect(self._on_stop)
|
||||
btn_layout2.addWidget(self._stop_btn)
|
||||
layout.addLayout(btn_layout2)
|
||||
|
||||
layout.addStretch()
|
||||
self._clear_btn.clicked.connect(self.clear_trajectory)
|
||||
layout.addWidget(self._clear_btn)
|
||||
|
||||
return frame
|
||||
|
||||
def _create_right_panel(self) -> QWidget:
|
||||
"""Create the right panel containing status and diagnostics."""
|
||||
# Container widget
|
||||
container = QWidget()
|
||||
container.setFixedWidth(300)
|
||||
container.setStyleSheet("background-color: #1a1a1a;")
|
||||
|
||||
layout = QVBoxLayout(container)
|
||||
layout.setContentsMargins(0, 0, 0, 0)
|
||||
layout.setSpacing(0)
|
||||
|
||||
# Scroll area for both panels
|
||||
scroll = QScrollArea()
|
||||
scroll.setWidgetResizable(True)
|
||||
scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
|
||||
scroll.setStyleSheet("""
|
||||
QScrollArea {
|
||||
border: none;
|
||||
background-color: #1a1a1a;
|
||||
}
|
||||
QScrollBar:vertical {
|
||||
background-color: #2a2a2a;
|
||||
width: 8px;
|
||||
}
|
||||
QScrollBar::handle:vertical {
|
||||
background-color: #555;
|
||||
border-radius: 4px;
|
||||
min-height: 20px;
|
||||
}
|
||||
QScrollBar::add-line:vertical, QScrollBar::sub-line:vertical {
|
||||
height: 0px;
|
||||
}
|
||||
""")
|
||||
|
||||
# Inner widget for scroll content
|
||||
scroll_content = QWidget()
|
||||
scroll_layout = QVBoxLayout(scroll_content)
|
||||
scroll_layout.setContentsMargins(0, 0, 0, 0)
|
||||
scroll_layout.setSpacing(0)
|
||||
|
||||
# Status panel (top)
|
||||
status_panel = self._create_status_panel()
|
||||
scroll_layout.addWidget(status_panel)
|
||||
|
||||
# Diagnostics panel (below status)
|
||||
self._diagnostics_panel = DiagnosticsPanel()
|
||||
scroll_layout.addWidget(self._diagnostics_panel)
|
||||
|
||||
scroll_layout.addStretch()
|
||||
|
||||
scroll.setWidget(scroll_content)
|
||||
layout.addWidget(scroll)
|
||||
|
||||
return container
|
||||
|
||||
def _toggle_follow(self):
|
||||
"""Toggle auto-follow mode."""
|
||||
self._auto_follow = self._follow_btn.isChecked()
|
||||
self._follow_btn.setText("Follow: ON" if self._auto_follow else "Follow: OFF")
|
||||
|
||||
# ==================== Data Update Slots ====================
|
||||
def _on_mouse_clicked(self, event):
|
||||
"""Handle mouse click events. Ctrl+Click sets navigation goal."""
|
||||
# Check if Ctrl key is held
|
||||
modifiers = event.modifiers()
|
||||
if not (modifiers & Qt.ControlModifier):
|
||||
return
|
||||
|
||||
# Get click position in scene coordinates
|
||||
pos = event.scenePos()
|
||||
|
||||
# Convert to view coordinates (map coordinates)
|
||||
view_pos = self._plot_widget.plotItem.vb.mapSceneToView(pos)
|
||||
x = view_pos.x()
|
||||
y = view_pos.y()
|
||||
|
||||
# Compute theta pointing from robot to click point
|
||||
dx = x - self._pose_x
|
||||
dy = y - self._pose_y
|
||||
theta = math.atan2(dy, dx)
|
||||
|
||||
logger.info(f"Goal selected: ({x:.2f}, {y:.2f}), theta={math.degrees(theta):.1f}°")
|
||||
|
||||
# Emit signal for navigation
|
||||
self.goal_selected.emit(x, y, theta)
|
||||
|
||||
# ==================== Data Update Methods ====================
|
||||
|
||||
def update_pose(self, x: float, y: float, theta: float):
|
||||
"""Update the robot pose and trajectory."""
|
||||
@@ -462,7 +308,7 @@ class SlamView(QWidget):
|
||||
list(self._trajectory_y)
|
||||
)
|
||||
|
||||
# Update robot marker
|
||||
# Update robot marker position
|
||||
self._robot_marker.setData([x], [y])
|
||||
|
||||
# Update heading arrow
|
||||
@@ -471,53 +317,20 @@ class SlamView(QWidget):
|
||||
arrow_end_y = y + arrow_length * math.sin(theta)
|
||||
self._heading_arrow.setData([x, arrow_end_x], [y, arrow_end_y])
|
||||
|
||||
# Update labels
|
||||
# Update pose label
|
||||
theta_deg = math.degrees(theta)
|
||||
self._pose_label.setText(f"X: {x:7.3f} Y: {y:7.3f} \u03b8: {theta_deg:6.1f}\u00b0")
|
||||
self._pos_x_label.setText(f"X: {x:.3f} m")
|
||||
self._pos_y_label.setText(f"Y: {y:.3f} m")
|
||||
self._pos_theta_label.setText(f"\u03b8: {theta_deg:.1f}\u00b0")
|
||||
self._pose_label.setText(f"X: {x:7.3f} Y: {y:7.3f} θ: {theta_deg:6.1f}°")
|
||||
|
||||
# Auto-follow
|
||||
# Auto-follow robot
|
||||
if self._auto_follow:
|
||||
self._center_on_robot()
|
||||
|
||||
def update_status(self, status: dict):
|
||||
"""Update SLAM status display."""
|
||||
self._slam_mode = status.get('mode', 'Idle')
|
||||
self._match_score = status.get('match_score', 0.0)
|
||||
self._num_keyframes = status.get('num_keyframes', 0)
|
||||
self._num_submaps = status.get('num_submaps', 0)
|
||||
self._is_lost = status.get('is_lost', False)
|
||||
|
||||
# Update UI
|
||||
self._match_bar.setValue(int(self._match_score * 100))
|
||||
|
||||
# Color code match bar
|
||||
if self._match_score > 0.7:
|
||||
self._match_bar.setStyleSheet("QProgressBar::chunk { background-color: #4CAF50; }")
|
||||
elif self._match_score > 0.3:
|
||||
self._match_bar.setStyleSheet("QProgressBar::chunk { background-color: #FFC107; }")
|
||||
else:
|
||||
self._match_bar.setStyleSheet("QProgressBar::chunk { background-color: #f44336; }")
|
||||
|
||||
self._keyframes_label.setText(f"Keyframes: {self._num_keyframes}")
|
||||
self._submaps_label.setText(f"Submaps: {self._num_submaps}")
|
||||
|
||||
if self._is_lost:
|
||||
self._lost_label.show()
|
||||
else:
|
||||
self._lost_label.hide()
|
||||
|
||||
# Update mode combo (without triggering signal)
|
||||
self._mode_combo.blockSignals(True)
|
||||
idx = self._mode_combo.findText(self._slam_mode)
|
||||
if idx >= 0:
|
||||
self._mode_combo.setCurrentIndex(idx)
|
||||
self._mode_combo.blockSignals(False)
|
||||
|
||||
def update_map(self, map_data: dict):
|
||||
"""Update occupancy grid map display."""
|
||||
"""Update occupancy grid map display.
|
||||
|
||||
Args:
|
||||
map_data: Dict with resolution, width, height, origin_x, origin_y, cells
|
||||
"""
|
||||
resolution = map_data.get('resolution', 0.05)
|
||||
width = map_data.get('width', 0)
|
||||
height = map_data.get('height', 0)
|
||||
@@ -525,44 +338,26 @@ class SlamView(QWidget):
|
||||
origin_y = map_data.get('origin_y', 0.0)
|
||||
cells = map_data.get('cells', b'')
|
||||
|
||||
logger.info(f"SlamView.update_map(): {width}x{height}, resolution={resolution}m, cells={len(cells) if cells else 0} bytes")
|
||||
|
||||
if not cells or width == 0 or height == 0:
|
||||
import logging
|
||||
logging.getLogger(__name__).warning(
|
||||
f"Skipping map update: cells_len={len(cells) if cells else 0}, w={width}, h={height}"
|
||||
)
|
||||
logger.debug(f"Skipping map update: empty data")
|
||||
return
|
||||
|
||||
# Decode cells - handle both raw bytes (from protobuf) and base64 (legacy)
|
||||
import logging
|
||||
logger = logging.getLogger(__name__)
|
||||
# Decode cells (raw bytes from protobuf)
|
||||
try:
|
||||
if isinstance(cells, bytes):
|
||||
# Raw bytes from protobuf
|
||||
cells_array = np.frombuffer(cells, dtype=np.uint8).reshape((height, width))
|
||||
else:
|
||||
# Base64 string (legacy JSON format)
|
||||
cells_bytes = base64.b64decode(cells)
|
||||
cells_array = np.frombuffer(cells_bytes, dtype=np.uint8).reshape((height, width))
|
||||
logger.debug(f"Decoded map cells: {len(cells)} bytes -> {height}x{width} array")
|
||||
logger.warning(f"Unexpected cells type: {type(cells)}")
|
||||
return
|
||||
except Exception as e:
|
||||
logger.error(f"Failed to decode map cells: {e} (cells type={type(cells)}, len={len(cells) if cells else 0}, w={width}, h={height})")
|
||||
logger.error(f"Failed to decode map cells: {e}")
|
||||
return
|
||||
|
||||
# Convert to RGB image
|
||||
# 0 = unknown (gray), 1-127 = free (white), 128-255 = occupied (black)
|
||||
rgb_image = np.zeros((height, width, 3), dtype=np.uint8)
|
||||
|
||||
# Unknown cells (value == 0)
|
||||
unknown_mask = cells_array == 0
|
||||
rgb_image[unknown_mask] = [128, 128, 128]
|
||||
|
||||
# Free cells (1-127) -> white
|
||||
free_mask = (cells_array > 0) & (cells_array < 128)
|
||||
rgb_image[free_mask] = [255, 255, 255]
|
||||
|
||||
# Occupied cells (128-255) -> black
|
||||
occupied_mask = cells_array >= 128
|
||||
rgb_image[occupied_mask] = [0, 0, 0]
|
||||
# Convert to RGB image using pre-computed lookup table (fast)
|
||||
# Dhruva encoding: 0 = free, 100 = occupied, 255 = unknown
|
||||
rgb_image = OCCUPANCY_COLOR_MAP[cells_array] # Shape: (height, width, 3)
|
||||
|
||||
# Store map metadata
|
||||
self._map_resolution = resolution
|
||||
@@ -571,19 +366,11 @@ class SlamView(QWidget):
|
||||
self._map_width = width
|
||||
self._map_height = height
|
||||
|
||||
# pyqtgraph ImageItem expects image data where:
|
||||
# - image[row, col] corresponds to (x=col, y=row) in image coordinates
|
||||
# - By default, row 0 is at the BOTTOM of the display (y increases upward)
|
||||
# - Our cells_array[y, x] has row 0 at top (y=0 is origin_y in world coords)
|
||||
#
|
||||
# To correctly map world coordinates:
|
||||
# - World (origin_x, origin_y) should map to image row=0, col=0
|
||||
# - World Y increases upward, image row increases upward in pyqtgraph
|
||||
# - So no flip needed if we set the rect correctly
|
||||
#
|
||||
# The cells are in row-major order: cells[y][x] where y=0 is at origin_y
|
||||
# pyqtgraph will display image[0,:] at y=origin_y (bottom of rect)
|
||||
self._map_item.setImage(rgb_image)
|
||||
# PyQtGraph expects image in (width, height, channels) format, so transpose
|
||||
rgb_image_transposed = np.transpose(rgb_image, (1, 0, 2)) # (h,w,c) -> (w,h,c)
|
||||
|
||||
# Update map image
|
||||
self._map_item.setImage(rgb_image_transposed)
|
||||
self._map_item.setRect(
|
||||
origin_x,
|
||||
origin_y,
|
||||
@@ -591,156 +378,45 @@ class SlamView(QWidget):
|
||||
height * resolution
|
||||
)
|
||||
|
||||
# Debug: log map stats
|
||||
occupied_count = np.sum(occupied_mask)
|
||||
free_count = np.sum(free_mask)
|
||||
# Update map info label (use efficient count methods)
|
||||
occupied_count = np.count_nonzero(cells_array == 100)
|
||||
free_count = np.count_nonzero(cells_array == 0)
|
||||
area_m2 = (width * height * resolution * resolution)
|
||||
self._map_info_label.setText(f"Map: {width}x{height} ({area_m2:.1f}m²)")
|
||||
|
||||
logger.debug(
|
||||
f"Map updated: {width}x{height} @ {resolution}m, "
|
||||
f"origin=({origin_x:.1f}, {origin_y:.1f}), "
|
||||
f"occupied={occupied_count}, free={free_count}"
|
||||
)
|
||||
|
||||
def update_scan(self, scan_data: dict):
|
||||
"""Update lidar scan overlay."""
|
||||
"""Update lidar scan overlay.
|
||||
|
||||
Args:
|
||||
scan_data: Dict with 'points' as list of [x, y] in robot frame
|
||||
"""
|
||||
points = scan_data.get('points', [])
|
||||
|
||||
if not points:
|
||||
self._lidar_scatter.setData([], [])
|
||||
return
|
||||
|
||||
# Points are already in global frame
|
||||
x_coords = [p[0] for p in points]
|
||||
y_coords = [p[1] for p in points]
|
||||
# Transform points from robot frame to global frame
|
||||
cos_theta = math.cos(self._pose_theta)
|
||||
sin_theta = math.sin(self._pose_theta)
|
||||
|
||||
self._lidar_scatter.setData(x_coords, y_coords)
|
||||
global_x = []
|
||||
global_y = []
|
||||
for p in points:
|
||||
# Rotate and translate
|
||||
rx = p[0] * cos_theta - p[1] * sin_theta + self._pose_x
|
||||
ry = p[0] * sin_theta + p[1] * cos_theta + self._pose_y
|
||||
global_x.append(rx)
|
||||
global_y.append(ry)
|
||||
|
||||
def update_diagnostics(self, data: dict):
|
||||
"""Update SLAM diagnostics panel.
|
||||
self._lidar_scatter.setData(global_x, global_y)
|
||||
|
||||
Args:
|
||||
data: Dictionary with timing, scan_match, mapping, loop_closure keys
|
||||
"""
|
||||
self._diagnostics_panel.update_diagnostics(data)
|
||||
|
||||
# ==================== Click-to-Move ====================
|
||||
|
||||
def _on_map_clicked(self, event):
|
||||
"""Handle map click for go-to-point."""
|
||||
if event.button() != Qt.LeftButton:
|
||||
return
|
||||
|
||||
# Get click position in plot coordinates
|
||||
pos = self._plot_widget.plotItem.vb.mapSceneToView(event.scenePos())
|
||||
target_x = pos.x()
|
||||
target_y = pos.y()
|
||||
|
||||
# Start go-to-point
|
||||
self._start_goto(target_x, target_y)
|
||||
|
||||
def _start_goto(self, target_x: float, target_y: float):
|
||||
"""Start go-to-point navigation."""
|
||||
self._target_x = target_x
|
||||
self._target_y = target_y
|
||||
|
||||
# Compute target angle
|
||||
dx = target_x - self._pose_x
|
||||
dy = target_y - self._pose_y
|
||||
self._target_angle = math.atan2(dy, dx)
|
||||
|
||||
# Show target marker
|
||||
self._target_marker.setData([target_x], [target_y])
|
||||
|
||||
# Start rotating
|
||||
self._goto_state = GoToState.ROTATING
|
||||
self._goto_label.setText(f"Going to ({target_x:.1f}, {target_y:.1f})")
|
||||
self._goto_timer.start()
|
||||
|
||||
def _goto_tick(self):
|
||||
"""Go-to-point control loop tick."""
|
||||
if self._goto_state == GoToState.IDLE or self._goto_state == GoToState.ARRIVED:
|
||||
self._goto_timer.stop()
|
||||
return
|
||||
|
||||
# Angular error (normalize to [-pi, pi])
|
||||
angle_to_target = math.atan2(
|
||||
self._target_y - self._pose_y,
|
||||
self._target_x - self._pose_x
|
||||
)
|
||||
angle_error = angle_to_target - self._pose_theta
|
||||
while angle_error > math.pi:
|
||||
angle_error -= 2 * math.pi
|
||||
while angle_error < -math.pi:
|
||||
angle_error += 2 * math.pi
|
||||
|
||||
# Distance to target
|
||||
distance = math.hypot(
|
||||
self._target_x - self._pose_x,
|
||||
self._target_y - self._pose_y
|
||||
)
|
||||
|
||||
if self._goto_state == GoToState.ROTATING:
|
||||
if abs(angle_error) < 0.1: # ~5 degrees
|
||||
self._goto_state = GoToState.DRIVING
|
||||
self._goto_label.setText(f"Driving to ({self._target_x:.1f}, {self._target_y:.1f})")
|
||||
else:
|
||||
# Rotate toward target
|
||||
angular_vel = 0.3 * (1 if angle_error > 0 else -1)
|
||||
self.velocity_command.emit({'linear': 0.0, 'angular': angular_vel})
|
||||
|
||||
elif self._goto_state == GoToState.DRIVING:
|
||||
if distance < 0.05: # 5cm tolerance
|
||||
self._goto_state = GoToState.ARRIVED
|
||||
self.velocity_command.emit({'linear': 0.0, 'angular': 0.0})
|
||||
self._goto_label.setText("Arrived")
|
||||
self._target_marker.setData([], [])
|
||||
self._goto_timer.stop()
|
||||
else:
|
||||
# Drive toward target, with heading correction
|
||||
linear_vel = min(0.2, distance) # Max 20cm/s
|
||||
angular_vel = 0.5 * angle_error # Proportional steering
|
||||
self.velocity_command.emit({'linear': linear_vel, 'angular': angular_vel})
|
||||
|
||||
def _cancel_goto(self):
|
||||
"""Cancel go-to-point navigation."""
|
||||
self._goto_state = GoToState.IDLE
|
||||
self._goto_timer.stop()
|
||||
self._target_marker.setData([], [])
|
||||
self._goto_label.setText("")
|
||||
self.velocity_command.emit({'linear': 0.0, 'angular': 0.0})
|
||||
|
||||
# ==================== Control Buttons ====================
|
||||
|
||||
def _on_mode_changed(self, mode: str):
|
||||
"""Handle mode selector change."""
|
||||
self.slam_command.emit({'command': 'set_mode', 'mode': mode})
|
||||
|
||||
def _on_clear_map(self):
|
||||
"""Handle Clear Map button."""
|
||||
self.slam_command.emit({'command': 'reset'})
|
||||
self._trajectory_x.clear()
|
||||
self._trajectory_y.clear()
|
||||
self._trajectory_line.setData([], [])
|
||||
self._map_item.clear()
|
||||
|
||||
def _on_save_map(self):
|
||||
"""Handle Save Map button."""
|
||||
path, _ = QFileDialog.getSaveFileName(
|
||||
self, "Save Map", "", "Map files (*.bin);;All files (*)"
|
||||
)
|
||||
if path:
|
||||
self.slam_command.emit({'command': 'save_map', 'path': path})
|
||||
QMessageBox.information(self, "Save Map", f"Map save requested: {path}")
|
||||
|
||||
def _on_reset_pose(self):
|
||||
"""Handle Reset Pose button."""
|
||||
self.slam_command.emit({'command': 'reset_pose', 'x': 0.0, 'y': 0.0, 'theta': 0.0})
|
||||
|
||||
def _on_stop(self):
|
||||
"""Handle STOP button."""
|
||||
self._cancel_goto()
|
||||
self.velocity_command.emit({'linear': 0.0, 'angular': 0.0})
|
||||
|
||||
# ==================== Utility ====================
|
||||
# ==================== Utility Methods ====================
|
||||
|
||||
def _center_on_robot(self):
|
||||
"""Center the view on the robot."""
|
||||
@@ -761,3 +437,52 @@ class SlamView(QWidget):
|
||||
self._trajectory_x.clear()
|
||||
self._trajectory_y.clear()
|
||||
self._trajectory_line.setData([], [])
|
||||
|
||||
def clear_map(self):
|
||||
"""Clear the map display."""
|
||||
self._map_item.clear()
|
||||
self._map_info_label.setText("No map")
|
||||
|
||||
def update_navigation(self, nav_data: dict):
|
||||
"""Update navigation visualization (path, goal, waypoints).
|
||||
|
||||
Args:
|
||||
nav_data: Dict with path, goal, current_waypoint_index, state
|
||||
"""
|
||||
path = nav_data.get('path', [])
|
||||
goal = nav_data.get('goal')
|
||||
current_idx = nav_data.get('current_waypoint_index', 0)
|
||||
state = nav_data.get('state', 'Idle')
|
||||
|
||||
# Store data
|
||||
self._nav_path = path
|
||||
self._nav_goal = goal
|
||||
self._current_waypoint_idx = current_idx
|
||||
|
||||
# Update path line
|
||||
if path:
|
||||
path_x = [p['x'] for p in path]
|
||||
path_y = [p['y'] for p in path]
|
||||
self._nav_path_line.setData(path_x, path_y)
|
||||
|
||||
# Update waypoint markers (show remaining waypoints)
|
||||
remaining_x = [p['x'] for p in path[current_idx:]]
|
||||
remaining_y = [p['y'] for p in path[current_idx:]]
|
||||
self._nav_waypoint_scatter.setData(remaining_x, remaining_y)
|
||||
else:
|
||||
self._nav_path_line.setData([], [])
|
||||
self._nav_waypoint_scatter.setData([], [])
|
||||
|
||||
# Update goal marker
|
||||
if goal and state not in ('Idle', 'Reached', 'Cancelled'):
|
||||
self._nav_goal_marker.setData([goal['x']], [goal['y']])
|
||||
else:
|
||||
self._nav_goal_marker.setData([], [])
|
||||
|
||||
def clear_navigation(self):
|
||||
"""Clear navigation visualization."""
|
||||
self._nav_path = []
|
||||
self._nav_goal = None
|
||||
self._nav_path_line.setData([], [])
|
||||
self._nav_waypoint_scatter.setData([], [])
|
||||
self._nav_goal_marker.setData([], [])
|
||||
|
||||
Reference in New Issue
Block a user