RoomMapper: Room Measurement with RPLidar and Raspberry Pi

#!/usr/bin/env python3
import pygame
import math
import time
import numpy as np
import threading
from datetime import datetime
import serial
import serial.tools.list_ports

class RoomMapper:
    def __init__(self):
        # Hardware configuration
        self.lidar = None
        self.is_scanning = False
        self.scan_thread = None
        self.simulation_mode = False
        
        # Display settings
        self.screen_size = (1024, 768)
        self.screen = None
        self.center_x = self.screen_size[0] // 2
        self.center_y = self.screen_size[1] // 2
        self.scale = 0.2  
        self.point_size = 3
        
        # Color scheme
        self.colors = {
            'bg': (15, 20, 30),
            'grid': (40, 45, 60),
            'robot': (220, 80, 60),
            'axes': (80, 180, 120),
            'text': (220, 230, 240),
            'accent': (65, 150, 255),
            'point_close': (100, 200, 255),
            'point_mid': (100, 255, 150),
            'point_far': (255, 200, 100)
        }
        
        # Data storage
        self.point_cloud = []
        self.scan_count = 0
        self.start_time = time.time()
        self.room_data = None
        self.buffer_lock = threading.Lock()
        
        # LiDAR settings
        self.min_distance = 50
        self.max_distance = 16000
        
        # Initialize fonts
        self.fonts = {}

    def initialize_system(self):
        """Initialize all system components"""
        print("🚀 Initializing RoomMapper System...")
        
        if not self.init_pygame():
            return False
            
        if not self.connect_lidar():
            print("❌ Failed to connect to LiDAR")
            return False
            
        self.load_fonts()
        print("✅ System initialization complete")
        return True

    def connect_lidar(self):
        """Connect to RPLidar"""
        try:
            # Try to find LiDAR port automatically
            ports = list(serial.tools.list_ports.comports())
            lidar_port = None
            
            for port in ports:
                print(f"🔍 Found port: {port.device} - {port.description}")
                if 'USB' in port.description or 'Serial' in port.description:
                    lidar_port = port.device
                    break
            
            if not lidar_port:
                lidar_port = '/dev/ttyUSB0'  # Default Linux port
                
            print(f"🔌 Connecting to LiDAR on {lidar_port}...")
            
            # Import RPLidar here to avoid issues if not installed
            try:
                from rplidar import RPLidar
                self.lidar = RPLidar(lidar_port)
                
                # Get device info
                info = self.lidar.get_info()
                health = self.lidar.get_health()
                
                print("✅ LiDAR Connected Successfully!")
                print(f"   Model: {info['model']}")
                print(f"   Firmware: {info['firmware']}")
                print(f"   Hardware: {info['hardware']}")
                print(f"   Health: {health}")
                
                # Start motor
                self.lidar.start_motor()
                time.sleep(1)
                
                return True
                
            except ImportError:
                print("❌ RPLidar library not installed")
                return False
                
        except Exception as e:
            print(f"❌ LiDAR connection failed: {e}")
            return False

    def init_pygame(self):
        """Initialize Pygame display"""
        try:
            pygame.init()
            self.screen = pygame.display.set_mode(self.screen_size)
            pygame.display.set_caption("RoomMapper Pro - Real LiDAR Data")
            print("✅ Display initialized")
            return True
        except Exception as e:
            print(f"❌ Display initialization failed: {e}")
            return False

    def load_fonts(self):
        """Load fonts"""
        try:
            self.fonts = {
                'small': pygame.font.SysFont('Arial', 14),
                'normal': pygame.font.SysFont('Arial', 18),
                'medium': pygame.font.SysFont('Arial', 22),
                'large': pygame.font.SysFont('Arial', 26)
            }
        except:
            self.fonts = {
                'small': pygame.font.Font(None, 24),
                'normal': pygame.font.Font(None, 28),
                'medium': pygame.font.Font(None, 32),
                'large': pygame.font.Font(None, 36)
            }

    def lidar_scan_worker(self):
        """Background thread for LiDAR scanning"""
        print("📡 Starting LiDAR scanning thread...")
        
        try:
            for scan_data in self.lidar.iter_scans():
                if not self.is_scanning:
                    break
                
                new_points = []
                valid_points = 0
                
                for measurement in scan_data:
                    if len(measurement) == 3:
                        quality, angle, distance = measurement
                        
                        # Filter valid measurements
                        if (distance > self.min_distance and 
                            distance < self.max_distance and
                            quality > 10):
                            
                            # Convert to Cartesian coordinates
                            angle_rad = math.radians(angle)
                            x = distance * math.cos(angle_rad)
                            y = distance * math.sin(angle_rad)
                            
                            point_data = {
                                'x': x, 
                                'y': y, 
                                'distance': distance, 
                                'angle': angle,
                                'quality': quality,
                                'timestamp': time.time()
                            }
                            new_points.append(point_data)
                            valid_points += 1
                
                if new_points:
                    with self.buffer_lock:
                        self.point_cloud.extend(new_points)
                        
                        # Keep last 2000 points
                        if len(self.point_cloud) > 2000:
                            self.point_cloud = self.point_cloud[-2000:]
                    
                    self.scan_count += 1
                    
                    if self.scan_count % 10 == 0:
                        print(f"📊 Scan {self.scan_count}: {valid_points} points")
                        
        except Exception as e:
            print(f"❌ LiDAR scanning error: {e}")
        finally:
            print("🛑 LiDAR scanning thread stopped")

    def process_real_data(self):
        """Process real LiDAR data"""
        if len(self.point_cloud) < 100:
            return None
            
        with self.buffer_lock:
            if not self.point_cloud:
                return None
            
            # Extract coordinates from REAL data
            xs = [p['x'] for p in self.point_cloud]
            ys = [p['y'] for p in self.point_cloud]
            distances = [p['distance'] for p in self.point_cloud]
            
            # Calculate actual room dimensions from REAL measurements
            min_x, max_x = min(xs), max(xs)
            min_y, max_y = min(ys), max(ys)
            
            width = max_x - min_x
            height = max_y - min_y
            area = (width * height) / 1000000
            
            # Calculate center
            center_x = (min_x + max_x) / 2
            center_y = (min_y + max_y) / 2
            
            # Calculate confidence based on real data
            confidence = min(95, (len(self.point_cloud) / 30))
            
            self.room_data = {
                'width_mm': width,
                'height_mm': height,
                'width_m': width / 1000,
                'height_m': height / 1000,
                'area_m2': area,
                'center': (center_x, center_y),
                'bounds': (min_x, min_y, max_x, max_y),
                'point_count': len(self.point_cloud),
                'avg_distance': sum(distances) / len(distances),
                'confidence': confidence,
                'scan_duration': time.time() - self.start_time,
                'timestamp': datetime.now().isoformat(),
                'data_source': 'REAL_LIDAR'
            }
            
            print(f"📏 Real Measurements: {width/1000:.2f}m x {height/1000:.2f}m, Area: {area:.2f}m²")
            return self.room_data

    def draw_real_point_cloud(self):
        """Draw REAL LiDAR points"""
        with self.buffer_lock:
            for point in self.point_cloud:
                x = point['x']
                y = point['y']
                distance = point['distance']
                
                # Convert to screen coordinates
                screen_x = int(x * self.scale) + self.center_x
                screen_y = int(y * self.scale) + self.center_y
                
                # Check if point is within screen bounds
                if (0 <= screen_x < self.screen_size[0] and 
                    0 <= screen_y < self.screen_size[1]):
                    
                    # Color based on REAL distance
                    if distance < 2000:
                        color = self.colors['point_close']
                    elif distance < 6000:
                        color = self.colors['point_mid']
                    else:
                        color = self.colors['point_far']
                    
                    pygame.draw.circle(self.screen, color, (screen_x, screen_y), self.point_size)

    def draw_room_boundary(self):
        """Draw room boundary from REAL measurements"""
        if not self.room_data:
            return
            
        try:
            min_x, min_y, max_x, max_y = self.room_data['bounds']
            
            # Convert to screen coordinates
            screen_min_x = int(min_x * self.scale) + self.center_x
            screen_min_y = int(min_y * self.scale) + self.center_y
            screen_max_x = int(max_x * self.scale) + self.center_x
            screen_max_y = int(max_y * self.scale) + self.center_y
            
            # Draw boundary
            rect_width = screen_max_x - screen_min_x
            rect_height = screen_max_y - screen_min_y
            pygame.draw.rect(self.screen, self.colors['accent'], 
                           (screen_min_x, screen_min_y, rect_width, rect_height), 3)
            
        except Exception as e:
            print(f"⚠️ Boundary drawing error: {e}")

    def draw_grid(self):
        """Draw measurement grid"""
        grid_size = 1000  # 1 meter
        screen_grid_size = int(grid_size * self.scale)
        
        # Draw vertical lines
        for x in range(0, self.screen_size[0], screen_grid_size):
            pygame.draw.line(self.screen, self.colors['grid'], 
                           (x, 0), (x, self.screen_size[1]), 1)
        
        # Draw horizontal lines
        for y in range(0, self.screen_size[1], screen_grid_size):
            pygame.draw.line(self.screen, self.colors['grid'],
                           (0, y), (self.screen_size[0], y), 1)

    def draw_robot(self):
        """Draw robot position"""
        # Robot body
        pygame.draw.circle(self.screen, self.colors['robot'], 
                         (self.center_x, self.center_y), 20)
        
        # Direction indicator
        pygame.draw.line(self.screen, (255, 255, 255),
                        (self.center_x, self.center_y),
                        (self.center_x + 30, self.center_y), 4)

    def draw_control_panel(self):
        """Draw control panel"""
        panel_rect = (20, 20, 350, 200)
        pygame.draw.rect(self.screen, (25, 30, 45), panel_rect)
        pygame.draw.rect(self.screen, self.colors['accent'], panel_rect, 2)
        
        # Title
        title = self.fonts['large'].render("RoomMapper Pro - REAL DATA", True, self.colors['accent'])
        self.screen.blit(title, (40, 30))
        
        # Status
        status = self.fonts['medium'].render("Status: SCANNING - REAL LIDAR", True, (100, 255, 100))
        self.screen.blit(status, (40, 70))
        
        # Real-time info
        info_lines = [
            f"Scan Cycles: {self.scan_count}",
            f"Data Points: {len(self.point_cloud)}",
            f"Running Time: {time.time() - self.start_time:.1f}s",
            f"Zoom Level: {self.scale:.2f}"
        ]
        
        for i, line in enumerate(info_lines):
            text = self.fonts['normal'].render(line, True, self.colors['text'])
            self.screen.blit(text, (40, 100 + i * 25))

    def draw_measurements(self):
        """Draw real measurements"""
        panel_rect = (self.screen_size[0] - 370, 20, 350, 300)
        pygame.draw.rect(self.screen, (25, 30, 45), panel_rect)
        pygame.draw.rect(self.screen, self.colors['accent'], panel_rect, 2)
        
        title = self.fonts['large'].render("Real Measurements", True, self.colors['accent'])
        self.screen.blit(title, (self.screen_size[0] - 350, 30))
        
        if self.room_data:
            measurements = [
                f"Room Width: {self.room_data['width_m']:.2f} m",
                f"Room Height: {self.room_data['height_m']:.2f} m", 
                f"Room Area: {self.room_data['area_m2']:.2f} m²",
                f"Avg Distance: {self.room_data['avg_distance']:.0f} mm",
                f"Data Points: {self.room_data['point_count']}",
                f"Confidence: {self.room_data['confidence']:.0f}%",
                f"Data Source: {self.room_data['data_source']}",
                f"Scan Duration: {self.room_data['scan_duration']:.1f}s"
            ]
            
            for i, measurement in enumerate(measurements):
                text = self.fonts['normal'].render(measurement, True, self.colors['text'])
                self.screen.blit(text, (self.screen_size[0] - 350, 70 + i * 25))
        else:
            status = self.fonts['normal'].render("Collecting real data...", True, (255, 200, 100))
            self.screen.blit(status, (self.screen_size[0] - 350, 70))

    def draw_help(self):
        """Draw help information"""
        help_text = [
            "CONTROLS:",
            "Z/X - Zoom In/Out",
            "R - Reset Scan", 
            "P - Process Data",
            "ESC - Exit"
        ]
        
        for i, text in enumerate(help_text):
            rendered = self.fonts['normal'].render(text, True, self.colors['text'])
            self.screen.blit(rendered, (20, self.screen_size[1] - 120 + i * 25))

    def handle_events(self):
        """Handle user input"""
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                return False
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_ESCAPE:
                    return False
                elif event.key == pygame.K_z:
                    self.scale = min(self.scale * 1.2, 1.0)
                    print(f"🔍 Zoom: {self.scale:.2f}")
                elif event.key == pygame.K_x:
                    self.scale = max(self.scale * 0.8, 0.05)
                    print(f"🔍 Zoom: {self.scale:.2f}")
                elif event.key == pygame.K_r:
                    self.reset_scan()
                elif event.key == pygame.K_p:
                    self.process_real_data()
                    print("🔄 Processing REAL LiDAR data...")
        return True

    def reset_scan(self):
        """Reset scan data"""
        with self.buffer_lock:
            self.point_cloud = []
            self.room_data = None
            self.scan_count = 0
            self.start_time = time.time()
        print("🔄 Scan reset - collecting fresh data")

    def draw_radar_display(self):
        """Draw main radar display"""
        self.draw_grid()
        self.draw_real_point_cloud()
        self.draw_room_boundary()
        self.draw_robot()

    def draw_frame(self):
        """Draw complete frame"""
        self.screen.fill(self.colors['bg'])
        self.draw_radar_display()
        self.draw_control_panel()
        self.draw_measurements()
        self.draw_help()
        pygame.display.flip()

    def start_mapping(self):
        """Start mapping process"""
        print("🚀 Starting REAL LiDAR Mapping...")
        self.is_scanning = True
        
        # Start LiDAR thread
        self.scan_thread = threading.Thread(target=self.lidar_scan_worker)
        self.scan_thread.daemon = True
        self.scan_thread.start()
        
        # Main loop
        clock = pygame.time.Clock()
        last_process_time = time.time()
        
        try:
            while self.is_scanning:
                current_time = time.time()
                if current_time - last_process_time > 3.0:
                    self.process_real_data()
                    last_process_time = current_time
                
                self.draw_frame()
                
                if not self.handle_events():
                    break
                
                clock.tick(60)
                
        except Exception as e:
            print(f"❌ Main loop error: {e}")
        finally:
            self.stop_mapping()

    def stop_mapping(self):
        """Stop mapping process"""
        print("🛑 Stopping mapping...")
        self.is_scanning = False
        
        if self.lidar:
            try:
                self.lidar.stop()
                self.lidar.stop_motor()
                self.lidar.disconnect()
                print("✅ LiDAR stopped and disconnected")
            except Exception as e:
                print(f"⚠️ Error stopping LiDAR: {e}")

    def cleanup(self):
        """Cleanup resources"""
        self.stop_mapping()
        pygame.quit()
        print("🔌 System shutdown complete")

def main():
    """Main function"""
    print("=" * 60)
    print("🎯 RoomMapper Pro - REAL LiDAR Data Acquisition")
    print("=" * 60)
    print("This version uses ACTUAL LiDAR measurements")
    print("Make sure LiDAR is properly connected and powered")
    print("=" * 60)
    
    mapper = RoomMapper()
    
    if not mapper.initialize_system():
        print("❌ System initialization failed")
        return
    
    try:
        mapper.start_mapping()
    except KeyboardInterrupt:
        print("\n⏹️ Stopped by user")
    except Exception as e:
        print(f"❌ Error: {e}")
    finally:
        mapper.cleanup()
    
    print("✨ Thank you for using RoomMapper Pro!")

if __name__ == "__main__":
    main()