import math
import numpy as np
import matplotlib.pyplot as plt
from math import cos, sin, radians, pi
def file_read(f):
"""
Reading LIDAR laser beams (angles and corresponding distance data)
"""
measures = [line.split(",") for line in open(f)]
angles = []
distances = []
for measure in measures:
angles.append(float(measure[0]))
distances.append(float(measure[1]))
angles = np.array(angles)
distances = np.array(distances)
return angles, distances
ang, dist = file_read("lidar01.csv")
ox = np.sin(ang) * dist
oy = np.cos(ang) * dist
plt.figure(figsize=(6,10))
plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-") # lines from 0,0 to the
plt.axis("equal")
bottom, top = plt.ylim() # return the current ylim
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
plt.grid(True)
plt.show()
import lidar_to_grid_map as lg
map1 = np.ones((50, 50)) * 0.5
line = lg.bresenham((2, 2), (40, 30))
for l in line:
map1[l[0]][l[1]] = 1
plt.imshow(map1)
plt.colorbar()
plt.show()
line = lg.bresenham((2, 30), (40, 30))
for l in line:
map1[l[0]][l[1]] = 1
line = lg.bresenham((2, 30), (2, 2))
for l in line:
map1[l[0]][l[1]] = 1
plt.imshow(map1)
plt.colorbar()
plt.show()
from collections import deque
def flood_fill(cpoint, pmap):
"""
cpoint: starting point (x,y) of fill
pmap: occupancy map generated from Bresenham ray-tracing
"""
# Fill empty areas with queue method
sx, sy = pmap.shape
fringe = deque()
fringe.appendleft(cpoint)
while fringe:
n = fringe.pop()
nx, ny = n
# West
if nx > 0:
if pmap[nx - 1, ny] == 0.5:
pmap[nx - 1, ny] = 0.0
fringe.appendleft((nx - 1, ny))
# East
if nx < sx - 1:
if pmap[nx + 1, ny] == 0.5:
pmap[nx + 1, ny] = 0.0
fringe.appendleft((nx + 1, ny))
# North
if ny > 0:
if pmap[nx, ny - 1] == 0.5:
pmap[nx, ny - 1] = 0.0
fringe.appendleft((nx, ny - 1))
# South
if ny < sy - 1:
if pmap[nx, ny + 1] == 0.5:
pmap[nx, ny + 1] = 0.0
fringe.appendleft((nx, ny + 1))
flood_fill((10, 20), map1)
map_float = np.array(map1)/10.0
plt.imshow(map1)
plt.colorbar()
plt.show()
xyreso = 0.02 # x-y grid resolution
yawreso = math.radians(3.1) # yaw angle resolution [rad]
ang, dist = file_read("lidar01.csv")
ox = np.sin(ang) * dist
oy = np.cos(ang) * dist
pmap, minx, maxx, miny, maxy, xyreso = lg.generate_ray_casting_grid_map(ox, oy, xyreso, False)
xyres = np.array(pmap).shape
plt.figure(figsize=(20,8))
plt.subplot(122)
plt.imshow(pmap, cmap = "PiYG_r")
plt.clim(-0.4, 1.4)
plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor = True)
plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor = True)
plt.grid(True, which="minor", color="w", linewidth = .6, alpha = 0.5)
plt.colorbar()
plt.show()
The grid map is 150 x 100 .