feat: 添加 A* 算法实现解释

AI/路径规划/AStar/README.md

@@ -0,0 +1,153 @@
+
+# AStar
+
+A* 算法结合了 Dijkstra 算法（保证找到最短路径）和 贪婪最佳优先搜索（优先探索目标方向）的优点
+
+1. 像 Dijkstra 一样，它记录从起点到当前节点的实际移动代价（g(n)）
+2. 像贪婪最佳优先搜索一样，它使用一个启发式函数（h(n)）来估计从当前节点到目标节点的剩余代价
+
+参考代码如下：
+
+https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/path_planner/path_planner/src/graph_planner/astar_planner.cpp
+
+```cpp
+template <typename T>
+class Node
+{
+protected:
+  T x_, y_;       // x 轴坐标 y 轴坐标
+  double g_;      // 从 StartNode 到 CurrentNode 的 Cast 
+  double h_;      // 从 CurrentNode 到 EndNode 的 欧几里得距离 
+  int id_;        // Node 节点的 ID
+  int pid_;       // Node 节点的 父节点
+
+public:
+  
+  // 提供排序的仿函数
+  struct compare_cost
+  {
+    bool operator()(const Node& n1, const Node& n2) const
+    {
+      return (n1.g() + n1.h() > n2.g() + n2.h()) || ((n1.g() + n1.h() == n2.g() + n2.h()) && (n1.h() > n2.h()));
+    };
+  };
+}
+```
+
+观察 Node 节点的代码，比较一个节点 Cast 的大小是将 g 值 + h 值进行比较，如果相等，则单纯比较 h 值
+
+解析来是 A* 算法的实现
+
+- `std::priority_queue<Node, std::vector<Node>, Node::compare_cost> open_list;` 使用 `priority_queue` 优先队列对插入的数据进行排序
+
+结合 `Node::compare_cost` 的排序函数，可知 `open_list` 中 `Node` 是按 `Cast` 从小到大进行排序
+
+每次都取出 `open_list` 中 Cast 最小的 Node，也就是第一个 Node
+
+对 Node 周围的 8 个 Node 进行遍历，将其添加到 `open_list` 中
+
+> 添加 Node 之前先判断其在 `open_list` 或者 `closed_list` 中是否已经存在，避免重复处理 Node 
+
+```cpp
+bool AStarPathPlanner::plan(const Point3d& start, const Point3d& goal, Points3d& path, Points3d& expand)
+{
+  double m_start_x, m_start_y, m_goal_x, m_goal_y;
+  if ((!validityCheck(start.x(), start.y(), m_start_x, m_start_y)) ||
+      (!validityCheck(goal.x(), goal.y(), m_goal_x, m_goal_y)))
+  {
+    return false;
+  }
+
+  Node start_node(m_start_x, m_start_y);
+  Node goal_node(m_goal_x, m_goal_y);
+  start_node.set_id(grid2Index(start_node.x(), start_node.y()));
+  goal_node.set_id(grid2Index(goal_node.x(), goal_node.y()));
+
+  // clear vector
+  path.clear();
+  expand.clear();
+
+  // open list and closed list
+  std::priority_queue<Node, std::vector<Node>, Node::compare_cost> open_list;
+  std::unordered_map<int, Node> closed_list;
+
+  open_list.push(start_node);
+
+  // main process
+  while (!open_list.empty())
+  {
+    // pop current node from open list
+    auto current = open_list.top();
+    open_list.pop();
+
+    // current node does not exist in closed list
+    if (closed_list.find(current.id()) != closed_list.end())
+      continue;
+
+    closed_list.insert(std::make_pair(current.id(), current));
+    expand.emplace_back(current.x(), current.y());
+
+    // goal found
+    if (current == goal_node)
+    {
+      const auto& backtrace = _convertClosedListToPath<Node>(closed_list, start_node, goal_node);
+      for (auto iter = backtrace.rbegin(); iter != backtrace.rend(); ++iter)
+      {
+        // convert to world frame
+        double wx, wy;
+        costmap_->mapToWorld(iter->x(), iter->y(), wx, wy);
+        path.emplace_back(wx, wy);
+      }
+      return true;
+    }
+
+    // explore neighbor of current node
+    for (const auto& motion : motions)
+    {
+      // explore a new node
+      auto node_new = current + motion;
+      node_new.set_g(current.g() + motion.g());
+      node_new.set_id(grid2Index(node_new.x(), node_new.y()));
+
+      // node_new in closed list
+      if (closed_list.find(node_new.id()) != closed_list.end())
+        continue;
+
+      node_new.set_pid(current.id());
+
+      // next node hit the boundary or obstacle
+      // prevent planning failed when the current within inflation
+      if ((node_new.id() < 0) || (node_new.id() >= map_size_) ||
+          (costmap_->getCharMap()[node_new.id()] >= costmap_2d::LETHAL_OBSTACLE * obstacle_factor_ &&
+           costmap_->getCharMap()[node_new.id()] >= costmap_->getCharMap()[current.id()]))
+        continue;
+
+      // if using dijkstra implementation, do not consider heuristics cost
+      if (!is_dijkstra_)
+        node_new.set_h(std::hypot(node_new.x() - goal_node.x(), node_new.y() - goal_node.y()));
+
+      // if using GBFS implementation, only consider heuristics cost
+      if (is_gbfs_)
+        node_new.set_g(0.0);
+      // else, g will be calculate through node_new = current + m
+
+      open_list.push(node_new);
+    }
+  }
+
+  return false;
+}
+```
+
+`motions` 的内容如下，是为了方便遍历 上下左右 + 斜方向 总共 八方向 的 Node 节点而定义的一个数组
+
+```cpp
+const std::vector<Node> motions = {
+    { 0, 1, 1.0 },          { 1, 0, 1.0 },           { 0, -1, 1.0 },          { -1, 0, 1.0 },
+    { 1, 1, std::sqrt(2) }, { 1, -1, std::sqrt(2) }, { -1, 1, std::sqrt(2) }, { -1, -1, std::sqrt(2) },
+};
+```
+
+> 分别对应 上、右、下、左、右上、右下、左上、左下
+
+

AI/路径规划/JPS/RAEDME.md

@@ -0,0 +1,37 @@
+# JPS
+
+> Jump Point Search
+
+参考文档： https://zhuanlan.zhihu.com/p/500807732
+参考代码： https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/path_planner/path_planner/src/graph_planner/jps_planner.cpp
+
+定义基本结构体 Node
+
+```cpp
+class JNode : public Node
+{
+  public:
+    /* @brief Construct a new JNode object
+     * @param x   节点对应 X 轴坐标
+     * @param y   节点对应 Y 轴坐标
+     * @param g   节点对应 到 StartNode 的 Cast
+     * @param h   节点对应 到 EndNode 的 Cast
+     * @param id  节点对应 的 ID
+     * @param pid 节点对应 的 Parent 的 ID
+     * @param fid 节点对应 强制邻居 Forced neighbor ID
+     */
+    JNode(int x = 0, int y = 0, double g = 0.0, double h = 0.0, int id = 0, int pid = -1, int fid = 0)
+      : Node(x, y, g, h, id, pid), fid_(fid)
+    {}  
+
+    struct compare_cost
+    {
+        bool operator()(const Node& n1, const Node& n2) const
+        {
+            return (n1.g() + n1.h() > n2.g() + n2.h()) || ((n1.g() + n1.h() == n2.g() + n2.h()) && (n1.h() > n2.h()));
+        };
+    };
+}
+```
+
+

AI/路径规划/README.md

@@ -0,0 +1,3 @@
+# 路径规划
+
+参考项目 https://github.com/ai-winter/ros_motion_planning