Skip to content

Commit 9e3dc08

Browse files
committed
keep implementation
1 parent 5f56c6a commit 9e3dc08

File tree

1 file changed

+32
-26
lines changed

1 file changed

+32
-26
lines changed

Mapping/grid_map/grid_map.py

Lines changed: 32 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -14,14 +14,14 @@
1414
AREA_WIDTH = 30.0
1515

1616

17-
def generate_gaussian_grid_map(ox, oy, reso):
17+
def generate_gaussian_grid_map(ox, oy, xyreso):
1818

1919
minx = min(ox) - AREA_WIDTH / 2.0
2020
miny = min(oy) - AREA_WIDTH / 2.0
2121
maxx = max(ox) + AREA_WIDTH / 2.0
2222
maxy = max(oy) + AREA_WIDTH / 2.0
23-
xw = round((maxx - minx) / reso)
24-
yw = round((maxy - miny) / reso)
23+
xw = round((maxx - minx) / xyreso)
24+
yw = round((maxy - miny) / xyreso)
2525

2626
# calc each potential
2727
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):
3131
for ix in range(xw):
3232
for iy in range(yw):
3333

34-
x = ix / reso + minx
35-
y = iy / reso + miny
34+
x = ix * xyreso + minx
35+
y = iy * xyreso + miny
3636

3737
# Search minimum distance
3838
mindis = float("inf")
@@ -44,7 +44,8 @@ def generate_gaussian_grid_map(ox, oy, reso):
4444
pdf = (1.0 - norm.cdf(mindis, 0.0, STD))
4545
pmap[ix][iy] = pdf
4646

47-
draw_heatmap(pmap)
47+
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
48+
plt.plot(ox, oy, "xr")
4849
plt.show()
4950

5051

@@ -62,21 +63,21 @@ def __str__(self):
6263
return str(self.px) + "," + str(self.py) + "," + str(self.d) + "," + str(self.angle)
6364

6465

65-
def precasting(minx, miny, xw, yw, reso, yawreso):
66+
def precasting(minx, miny, xw, yw, xyreso, yawreso):
6667

6768
precast = [[] for i in range(round((math.pi * 2.0) / yawreso) + 1)]
6869

6970
for ix in range(xw):
7071
for iy in range(yw):
71-
px = ix / reso + minx
72-
py = iy / reso + miny
72+
px = ix * xyreso + minx
73+
py = iy * xyreso + miny
7374

7475
d = math.sqrt(px**2 + py**2)
7576
angle = math.atan2(py, px)
7677
if angle < 0.0:
7778
angle += math.pi * 2.0
7879

79-
angleid = round(angle / yawreso)
80+
angleid = math.floor(angle / yawreso)
8081

8182
pc = precastDB()
8283

@@ -92,46 +93,51 @@ def precasting(minx, miny, xw, yw, reso, yawreso):
9293
return precast
9394

9495

95-
def generate_ray_casting_grid_map(ox, oy, reso):
96+
def generate_ray_casting_grid_map(ox, oy, xyreso):
9697

9798
minx = min(ox) - AREA_WIDTH / 2.0
9899
miny = min(oy) - AREA_WIDTH / 2.0
99100
maxx = max(ox) + AREA_WIDTH / 2.0
100101
maxy = max(oy) + AREA_WIDTH / 2.0
101-
xw = round((maxx - minx) / reso)
102-
yw = round((maxy - miny) / reso)
102+
xw = round((maxx - minx) / xyreso)
103+
yw = round((maxy - miny) / xyreso)
103104

104105
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
105106

106107
yawreso = math.radians(10.0)
107108

108-
precast = precasting(minx, miny, xw, yw, reso, yawreso)
109+
precast = precasting(minx, miny, xw, yw, xyreso, yawreso)
109110

110111
for (x, y) in zip(ox, oy):
111112

112113
d = math.sqrt(x**2 + y**2)
113114
angle = math.atan2(y, x)
115+
if angle < 0.0:
116+
angle += math.pi * 2.0
114117

115-
angleid = round(angle / yawreso)
118+
angleid = math.floor(angle / yawreso)
116119

117120
gridlist = precast[angleid]
118121

119-
ix = round(x * reso - minx)
120-
iy = round(y * reso - miny)
122+
ix = round((x - minx) / xyreso)
123+
iy = round((y - miny) / xyreso)
124+
125+
pmap[ix][iy] = 1.0
121126

122127
for grid in gridlist:
123128

124-
if ix == grid.ix and iy == grid.iy:
125-
pmap[grid.ix][grid.iy] = 1.0
126-
elif grid.d > d:
129+
if grid.d > (d):
127130
pmap[grid.ix][grid.iy] = 0.5
128131

129-
draw_heatmap(pmap)
132+
draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
130133
plt.show()
131134

132135

133-
def draw_heatmap(data):
134-
data = np.array(data).T
136+
def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
137+
y, x = np.mgrid[slice(minx, maxx + xyreso, xyreso),
138+
slice(miny, maxy + xyreso, xyreso)]
139+
# print(y)
140+
# data = np.array(data).T
135141
plt.pcolor(data, vmax=1.0, cmap=plt.cm.Blues)
136142
plt.axis("equal")
137143

@@ -142,10 +148,10 @@ def main():
142148
# obstacle positions
143149
ox = [-5.0, 5.0, 0.0, 10.0]
144150
oy = [0.0, 5.0, 10.0, -5.0]
145-
reso = 1.0
151+
xyreso = 0.5
146152

147-
# generate_gaussian_grid_map(ox, oy, reso)
148-
generate_ray_casting_grid_map(ox, oy, reso)
153+
# generate_gaussian_grid_map(ox, oy, xyreso)
154+
generate_ray_casting_grid_map(ox, oy, xyreso)
149155

150156

151157
if __name__ == '__main__':

0 commit comments

Comments
 (0)