Add lidar model

lidar_model.py

@@ -0,0 +1,148 @@
+from map import Map
+import matplotlib.pyplot as plt
+import numpy as np
+import math
+import time
+
+SCAN = '#5DADE2'
+
+
+class LidarModel:
+    """
+    Lidar Model
+    """
+    def __init__(self, FoV, range, resolution):
+        """
+        Constructor for Lidar object.
+        :param FoV: Sensor's Field of View in °
+        :param range: range in m
+        :param resolution: resolution in °
+        """
+
+        # set sensor parameters
+        self.FoV = FoV
+        self.range = range
+        self.resolution = resolution
+
+        # number of measurements
+        self.n_measurements = int(self.FoV/self.resolution + 1)
+
+        # construct measurement container
+        angles = np.linspace(-math.pi / 360 * self.FoV,
+                             math.pi / 360 * self.FoV,
+                             self.n_measurements)
+        ranges = np.ones(self.n_measurements) * self.range
+        self.measurements = np.stack((angles, ranges), axis=0)
+
+    def scan(self, car, map):
+        """
+        Get a Lidar Scan estimate
+        :param car: state containing x and y coordinates of the sensor
+        :param map: map object
+        :return: self with updated self.measurements
+        """
+
+        start = time.time()
+        # reset measurements
+        self.measurements[1, :] = np.ones(self.n_measurements) * self.range
+
+        # flip map upside-down to allow for normal indexing of y axis
+        #map.data = np.flipud(map.data)
+
+        # get sensor's map pose
+        x, y = map.w2m(car.x, car.y)
+        # get center of mass
+        xc = x + 0.5
+        yc = y + 0.5
+
+        # get sensor range in px values
+        range_px = int(self.range / map.resolution)
+
+        # iterate over area within sensor's range
+        for i in range(x - range_px, x + range_px + 1):
+            if 0 <= i < map.width:
+                for j in range(y - range_px, y + range_px + 1):
+                    if 0 <= j < map.height:
+                        # if obstacle detected
+                        if map.data[j, i] == 0:
+
+                            # get center of mass of cell
+                            xc_target = i + 0.5
+                            yc_target = j + 0.5
+
+                            # check all corner's of cell
+                            cell_angles = []
+                            for k in range(-1, 2):
+                                for l in range(-1, 2):
+                                    dy = yc_target + l/2 - yc
+                                    dx = xc_target + k/2 - xc
+                                    cell_angle = np.arctan2(dy, dx) - car.psi
+                                    if cell_angle < - math.pi:
+                                        cell_angle = -np.mod(math.pi+cell_angle, 2*math.pi) + math.pi
+                                    else:
+                                        cell_angle = np.mod(math.pi+cell_angle, 2*math.pi) - math.pi
+                                    cell_angles.append(cell_angle)
+
+                            # get min and max angle hitting respective cell
+                            min_angle = np.min(cell_angles)
+                            max_angle = np.max(cell_angles)
+
+                            # get distance to mass center of cell
+                            cell_distance = np.sqrt(
+                                (xc - xc_target)**2 + (yc - yc_target)**2)
+
+                            # get IDs of all laser beams hitting cell
+                            valid_beam_ids = []
+                            if min_angle < -math.pi/2 and max_angle > math.pi/2:
+                                for beam_id in range(self.n_measurements):
+                                    if max_angle <= self.measurements[0, beam_id] <= min_angle:
+                                        valid_beam_ids.append(beam_id)
+                            else:
+                                for beam_id in range(self.n_measurements):
+                                    if min_angle <= self.measurements[0, beam_id] <= max_angle:
+                                        valid_beam_ids.append(beam_id)
+
+                            # update distance for all valid laser beams
+                            for beam_id in valid_beam_ids:
+                                if cell_distance < self.measurements[1, beam_id] / map.resolution:
+                                    self.measurements[1, beam_id] = cell_distance * map.resolution
+
+        #map.data = np.flipud(map.data)
+        end = time.time()
+        print('Time elapsed: ', end - start)
+
+    def plot_scan(self, car):
+        """
+        Display current sensor measurements.
+        :param car: state containing x and y coordinate of sensor
+        """
+
+        start = time.time()
+        # get beam endpoints
+        beam_end_x = self.measurements[1, :] * np.cos(self.measurements[0, :] + car.psi)
+        beam_end_y = self.measurements[1, :] * np.sin(self.measurements[0, :] + car.psi)
+
+        # plot all laser beams
+        for i in range(self.n_measurements):
+            plt.plot((car.x, car.x+beam_end_x[i]), (car.y, car.y+beam_end_y[i]), c=SCAN)
+        end = time.time()
+        print('Time elapsed: ', end - start)
+
+
+if __name__ == '__main__':
+
+    # Create Map
+    map = Map('map_floor2.png')
+    plt.imshow(map.data, cmap='gray',
+               extent=[map.origin[0], map.origin[0] +
+                       map.width * map.resolution,
+                       map.origin[1], map.origin[1] +
+                       map.height * map.resolution])
+
+    car = BicycleModel(x=-4.9, y=-5.0, yaw=0.9)
+    sensor = LidarModel(FoV=180, range=5, resolution=1)
+    sensor.scan(car, map)
+    sensor.plot_scan(car)
+
+    plt.axis('equal')
+    plt.show()