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)
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_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 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")
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 neighbor_index in closedList:
continue
if neighbor not 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
path = get_final_path(closedList, fpath, nstart, config)
return path