AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

keep implementation

Browse Source
This commit is contained in:
Atsushi Sakai
2018-02-17 10:19:13 -08:00
parent 5f56c6a552
commit 9e3dc080a9
1 changed files with 32 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
Mapping/grid_map/grid_map.py
View File

@@ -14,14 +14,14 @@ from scipy.stats import norm
AREA_WIDTH = 30.0
def generate_gaussian_grid_map(ox, oy, reso):
def generate_gaussian_grid_map(ox, oy, xyreso):
minx = min(ox) - AREA_WIDTH / 2.0
miny = min(oy) - AREA_WIDTH / 2.0
maxx = max(ox) + AREA_WIDTH / 2.0
maxy = max(oy) + AREA_WIDTH / 2.0
xw = round((maxx - minx) / reso)
yw = round((maxy - miny) / reso)
xw = round((maxx - minx) / xyreso)
yw = round((maxy - miny) / xyreso)
# calc each potential
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
@@ -31,8 +31,8 @@ def generate_gaussian_grid_map(ox, oy, reso):
for ix in range(xw):
for iy in range(yw):
x = ix / reso + minx
y = iy / reso + miny
x = ix * xyreso + minx
y = iy * xyreso + miny
# Search minimum distance
mindis = float("inf")
@@ -44,7 +44,8 @@ def generate_gaussian_grid_map(ox, oy, reso):
pdf = (1.0 - norm.cdf(mindis, 0.0, STD))
pmap[ix][iy] = pdf
draw_heatmap(pmap)
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
plt.plot(ox, oy, "xr")
plt.show()
@@ -62,21 +63,21 @@ class precastDB:
return str(self.px) + "," + str(self.py) + "," + str(self.d) + "," + str(self.angle)
def precasting(minx, miny, xw, yw, reso, yawreso):
def precasting(minx, miny, xw, yw, xyreso, yawreso):
precast = [[] for i in range(round((math.pi * 2.0) / yawreso) + 1)]
for ix in range(xw):
for iy in range(yw):
px = ix / reso + minx
py = iy / reso + miny
px = ix * xyreso + minx
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
angleid = round(angle / yawreso)
angleid = math.floor(angle / yawreso)
pc = precastDB()
@@ -92,46 +93,51 @@ def precasting(minx, miny, xw, yw, reso, yawreso):
return precast
def generate_ray_casting_grid_map(ox, oy, reso):
def generate_ray_casting_grid_map(ox, oy, xyreso):
minx = min(ox) - AREA_WIDTH / 2.0
miny = min(oy) - AREA_WIDTH / 2.0
maxx = max(ox) + AREA_WIDTH / 2.0
maxy = max(oy) + AREA_WIDTH / 2.0
xw = round((maxx - minx) / reso)
yw = round((maxy - miny) / reso)
xw = round((maxx - minx) / xyreso)
yw = round((maxy - miny) / 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, reso, yawreso)
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
angleid = round(angle / yawreso)
angleid = math.floor(angle / yawreso)
gridlist = precast[angleid]
ix = round(x * reso - minx)
iy = round(y * reso - miny)
ix = round((x - minx) / xyreso)
iy = round((y - miny) / xyreso)
pmap[ix][iy] = 1.0
for grid in gridlist:
if ix == grid.ix and iy == grid.iy:
pmap[grid.ix][grid.iy] = 1.0
elif grid.d > d:
if grid.d > (d):
pmap[grid.ix][grid.iy] = 0.5
draw_heatmap(pmap)
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
plt.show()
def draw_heatmap(data):
data = np.array(data).T
def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
y, x = np.mgrid[slice(minx, maxx + xyreso, xyreso),
slice(miny, maxy + xyreso, xyreso)]
# print(y)
# data = np.array(data).T
plt.pcolor(data, vmax=1.0, cmap=plt.cm.Blues)
plt.axis("equal")
@@ -142,10 +148,10 @@ def main():
# obstacle positions
ox = [-5.0, 5.0, 0.0, 10.0]
oy = [0.0, 5.0, 10.0, -5.0]
reso = 1.0
xyreso = 0.5
# generate_gaussian_grid_map(ox, oy, reso)
generate_ray_casting_grid_map(ox, oy, reso)
# generate_gaussian_grid_map(ox, oy, xyreso)
generate_ray_casting_grid_map(ox, oy, xyreso)
if __name__ == '__main__':