From a382689a30aeb822039c01609565c4260cb9e4b6 Mon Sep 17 00:00:00 2001 From: zks Date: Sun, 24 Mar 2019 10:03:37 +0800 Subject: [PATCH 1/3] hybrid a star --- PathPlanning/HybridAStar/a_star.py | 241 +++++++++++ PathPlanning/HybridAStar/car.py | 92 +++++ PathPlanning/HybridAStar/hybrid_a_star.py | 470 ++++++++++++++++++++++ 3 files changed, 803 insertions(+) create mode 100644 PathPlanning/HybridAStar/a_star.py create mode 100644 PathPlanning/HybridAStar/car.py create mode 100644 PathPlanning/HybridAStar/hybrid_a_star.py diff --git a/PathPlanning/HybridAStar/a_star.py b/PathPlanning/HybridAStar/a_star.py new file mode 100644 index 00000000..f5753edc --- /dev/null +++ b/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 openset.has_key(c_id): + 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() \ No newline at end of file diff --git a/PathPlanning/HybridAStar/car.py b/PathPlanning/HybridAStar/car.py new file mode 100644 index 00000000..e7d35e99 --- /dev/null +++ b/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 len(ids) == 0: + 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() + \ No newline at end of file diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py new file mode 100644 index 00000000..35af0c94 --- /dev/null +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -0,0 +1,470 @@ +""" + +Hybrid A* path planning + +author: Atsushi Sakai (@Atsushi_twi) + +""" + +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 + else: + 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): + # import ipdb; ipdb.set_trace() + 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): + # import ipdb; ipdb.set_trace() + 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 len(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 # no update + + +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 = [] + + _, _, 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): + current = openList.pop(c_id) + closedList[c_id] = current + + 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): + continue + if not openList.has_key(neighbor_index) \ + or openList[neighbor_index].cost > neighbor.cost: # TODO huristic + heapq.heappush(pq, (calc_cost(neighbor, h_dp, ngoal, config), neighbor_index)) + openList[neighbor_index] = neighbor + + # print(current) + + rx, ry, ryaw = [], [], [] + + 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 h_dp.has_key(ind): + 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() From e102b5dd5f718b9eafffac2fa0198851bc353fd5 Mon Sep 17 00:00:00 2001 From: zks Date: Sun, 24 Mar 2019 14:09:24 +0800 Subject: [PATCH 2/3] hybrid_a_star python 3.5 --- PathPlanning/HybridAStar/a_star.py | 8 ++--- PathPlanning/HybridAStar/car.py | 2 +- PathPlanning/HybridAStar/hybrid_a_star.py | 38 +++++++++++------------ 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/PathPlanning/HybridAStar/a_star.py b/PathPlanning/HybridAStar/a_star.py index f5753edc..ada80e1c 100644 --- a/PathPlanning/HybridAStar/a_star.py +++ b/PathPlanning/HybridAStar/a_star.py @@ -13,9 +13,9 @@ import matplotlib.pyplot as plt import math import heapq -_round = round -def round(x): - return int(_round(x)) +# _round = round +# def round(x): +# return int(_round(x)) show_animation = False @@ -73,7 +73,7 @@ def dp_planning(sx, sy, gx, gy, ox, oy, reso, rr): if not pq: break cost, c_id = heapq.heappop(pq) - if openset.has_key(c_id): + if c_id in openset: current = openset[c_id] closedset[c_id] = current openset.pop(c_id) diff --git a/PathPlanning/HybridAStar/car.py b/PathPlanning/HybridAStar/car.py index e7d35e99..2701d0da 100644 --- a/PathPlanning/HybridAStar/car.py +++ b/PathPlanning/HybridAStar/car.py @@ -23,7 +23,7 @@ def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R) - if len(ids) == 0: + if not ids: continue if not rectangle_check(x, y, yaw, diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 35af0c94..eb69de4c 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -16,7 +16,7 @@ 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 +from a_star import dp_planning #, calc_obstacle_map XY_GRID_RESOLUTION = 2.0 #[m] YAW_GRID_RESOLUTION = np.deg2rad(15.0) #[rad] @@ -38,9 +38,9 @@ H_COST = 5.0 # Heuristic cost show_animation = True -_round = round -def round(x): - return int(_round(x)) +# _round = round +# def round(x): +# return int(_round(x)) class Node: @@ -92,9 +92,9 @@ class KDTree: dist.append(idist) return index, dist - else: - dist, index = self.tree.query(inp, k=k) - return index, dist + + dist, index = self.tree.query(inp, k=k) + return index, dist def search_in_distance(self, inp, r): """ @@ -211,7 +211,7 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree): paths = rs.calc_paths(sx,sy,syaw,gx, gy, gyaw, max_curvature, step_size=MOTION_RESOLUTION) - if not len(paths): + if not paths: return None best_path, best = None, None @@ -223,7 +223,7 @@ def analytic_expantion(current, goal, c, ox, oy, kdtree): best = cost best_path = path - return best_path # no update + return best_path def update_node_with_analystic_expantion(current, goal, @@ -309,7 +309,6 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): True, [goal[0]], [goal[1]], [goal[2]], [True]) openList, closedList = {}, {} - h = [] _, _, h_dp = dp_planning(nstart.xlist[-1], nstart.ylist[-1], ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR) @@ -324,9 +323,12 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): return [], [], [] cost, c_id = heapq.heappop(pq) - if openList.has_key(c_id): + # 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") @@ -341,23 +343,21 @@ def hybrid_a_star_planning(start, goal, ox, oy, xyreso, yawreso): for neighbor in get_neighbors(current, config, ox, oy, obkdtree): neighbor_index = calc_index(neighbor, config) - if closedList.has_key(neighbor_index): + # if closedList.has_key(neighbor_index): + if neighbor_index in closedList: continue - if not openList.has_key(neighbor_index) \ - or openList[neighbor_index].cost > neighbor.cost: # TODO huristic + 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 - # print(current) - - rx, ry, ryaw = [], [], [] 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 h_dp.has_key(ind): + if not ind in h_dp: return (n.cost + 999999999) # collision cost return (n.cost + H_COST*h_dp[ind].cost) @@ -451,7 +451,7 @@ def main(): x = path.xlist y = path.ylist yaw = path.yawlist - direction = path.directionlist + # direction = path.directionlist for ix, iy, iyaw in zip(x, y, yaw): plt.cla() From d8367b56b77dfb2008823468e56db1a5f151d445 Mon Sep 17 00:00:00 2001 From: zks Date: Sun, 24 Mar 2019 14:27:03 +0800 Subject: [PATCH 3/3] hybrid a star python 3.5 --- PathPlanning/HybridAStar/hybrid_a_star.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index eb69de4c..1c9a3988 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -3,6 +3,7 @@ Hybrid A* path planning author: Atsushi Sakai (@Atsushi_twi) + Zheng Zh (@Zhengzh) """ @@ -143,7 +144,7 @@ def calc_motion_inputs(): def get_neighbors(current, config, ox, oy, kdtree): - # import ipdb; ipdb.set_trace() + 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): @@ -163,7 +164,6 @@ def calc_next_node(current, steer, direction, config, ox, oy, kdtree): # plt.plot(xlist, ylist) if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): - # import ipdb; ipdb.set_trace() return None d = direction==1