def analystic_expantion(n, ngoal, ox, oy, kdtree): sx = n.x[-1] sy = n.y[-1] syaw = n.yaw[-1] max_curvature = math.tan(MAX_STEER) / WB paths = rs_path.calc_paths(sx, sy, syaw, ngoal.x[-1], ngoal.y[-1], ngoal.yaw[-1], max_curvature, step_size=MOTION_RESOLUTION) if len(paths) == 0: return None pathset = {} path_id = 0 for path in paths: pathset[path_id] = path path_id = path_id + 1 ind = 0 for i in range(len(pathset)): p_id = min(pathset, key=lambda o: calc_rs_path_cost(pathset[o])) path = pathset[p_id] if vehicle_lib.check_collision(ox, oy, path.x, path.y, path.yaw, kdtree): return path return None
def main_middle(sx, sy, syaw, mx, my, myaw, other_sx, other_sy, other_syaw, other_gx, other_gy, other_gyaw, other_other_x=None, other_other_y=None, other_other_yaw=None): # set obstacles global ox, oy oox = ox[:] ooy = oy[:] tox, toy = ox[:], oy[:] kdtree = planner.KDTree(np.vstack((tox, toy)).T) if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw], kdtree) == False: return None, None, None vehicle_x, vehicle_y = planner.vehicle.plot_trailer( mx, my, myaw, 0.0, True) oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy) path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw, other_gx, other_gy, other_gyaw, oox_add, ooy_add, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path2 == []: print 'The middle state is invalid(2)' return None, None, None # path generation path1 = planner.calc_hybrid_astar_path(sx, sy, syaw, mx, my, myaw, oox, ooy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path1 == []: print 'Cannot find path.(1)' return None, None, None vehicle.plot_trailer(other_sx, other_sy, other_syaw, 0) show_animation(path1, ox, oy, other_sx, other_sy, other_syaw, other_other_x, other_other_y, other_other_yaw) show_animation(path2, ox, oy, mx, my, myaw, other_other_x, other_other_y, other_other_yaw) plt.pause(0.01) return mx, my, myaw
def verify_index(node, c, ox, oy, kdtree): if (node.xind - c.minx) >= c.xw or (node.xind - c.minx) <= 0 or ( node.yind - c.miny) >= c.yw or (node.yind - c.miny) <= 0: return False if vehicle_lib.check_collision(ox, oy, node.x, node.y, node.yaw, kdtree) == False: return False return True
def analystic_expantion(n, ngoal, ox, oy, kdtree, draw): sx = n.x[-1] sy = n.y[-1] syaw = n.yaw[-1] max_curvature = math.tan(MAX_STEER) / WB paths = rs_path.calc_paths(sx, sy, syaw, ngoal.x[-1], ngoal.y[-1], ngoal.yaw[-1], max_curvature, step_size=MOTION_RESOLUTION) if len(paths) == 0: return None #pathqueue = PriorityQueue{rs_path.Path} #pathqueue = Queue.PriorityQueue() pathset = {} path_id = 0 for path in paths: #yaw1 = vehicle_lib.calc_trailer_yaw_from_xyyaw(path.x, path.y, path.yaw, n.yaw1[-1], steps) #pathqueue.put(path, calc_rs_path_cost(path)) pathset[path_id] = path path_id = path_id + 1 ind = 0 for i in range(len(pathset)): #path = pathqueue.get() p_id = min(pathset, key=lambda o: calc_rs_path_cost(pathset[o])) path = pathset[p_id] # if draw == True: # plt.grid(True) # plt.axis("equal") # plt.plot(path.x, path.y, linewidth = '0.3', color= 'red') # plt.pause(0.01) if vehicle_lib.check_collision(ox, oy, path.x, path.y, path.yaw, kdtree): #plt.plot(path.x, path.y, "-^b") return path # path is ok return None
def verify_index(node, c, ox, oy, kdtree): # overflow map if (node.xind - c.minx) >= c.xw: return False elif (node.xind - c.minx) <= 0: return False if (node.yind - c.miny) >= c.yw: return False elif (node.yind - c.miny) <= 0: return False # check collisiton #steps = MOTION_RESOLUTION*node.directions #yaw1 = vehicle_lib.calc_trailer_yaw_from_xyyaw(node.x, node.y, node.yaw, inityaw1, steps) #ind = 1:SKIP_COLLISION_CHECK:length(node.x) #if !trailerlib.check_trailer_collision(ox, oy, node.x[ind], node.y[ind], node.yaw[ind], yaw1[ind], kdtree = kdtree): #return False if vehicle_lib.check_collision(ox, oy, node.x, node.y, node.yaw, kdtree) == False: return False return True #index is ok"
def main(x, y, yaw): sx = SX # [m] sy = SY # [m] syaw0 = SYAW # goal state gx = GX # [m] gy = GY # [m] gyaw0 = GYAW other_sx = OTHER_SX # [m] other_sy = OTHER_SY # [m] other_syaw0 = OTHER_SYAW other_gx = OTHER_GX # [m] other_gy = OTHER_GY # [m] other_gyaw0 = OTHER_GYAW mx = x # [m] my = y # [m] myaw0 = yaw # set obstacles global ox, oy oox = ox[:] ooy = oy[:] tox, toy = ox[:], oy[:] kdtree = planner.KDTree(np.vstack((tox, toy)).T) if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw0], kdtree) == False: return # path generation vehicle_x, vehicle_y = planner.vehicle.plot_trailer( mx, my, myaw0, 0.0, True) oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy) path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw0, other_gx, other_gy, other_gyaw0, oox_add, ooy_add, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path2 == []: print('The middle state is invalid(2)') return path1 = planner.calc_hybrid_astar_path(sx, sy, syaw0, mx, my, myaw0, oox, ooy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path1 == []: print('Cannot find path.(1)') return path3 = planner.calc_hybrid_astar_path(mx, my, myaw0, gx, gy, gyaw0, oox, ooy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path3 == []: print('Cannot find path.(3)') return # global total_cost # global final_path1 # global final_path2 # global final_path3,final_mx,final_my,final_myaw0 # # if path1.cost + path2.cost + path3.cost < total_cost: # final_path1 = path1 # final_path2 = path2 # final_path3 = path3 # final_mx = mx # final_my = my # final_myaw0 = myaw0 plt.grid(True) plt.axis("equal") plt.plot(ox, oy, '.k') plt.pause(5) show_animation(path1, ox, oy, other_sx, other_sy, other_syaw0) show_animation(path2, ox, oy, mx, my, myaw0) show_animation(path3, ox, oy, other_gx, other_gy, other_gyaw0)
def calc_hybrid_astar_path(sx, sy, syaw, gx, gy, gyaw, ox, oy, xyreso, yawreso): # sx: start x position[m] # sy: start y position[m] # gx: goal x position[m] # gx: goal x position[m] # ox: x position list of Obstacles[m] # oy: y position list of Obstacles[m] # xyreso: grid resolution[m] # yawreso: yaw angle resolution[rad] syaw0 = rs_path.pi_2_pi(syaw) gyaw0 = rs_path.pi_2_pi(gyaw) global tox, toy ox, oy = ox[:], oy[:] tox, toy = ox[:], oy[:] kdtree = KDTree(np.vstack((tox, toy)).T) # a = np.array((1,2,3)) # b = np.array((2,3,4)) # c = np.hstack((a,b)) # tree = spatial.KDTree((a,b),10) # print tree.date c = calc_config(ox, oy, xyreso, yawreso) #c.prn_obj() nstart = Node(round(sx / xyreso), round(sy / xyreso), round(syaw0 / yawreso), True, [sx], [sy], [syaw0], [True], 0.0, 0.0, -1) ngoal = Node(round(gx / xyreso), round(gy / xyreso), round(gyaw0 / yawreso), True, [gx], [gy], [gyaw0], [True], 0.0, 0.0, -1) h_dp = calc_holonomic_with_obstacle_heuristic(ngoal, ox, oy, xyreso) openset, closedset = {}, {} fnode = None openset[calc_index(nstart, c)] = nstart pq = Q.PriorityQueue() pq.put(calc_index(nstart, c), calc_cost(nstart, h_dp, ngoal, c)) u, d = calc_motion_inputs() nmotion = len(u) if vehicle_lib.check_collision(ox, oy, [sx], [sy], [syaw0], kdtree) == False: print('1111111') return [] if vehicle_lib.check_collision(ox, oy, [gx], [gy], [gyaw0], kdtree) == False: print('2222222') return [] times = 0 while 1: # if times >100: # return [] if len(openset) == 0: print("Error: Cannot find path, No open set") return [] c_id = min( openset, key=lambda o: openset[o].cost + calc_cost(nstart, h_dp, ngoal, c)) #c_id = pq.get() current = openset[c_id] # move current node from open to closed del openset[c_id] closedset[c_id] = current # visualize_x = [] # visualize_y = [] # for v in closedset.values(): # visualize_x.append(v.x[-1]) # visualize_y.append(v.y[-1]) # print visualize_x,visualize_y # plt.plot(tox, toy, ".k") # plt.plot(visualize_x,visualize_y,'.k') # plt.pause(0.1) isupdated, fpath = update_node_with_analystic_expantion( current, ngoal, c, ox, oy, kdtree, True) if isupdated: # found fnode = fpath break for i in range(nmotion): node = calc_next_node(current, c_id, u[i], d[i], c) if verify_index(node, c, ox, oy, kdtree) == False: continue node_ind = calc_index(node, c) #print 'expend node id:', node_ind # If it is already in the closed set, skip it if node_ind in closedset: continue if node_ind not in openset: openset[node_ind] = node pq.put(node_ind, calc_cost(nstart, h_dp, ngoal, c)) else: if openset[node_ind].cost > node.cost: # If so, update the node to have a new parent openset[node_ind] = node times = times + 1 print("final expand node:", len(openset) + len(closedset)) path = get_final_path(closedset, fnode, nstart, c) return path
def main(x, y, yaw): sx = SX # [m] sy = SY # [m] syaw0 = SYAW # goal state gx = GX # [m] gy = GY # [m] gyaw0 = GYAW other_sx = OTHER_SX # [m] other_sy = OTHER_SY # [m] other_syaw0 = OTHER_SYAW other_gx = OTHER_GX # [m] other_gy = OTHER_GY # [m] other_gyaw0 = OTHER_GYAW mx = x # [m] my = y # [m] myaw0 = np.deg2rad(yaw) # set obstacles ox = [] oy = [] # for i in range(-8, 8): # ox.append(i) # oy.append(12.0) for i in range(-25, -8): ox.append(i) oy.append(9.0) for i in range(8, 25): ox.append(i) oy.append(9.0) for i in range(-25, -8): ox.append(i) oy.append(4.0) for i in range(-3, 5): ox.append(-8.0) oy.append(i) for i in range(-3, 5): ox.append(8.0) oy.append(i) for i in range(8, 25): ox.append(i) oy.append(4.0) # for i in range(-8, 8): # ox.append(i) # oy.append(1.0) for i in range(-8, 8): ox.append(i) oy.append(12.0) for i in range(4, 9): ox.append(-25) oy.append(i) for i in range(4, 9): ox.append(25) oy.append(i) oox = ox[:] ooy = oy[:] tox, toy = ox[:], oy[:] kdtree = planner.KDTree(np.vstack((tox, toy)).T) # plt.plot(ox,oy,'.k') # plt.show() if vehicle_lib.check_collision(oox, ooy, [mx], [my], [myaw0], kdtree) == False: return # path generation path1 = planner.calc_hybrid_astar_path(sx, sy, syaw0, mx, my, myaw0, ox, oy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path1 == []: print 'Cannot find path.(1)' return vehicle_x, vehicle_y = planner.vehicle.plot_trailer( mx, my, myaw0, 0.0, True) oox_add, ooy_add = add_obstacle(vehicle_x, vehicle_y, oox, ooy) path2 = planner.calc_hybrid_astar_path(other_sx, other_sy, other_syaw0, other_gx, other_gy, other_gyaw0, oox_add, ooy_add, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path2 == []: print 'The middle state is invalid(2)' return path3 = planner.calc_hybrid_astar_path(mx, my, myaw0, gx, gy, gyaw0, ox, oy, planner.XY_GRID_RESOLUTION, planner.YAW_GRID_RESOLUTION) if path3 == []: print 'Cannot find path.(3)' return # global total_cost # global final_path1 # global final_path2 # global final_path3,final_mx,final_my,final_myaw0 # # if path1.cost + path2.cost + path3.cost < total_cost: # final_path1 = path1 # final_path2 = path2 # final_path3 = path3 # final_mx = mx # final_my = my # final_myaw0 = myaw0 vehicle.plot_trailer(other_sx, other_sy, other_syaw0, 0) show_animation(path1, oox, ooy, other_sx, other_sy, other_syaw0) show_animation(path2, oox, ooy, mx, my, myaw0) show_animation(path3, oox, ooy, other_gx, other_gy, other_gyaw0) plt.show()