From a5f5fd9f8e6b3084a9f84d9f693ab6d0894f35c7 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Wed, 4 Dec 2019 23:28:29 +0100 Subject: [PATCH] Add lidar model --- lidar_model.py | 148 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 148 insertions(+) create mode 100644 lidar_model.py diff --git a/lidar_model.py b/lidar_model.py new file mode 100644 index 0000000..82e8c68 --- /dev/null +++ b/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()