add a star code and animation

.gitmodules

@@ -13,3 +13,6 @@
 [submodule "PathPlanning/Dijkstra/matplotrecorder"]
 	path = PathPlanning/Dijkstra/matplotrecorder
 	url = https://github.com/AtsushiSakai/matplotrecorder
+[submodule "PathPlanning/AStar/matplotrecorder"]
+	path = PathPlanning/AStar/matplotrecorder
+	url = https://github.com/AtsushiSakai/matplotrecorder

PathPlanning/AStar/a_star.py

@@ -0,0 +1,225 @@
+"""
+A* grid based planning
+
+author: Atsushi Sakai
+"""
+
+import matplotlib.pyplot as plt
+import math
+from matplotrecorder import matplotrecorder
+matplotrecorder.donothing = True
+
+
+class Node:
+
+    def __init__(self, x, y, cost, pind):
+        self.x = x
+        self.y = y
+        self.cost = cost
+        self.pind = pind
+
+    def __str__(self):
+        return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
+
+
+def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
+    """
+    gx: goal x position [m]
+    gx: goal x position [m]
+    ox: x position list of Obstacles [m]
+    oy: y position list of Obstacles [m]
+    reso: grid resolution [m]
+    rr: robot radius[m]
+    """
+
+    nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
+    ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
+    ox = [iox / reso for iox in ox]
+    oy = [ioy / reso for ioy in oy]
+
+    obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, 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_h(ngoal, openset[o].x, openset[o].y))
+        current = openset[c_id]
+        #  print("current", current)
+
+        # show graph
+        plt.plot(current.x, current.y, "xc")
+        if len(closedset.keys()) % 10 == 0:
+            plt.pause(0.001)
+            matplotrecorder.save_frame()
+
+        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 range(len(motion)):
+            node = Node(current.x + motion[i][0], current.y + motion[i][1],
+                        current.cost + motion[i][2], c_id)
+            n_id = calc_index(node, xw, minx, miny)
+
+            if not verify_node(node, obmap, minx, miny, maxx, maxy):
+                continue
+
+            if n_id in closedset:
+                continue
+            # Otherwise if it is already in the open set
+            if n_id in openset:
+                if openset[n_id].cost > node.cost:
+                    openset[n_id].cost = node.cost
+                    openset[n_id].pind = c_id
+            else:
+                openset[n_id] = node
+
+    # generate final course
+    rx, ry = [ngoal.x], [ngoal.y]
+    pind = ngoal.pind
+    while pind != -1:
+        n = closedset[pind]
+        rx.append(n.x)
+        ry.append(n.y)
+        pind = n.pind
+
+    return rx, ry
+
+
+def calc_h(ngoal, x, y):
+    w = 10.0  # weight of heuristic
+    d = w * math.sqrt((ngoal.x - x)**2 + (ngoal.y - y)**2)
+    return d
+
+
+def verify_node(node, obmap, minx, miny, maxx, maxy):
+
+    if obmap[node.x][node.y]:
+        return False
+
+    if node.x < minx:
+        return False
+    elif node.y < miny:
+        return False
+    elif node.x > maxx:
+        return False
+    elif node.y > maxy:
+        return False
+
+    return True
+
+
+def calc_obstacle_map(ox, oy, reso, vr):
+
+    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(xwidth)] for i in range(ywidth)]
+    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 <= vr / reso:
+                    obmap[ix][iy] = True
+                    break
+
+    return obmap, minx, miny, maxx, maxy, xwidth, ywidth
+
+
+def calc_index(node, xwidth, xmin, ymin):
+    return (node.y - ymin) * xwidth + (node.x - xmin)
+
+
+def get_motion_model():
+    # dx, dy, cost
+    motion = [[1, 0, 1],
+              [0, 1, 1],
+              [-1, 0, 1],
+              [0, -1, 1],
+              [-1, -1, math.sqrt(2)],
+              [-1, 1, math.sqrt(2)],
+              [1, -1, math.sqrt(2)],
+              [1, 1, math.sqrt(2)]]
+
+    return motion
+
+
+def main():
+    print(__file__ + " start!!")
+
+    # start and goal position
+    sx = 10.0  # [m]
+    sy = 10.0  # [m]
+    gx = 50.0  # [m]
+    gy = 50.0  # [m]
+    grid_size = 1.0  # [m]
+    robot_size = 1.0  # [m]
+
+    ox = []
+    oy = []
+
+    for i in range(60):
+        ox.append(i)
+        oy.append(0.0)
+    for i in range(60):
+        ox.append(60.0)
+        oy.append(i)
+    for i in range(61):
+        ox.append(i)
+        oy.append(60.0)
+    for i in range(61):
+        ox.append(0.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(20.0)
+        oy.append(i)
+    for i in range(40):
+        ox.append(40.0)
+        oy.append(60.0 - i)
+
+    plt.plot(ox, oy, ".k")
+    plt.plot(sx, sy, "xr")
+    plt.plot(gx, gy, "xb")
+    plt.grid(True)
+    plt.axis("equal")
+
+    rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
+
+    plt.plot(rx, ry, "-r")
+
+    for i in range(20):
+        matplotrecorder.save_frame()
+    plt.show()
+
+    matplotrecorder.save_movie("animation.gif", 0.1)
+
+
+if __name__ == '__main__':
+    main()