github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 01:28:23 -05:00
Code Issues Packages Projects Releases Wiki Activity

first release raycasting_grid_map

Browse Source
This commit is contained in:
Atsushi Sakai
2018-02-19 11:30:23 -08:00
parent 4c71239099
commit 1ee13a74e4
1 changed files with 30 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
Mapping/raycasting_grid_map/raycasting_grid_map.py
View File

@@ -1,6 +1,6 @@
"""
2D grid map sample
Ray casting 2D grid map example
author: Atsushi Sakai (@Atsushi_twi)
@@ -9,46 +9,17 @@ author: Atsushi Sakai (@Atsushi_twi)
import math
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
AREA_WIDTH = 10.0
EXTEND_AREA = 10.0
STD = 10.0 # standard diviation
def generate_gaussian_grid_map(ox, oy, xyreso):
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
# calc each potential
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
for ix in range(xw):
for iy in range(yw):
x = ix * xyreso + minx
y = iy * xyreso + miny
# Search minimum distance
mindis = float("inf")
for (iox, ioy) in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if mindis >= d:
mindis = d
pdf = (1.0 - norm.cdf(mindis, 0.0, STD))
pmap[ix][iy] = pdf
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")
plt.plot(0.0, 0.0, "ob")
show_animation = True
def calc_grid_map_config(ox, oy, xyreso):
minx = round(min(ox) - AREA_WIDTH / 2.0)
miny = round(min(oy) - AREA_WIDTH / 2.0)
maxx = round(max(ox) + AREA_WIDTH / 2.0)
maxy = round(max(oy) + AREA_WIDTH / 2.0)
minx = round(min(ox) - EXTEND_AREA / 2.0)
miny = round(min(oy) - EXTEND_AREA / 2.0)
maxx = round(max(ox) + EXTEND_AREA / 2.0)
maxy = round(max(oy) + EXTEND_AREA / 2.0)
xw = int(round((maxx - minx) / xyreso))
yw = int(round((maxy - miny) / xyreso))
@@ -69,6 +40,14 @@ class precastDB:
return str(self.px) + "," + str(self.py) + "," + str(self.d) + "," + str(self.angle)
def atan_zero_to_twopi(y, x):
angle = math.atan2(y, x)
if angle < 0.0:
angle += math.pi * 2.0
return angle
def precasting(minx, miny, xw, yw, xyreso, yawreso):
precast = [[] for i in range(round((math.pi * 2.0) / yawreso) + 1)]
@@ -79,10 +58,7 @@ def precasting(minx, miny, xw, yw, xyreso, yawreso):
py = iy * xyreso + miny
d = math.sqrt(px**2 + py**2)
angle = math.atan2(py, px)
if angle < 0.0:
angle += math.pi * 2.0
angle = atan_zero_to_twopi(py, px)
angleid = math.floor(angle / yawreso)
pc = precastDB()
@@ -99,23 +75,18 @@ def precasting(minx, miny, xw, yw, xyreso, yawreso):
return precast
def generate_ray_casting_grid_map(ox, oy, xyreso):
def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso):
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
yawreso = math.radians(10.0)
precast = precasting(minx, miny, xw, yw, xyreso, yawreso)
for (x, y) in zip(ox, oy):
d = math.sqrt(x**2 + y**2)
angle = math.atan2(y, x)
if angle < 0.0:
angle += math.pi * 2.0
angle = atan_zero_to_twopi(y, x)
angleid = math.floor(angle / yawreso)
gridlist = precast[angleid]
@@ -124,14 +95,12 @@ def generate_ray_casting_grid_map(ox, oy, xyreso):
iy = int(round((y - miny) / xyreso))
for grid in gridlist:
if grid.d > (d):
if grid.d > d:
pmap[grid.ix][grid.iy] = 0.5
pmap[ix][iy] = 1.0
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")
plt.plot(0.0, 0.0, "ob")
return pmap, minx, maxx, miny, maxy, xyreso
def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
@@ -144,18 +113,20 @@ def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
def main():
print(__file__ + " start!!")
xyreso = 0.5
xyreso = 0.25 # x-y grid resolution [m]
yawreso = math.radians(10.0) # yaw angle resolution [rad]
for i in range(5):
ox = (np.random.rand(4) - 0.5) * 10.0
oy = (np.random.rand(4) - 0.5) * 10.0
plt.cla()
generate_gaussian_grid_map(ox, oy, xyreso)
plt.pause(1.0)
plt.cla()
generate_ray_casting_grid_map(ox, oy, xyreso)
plt.pause(1.0)
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(
ox, oy, xyreso, yawreso)
if show_animation:
plt.cla()
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")
plt.plot(0.0, 0.0, "ob")
plt.pause(1.0)
if __name__ == '__main__':