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
