grid: take steering transitions into account.

src/nppilot/lib/grid.py

@@ -1,5 +1,6 @@
 import math
 import collections
+import random
 
 import numpy as np
 import matplotlib.patches
@@ -23,7 +24,8 @@ class Grid:
         return self._cells[p[0]][p[1]]
 
     def set(self, p: tuple, v: float) -> None:
-        self._cells[p[0]][p[1]] = v
+        if self.contains(p):
+            self._cells[p[0]][p[1]] = v
 
     def line(self, p: tuple, dp: tuple, c: int, v: float) -> None:
         for i in range(c):
@@ -38,16 +40,16 @@ class Grid:
                 if acc >= INF:
                     out[y][x] = acc
                 else:
-                    for d in (-1, 0), (1, 0), (0, 1), (0, -1):
+                    for d in DIRS.values():
                         v = self.get((y + d[0], x + d[1]))
                         if v >= INF:
-                            v = 1
+                            v = 10
                         acc += v
-                    out[y][x] = acc / 5
+                    out[y][x] = acc / len(DIRS)
         self._cells = out
 
     def contains(self, p):
-        y, x = p
+        y, x = p[:2]
         if y < 0 or x < 0:
             return False
         if y >= len(self._cells) or x >= len(self._cells[0]):
@@ -68,16 +70,22 @@ def plot(g: Grid, keepout: Grid):
             xx = x * s - s / 2
             if keepout is not None and keepout._cells[y][x] >= INF:
                 color = 'purple'
+            elif cell <= 0:
+                color = '#eeeeee'
             else:
                 color = cmap(cell)
             r = matplotlib.patches.Rectangle((xx, yy), w, w, facecolor=color, linewidth=s)
             ax.add_patch(r)
 
 
-def h(n, objective):
+def h_city_block(n, objective):
     return abs(n[0] - objective[0]) + abs(n[1] - objective[1])
 
 
+def h(n, objective):
+    return math.sqrt((n[0] - objective[0])**2 + (n[1] - objective[1])**2)
+
+
 def gf(g, p):
     v = g.get(p)
     if v <= 0:
@@ -90,63 +98,84 @@ def kf(p, keepout):
 
 
 DIRS = {
-    None: ((1, 0), (-1, 0), (0, 1), (0, -1)),
-    0: ((1, 0), (1, -1), (1, 1)),  # North
-    1: ((0, 1), (1, 1), (-1, 1)),  # East
-    2: ((-1, 0), (-1, -1), (-1, 1)),  # South
-    3: ((0, -1), (1, -1), (-1, -1)),  # West
+    0: (1, 0, 1),  # North
+    1: (1, 1, 1.4),  # NE
+    2: (0, 1, 1),  # East
+    3: (-1, 1, 1.4),  # SE
+    4: (-1, 0, 1),  # South
+    5: (-1, -1, 1.4),  # SW
+    6: (0, -1, 1),  # West
+    7: (1, -1, 1.4),  # NW
 }
 
 
-def plan(g: Grid, keepout: Grid, start, goal):
+def plan(gr: Grid, keepout: Grid, start, goal):
     closed = set()
     open = [start]
     came_from = {}
-    g.set(start, 0.001)
+    g = {start: 0}
 
     while open:
         c = open[0]
-        if c == goal:
+        open = open[1:]
+        if c[:2] == goal[:2]:
             path = []
-            p = goal
+            p = c
             while True:
                 path.append(p)
-                if p == start:
+                if p[:2] == start[:2]:
                     path.reverse()
                     return path
                 p = came_from[p]
-        open = open[1:]
         closed.add(c)
 
-        for d in DIRS[None]:
-            n = (c[0] + d[0], c[1] + d[1])
+        for d in (c[-1] - 1, c[-1], c[-1] + 1):
+            if d < 0:
+                d += 8
+            if d >= 8:
+                d -= 8
+            di = DIRS[d]
+            n = (c[0] + di[0], c[1] + di[1], d)
             if n in closed:
                 continue
-            if not g.contains(n):
+            if not keepout.contains(n):
                 continue
-            gn = g.get(c) + 1
+            if keepout.get(n) >= INF:
+                continue
+            gn = g[c] + di[-1]
+            if gr.get(n) < gn:
+                gr.set(n, gn)
             if n not in open:
                 open.append(n)
-            elif gn >= g.get(n):
+            elif gn >= g[n]:
                 continue
             came_from[n] = c
-            g.set(n, gn)
+            g[n] = gn
         open.sort(key=lambda x: gf(g, x) + h(x, goal) + kf(x, keepout))
 
 
 def main():
-    g = Grid(30, 30, 1)
-    keepout = Grid(30, 30, 1)
-    keepout.line((10, 0), (0, 1), 5, INF)
-    keepout.line((12, 12), (1, 0), 15, INF)
+    w, h = 30, 30
+    g = Grid(h, w, 1)
+    keepout = Grid(h, w, 1)
+
+    for i in range(20):
+        d = (1, 0)
+        if random.randint(0, 1) == 1:
+            d = (0, 1)
+        keepout.line((random.randint(0, h), random.randint(0, w)), d, random.randint(5, h // 2),
+                     INF)
     keepout.smear()
 
-    path = plan(g, keepout, (1, 2), (29, 18))
+    start = (random.randint(0, h), random.randint(0, w), 0)
+    goal = (random.randint(0, h), random.randint(0, w))
+    path = plan(g, keepout, start, goal)
     path = np.array(path)
+
     plot(g, keepout)
+    plt.tight_layout()
     plt.plot(path[:, 1], path[:, 0])
     plt.axis('equal')
-    #    plt.plot(X[:, 0, ], X[:, 1])
     plt.savefig('fig.png')
     plt.show()