def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution,
yaw_resolution):
"""
start: start node
goal: goal node
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
xy_resolution: grid resolution [m]
yaw_resolution: yaw angle resolution [rad]
"""
start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
tox, toy = ox[:], oy[:]
obstacle_kd_tree = cKDTree(np.vstack((tox, toy)).T)
config = Config(tox, toy, xy_resolution, yaw_resolution)
start_node = Node(
round(start[0] / xy_resolution),
round(start[1] / xy_resolution),
round(start[2] / yaw_resolution),
True, [start[0]], [start[1]], [start[2]], [True],
cost=0)
goal_node = Node(
round(goal[0] / xy_resolution), round(goal[1] / xy_resolution),
round(goal[2] / yaw_resolution), True, [goal[0]], [goal[1]], [goal[2]],
[True])
openList, closedList = {}, {}
h_dp = calc_distance_heuristic(goal_node.x_list[-1], goal_node.y_list[-1],
ox, oy, xy_resolution, VR)
pq = []
openList[calc_index(start_node, config)] = start_node
heapq.heappush(
pq,
(calc_cost(start_node, h_dp, config), calc_index(start_node, config)))
final_path = None
while True:
# The waypoints contained in the closed list lead to the waypoints that
# can be reached from the starting point, but this is not the final
# DESTINATION waypoint. This means that we did not find a feasible path.
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.x_list[-1], current.y_list[-1], "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if len(closedList.keys()) % 10 == 0:
plt.pause(0.001)
# Main difference with traditional A-star algorithm. Each iteration
# when we reached a new station, try to find a path guide to goal
# node.
# Actually, final path does not necessarily exist, when the path
# violates the physical feasibility limits or collides with obstacles.
# In this case, the program will return after the open-list has been
# traversed. See line 334.
is_updated, final_path = update_node_with_analytic_expansion(
current, goal_node, config, ox, oy, obstacle_kd_tree)
if is_updated:
print("path found")
break
for neighbor in get_neighbors(current, config, ox, oy, obstacle_kd_tree):
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, config), neighbor_index))
openList[neighbor_index] = neighbor
path = get_final_path(closedList, final_path)
return path
def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
"""
start: start node
goal: goal node
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
xy_resolution: grid resolution [m]
yaw_resolution: yaw angle resolution [rad]
"""
start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
tox, toy = ox[:], oy[:]
obstacle_kd_tree = cKDTree(np.vstack((tox, toy)).T)
config = Config(tox, toy, xy_resolution, yaw_resolution)
start_node = Node(round(start[0] / xy_resolution),
round(start[1] / xy_resolution),
round(start[2] / yaw_resolution), True,
[start[0]], [start[1]], [start[2]], [True], cost=0)
goal_node = Node(round(goal[0] / xy_resolution),
round(goal[1] / xy_resolution),
round(goal[2] / yaw_resolution), True,
[goal[0]], [goal[1]], [goal[2]], [True])
openList, closedList = {}, {}
h_dp = calc_distance_heuristic(
goal_node.x_list[-1], goal_node.y_list[-1],
ox, oy, xy_resolution, BUBBLE_R)
pq = []
openList[calc_index(start_node, config)] = start_node
heapq.heappush(pq, (calc_cost(start_node, h_dp, config),
calc_index(start_node, config)))
final_path = None
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.x_list[-1], current.y_list[-1], "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if len(closedList.keys()) % 10 == 0:
plt.pause(0.001)
is_updated, final_path = update_node_with_analytic_expansion(
current, goal_node, config, ox, oy, obstacle_kd_tree)
if is_updated:
print("path found")
break
for neighbor in get_neighbors(current, config, ox, oy,
obstacle_kd_tree):
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, config),
neighbor_index))
openList[neighbor_index] = neighbor
path = get_final_path(closedList, final_path)
return path
def hybrid_a_star_planning(start, goal, ox, oy, xy_resolution, yaw_resolution):
"""
start: start node
goal: goal node
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
xy_resolution: grid resolution [m]
yaw_resolution: yaw angle resolution [rad]
"""
# 将航向角转化到[-pi,pi]
start[2], goal[2] = rs.pi_2_pi(start[2]), rs.pi_2_pi(goal[2])
tox, toy = ox[:], oy[:]
# 创建障碍物KDTree，后面需要对积分推出的路径点挨个进行障碍物检测，就需要计算每个路径点按车辆最大轮廓为半径的圆，
# 看障碍物是否落在圆内，若在，则发生碰撞，因此有多次查找一定范围内最近邻节点的操作，因此KD-Tree是很适合的数据结构
obstacle_kd_tree = cKDTree(np.vstack((tox, toy)).T)
config = Config(tox, toy, xy_resolution, yaw_resolution)
# todo 赋值方式看不懂 为什么方向设为True or False？
start_node = Node(round(start[0] / xy_resolution),
round(start[1] / xy_resolution),
round(start[2] / yaw_resolution), True,
[start[0]], [start[1]], [start[2]], [True], cost=0)
goal_node = Node(round(goal[0] / xy_resolution),
round(goal[1] / xy_resolution),
round(goal[2] / yaw_resolution), True,
[goal[0]], [goal[1]], [goal[2]], [True])
openList, closedList = {}, {}
# 搜索前预先计算终点到图中所有可行节点的启发值h(n),这里方法采用迪杰斯特拉算出h(n)为节点到终点的最短距离
h_dp = calc_distance_heuristic(
goal_node.x_list[-1], goal_node.y_list[-1],
ox, oy, xy_resolution, VR)
# 创建一个堆，即优先级队列，堆的第一个元素总是所有元素中最小的，每个元素用元组表示(cost,index)
pq = []
openList[calc_index(start_node, config)] = start_node
heapq.heappush(pq, (calc_cost(start_node, h_dp, config),
calc_index(start_node, config)))
final_path = None
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.x_list[-1], current.y_list[-1], "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if len(closedList.keys()) % 10 == 0:
plt.pause(0.001)
is_updated, final_path = update_node_with_analytic_expansion(
current, goal_node, config, ox, oy, obstacle_kd_tree)
if is_updated:
print("path found")
break
for neighbor in get_neighbors(current, config, ox, oy,
obstacle_kd_tree):
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, config),
neighbor_index))
openList[neighbor_index] = neighbor
path = get_final_path(closedList, final_path)
return path