【运动规划算法项目实战】如何在栅格地图中实现Dijkstra算法（附ROS C++代码）

---

简介

Dijkstra算法是一种用于图中单源最短路径的贪心算法。在计算机科学和网络设计中广泛应用。该算法从起点开始，通过优先选择距离起点最近的未标记节点来逐步扩展最短路径树。它最终确定到所有节点的最短路径。

---

一、算法介绍

算法的基本思想是从起始节点开始，计算出所有其他节点到该节点的最短距离，并将该节点加入最短路径树中。然后重复这个过程，直到所有节点都在最短路径树中。

1.1 Dijkstra算法流程

算法步骤如下：

1. 创建一个距离数组dist[]，用于存储起点到每个节点的距离，并将其初始化为无穷大。
2. 创建一个访问数组visited[]，用于标记每个节点是否已访问，并将其初始化为false。
3. 将起点的距离设为0，将其加入到待访问队列中。
4. 从待访问队列中取出距离起点最近的节点，并将其标记为已访问。
5. 遍历该节点的所有邻居节点，如果邻居节点未被访问，则计算出起点到该邻居节点的距离。如果该距离小于该邻居节点当前存储的距离，则更新该邻居节点的距离，并将其加入到待访问队列中。
6. 重复