14
14
AREA_WIDTH = 30.0
15
15
16
16
17
- def generate_gaussian_grid_map (ox , oy , reso ):
17
+ def generate_gaussian_grid_map (ox , oy , xyreso ):
18
18
19
19
minx = min (ox ) - AREA_WIDTH / 2.0
20
20
miny = min (oy ) - AREA_WIDTH / 2.0
21
21
maxx = max (ox ) + AREA_WIDTH / 2.0
22
22
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 )
25
25
26
26
# calc each potential
27
27
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):
31
31
for ix in range (xw ):
32
32
for iy in range (yw ):
33
33
34
- x = ix / reso + minx
35
- y = iy / reso + miny
34
+ x = ix * xyreso + minx
35
+ y = iy * xyreso + miny
36
36
37
37
# Search minimum distance
38
38
mindis = float ("inf" )
@@ -44,7 +44,8 @@ def generate_gaussian_grid_map(ox, oy, reso):
44
44
pdf = (1.0 - norm .cdf (mindis , 0.0 , STD ))
45
45
pmap [ix ][iy ] = pdf
46
46
47
- draw_heatmap (pmap )
47
+ draw_heatmap (pmap , minx , maxx , miny , maxy , xyreso )
48
+ plt .plot (ox , oy , "xr" )
48
49
plt .show ()
49
50
50
51
@@ -62,21 +63,21 @@ def __str__(self):
62
63
return str (self .px ) + "," + str (self .py ) + "," + str (self .d ) + "," + str (self .angle )
63
64
64
65
65
- def precasting (minx , miny , xw , yw , reso , yawreso ):
66
+ def precasting (minx , miny , xw , yw , xyreso , yawreso ):
66
67
67
68
precast = [[] for i in range (round ((math .pi * 2.0 ) / yawreso ) + 1 )]
68
69
69
70
for ix in range (xw ):
70
71
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
73
74
74
75
d = math .sqrt (px ** 2 + py ** 2 )
75
76
angle = math .atan2 (py , px )
76
77
if angle < 0.0 :
77
78
angle += math .pi * 2.0
78
79
79
- angleid = round (angle / yawreso )
80
+ angleid = math . floor (angle / yawreso )
80
81
81
82
pc = precastDB ()
82
83
@@ -92,46 +93,51 @@ def precasting(minx, miny, xw, yw, reso, yawreso):
92
93
return precast
93
94
94
95
95
- def generate_ray_casting_grid_map (ox , oy , reso ):
96
+ def generate_ray_casting_grid_map (ox , oy , xyreso ):
96
97
97
98
minx = min (ox ) - AREA_WIDTH / 2.0
98
99
miny = min (oy ) - AREA_WIDTH / 2.0
99
100
maxx = max (ox ) + AREA_WIDTH / 2.0
100
101
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 )
103
104
104
105
pmap = [[0.0 for i in range (yw )] for i in range (xw )]
105
106
106
107
yawreso = math .radians (10.0 )
107
108
108
- precast = precasting (minx , miny , xw , yw , reso , yawreso )
109
+ precast = precasting (minx , miny , xw , yw , xyreso , yawreso )
109
110
110
111
for (x , y ) in zip (ox , oy ):
111
112
112
113
d = math .sqrt (x ** 2 + y ** 2 )
113
114
angle = math .atan2 (y , x )
115
+ if angle < 0.0 :
116
+ angle += math .pi * 2.0
114
117
115
- angleid = round (angle / yawreso )
118
+ angleid = math . floor (angle / yawreso )
116
119
117
120
gridlist = precast [angleid ]
118
121
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
121
126
122
127
for grid in gridlist :
123
128
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 ):
127
130
pmap [grid .ix ][grid .iy ] = 0.5
128
131
129
- draw_heatmap (pmap )
132
+ draw_heatmap (pmap , minx , maxx , miny , maxy , xyreso )
130
133
plt .show ()
131
134
132
135
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
135
141
plt .pcolor (data , vmax = 1.0 , cmap = plt .cm .Blues )
136
142
plt .axis ("equal" )
137
143
@@ -142,10 +148,10 @@ def main():
142
148
# obstacle positions
143
149
ox = [- 5.0 , 5.0 , 0.0 , 10.0 ]
144
150
oy = [0.0 , 5.0 , 10.0 , - 5.0 ]
145
- reso = 1.0
151
+ xyreso = 0.5
146
152
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 )
149
155
150
156
151
157
if __name__ == '__main__' :
0 commit comments