def 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]) 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]) path_x, path_y, _ = dp_planning(nstart.xlist[-1], nstart.ylist[-1], ngoal.xlist[-1], ngoal.ylist[-1], ox, oy, xyreso, VR) return path_x, path_y
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) c = Config(tox, toy, xyreso, yawreso) nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso), True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1) ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso), True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1) rx, ry, ryaw = [], [], [] return rx, ry, ryaw
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) c = Config(tox, toy, xyreso, yawreso) nstart = Node(int(start[0] / xyreso), int(start[1] / xyreso), int(start[2] / yawreso), True, [start[0]], [start[1]], [start[2]], [True], 0.0, 0.0, -1) ngoal = Node(int(goal[0] / xyreso), int(goal[1] / xyreso), int(goal[2] / yawreso), True, [goal[0]], [goal[1]], [goal[2]], [True], 0.0, 0.0, -1) openList, closedList = {}, {} h = [] # goalqueue = queue.PriorityQueue() pq = [] openList[calc_index(nstart, c)] = nstart heapq.heappush(pq, (calc_index(nstart, c), calc_cost(nstart, h, ngoal, c))) while True: if not openList: print("Error: Cannot find path, No open set") return [], [], [] c_id, cost = heapq.heappop(pq) current = openList.pop(c_id) closedList[c_id] = current isupdated, fpath = analytic_expantion( current, ngoal, c, ox, oy, obkdtree) # print(current) rx, ry, ryaw = [], [], [] return rx, ry, ryaw
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) c = Config(tox, toy, xyreso, yawreso) rx, ry, ryaw = [], [], [] return rx, ry, ryaw