diff --git a/PathPlanning/HybridAStar/a_star.py b/PathPlanning/HybridAStar/a_star.py index ada80e1c..7f26640f 100644 --- a/PathPlanning/HybridAStar/a_star.py +++ b/PathPlanning/HybridAStar/a_star.py @@ -2,23 +2,19 @@ A* grid based planning -author: Atsushi Sakai(@Atsushi_twi) - Nikos Kanargias (nkana@tee.gr) +author: 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)) +import matplotlib.pyplot as plt show_animation = False + class Node: def __init__(self, x, y, cost, pind): @@ -67,7 +63,6 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): openset[calc_index(ngoal, xw, minx, miny)] = ngoal pq = [] pq.append((0, calc_index(ngoal, xw, minx, miny))) - while 1: if not pq: @@ -87,7 +82,6 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): plt.pause(0.001) # Remove the item from the open set - # expand search grid based on motion model for i, _ in enumerate(motion): @@ -104,15 +98,17 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): 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))) + 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))) + 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) + rx, ry = calc_final_path(closedset[calc_index( + nstart, xw, minx, miny)], closedset, reso) return rx, ry, closedset diff --git a/PathPlanning/HybridAStar/car.py b/PathPlanning/HybridAStar/car.py index 2701d0da..10b7a296 100644 --- a/PathPlanning/HybridAStar/car.py +++ b/PathPlanning/HybridAStar/car.py @@ -1,36 +1,45 @@ +""" + +Car model for Hybrid A* path planning + +author: Zheng Zh (@Zhengzh) + +""" + 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 +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) +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] +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) + 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 + [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 @@ -38,14 +47,14 @@ def rectangle_check(x, y, yaw, ox, oy): for iox, ioy in zip(ox, oy): tx = iox - x ty = ioy - y - rx = c*tx - s*ty - ry = s*tx + c*ty + 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): + 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 + return False # no collision + + return True # collision def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): @@ -58,35 +67,38 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): 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 + 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 + + 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 + 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() - \ No newline at end of file + plt.show() \ No newline at end of file diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 1c9a3988..fb0e8a77 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -2,40 +2,43 @@ Hybrid A* path planning -author: Atsushi Sakai (@Atsushi_twi) - Zheng Zh (@Zhengzh) +author: Zheng Zh (@Zhengzh) """ +import heapq +import matplotlib.pyplot as plt +import scipy.spatial +import numpy as np +import math + import sys sys.path.append("../ReedsSheppPath/") +try: + from a_star import dp_planning # , calc_obstacle_map + import reeds_shepp_path_planning as rs + from car import move, check_car_collision, MAX_STEER, WB, plot_car +except: + raise -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 +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 +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 +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 +H_COST = 5.0 # Heuristic cost show_animation = True @@ -43,6 +46,7 @@ show_animation = True # 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): @@ -58,6 +62,7 @@ class Node: self.pind = pind self.cost = cost + class Path: def __init__(self, xlist, ylist, yawlist, directionlist, cost): @@ -93,7 +98,7 @@ class KDTree: dist.append(idist) return index, dist - + dist, index = self.tree.query(inp, k=k) return index, dist @@ -109,10 +114,10 @@ class KDTree: 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 + 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) @@ -137,10 +142,10 @@ def show_expansion_tree(): def calc_motion_inputs(): - + for steer in np.linspace(-MAX_STEER, MAX_STEER, N_STEER): for d in [1, -1]: - yield [steer, d] + yield [steer, d] def get_neighbors(current, config, ox, oy, kdtree): @@ -150,39 +155,40 @@ def get_neighbors(current, 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) + 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) - + 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) + addedcost += STEER_COST * abs(steer) # steer change penalty - addedcost += STEER_CHANGE_COST*abs(current.steer-steer) + addedcost += STEER_CHANGE_COST * abs(current.steer - steer) - cost = current.cost + addedcost + arc_l + cost = current.cost + addedcost + arc_l node = Node(xind, yind, yawind, d, xlist, ylist, yawlist, [d], @@ -193,10 +199,11 @@ def calc_next_node(current, steer, direction, config, ox, oy, kdtree): def is_same_grid(n1, n2): - if n1.xind==n2.xind and n1.yind==n2.yind and n1.yawind==n2.yawind: + 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] @@ -207,9 +214,9 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree): 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) + 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 @@ -222,13 +229,13 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree): 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) +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) @@ -241,11 +248,11 @@ def update_node_with_analystic_expantion(current, goal, fd = [] for d in apath.directions[1:]: - fd.append(d>=0) - + 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) + 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 @@ -255,33 +262,33 @@ def calc_rs_path_cost(rspath): cost = 0.0 for l in rspath.lengths: - if l >= 0: # forward + if l >= 0: # forward cost += l - else: # back + 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 + 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) + if ctype != "S": # curve + cost += STEER_COST * abs(MAX_STEER) # ==steer change penalty # calc steer profile nctypes = len(rspath.ctypes) - ulist = [0.0]*nctypes + ulist = [0.0] * nctypes for i in range(nctypes): - if rspath.ctypes[i] == "R" : + 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]) + + for i in range(len(rspath.ctypes) - 1): + cost += STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i]) return cost @@ -311,11 +318,12 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): openList, closedList = {}, {} _, _, h_dp = dp_planning(nstart.xlist[-1], nstart.ylist[-1], - ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR) + 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))) + heapq.heappush(pq, (calc_cost(nstart, h_dp, ngoal, config), + calc_index(nstart, config))) while True: if not openList: @@ -340,29 +348,32 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): 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)) + 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) + 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)) + 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 @@ -373,7 +384,7 @@ def get_final_path(closed, ngoal, nstart, config): 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)) @@ -391,10 +402,10 @@ def get_final_path(closed, ngoal, nstart, config): 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: + if xind >= c.minx and xind <= c.maxx and yind >= c.miny \ + and yind <= c.maxy: return True - + return False @@ -445,7 +456,7 @@ def main(): path = hybrid_a_star_planning( start, goal, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION) - + # plot_car(*start) # plot_car(*goal) x = path.xlist