Browse Source

Update to implement 3d astar from 2d astar

Marc Savioz 1 year ago
parent
commit
a447f04de8
2 changed files with 175 additions and 30 deletions
  1. BIN
      software/Active_Obstacles.kmz
  2. 175 30
      software/astar.py

BIN
software/Active_Obstacles.kmz


+ 175 - 30
software/astar.py

@@ -1,12 +1,175 @@
+import math
 import numpy as np
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
 from scipy import signal
+class Node:
 
-goal = (69, 75)
-start = (2, 43)
+    def __init__(self, x, y, z, cost, pind):
+        self.x = x
+        self.y = y
+        self.z = z
+        self.cost = cost
+        self.pind = pind
+
+    def __str__(self):
+        return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
+
+def calc_final_path(ngoal, closedset, reso):
+    # generate final course
+    rx, ry = [ngoal.x * reso], [ngoal.y * reso]
+    pind = ngoal.pind
+    while pind != -1:
+        n = closedset[pind]
+        rx.append(n.x * reso)
+        ry.append(n.y * reso)
+        pind = n.pind
+
+    return rx, ry
+
+
+def calc_heuristic(n1, n2):
+    w = 1.0  # weight of heuristic
+    d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
+    return d
+
+def calc_index(node, xwidth, xmin, ymin):
+    return (node.y - ymin) * xwidth + (node.x - xmin)
+
+def get_motion_model():
+    # Always go forward one step (1) so cost is
+    # dx, dy, dz, cost
+    motion = [
+            # z = 0
+            [0, 1,  0, 1],
+            [1, 0,  0, 1],
+            [-1, 0, 0, 1],
+            [0, -1, 0, 1],
+            [-1, -1,0, math.sqrt(2)],
+            [1, 1,  0, math.sqrt(2)],
+            [-1, 1, 0, math.sqrt(2)],
+            [1, -1, 0, math.sqrt(2)],
+
+            # z = 1
+            [0, 1,  0.1, math.sqrt(2)],
+            [1, 0,  0.1, math.sqrt(2)],
+            [-1, 0, 0.1, math.sqrt(2)],
+            [0, -1, 0.1, math.sqrt(2)],
+            [-1, -1,0.1, math.sqrt(math.sqrt(2)+1)],
+            [1, 1,  0.1, math.sqrt(math.sqrt(2)+1)],
+            [-1, 1, 0.1, math.sqrt(math.sqrt(2)+1)],
+            [1,-1,  0.1,  math.sqrt(math.sqrt(2)+1)],
+
+            # z = -1
+            [0, 1,  -0.1, math.sqrt(2)],
+            [1, 0,  -0.1, math.sqrt(2)],
+            [-1, 0, -0.1, math.sqrt(2)],
+            [0, -1, -0.1, math.sqrt(2)],
+            [-1, -1,-0.1, math.sqrt(math.sqrt(2)+1)],
+            [1, 1,  -0.1, math.sqrt(math.sqrt(2)+1)],
+            [-1, 1, -0.1, math.sqrt(math.sqrt(2)+1)],
+            [1,-1,  -0.1,  math.sqrt(math.sqrt(2)+1)],
+
+            ]
+    return motion
+
+def calc_obstacle_map(ox, oy, oz, reso, rr):
+    minx = round(min(ox))
+    miny = round(min(oy))
+    maxx = round(max(ox))
+    maxy = round(max(oy))
+    #  print("minx:", minx)
+    #  print("miny:", miny)
+    #  print("maxx:", maxx)
+    #  print("maxy:", maxy)
+
+    xwidth = round(maxx - minx)
+    ywidth = round(maxy - miny)
+    #  print("xwidth:", xwidth)
+    #  print("ywidth:", ywidth)
+
+    # obstacle map generation
+    obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
+    for ix in range(xwidth):
+        x = ix + minx
+        for iy in range(ywidth):
+            y = iy + miny
+            #  print(x, y)
+            for iox, ioy in zip(ox, oy):
+                d = math.sqrt((iox - x)**2 + (ioy - y)**2)
+                if d <= rr / reso:
+                    obmap[ix][iy] = True
+                    break
+
+    return obmap, minx, miny, maxx, maxy, xwidth, ywidth
+
+
+def astar(sx, sy, sz, gx, gy, gz, ox, oy, oz, reso, rr):
+    nstart = Node(round(sx/reso), round(sy/reso), round(sz/reso), 0.0, -1)
+    ngoal = Node(round(sx/reso), round(sy/reso), round(sz/reso), 0.0, -1)
+    ox = [iox / reso for iox in ox]
+    oy = [ioy / reso for ioy in oy]
+    oz = [ioz / reso for ioz in oz]
+
+    obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, oz, reso, rr)
+
+    motion = get_motion_model()
+
+    openset, closedset = dict(), dict()
+    openset[calc_index(nstart, xw, minx, miny)] = nstart
+
+    while 1:
+        c_id = min(
+            openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
+        current = openset[c_id]
+
+        # show graph
+        if show_animation:  # pragma: no cover
+            plt.plot(current.x * reso, current.y * reso, "xc")
+            if len(closedset.keys()) % 10 == 0:
+                plt.pause(0.001)
+
+        if current.x == ngoal.x and current.y == ngoal.y:
+            print("Find goal")
+            ngoal.pind = current.pind
+            ngoal.cost = current.cost
+            break
+
+        # Remove the item from the open set
+        del openset[c_id]
+        # Add it to the closed set
+        closedset[c_id] = current
+
+        # expand search grid based on motion model
+        for i, _ in enumerate(motion):
+            node = Node(current.x + motion[i][0],
+                        current.y + motion[i][1],
+                        current.z + motion[i][2],
+                        current.cost + motion[i][3], c_id)
+            n_id = calc_index(node, xw, minx, miny)
+
+            if n_id in closedset:
+                continue
+
+            if not verify_node(node, obmap, minx, miny, maxx, maxy):
+                continue
+
+            if n_id not in openset:
+                openset[n_id] = node  # Discover a new node
+            else:
+                if openset[n_id].cost >= node.cost:
+                    # This path is the best until now. record it!
+                    openset[n_id] = node
+
+    rx, ry = calc_final_path(ngoal, closedset, reso)
+
+    return rx, ry
+
+show_animation = True
+goal = (22, 30)
+start = (2, 32)
 # Set up grid and test data
-nx, ny = 128, 128
+nx, ny = 64, 64
 x = range(nx)
 y = range(ny)
 dem1 = np.random.rand(nx,ny)
@@ -34,39 +197,21 @@ for x in range(0, len(demSmooth)):
         oy.append(y)
         oz.append(demSmooth[x, y])
 
+if show_animation:  # pragma: no cover
+    plt.plot(ox, oy, ".k")
+    plt.plot(start[0], start[1], "xr")
+    plt.plot(goal[0], goal[1], "xb")
+    plt.axis("equal")
+rx, ry = astar(start[0], start[1], demSmooth[start[0], start[1]], goal[0], goal[1], demSmooth[goal[0], goal[1]], ox, oy, oz, 1, 1)
 
-plt.show()
+if show_animation:  # pragma: no cover
+    plt.plot(rx, ry, "-r")
+    plt.show()
 
 
-def get_motion_model():
-    # Always go forward one step so cost is
-    # dlateral, dz, cost
-    motion = [
-            [-1, -1, math.sqrt(math.sqrt(2)+1)],
-            [-1,  0,  math.sqrt(2)],
-            [-1,  1,  math.sqrt(math.sqrt(2)+1)],
-            [0,  -1,  math.sqrt(2)],
-            [0,   0,  1],
-            [0,   1,  math.sqrt(math.sqrt(2)+1)],
-            [1,   -1, math.sqrt(math.sqrt(2)+1)],
-            [1,   0,  math.sqrt(2)],
-            [1,   1,  math.sqrt(math.sqrt(2)+1)],
-            ]
-    return motion
 
 def firstpass():
     """
     Gets height across a 150 meters wide section between the start and the goal.
     """
     return None
-
-
-
-def astar(sx, sy, sz, gx, gy, gz, ox, oy, oz, reso, rr):
-    nstart = Node(round(sx/reso), round(sy/reso), 0.0, -1)
-    ngoal = Node(round(sx/reso), round(sy/reso), 0.0, -1)
-
-    return None
-
-
-