Working main program

slam/main.py

@@ -3,21 +3,41 @@ import math
 import pygame
 
 from slam.environment import Environment
+from slam.sensors import Lidar2DSensor
 
 PYGAME_RUNNING = True
+SENSOR_ON = False  # Mouse cursor
 
 
 def main():
     global PYGAME_RUNNING
+    global SENSOR_ON
     environment = Environment(map_path="../data/slam_demo_floor_plan.png", map_height=600, map_width=1200)
+    environment.original_map = environment.map.copy()
+    laser = Lidar2DSensor(200, environment.original_map, uncertainty=(0.5, 0.01))
+    environment.map.fill(environment.colors.black)
+    environment.info_map = environment.map.copy()
 
     while PYGAME_RUNNING:
 
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
                 PYGAME_RUNNING = False
-
-        # Updates the map
+            if pygame.mouse.get_focused():
+                SENSOR_ON = True
+            else:
+                SENSOR_ON = False
+
+        if SENSOR_ON:
+            sim_robot_position = pygame.mouse.get_pos()
+            laser.position = sim_robot_position
+            sensor_data = laser.sense_obstacles()
+            # Draw data to map
+            environment.store_points(sensor_data)
+            environment.show_sensor_data()
+        # update the map
+        environment.map.blit(environment.info_map, (0, 0))
+        # display the map
         pygame.display.update()
 
     exit(0)

slam/sensors.py

@@ -5,10 +5,10 @@ import numpy as np
 
 
 def add_noise(distance, angle, sigma):
-    mean = np.array[distance, angle]
+    _mean = np.array([distance, angle])
     # Noise for distance or angle are not correlated
     covariance = np.diag(sigma ** 2)
-    new_distance, new_angle = np.random.multivariate_normal(mean, covariance)
+    new_distance, new_angle = np.random.multivariate_normal(_mean, covariance)
     # Don't want negative values
     new_distance = max(new_distance, 0)
     new_angle = max(new_angle, 0)
@@ -32,7 +32,7 @@ class Lidar2DSensor:
         self.sigma = np.array([self._uncertainty[0], self._uncertainty[1]])
         # TODO make this a value of the robot.
         self.position = (0, 0)
-        self.map_width, self.map_height = self.map.get_surface().get_size()
+        self.map_width, self.map_height = self.map.get_size()
         self.sensor_point_cloud = []
         self.colors = colors