自动驾驶中的运动规划

# A*
'''
g: 路径代价,哪个点值得搜索 找到的路径最小
h(g+h): 哪个点最先被搜索 最快地找到路径

在进行代价比较时只使用了g_cost，这是因为规划的最终目的就是使得这个cost最小。

而在取下一个待访问节点时比较的是 g+h，这是为了通过诱导cost更快地找到路径
'''
for next in graph.neighbors(current):
      new_cost = cost_so_far[current] + graph.cost(current, next)
      if next not in cost_so_far or new_cost < cost_so_far[next]:
         cost_so_far[next] = new_cost
         priority = new_cost + heuristic(goal, next)
         frontier.put(next, priority)
         came_from[next] = current

# hrbrid A*
g_cost: 通过离散化转向和reeds_shepp路径计算cost, 这里取邻近点时只能预定义几个
h_cost: 从目标点开始，计算到当前点的dijkstra路径代价

？ 那使用rrt也能使用这种算法

 frontier: g_cost + h_cost
            # node self only save g_cost
         # g_cost: 哪个点值得搜索 找到的路径最小
        # g+h: 哪个点最先被搜索 最快地找到路径