#!/usr/bin/env python3

"""

Python-SLAM Unified Entry Point

This is the main entry point for the complete Python-SLAM system,

integrating GUI, benchmarking, GPU acceleration, ROS2 Nav2, and embedded optimization.

"""

import sys

import os

import argparse

import logging

import signal

from pathlib import Path

from typing import Optional, Dict, Any

import threading

import time

# Add src directory to path

sys.path.insert(0, os.path.join(os.path.dirname(__file__), 'src'))

try:

# GUI Components

from python_slam.gui.main_window import SlamMainWindow

from python_slam.gui.utils import MaterialDesignManager

# Benchmarking

from python_slam.benchmarking.benchmark_runner import BenchmarkRunner, BenchmarkConfig

# GPU Acceleration

from python_slam.gpu_acceleration import GPUManager, get_gpu_accelerator, is_gpu_available

# ROS2 Integration (optional)

try:

from python_slam.ros2_nav2_integration.nav2_bridge import Nav2Bridge

ROS2_AVAILABLE = True

except ImportError:

ROS2_AVAILABLE = False

# Embedded Optimization

from python_slam.embedded_optimization.arm_optimization import ARMOptimizer

# Core SLAM

from python_slam.basic_slam_pipeline import BasicSLAMPipeline

# PyQt6/PySide6 for GUI

try:

from PyQt6.QtWidgets import QApplication

from PyQt6.QtCore import QTimer

GUI_BACKEND = "PyQt6"

except ImportError:

try:

from PySide6.QtWidgets import QApplication

from PySide6.QtCore import QTimer

GUI_BACKEND = "PySide6"

except ImportError:

GUI_BACKEND = None

except ImportError as e:

print(f"Import error: {e}")

print("Please ensure all dependencies are installed.")

sys.exit(1)

# Configure logging

logging.basicConfig(

level=logging.INFO,

format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',

handlers=[

logging.StreamHandler(),

logging.FileHandler('python_slam.log')

]

)

logger = logging.getLogger(__name__)

class PythonSLAMSystem:

"""Main Python-SLAM system integrating all components."""

def __init__(self, config: Optional[Dict[str, Any]] = None):

self.config = config or {}

# System components

self.slam_pipeline = None

self.gui_app = None

self.main_window = None

self.gpu_manager = None

self.arm_optimizer = None

self.nav2_bridge = None

self.benchmark_runner = None

# System state

self.running = False

self.shutdown_event = threading.Event()

# Performance monitoring

self.performance_monitor = None

self.start_time = None

# Register signal handlers

signal.signal(signal.SIGINT, self._signal_handler)

signal.signal(signal.SIGTERM, self._signal_handler)

def initialize(self, mode: str = "full") -> bool:

"""Initialize the Python-SLAM system."""

logger.info("Initializing Python-SLAM system...")

self.start_time = time.time()

try:

# Initialize core SLAM pipeline

if not self._initialize_slam_pipeline():

logger.error("Failed to initialize SLAM pipeline")

return False

# Initialize GPU acceleration

if self.config.get("enable_gpu", True):

self._initialize_gpu_acceleration()

# Initialize ARM optimization

if self.config.get("enable_arm_optimization", True):

self._initialize_arm_optimization()

# Initialize GUI (if requested)

if mode in ["full", "gui"] and GUI_BACKEND:

if not self._initialize_gui():

if mode == "gui":

logger.error("GUI initialization failed in GUI mode")

return False

# Initialize ROS2 Nav2 (if available and requested)

if mode in ["full", "ros2"] and ROS2_AVAILABLE and self.config.get("enable_ros2", False):

self._initialize_ros2_nav2()

# Initialize benchmarking

if mode in ["full", "benchmark"]:

self._initialize_benchmarking()

# Start performance monitoring

self._start_performance_monitoring()

logger.info(f"Python-SLAM system initialized successfully in {mode} mode")

self.running = True

return True

except Exception as e:

logger.error(f"System initialization failed: {e}")

return False

def _initialize_slam_pipeline(self) -> bool:

"""Initialize the core SLAM pipeline."""

try:

slam_config = self.config.get("slam", {})

self.slam_pipeline = BasicSLAMPipeline(slam_config)

logger.info("SLAM pipeline initialized")

return True

except Exception as e:

logger.error(f"SLAM pipeline initialization failed: {e}")

return False

def _initialize_gpu_acceleration(self):

"""Initialize GPU acceleration."""

try:

if is_gpu_available():

self.gpu_manager = GPUManager()

if self.gpu_manager.initialize_accelerators():

available_backends = self.gpu_manager.get_available_backends()

logger.info(f"GPU acceleration initialized: {[b.value for b in available_backends]}")

else:

logger.warning("GPU acceleration initialization failed")

else:

logger.info("No GPU acceleration available")

except Exception as e:

logger.error(f"GPU acceleration initialization failed: {e}")

def _initialize_arm_optimization(self):

"""Initialize ARM processor optimization."""

try:

from python_slam.embedded_optimization.arm_optimization import ARMConfig

arm_config = ARMConfig(**self.config.get("arm", {}))

self.arm_optimizer = ARMOptimizer(arm_config)

logger.info("ARM optimization initialized")

except Exception as e:

logger.error(f"ARM optimization initialization failed: {e}")

def _initialize_gui(self) -> bool:

"""Initialize the GUI application."""

try:

if not GUI_BACKEND:

logger.error("No GUI backend available (PyQt6/PySide6)")

return False

self.gui_app = QApplication(sys.argv)

# Apply Material Design theme

material_manager = MaterialDesignManager()

material_manager.apply_theme(self.gui_app, "dark")

# Create main window

self.main_window = SlamMainWindow(

slam_system=self.slam_pipeline,

gpu_manager=self.gpu_manager

)

logger.info(f"GUI initialized with {GUI_BACKEND}")

return True

except Exception as e:

logger.error(f"GUI initialization failed: {e}")

return False

def _initialize_ros2_nav2(self):

"""Initialize ROS2 Nav2 integration."""

try:

import rclpy

rclpy.init()

self.nav2_bridge = Nav2Bridge(slam_system=self.slam_pipeline)

# Start ROS2 spinning in a separate thread

self.ros2_thread = threading.Thread(

target=self._ros2_spin_thread,

daemon=True

)

self.ros2_thread.start()

logger.info("ROS2 Nav2 integration initialized")

except Exception as e:

logger.error(f"ROS2 Nav2 initialization failed: {e}")

def _initialize_benchmarking(self):

"""Initialize benchmarking system."""

try:

benchmark_config = BenchmarkConfig(**self.config.get("benchmark", {}))

self.benchmark_runner = BenchmarkRunner(benchmark_config)

logger.info("Benchmarking system initialized")

except Exception as e:

logger.error(f"Benchmarking initialization failed: {e}")

def _start_performance_monitoring(self):

"""Start system performance monitoring."""

try:

self.performance_monitor = threading.Thread(

target=self._performance_monitor_thread,

daemon=True

)

self.performance_monitor.start()

logger.info("Performance monitoring started")

except Exception as e:

logger.error(f"Performance monitoring failed to start: {e}")

def _ros2_spin_thread(self):

"""ROS2 spinning thread."""

try:

import rclpy

while self.running and not self.shutdown_event.is_set():

rclpy.spin_once(self.nav2_bridge, timeout_sec=0.1)

except Exception as e:

logger.error(f"ROS2 spin thread error: {e}")

def _performance_monitor_thread(self):

"""Performance monitoring thread."""

try:

import psutil

while self.running and not self.shutdown_event.is_set():

# Monitor system resources

cpu_percent = psutil.cpu_percent(interval=1)

memory = psutil.virtual_memory()

# Log performance metrics

if cpu_percent > 80:

logger.warning(f"High CPU usage: {cpu_percent}%")

if memory.percent > 80:

logger.warning(f"High memory usage: {memory.percent}%")

# Update GUI if available

if self.main_window:

# This would update the metrics dashboard

pass

time.sleep(5) # Monitor every 5 seconds

except Exception as e:

logger.error(f"Performance monitoring error: {e}")

def run(self, mode: str = "full"):

"""Run the Python-SLAM system."""

if not self.running:

logger.error("System not initialized")

return False

try:

logger.info(f"Starting Python-SLAM system in {mode} mode")

if mode == "gui" and self.main_window:

# Run GUI event loop

self.main_window.show()

return self.gui_app.exec()

elif mode == "benchmark" and self.benchmark_runner:

# Run benchmarks

self._run_benchmarks()

return True

elif mode == "headless":

# Run in headless mode

self._run_headless()

return True

elif mode == "full":

# Run full system

if self.main_window:

self.main_window.show()

return self.gui_app.exec()

else:

self._run_headless()

return True

else:

logger.error(f"Unknown run mode: {mode}")

return False

except KeyboardInterrupt:

logger.info("Received keyboard interrupt")

return True

except Exception as e:

logger.error(f"System run error: {e}")

return False

def _run_benchmarks(self):

"""Run comprehensive benchmarks."""

logger.info("Starting comprehensive benchmarks...")

# SLAM benchmarks

if self.benchmark_runner:

results = self.benchmark_runner.run_benchmarks()

logger.info("SLAM benchmarks completed")

# GPU benchmarks

if self.gpu_manager:

gpu_results = self.gpu_manager.run_benchmark()

logger.info("GPU benchmarks completed")

# ARM benchmarks

if self.arm_optimizer:

arm_results = self.arm_optimizer.benchmark_operations()

logger.info("ARM benchmarks completed")

# Save results

self._save_benchmark_results({

"slam": results if 'results' in locals() else {},

"gpu": gpu_results if 'gpu_results' in locals() else {},

"arm": arm_results if 'arm_results' in locals() else {}

})

def _run_headless(self):

"""Run in headless mode without GUI."""

logger.info("Running in headless mode - press Ctrl+C to stop")

try:

while not self.shutdown_event.is_set():

# Process SLAM data if available

if self.slam_pipeline:

# This would process incoming sensor data

pass

time.sleep(0.1) # 10 Hz update rate

except KeyboardInterrupt:

logger.info("Received shutdown signal")

def _save_benchmark_results(self, results: Dict[str, Any]):

"""Save benchmark results to file."""

try:

import json

from datetime import datetime

timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")

filename = f"benchmark_results_{timestamp}.json"

with open(filename, 'w') as f:

json.dump(results, f, indent=2, default=str)

logger.info(f"Benchmark results saved to {filename}")

except Exception as e:

logger.error(f"Failed to save benchmark results: {e}")

def _signal_handler(self, signum, frame):

"""Handle system signals for graceful shutdown."""

logger.info(f"Received signal {signum}")

self.shutdown()

def shutdown(self):

"""Shutdown the Python-SLAM system."""

logger.info("Shutting down Python-SLAM system...")

self.running = False

self.shutdown_event.set()

try:

# Shutdown components

if self.nav2_bridge:

self.nav2_bridge.shutdown()

if self.gpu_manager:

self.gpu_manager.cleanup()

if self.slam_pipeline:

# self.slam_pipeline.shutdown()

pass

# Shutdown ROS2

if ROS2_AVAILABLE and hasattr(self, 'nav2_bridge'):

try:

import rclpy

rclpy.shutdown()

except:

pass

# Calculate uptime

if self.start_time:

uptime = time.time() - self.start_time

logger.info(f"System uptime: {uptime:.2f} seconds")

logger.info("Python-SLAM system shutdown completed")

except Exception as e:

logger.error(f"Shutdown error: {e}")

def get_system_status(self) -> Dict[str, Any]:

"""Get comprehensive system status."""

status = {

"running": self.running,

"uptime": time.time() - self.start_time if self.start_time else 0,

"components": {}

}

# SLAM status

if self.slam_pipeline:

status["components"]["slam"] = {"initialized": True, "active": True}

# GPU status

if self.gpu_manager:

status["components"]["gpu"] = self.gpu_manager.get_accelerator_status()

# ARM status

if self.arm_optimizer:

status["components"]["arm"] = self.arm_optimizer.get_performance_stats()

# ROS2 status

if self.nav2_bridge:

status["components"]["ros2"] = self.nav2_bridge.get_status().__dict__

return status

def create_default_config() -> Dict[str, Any]:

"""Create default configuration for Python-SLAM."""

return {

"enable_gpu": True,

"enable_arm_optimization": True,

"enable_ros2": False,

"slam": {

"feature_detector": "SIFT",

"max_features": 1000,

"enable_loop_closure": True

},

"gpu": {

"preferred_backend": None,

"enable_mixed_precision": True

},

"arm": {

"enable_neon": True,

"optimization_level": "balanced"

},

"benchmark": {

"enable_parallel_execution": True,

"timeout_seconds": 300

},

"gui": {

"theme": "dark",

"enable_real_time_updates": True

}

}

def main():

"""Main entry point for Python-SLAM."""

parser = argparse.ArgumentParser(description="Python-SLAM Unified System")

parser.add_argument("--mode", choices=["full", "gui", "headless", "benchmark", "ros2"],

default="full", help="Run mode")

parser.add_argument("--config", type=str, help="Configuration file path")

parser.add_argument("--log-level", choices=["DEBUG", "INFO", "WARNING", "ERROR"],

default="INFO", help="Logging level")

parser.add_argument("--enable-gpu", action="store_true", default=True,

help="Enable GPU acceleration")

parser.add_argument("--disable-gpu", action="store_false", dest="enable_gpu",

help="Disable GPU acceleration")

parser.add_argument("--enable-ros2", action="store_true", default=False,

help="Enable ROS2 Nav2 integration")

parser.add_argument("--benchmark-only", action="store_true",

help="Run benchmarks only")

args = parser.parse_args()

# Set logging level

logging.getLogger().setLevel(getattr(logging, args.log_level))

# Load configuration

if args.config and os.path.exists(args.config):

try:

import json

with open(args.config, 'r') as f:

config = json.load(f)

logger.info(f"Loaded configuration from {args.config}")

except Exception as e:

logger.error(f"Failed to load config: {e}")

config = create_default_config()

else:

config = create_default_config()

# Override config with command line arguments

config["enable_gpu"] = args.enable_gpu

config["enable_ros2"] = args.enable_ros2

# Determine run mode

run_mode = "benchmark" if args.benchmark_only else args.mode

# Create and run system

try:

system = PythonSLAMSystem(config)

if system.initialize(mode=run_mode):

success = system.run(mode=run_mode)

exit_code = 0 if success else 1

else:

logger.error("System initialization failed")

exit_code = 1

system.shutdown()

except Exception as e:

logger.error(f"System error: {e}")

exit_code = 1

sys.exit(exit_code)

if __name__ == "__main__":

main()