Merge pull request #180 from zhengzh/master

hybrid a star

PathPlanning/HybridAStar/a_star.py

@@ -0,0 +1,241 @@
+"""
+
+A* grid based planning
+
+author: Atsushi Sakai(@Atsushi_twi)
+        Nikos Kanargias (nkana@tee.gr)
+
+See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
+
+"""
+
+import matplotlib.pyplot as plt
+import math
+import heapq
+
+# _round = round
+# def round(x):
+#     return int(_round(x))
+
+show_animation = False
+
+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 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 dp_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(ngoal, xw, minx, miny)] = ngoal
+    pq = []
+    pq.append((0, calc_index(ngoal, xw, minx, miny)))
+    
+
+    while 1:
+        if not pq:
+            break
+        cost, c_id = heapq.heappop(pq)
+        if c_id in openset:
+            current = openset[c_id]
+            closedset[c_id] = current
+            openset.pop(c_id)
+        else:
+            continue
+
+        # 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)
+
+        # Remove the item from the open set
+        
+
+        # 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.cost + motion[i][2], 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
+                heapq.heappush(pq, (node.cost, calc_index(node, xw, minx, miny)))
+            else:
+                if openset[n_id].cost >= node.cost:
+                    # This path is the best until now. record it!
+                    openset[n_id] = node
+                    heapq.heappush(pq, (node.cost, calc_index(node, xw, minx, miny)))
+
+
+    rx, ry = calc_final_path(closedset[calc_index(nstart, xw, minx, miny)], closedset, reso)
+
+    return rx, ry, closedset
+
+
+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 verify_node(node, obmap, minx, miny, maxx, maxy):
+
+    if node.x < minx:
+        return False
+    elif node.y < miny:
+        return False
+    elif node.x >= maxx:
+        return False
+    elif node.y >= maxy:
+        return False
+
+    if obmap[node.x][node.y]:
+        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 = 2.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)
+
+    if show_animation:  # pragma: no cover
+        plt.plot(ox, oy, ".k")
+        plt.plot(sx, sy, "xr")
+        plt.plot(gx, gy, "xb")
+        plt.grid(True)
+        plt.axis("equal")
+
+    rx, ry, _ = dp_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
+
+    if show_animation:  # pragma: no cover
+        plt.plot(rx, ry, "-r")
+        plt.show()
+
+
+if __name__ == '__main__':
+    show_animation = True
+    main()

PathPlanning/HybridAStar/car.py

@@ -0,0 +1,92 @@
+
+import matplotlib.pyplot as plt
+from math import sqrt, cos, sin, tan, pi
+
+WB = 3. # rear to front wheel
+W = 2. # width of car
+LF = 3.3 # distance from rear to vehicle front end
+LB = 1.0 # distance from rear to vehicle back end
+MAX_STEER = 0.6 #[rad] maximum steering angle 
+
+WBUBBLE_DIST = (LF-LB)/2.0
+WBUBBLE_R = sqrt(((LF+LB)/2.0)**2+1)
+
+# vehicle rectangle verticles
+VRX = [LF, LF, -LB, -LB, LF]
+VRY = [W/2,-W/2,-W/2,W/2,W/2]
+
+
+def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
+    for x, y, yaw in zip(xlist, ylist, yawlist):
+        cx = x + WBUBBLE_DIST*cos(yaw)
+        cy = y + WBUBBLE_DIST*sin(yaw)
+
+        ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R)
+
+        if not ids:
+            continue
+        
+        if not rectangle_check(x, y, yaw,
+                [ox[i] for i in ids], [oy[i] for i in ids]):
+            return False # collision
+    
+    return True # no collision
+
+def rectangle_check(x, y, yaw, ox, oy):
+    # transform obstacles to base link frame
+    c, s = cos(-yaw), sin(-yaw)
+    for iox, ioy in zip(ox, oy):
+        tx = iox - x
+        ty = ioy - y
+        rx = c*tx - s*ty
+        ry = s*tx + c*ty
+
+        if not (rx > LF or rx < -LB or ry > W/2.0 or ry < -W/2.0):
+            # print (x, y, yaw, iox, ioy, c, s, rx, ry)
+            return False # no collision
+    
+    return True # collision
+
+
+def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
+    """Plot arrow."""
+    if not isinstance(x, float):
+        for (ix, iy, iyaw) in zip(x, y, yaw):
+            plot_arrow(ix, iy, iyaw)
+    else:
+        plt.arrow(x, y, length * cos(yaw), length * sin(yaw),
+                  fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4)
+        # plt.plot(x, y)
+
+def plot_car(x, y, yaw):
+    car_color = '-k'
+    c, s = cos(yaw), sin(yaw)
+    
+    car_outline_x, car_outline_y = [], []
+    for rx, ry in zip(VRX, VRY):
+        tx = c*rx-s*ry + x
+        ty = s*rx+c*ry + y
+        car_outline_x.append(tx)
+        car_outline_y.append(ty)
+    
+    arrow_x, arrow_y, arrow_yaw = c*1.5+x, s*1.5+y, yaw
+    plot_arrow(arrow_x, arrow_y, arrow_yaw)
+    
+    plt.plot(car_outline_x, car_outline_y, car_color)
+
+def pi_2_pi(angle):
+    return (angle + pi) % (2 * pi) - pi
+
+def move(x, y, yaw, distance, steer, L=WB):
+    x += distance * cos(yaw)
+    y += distance * sin(yaw)
+    yaw += pi_2_pi(distance * tan(steer) / L) # distance/2
+
+    return x, y, yaw
+
+if __name__ == '__main__':
+    x, y, yaw = 0., 0., 1.
+    plt.axis('equal')
+    plot_car(x, y, yaw)
+    plt.show()
+    

PathPlanning/HybridAStar/hybrid_a_star.py

@@ -0,0 +1,470 @@
+"""
+
+Hybrid A* path planning
+
+author: Atsushi Sakai (@Atsushi_twi)
+        Zheng Zh (@Zhengzh)
+
+"""
+
+import sys
+sys.path.append("../ReedsSheppPath/")
+
+import math
+import numpy as np
+import scipy.spatial
+import matplotlib.pyplot as plt
+import reeds_shepp_path_planning as rs
+import heapq
+from car import move, check_car_collision, MAX_STEER, WB, plot_car
+from a_star import dp_planning #, calc_obstacle_map
+
+XY_GRID_RESOLUTION = 2.0 #[m]
+YAW_GRID_RESOLUTION = np.deg2rad(15.0) #[rad]
+GOAL_TYAW_TH = np.deg2rad(5.0) #[rad]
+MOTION_RESOLUTION = 0.1 #[m] path interporate resolution
+N_STEER = 20.0 # number of steer command
+EXTEND_AREA = 0.0  # [m]
+H_COST = 1.0
+MOTION_RESOLUTION = 0.1
+SKIP_COLLISION_CHECK = 4
+VR = 1.0 # robot radius
+
+SB_COST = 100.0 # switch back penalty cost
+BACK_COST = 5.0 # backward penalty cost
+STEER_CHANGE_COST = 5.0 # steer angle change penalty cost
+STEER_COST = 1.0 # steer angle change penalty cost
+# JACKKNIF_COST= 200.0 # Jackknif cost
+H_COST = 5.0 # Heuristic cost
+
+show_animation = True
+
+# _round = round
+# def round(x):
+#     return int(_round(x))
+
+class Node:
+
+    def __init__(self, xind, yind, yawind, direction, xlist, ylist, yawlist, directions, steer=0.0, pind=None, cost=None):
+        self.xind = xind
+        self.yind = yind
+        self.yawind = yawind
+        self.direction = direction
+        self.xlist = xlist
+        self.ylist = ylist
+        self.yawlist = yawlist
+        self.directions = directions
+        self.steer = steer
+        self.pind = pind
+        self.cost = cost
+
+class Path:
+
+    def __init__(self, xlist, ylist, yawlist, directionlist, cost):
+        self.xlist = xlist
+        self.ylist = ylist
+        self.yawlist = yawlist
+        self.directionlist = directionlist
+        self.cost = cost
+
+
+class KDTree:
+    """
+    Nearest neighbor search class with KDTree
+    """
+
+    def __init__(self, data):
+        # store kd-tree
+        self.tree = scipy.spatial.cKDTree(data)
+
+    def search(self, inp, k=1):
+        """
+        Search NN
+        inp: input data, single frame or multi frame
+        """
+
+        if len(inp.shape) >= 2:  # multi input
+            index = []
+            dist = []
+
+            for i in inp.T:
+                idist, iindex = self.tree.query(i, k=k)
+                index.append(iindex)
+                dist.append(idist)
+
+            return index, dist
+        
+        dist, index = self.tree.query(inp, k=k)
+        return index, dist
+
+    def search_in_distance(self, inp, r):
+        """
+        find points with in a distance r
+        """
+
+        index = self.tree.query_ball_point(inp, r)
+        return index
+
+
+class Config:
+
+    def __init__(self, ox, oy, xyreso, yawreso):
+        min_x_m = min(ox) #- EXTEND_AREA
+        min_y_m = min(oy) #- EXTEND_AREA
+        max_x_m = max(ox) #+ EXTEND_AREA
+        max_y_m = max(oy) #+ EXTEND_AREA
+
+        ox.append(min_x_m)
+        oy.append(min_y_m)
+        ox.append(max_x_m)
+        oy.append(max_y_m)
+
+        self.minx = round(min_x_m / xyreso)
+        self.miny = round(min_y_m / xyreso)
+        self.maxx = round(max_x_m / xyreso)
+        self.maxy = round(max_y_m / xyreso)
+
+        self.xw = round(self.maxx - self.minx)
+        self.yw = round(self.maxy - self.miny)
+
+        self.minyaw = round(- math.pi / yawreso) - 1
+        self.maxyaw = round(math.pi / yawreso)
+        self.yaww = round(self.maxyaw - self.minyaw)
+
+
+def show_expansion_tree():
+    pass
+
+
+def calc_motion_inputs():
+    
+    for steer in np.linspace(-MAX_STEER, MAX_STEER, N_STEER):
+        for d in [1, -1]:
+            yield [steer, d]         
+
+
+def get_neighbors(current, config, ox, oy, kdtree):
+
+    for steer, d in calc_motion_inputs():
+        node = calc_next_node(current, steer, d, config, ox, oy, kdtree)
+        if node and verify_index(node, config):
+            yield node
+
+def calc_next_node(current, steer, direction, config, ox, oy, kdtree):
+    
+    x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1]
+
+    arc_l = XY_GRID_RESOLUTION * 1.5
+    xlist, ylist, yawlist = [], [], []
+    for dist in np.arange(0, arc_l, MOTION_RESOLUTION):
+        x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION*direction, steer)
+        xlist.append(x)
+        ylist.append(y)
+        yawlist.append(yaw)
+    
+    # plt.plot(xlist, ylist)
+    if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree):
+        return None
+
+    d = direction==1
+    xind = round(x/XY_GRID_RESOLUTION)
+    yind = round(y/XY_GRID_RESOLUTION)
+    yawind = round(yaw/YAW_GRID_RESOLUTION)
+    
+    addedcost = 0.0
+
+    if d != current.direction:
+        addedcost += SB_COST
+    
+    # steer penalty
+    addedcost += STEER_COST*abs(steer)
+
+    # steer change penalty
+    addedcost += STEER_CHANGE_COST*abs(current.steer-steer)
+
+    cost = current.cost + addedcost + arc_l   
+
+    node = Node(xind, yind, yawind, d, xlist,
+                ylist, yawlist, [d],
+                pind=calc_index(current, config),
+                cost=cost, steer=steer)
+
+    return node
+
+
+def is_same_grid(n1, n2):
+    if n1.xind==n2.xind and n1.yind==n2.yind and n1.yawind==n2.yawind:
+        return True
+    return False
+
+def analytic_expantion(current, goal, c, ox, oy, kdtree):
+
+    sx = current.xlist[-1]
+    sy = current.ylist[-1]
+    syaw = current.yawlist[-1]
+
+    gx = goal.xlist[-1]
+    gy = goal.ylist[-1]
+    gyaw = goal.yawlist[-1]
+
+    max_curvature = math.tan(MAX_STEER)/WB
+    paths = rs.calc_paths(sx,sy,syaw,gx, gy, gyaw,
+                            max_curvature, step_size=MOTION_RESOLUTION)
+
+    if not paths:
+        return None
+
+    best_path, best = None, None
+
+    for path in paths:
+        if check_car_collision(path.x, path.y, path.yaw, ox, oy, kdtree):
+            cost = calc_rs_path_cost(path)
+            if not best or best > cost:
+                best = cost
+                best_path = path
+  
+    return best_path
+
+
+def update_node_with_analystic_expantion(current, goal, 
+                                        c, ox, oy, kdtree):
+    apath = analytic_expantion(current, goal, c, ox,oy, kdtree)
+
+    if apath:
+        plt.plot(apath.x, apath.y)
+        fx = apath.x[1:]
+        fy = apath.y[1:]
+        fyaw = apath.yaw[1:]
+
+        fcost = current.cost + calc_rs_path_cost(apath)
+        fpind = calc_index(current, c)
+
+        fd = []
+        for d in apath.directions[1:]:
+            fd.append(d>=0)
+        
+        fsteer = 0.0
+        fpath = Node(current.xind, current.yind, current.yawind, 
+            current.direction, fx, fy, fyaw, fd, cost=fcost, pind=fpind, steer=fsteer)
+        return True, fpath
+
+    return False, None
+
+
+def calc_rs_path_cost(rspath):
+
+    cost = 0.0
+    for l in rspath.lengths:
+        if l >= 0: # forward
+            cost += l
+        else: # back
+            cost += abs(l) * BACK_COST
+
+    # swich back penalty
+    for i in range(len(rspath.lengths)-1):
+        if rspath.lengths[i] * rspath.lengths[i+1] < 0.0: # switch back
+            cost += SB_COST
+
+    # steer penalyty
+    for ctype in rspath.ctypes:
+        if ctype != "S": # curve
+            cost += STEER_COST*abs(MAX_STEER)
+
+    # ==steer change penalty
+    # calc steer profile
+    nctypes = len(rspath.ctypes)
+    ulist = [0.0]*nctypes
+    for i in range(nctypes):
+        if rspath.ctypes[i] == "R" :
+            ulist[i] = - MAX_STEER
+        elif rspath.ctypes[i] == "L":
+            ulist[i] = MAX_STEER
+ 
+    for i in range(len(rspath.ctypes)-1):
+        cost += STEER_CHANGE_COST*abs(ulist[i+1] - ulist[i])
+
+    return cost
+
+
+def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso):
+    """
+    start
+    goal
+    ox: x position list of Obstacles [m]
+    oy: y position list of Obstacles [m]
+    xyreso: grid resolution [m]
+    yawreso: yaw angle resolution [rad]
+    """
+
+    start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
+    tox, toy = ox[:], oy[:]
+
+    obkdtree = KDTree(np.vstack((tox, toy)).T)
+
+    config = Config(tox, toy, xyreso, yawreso)
+
+    nstart = Node(round(start[0] / xyreso), round(start[1] / xyreso), round(start[2] / yawreso),
+                  True, [start[0]], [start[1]], [start[2]], [True], cost=0)
+    ngoal = Node(round(goal[0] / xyreso), round(goal[1] / xyreso), round(goal[2] / yawreso),
+                 True, [goal[0]], [goal[1]], [goal[2]], [True])
+
+    openList, closedList = {}, {}
+
+    _, _, h_dp = dp_planning(nstart.xlist[-1], nstart.ylist[-1],
+            ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR)
+
+    pq = []
+    openList[calc_index(nstart, config)] = nstart
+    heapq.heappush(pq, (calc_cost(nstart, h_dp, ngoal, config), calc_index(nstart, config)))
+
+    while True:
+        if not openList:
+            print("Error: Cannot find path, No open set")
+            return [], [], []
+
+        cost, c_id = heapq.heappop(pq)
+        # if openList.has_key(c_id): # python 2.7
+        if c_id in openList:
+            current = openList.pop(c_id)
+            closedList[c_id] = current
+        else:
+            continue
+
+        if show_animation:  # pragma: no cover
+            plt.plot(current.xlist[-1], current.ylist[-1], "xc")
+            if len(closedList.keys()) % 10 == 0:
+                plt.pause(0.001)
+
+        isupdated, fpath = update_node_with_analystic_expantion(
+            current, ngoal, config, ox, oy, obkdtree)
+
+        if isupdated:
+            break
+        
+        for neighbor in get_neighbors(current, config, ox, oy, obkdtree):
+            neighbor_index = calc_index(neighbor, config)
+            # if closedList.has_key(neighbor_index):
+            if neighbor_index in closedList:
+                continue
+            if not neighbor in openList \
+                or openList[neighbor_index].cost > neighbor.cost:
+                heapq.heappush(pq, (calc_cost(neighbor, h_dp, ngoal, config), neighbor_index))
+                openList[neighbor_index] = neighbor
+        
+
+    path = get_final_path(closedList, fpath, nstart, config)
+    return path
+
+def calc_cost(n, h_dp, goal, c):
+    ind = (n.yind - c.miny) * c.xw + (n.xind - c.minx)
+    if not ind in h_dp:
+        return (n.cost + 999999999) # collision cost
+    return (n.cost + H_COST*h_dp[ind].cost)
+
+def get_final_path(closed, ngoal, nstart, config):
+    rx, ry, ryaw = list(reversed(ngoal.xlist)), list(reversed(ngoal.ylist)), list(reversed(ngoal.yawlist))
+    direction = list(reversed(ngoal.directions))
+    nid = ngoal.pind
+    finalcost = ngoal.cost
+
+    while nid:
+        n = closed[nid]
+        rx.extend(list(reversed(n.xlist)))
+        ry.extend(list(reversed(n.ylist)))
+        ryaw.extend(list(reversed(n.yawlist)))
+        direction.extend(list(reversed(n.directions)))
+       
+        nid = n.pind
+
+    rx = list(reversed(rx))
+    ry = list(reversed(ry))
+    ryaw = list(reversed(ryaw))
+    direction = list(reversed(direction))
+
+    # adjuct first direction
+    direction[0] = direction[1]
+
+    path = Path(rx, ry, ryaw, direction, finalcost)
+
+    return path
+
+
+def verify_index(node, c):
+    xind, yind = node.xind, node.yind
+    if xind>=c.minx and xind<=c.maxx and yind >=c.miny \
+        and yind<=c.maxy:
+        return True
+    
+    return False
+
+
+def calc_index(node, c):
+    ind = (node.yawind - c.minyaw) * c.xw * c.yw + \
+        (node.yind - c.miny) * c.xw + (node.xind - c.minx)
+
+    if ind <= 0:
+        print("Error(calc_index):", ind)
+
+    return ind
+
+
+def main():
+    print("Start Hybrid A* planning")
+
+    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)
+
+    # Set Initial parameters
+    start = [10.0, 10.0, np.deg2rad(90.0)]
+    goal = [50.0, 50.0, np.deg2rad(-90.0)]
+
+    plt.plot(ox, oy, ".k")
+    rs.plot_arrow(start[0], start[1], start[2], fc='g')
+    rs.plot_arrow(goal[0], goal[1], goal[2])
+
+    plt.grid(True)
+    plt.axis("equal")
+
+    path = hybrid_a_star_planning(
+        start, goal, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION)
+    
+    # plot_car(*start)
+    # plot_car(*goal)
+    x = path.xlist
+    y = path.ylist
+    yaw = path.yawlist
+    # direction = path.directionlist
+
+    for ix, iy, iyaw in zip(x, y, yaw):
+        plt.cla()
+        plt.plot(ox, oy, ".k")
+        plt.plot(x, y, "-r", label="Hybrid A* path")
+        plt.grid(True)
+        plt.axis("equal")
+        plot_car(ix, iy, iyaw)
+        plt.pause(0.0001)
+
+    plt.show()
+    print(__file__ + " start!!")
+
+
+if __name__ == '__main__':
+    main()