basic implementation

requirements.in

@@ -1 +1,2 @@
+numpy
 pygame

requirements.txt

@@ -4,5 +4,7 @@
 #
 #    pip-compile requirements.in
 #
+numpy==1.20.3
+    # via -r requirements.in
 pygame==2.0.1
     # via -r requirements.in

slam/environment.py

@@ -1,3 +1,4 @@
+import math
 from collections import namedtuple
 
 import pygame
@@ -32,3 +33,26 @@ class Environment:
         self.map = pygame.display.set_mode((self.map_width, self.map_height))
         # overlay external map
         self.map.blit(self.external_map, (0, 0))
+
+    def calc_2d_position(self, distance, angle, robot_position):
+        """Calculates a 2D point from a robots position and a Lidar sensor reading."""
+        x = distance * math.cos(angle) + robot_position[0]
+        y = -distance * math.sin(angle) + robot_position[1]
+        return (int(x), int(y))
+
+    # TODO add type hints
+    def store_points(self, sensor_sweep_data):
+        """Stores sensor reading in the environment's point cloud."""
+        for element in sensor_sweep_data:
+            # TODO this could be refactored into a LidarReading & RobotPosition named tuples
+            point = self.calc_2d_position(element[0], element[1], element[2])
+            # check for dupes
+            # TODO ok an optimization could probably be added here too
+            if point not in self.point_cloud:
+                self.point_cloud.append(point)
+
+    def show_sensor_data(self):
+        # Draw data on new map
+        self.info_map = self.map.copy()
+        for point in self.point_cloud:
+            self.info_map.set_at((int(point[0]), int(point[1])), self.colors.red)

slam/sensors.py

@@ -0,0 +1,88 @@
+"""Module containing simulated robot sensors"""
+import math
+
+import numpy as np
+
+
+def add_noise(distance, angle, sigma):
+    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)
+    # Don't want negative values
+    new_distance = max(new_distance, 0)
+    new_angle = max(new_angle, 0)
+    return [new_distance, new_angle]
+
+
+class Lidar2DSensor:
+
+    def __init__(self, senor_range, environment_map, uncertainty, speed=4, colors=None):
+        """
+        :param range: range of the sensor
+        :param map: the map
+        :param uncertainty: uncertainty of sensor measurements
+        :param speed: revolutions per second
+        """
+        self.range = senor_range
+        self.map = environment_map
+        self._uncertainty = uncertainty
+        self.speed = speed
+        # The sensor noise represented as a distance and an angle
+        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.sensor_point_cloud = []
+        self.colors = colors
+
+    def calc_distance(self, obstacle_position):
+        """Calculates the distance from robot position to obstacle.
+
+        Uses the Euclidean distance calculation.
+            distance = sqrt((Xb - Xa)^2 + (Yb-Ya)^2)
+        """
+        px = (obstacle_position[0] - self.position[0]) ** 2
+        py = (obstacle_position[1] - self.position[1]) ** 2
+        return math.sqrt(px + py)
+
+    def sense_obstacles(self):
+        data = []
+        x1, y1 = self.position[0], self.position[1]
+
+        # 360 degrees == 2Pi radians
+        # np.linspace's use allows up to set the number of values which affects the simulated map resolution.
+        for angle in np.linspace(0, 2 * math.pi, 60, False):
+            # Calculate the end point of the lidar range's line segment in the 2D plane
+            x2, y2 = (x1 + self.range * math.cos(angle), y1 - self.range * math.sin(angle))
+            # Sample the line segment and detext if the sampled point is "black" when overlayed on the simulated map
+            for i in range(0, 100):
+                # TODO break this out to a function
+                u = i / 100
+                # TODO add comment explaining this calculation
+                x = int(x2 * u + x1 * (1 - u))
+                y = int(y2 * u + y1 * (1 - u))
+                # TODO an optimization could be performed to not sample any further
+                # if the point has fallen off the map.
+                # Check if the sampled point is on the map
+                if 0 < x < self.map_width and 0 < y < self.map_height:
+                    map_point_color = self.map.get_at((x, y))
+                    if (map_point_color[0], map_point_color[1], map_point_color[2]) == (0, 0, 0):
+                        # if black add some noise
+                        distance = self.calc_distance((x, y))
+                        noised_distance = add_noise(distance, angle, self.sigma)
+                        # TODO there is definitely a better datastructure to use. Maybe a named tuple?
+                        noised_distance.append(self.position)
+                        data.append(noised_distance)
+                        # if obstacle found stop further sampling this line
+                        break
+                else:
+                    # line fell off of map break this loop iteration
+                    break
+        # TODO OMG I hate this guys code
+        # Callers of this function need to check truthiness not in the return  statement
+        # if len(data)>0:
+        #     return data
+        # else:
+        #     return False
+        return data