keep implementation

Author: AtsushiSakai

Mapping/grid_map/grid_map.py

@@ -14,14 +14,14 @@
 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 @@ def __str__(self):
         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__':