├── .gitignore
├── CMakeLists.txt
├── README.md
├── README_zh-CN.md
├── a_star
    ├── a_star.cc
    ├── a_star.md
    └── a_star_zh-CN.md
├── common
    ├── map.cc
    └── map.h
└── rrt
    ├── rrt.cc
    └── rrt.h


/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | .vscode/


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.10)
 2 | project(planning_algorithms)
 3 | 
 4 | set(ASTAR
 5 |     common/map.cc
 6 |     a_star/a_star.cc
 7 | )
 8 | 
 9 | add_executable(a_star ${ASTAR})
10 | 
11 | find_package(OpenCV REQUIRED)
12 | include_directories(${OpenCV_INCLUDE_DIRS})
13 | link_directories(${OpenCV_LIBRARY_DIRS})
14 | target_link_libraries(a_star ${OpenCV_LIBS})
15 | 
16 | target_include_directories(a_star
17 |     PRIVATE 
18 |         ${PROJECT_SOURCE_DIR}/common
19 | )
20 | 
21 | add_executable(rrt rrt/rrt.cc)
22 | target_link_libraries(rrt ${OpenCV_LIBS})


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Planning Algorithm
 2 | ## 1. How to Use
 3 | ### 1.1 Environment config
 4 | [Ubuntu20.04](https://ubuntu.com/)(recommend) or Ubunt18.04<br>
 5 | [CMake](https://cmake.org/)<br>
 6 | [OpenCV](https://docs.opencv.org/4.5.3/d7/d9f/tutorial_linux_install.html)(OpenCV4 recommend)
 7 | 
 8 | ### 1.2 Build program
 9 | ```
10 | mkdir build
11 | cd build
12 | cmake ..
13 | make -j10
14 | ./a_star
15 | ./rrt
16 | ```
17 | ### 1.3 Supplementary Instruction
18 | The rightward direction is the positive direction of the X-axis<br>
19 | The downward direction is the positive direction of the Y-axis.
20 | 
21 | ## 2. Taxonomy of motion planning techniques applied in automated driving scenarios
22 | <table>
23 |     <tr>
24 |         <td>Algorithm group</td>
25 |         <td width="200">Technique</td>
26 |         <td>Technique description</td>
27 |     </tr>
28 |     <tr>
29 |         <td rowspan="3">Graph search based planners</td>
30 |         <td>Dijkstra's Algorithm</td>
31 |         <td>
32 |             Known nodes/cells search space with associated weights<br/>
33 |             Grid and node/cells weights computation according to the environment
34 |         </td>
35 |     </tr>
36 |     <tr>
37 |         <td>A* algorithm family</td>
38 |         <td>
39 |             Anytime D* with Voronoi cost functions Hybrid-heuristics A*<br/>
40 |             A* with Voronoi/Lattice enviroment represeantation. PAO*
41 |         </td>
42 |     </tr>
43 |     <tr>
44 |         <td>State Lattices</td>
45 |         <td>
46 |             Enviroment decomposed in a local variable grid, depending on the complexity of the maneuver<br/>
47 |             Spatio-temporal lattices(considering time and velocity dimensions)
48 |         </td>
49 |     </tr>
50 |     <tr>
51 |         <td>Sampling based planners</td>
52 |         <td>RRT</td>
53 |         <td>
54 |             Physical and logical bias are used to generate the random-tree<br/>
55 |             Anytime planning with RRT*<br/>
56 |             Trajectory coordination with RRT
57 |         </td>
58 |     </tr>
59 |     <tr>
60 |         <td rowspan="5">Interpolating curve planners</td>
61 |         <td>Line and circle</td>
62 |         <td>Road fitting and interpolation of known waypoints</td>
63 |     </tr>
64 |     <tr>
65 |         <td>Clothoid Curves</td>
66 |         <td>
67 |             Piecewise trajectory generation with straight, clothoid and circular segments<br/>
68 |             Off-line generation of clothoid primitives from which the best will be taken in on-line evaluation
69 |         </td>
70 |     </tr>
71 |     <tr>
72 |         <td>Polynomial Curves</td>
73 |         <td>
74 |             Cubic order polynomial curves<br/>
75 |             Higher order polynomial curves
76 |         </td>
77 |     </tr>
78 |     <tr>
79 |         <td>Bezier Curves</td>
80 |         <td>
81 |             Selection of the optimal control points location for the situation in hand<br/>
82 |             Rational Bezier curves impletation
83 |         </td>
84 |     </tr>
85 |     <tr>
86 |         <td>Spline Curves</td>
87 |         <td>Polynomial piecewise implementation Basis splines(b-splines)</td>
88 |     </tr>
89 |     <tr>
90 |         <td>Numerical optimization approaches</td>
91 |         <td>Function optimization</td>
92 |         <td>
93 |             Trajectory generation optimizing parameters such as speed, steering speed, rollover constraints, lateral accelerations, jerk(lateral comort optimization), among others
94 |         </td>
95 |     </tr>
96 | </table>
97 | 


--------------------------------------------------------------------------------
/README_zh-CN.md:
--------------------------------------------------------------------------------
 1 | # 路径规划算法
 2 | ## 1. 如何使用
 3 | ### 1.1 环境配置
 4 | [Ubuntu20.04](https://ubuntu.com/)(建议) 或者 Ubunt18.04<br>
 5 | [CMake](https://cmake.org/)<br>
 6 | [OpenCV](https://docs.opencv.org/4.5.3/d7/d9f/tutorial_linux_install.html)(建议安装OpenCV4)
 7 | 
 8 | ### 1.2 编译代码
 9 | ```
10 | mkdir build
11 | cd build
12 | cmake ..
13 | make -j10
14 | ./a_star
15 | ./rrt
16 | ```
17 | ### 1.3 补充说明
18 | 向右为X轴正方向, 向下为Y轴正方向
19 | 
20 | ## 2. 自动驾驶技术中路径规划算法的分类
21 | <table>
22 |     <tr>
23 |         <td>分类</td>
24 |         <td width="200">算法</td>
25 |         <td>说明</td>
26 |     </tr>
27 |     <tr>
28 |         <td rowspan="3">基于图搜索的算法</td>
29 |         <td>Dijkstra算法</td>
30 |         <td>
31 |             已知带有权重的节点搜索空间<br/>
32 |             节点的权重和周围的环境有关
33 |         </td>
34 |     </tr>
35 |     <tr>
36 |         <td>A*算法</td>
37 |         <td>
38 |             A*在Dijkstra算法的基础上增加了启发式代价函数<br/>
39 |         </td>
40 |     </tr>
41 |     <tr>
42 |         <td>Lattices</td>
43 |         <td>
44 |             根据环境的复杂性不同, 将其建模到本地的离散化网格中<br/>
45 |             时间-空间lattice, 考虑时间和速度两个维度
46 |         </td>
47 |     </tr>
48 |     <tr>
49 |         <td>基于取样的算法</td>
50 |         <td>RRT</td>
51 |         <td>
52 |             用物理和逻辑上的偏差来生成随即搜索树<br/>
53 |         </td>
54 |     </tr>
55 |     <tr>
56 |         <td rowspan="5">曲线插值</td>
57 |         <td>直线和圆</td>
58 |         <td>利用已知的点进行插值形成路径</td>
59 |     </tr>
60 |     <tr>
61 |         <td>回旋线</td>
62 |         <td>
63 |             利用直线、螺旋曲线、圆弧等分段组成轨迹<br/>
64 |             离线生成原始曲线然后放到线上进行评估
65 |         </td>
66 |     </tr>
67 |     <tr>
68 |         <td>多项式曲线</td>
69 |         <td>
70 |             三阶多项式曲线<br/>
71 |         </td>
72 |     </tr>
73 |     <tr>
74 |         <td>贝塞尔曲线</td>
75 |         <td>
76 |             为现有的情况选择最佳的控制点<br/>
77 |             有理贝塞尔曲线的实现
78 |         </td>
79 |     </tr>
80 |     <tr>
81 |         <td>样条曲线</td>
82 |         <td>分段的多项式函数组成样条曲线</td>
83 |     </tr>
84 |     <tr>
85 |         <td>数值优化</td>
86 |         <td>函数优化</td>
87 |         <td>
88 |             优化生成的轨迹的参数(比如速度、转向速度、滚动约束、侧向加速度、Jerk(速度的导数, 表示舒适性)等等)<br/>
89 |         </td>
90 |     </tr>
91 | </table>
92 | 


--------------------------------------------------------------------------------
/a_star/a_star.cc:
--------------------------------------------------------------------------------
  1 | #include <algorithm>
  2 | #include <cmath>
  3 | #include <iostream>
  4 | #include <opencv2/core/core.hpp>
  5 | #include <opencv2/highgui/highgui.hpp>
  6 | #include <opencv2/opencv.hpp>
  7 | #include <queue>
  8 | #include <unordered_set>
  9 | #include <vector>
 10 | 
 11 | #include "map.h"
 12 | 
 13 | namespace {
 14 | static constexpr int32_t kMapDimensionX = 50;
 15 | static constexpr int32_t kMapDimensionY = 50;
 16 | static Map map(kMapDimensionX, kMapDimensionY);
 17 | }  // namespace
 18 | 
 19 | bool IsCoordinateValid(int32_t x, int32_t y) {
 20 |   return x >= 0 && x < kMapDimensionX && y >= 0 && y < kMapDimensionY;
 21 | }
 22 | 
 23 | // Return the set of nodes which are adjacent to n.
 24 | void Star(Node* n, std::vector<Node*>& adjacent_nodes) {
 25 |   int32_t x = n->point().x, y = n->point().y;
 26 |   if (IsCoordinateValid(x, y - 1)) {
 27 |     Node* upper_node = map.mutable_node(x, y - 1);
 28 |     adjacent_nodes.push_back(upper_node);
 29 |   }
 30 |   if (IsCoordinateValid(x, y + 1)) {
 31 |     Node* down_node = map.mutable_node(x, y + 1);
 32 |     adjacent_nodes.push_back(down_node);
 33 |   }
 34 |   if (IsCoordinateValid(x - 1, y)) {
 35 |     Node* left_node = map.mutable_node(x - 1, y);
 36 |     adjacent_nodes.push_back(left_node);
 37 |   }
 38 |   if (IsCoordinateValid(x + 1, y)) {
 39 |     Node* right_node = map.mutable_node(x + 1, y);
 40 |     adjacent_nodes.push_back(right_node);
 41 |   }
 42 |   if (IsCoordinateValid(x - 1, y - 1)) {
 43 |     Node* upper_left_node = map.mutable_node(x - 1, y - 1);
 44 |     adjacent_nodes.push_back(upper_left_node);
 45 |   }
 46 |   if (IsCoordinateValid(x + 1, y - 1)) {
 47 |     Node* upper_right_node = map.mutable_node(x + 1, y - 1);
 48 |     adjacent_nodes.push_back(upper_right_node);
 49 |   }
 50 |   if (IsCoordinateValid(x - 1, y + 1)) {
 51 |     Node* down_left_node = map.mutable_node(x - 1, y + 1);
 52 |     adjacent_nodes.push_back(down_left_node);
 53 |   }
 54 |   if (IsCoordinateValid(x + 1, y + 1)) {
 55 |     Node* down_right_node = map.mutable_node(x + 1, y + 1);
 56 |     adjacent_nodes.push_back(down_right_node);
 57 |   }
 58 |   return;
 59 | }
 60 | 
 61 | void CalculateHeuristicCost(Node* goal_node) {
 62 |   for (int i = 0; i < kMapDimensionX; ++i) {
 63 |     for (int j = 0; j < kMapDimensionY; ++j) {
 64 |       double heuristic_cost =
 65 |           std::hypot(i - goal_node->point().x, j - goal_node->point().y);
 66 |       map.mutable_node(i, j)->set_heuristic_cost(heuristic_cost);
 67 |     }
 68 |   }
 69 | }
 70 | 
 71 | void AddObstacles() {
 72 |   Obstacle obstacle;
 73 |   obstacle.upper_left_x_ = 2;
 74 |   obstacle.upper_left_y_ = 4;
 75 |   obstacle.dimension_x_ = 8;
 76 |   obstacle.dimension_y_ = 2;
 77 |   map.AddObstacle(obstacle);
 78 | 
 79 |   obstacle.upper_left_x_ = 20;
 80 |   obstacle.upper_left_y_ = 30;
 81 |   obstacle.dimension_x_ = 4;
 82 |   obstacle.dimension_y_ = 4;
 83 |   map.AddObstacle(obstacle);
 84 | 
 85 |   map.DrawObstacles();
 86 | }
 87 | 
 88 | double c(Node* n_best, Node* adjacent_node) {
 89 |   double cost = 0.0;
 90 |   cost += std::hypot(n_best->point().x - adjacent_node->point().x,
 91 |                      n_best->point().y - adjacent_node->point().y);
 92 |   return cost;
 93 | }
 94 | 
 95 | bool AStarAlgorithm(Node* start_node, Node* goal_node) {
 96 |   CalculateHeuristicCost(goal_node);
 97 | 
 98 |   AddObstacles();
 99 | 
100 |   std::unordered_set<Node*> C;  // processed nodes
101 |   auto compare = [](Node* lhs, Node* rhs) {
102 |     return lhs->total_cost() > rhs->total_cost();
103 |   };
104 |   std::priority_queue<Node*, std::vector<Node*>, decltype(compare)> O(compare);
105 |   O.push(start_node);
106 |   C.insert(start_node);
107 |   while (!O.empty()) {
108 |     // Pick nbest from O such that f(nbest) <= f(n).
109 |     Node* n_best = O.top();
110 |     // Remove n_best from O and add to C.
111 |     O.pop();
112 |     C.insert(n_best);
113 |     if (n_best == goal_node) break;
114 |     std::vector<Node*> adjacent_nodes;
115 |     Star(n_best, adjacent_nodes);
116 |     for (Node* adjacent_node : adjacent_nodes) {
117 |       if (C.find(adjacent_node) != C.end() || adjacent_node->is_visited() ||
118 |           adjacent_node->is_obstacle())
119 |         continue;
120 |       adjacent_node->set_path_length_cost(n_best->path_length_cost() +
121 |                                           c(n_best, adjacent_node));
122 |       adjacent_node->set_total_cost();
123 |       O.push(adjacent_node);
124 |       adjacent_node->set_is_visited();
125 |       adjacent_node->set_pre_node(n_best);
126 |       map.DrawNode(*adjacent_node, cv::Scalar(0, 255, 0), 1);
127 |       cv::imshow("a_star", map.background());
128 |       cv::waitKey(5);
129 |     }
130 |   }
131 | 
132 |   return true;
133 | }
134 | 
135 | void DrawPath(Node* start_node, Node* goal_node) {
136 |   const Node* path_node = goal_node->pre_node();
137 |   while (path_node != start_node) {
138 |     map.DrawNode(*path_node, cv::Scalar(128, 128, 128));
139 |     path_node = path_node->pre_node();
140 |     cv::imshow("a_star", map.background());
141 |     cv::waitKey(5);
142 |   }
143 | }
144 | 
145 | int main(int argc, char* argv[]) {
146 |   Node* start_node = map.mutable_node(10, 40);
147 |   Node* goal_node = map.mutable_node(0, 0);
148 |   map.DrawNode(*start_node, cv::Scalar(0, 0, 255));
149 |   map.DrawNode(*goal_node, cv::Scalar(255, 0, 0));
150 |   AStarAlgorithm(start_node, goal_node);
151 |   DrawPath(start_node, goal_node);
152 |   cv::imshow("a_star", map.background());
153 |   cv::waitKey();
154 |   return -1;
155 | }
156 | 


--------------------------------------------------------------------------------
/a_star/a_star.md:
--------------------------------------------------------------------------------
 1 | # A* Algorithm
 2 | ## 1. pseudocode
 3 | **Input**: A graph<br>
 4 | **Output**: A path between start and goal nodes<br>
 5 | **repeat**<br>
 6 | 　Pick $n_{best}$ from $O$ such that $f(n_{best}) \leq f(n)$, $\forall n \in O$.<br>
 7 | 　Remove $n_{best}$ from $O$ and add to C.<br>
 8 | 　If $n_{best} =  q{goal}$, EXIT.<br>
 9 | 　Expand $n_{best}$: for all $x \in Star(n{best})$ that are not in C.<br>
10 | 　**if** $x \notin O$ **then**<br>
11 | 　　add $x$ to $O$.<br>
12 | 　**else if** $g(n{best})$ + c(n{best}, x) < g(x)$ **then**<br>
13 | 　　update $x$'s backpointer to point to $n{best}$]<br>
14 | 　**end if**<br>
15 | **util** $O$ is empty.<br>
16 | 
17 | $O$ is a priority queue.<br>
18 | $C$ is a set of Node which has been processed.<br>
19 | Star($n$) represents the set of nodes which are adjacent to n.<br>
20 | $c(n_1, n_2)$ is the length of edge connecting $n_1$ and $n_2$.<br>
21 | $g(n)$ is the total length of a back pointer path from n to $q_{start}$.<br>
22 | $h(n)$ is the heuristic cost function, which returns the estimated cost of shortest path from $n$ to $q_{goal}$.<br>
23 | $f(n) = g(n) + h(n)$ is the estimated cost of shortest path from $q_{start}$ to $q_{goal}$ via $n$.<br>
24 | 


--------------------------------------------------------------------------------
/a_star/a_star_zh-CN.md:
--------------------------------------------------------------------------------
 1 | # A* 算法
 2 | ## 1. 伪代码
 3 | **输入**: 图数据结构<br>
 4 | **输出**: 从起点到终点的一条路径<br>
 5 | **repeat**<br>
 6 | 　Pick $n_{best}$ from $O$ such that $f(n_{best}) \leq f(n)$, $\forall n \in O$.<br>
 7 | 　Remove $n_{best}$ from $O$ and add to C.<br>
 8 | 　If $n_{best} =  q{goal}$, EXIT.<br>
 9 | 　Expand $n_{best}$: for all $x \in Star(n{best})$ that are not in C.<br>
10 | 　**if** $x \notin O$ **then**<br>
11 | 　　add $x$ to $O$.<br>
12 | 　**else if** $g(n{best})$ + c(n{best}, x) < g(x)$ **then**<br>
13 | 　　update $x$'s backpointer to point to $n{best}$]<br>
14 | 　**end if**<br>
15 | **util** $O$ is empty.<br>
16 | 
17 | $O$ 是一个优先队列.<br>
18 | $C$ 存储已经处理好的节点.<br>
19 | Star($n$) 代表邻近n的所有节点.<br>
20 | $c(n_1, n_2)$ $n_1$和$n_2$之间边的长度.<br>
21 | $g(n)$ 从 n 到 $q_{start}$的路径总长度.<br>
22 | $h(n)$是启发式代价函数, 返回从 $n$ 到 $q_{goal}$理论上的最短距离(两点之间的直线).<br>
23 | $f(n) = g(n) + h(n)$ 从 $q_{start}$ 到 $q_{goal}$且通过$n$的最短距离估计.<br>
24 | 


--------------------------------------------------------------------------------
/common/map.cc:
--------------------------------------------------------------------------------
 1 | #include "map.h"
 2 | 
 3 | void Map::Init() {
 4 |   for (int i = 0; i < dimension_x_; ++i) {
 5 |     std::vector<Node> row;
 6 |     for (int j = 0; j < dimension_y_; ++j) {
 7 |       row.push_back(Node(Point(i, j)));
 8 |     }
 9 |     nodes_.push_back(row);
10 |   }
11 |   cv::Mat background(dimension_x_ * kMapGridSize, dimension_y_ * kMapGridSize,
12 |                      CV_8UC3, cv::Scalar(255, 255, 255));
13 |   background_ = background;
14 | }
15 | 
16 | void Map::AddObstacle(const Obstacle& obstacle) {
17 |   for (int i = 0; i < obstacle.dimension_x_; ++i) {
18 |     for (int j = 0; j < obstacle.dimension_y_; ++j) {
19 |       nodes_[obstacle.upper_left_x_ + i][obstacle.upper_left_y_ + j]
20 |           .set_is_obstacle();
21 |     }
22 |   }
23 | }
24 | 
25 | void Map::DrawObstacles() {
26 |   for (int i = 0; i < dimension_x_; ++i) {
27 |     for (int j = 0; j < dimension_y_; ++j) {
28 |       if (nodes_[i][j].is_obstacle()) DrawNode(nodes_[i][j]);
29 |     }
30 |   }
31 | }
32 | 
33 | void Map::DrawNode(Node node, cv::Scalar color,
34 |                    int thickness) {
35 |   int32_t x = node.point().x, y = node.point().y;
36 |   cv::rectangle(background_, cv::Point(x * kMapGridSize, y * kMapGridSize),
37 |                 cv::Point((x + 1) * kMapGridSize, (y + 1) * kMapGridSize),
38 |                 color, thickness);
39 | }
40 | 


--------------------------------------------------------------------------------
/common/map.h:
--------------------------------------------------------------------------------
 1 | #ifndef PATHPLANNING_COMMON_MAP_H_
 2 | #define PATHPLANNING_COMMON_MAP_H_
 3 | 
 4 | #include <iostream>
 5 | #include <memory>
 6 | #include <opencv2/core/core.hpp>
 7 | #include <opencv2/highgui/highgui.hpp>
 8 | #include <opencv2/opencv.hpp>
 9 | #include <vector>
10 | 
11 | namespace {
12 | static constexpr int32_t kMapGridSize = 10;
13 | }  // namespace
14 | 
15 | struct Point {
16 |   Point(int32_t x, int32_t y) : x(x), y(y) {}
17 |   int32_t x;
18 |   int32_t y;
19 | };
20 | 
21 | class Node {
22 |  public:
23 |   Node(Point point)
24 |       : point_(point),
25 |         path_length_cost_(0.0),
26 |         heuristic_cost_(0.0),
27 |         total_cost_(0.0),
28 |         is_obstacle_(false),
29 |         is_visited_(false){};
30 | 
31 |   const Point& point() { return point_; }
32 |   const Node* pre_node() const { return pre_node_; }
33 |   void set_pre_node(Node* pre_node) { pre_node_ = pre_node; }
34 | 
35 |   double set_path_length_cost(double path_length_cost) {
36 |     return path_length_cost_ = path_length_cost;
37 |   }
38 |   double path_length_cost() { return path_length_cost_; }
39 |   void set_heuristic_cost(double heuristic_cost) { heuristic_cost_ = heuristic_cost; }
40 |   double heuristic_cost() { return heuristic_cost_; }
41 |   void set_total_cost() { total_cost_ = path_length_cost_ + heuristic_cost_; }
42 |   double total_cost() { return total_cost_; }
43 | 
44 |   void set_is_visited() { is_visited_ = true; }
45 |   bool is_visited() { return is_visited_; }
46 |   void set_is_obstacle() { is_obstacle_ = true; }
47 |   bool is_obstacle() { return is_obstacle_; }
48 | 
49 |  private:
50 |   Point point_;
51 |   const Node* pre_node_ = nullptr;
52 | 
53 |   double path_length_cost_;  // g(n)
54 |   double heuristic_cost_;    // h(n)
55 |   double total_cost_;        // f(n) = g(n) + h(n)
56 | 
57 |   bool is_obstacle_;
58 |   bool is_visited_;
59 | };
60 | 
61 | struct Obstacle {
62 |   int32_t upper_left_x_;
63 |   int32_t upper_left_y_;
64 |   int32_t dimension_x_;
65 |   int32_t dimension_y_;
66 | };
67 | 
68 | class Map {
69 |  public:
70 |   Map(int32_t dimension_x = 0, int32_t dimension_y = 0)
71 |       : dimension_x_(dimension_x), dimension_y_(dimension_y) {
72 |     Init();
73 |   };
74 | 
75 |   void Init();
76 |   void AddObstacle(const Obstacle& obstacle);
77 |   void DrawObstacles();
78 |   void DrawNode(Node node,
79 |                    cv::Scalar color = cv::Scalar(0, 0, 0), int thickness = -1);
80 | 
81 |   Node* mutable_node(int32_t i, int32_t j) {
82 |     return &nodes_[i][j];
83 |   }
84 |   const cv::Mat& background() { return background_; }
85 | 
86 |  private:
87 |   std::vector<std::vector<Node>> nodes_;
88 |   int32_t dimension_x_;
89 |   int32_t dimension_y_;
90 |   cv::Mat background_;
91 | };
92 | 
93 | #endif  // PATHPLANNING_COMMON_MAP_H_
94 | 


--------------------------------------------------------------------------------
/rrt/rrt.cc:
--------------------------------------------------------------------------------
  1 | #include "rrt.h"
  2 | 
  3 | Node* RRT::GetNearestNode(const std::vector<double>& random_position) {
  4 |   int min_id = -1;
  5 |   double min_distance = std::numeric_limits<double>::max();
  6 |   for (int i = 0; i < node_list_.size(); i++) {
  7 |     double square_distance =
  8 |         std::pow(node_list_[i]->point().x - random_position[0], 2) +
  9 |         std::pow(node_list_[i]->point().y - random_position[1], 2);
 10 |     if (square_distance < min_distance) {
 11 |       min_distance = square_distance;
 12 |       min_id = i;
 13 |     }
 14 |   }
 15 | 
 16 |   return node_list_[min_id];
 17 | }
 18 | 
 19 | bool RRT::CollisionCheck(Node* newNode) {
 20 |   for (auto item : obstacle_list_) {
 21 |     if (std::sqrt(std::pow((item[0] - newNode->point().x), 2) +
 22 |                   std::pow((item[1] - newNode->point().y), 2)) <= item[2])
 23 |       return false;
 24 |   }
 25 |   return true;
 26 | }
 27 | 
 28 | std::vector<Node*> RRT::Planning() {
 29 |   cv::namedWindow("RRT");
 30 | 
 31 |   int count = 0;
 32 | 
 33 |   constexpr int kImageSize = 15;
 34 |   constexpr int kImageResolution = 50;
 35 |   cv::Mat background(kImageSize * kImageResolution,
 36 |                      kImageSize * kImageResolution, CV_8UC3,
 37 |                      cv::Scalar(255, 255, 255));
 38 | 
 39 |   circle(background,
 40 |          cv::Point(start_node_->point().x * kImageResolution,
 41 |                    start_node_->point().y * kImageResolution),
 42 |          20, cv::Scalar(0, 0, 255), -1);
 43 |   circle(background,
 44 |          cv::Point(goal_node_->point().x * kImageResolution,
 45 |                    goal_node_->point().y * kImageResolution),
 46 |          20, cv::Scalar(255, 0, 0), -1);
 47 | 
 48 |   for (auto item : obstacle_list_) {
 49 |     circle(background,
 50 |            cv::Point(item[0] * kImageResolution, item[1] * kImageResolution),
 51 |            item[2] * kImageResolution, cv::Scalar(0, 0, 0), -1);
 52 |   }
 53 | 
 54 |   node_list_.push_back(start_node_);
 55 |   while (1) {
 56 |     std::vector<double> random_position;
 57 |     if (goal_dis_(goal_gen_) > goal_sample_rate_) {
 58 |       double randX = area_dis_(goal_gen_);
 59 |       double randY = area_dis_(goal_gen_);
 60 |       random_position.push_back(randX);
 61 |       random_position.push_back(randY);
 62 |     } else {
 63 |       random_position.push_back(goal_node_->point().x);
 64 |       random_position.push_back(goal_node_->point().y);
 65 |     }
 66 | 
 67 |     Node* nearestNode = GetNearestNode(random_position);
 68 |     double theta = atan2(random_position[1] - nearestNode->point().y,
 69 |                          random_position[0] - nearestNode->point().x);
 70 |     Node* newNode = new Node(nearestNode->point().x + step_size_ * cos(theta),
 71 |                              nearestNode->point().y + step_size_ * sin(theta));
 72 |     newNode->set_parent(nearestNode);
 73 | 
 74 |     if (!CollisionCheck(newNode)) continue;
 75 |     node_list_.push_back(newNode);
 76 | 
 77 |     line(background,
 78 |          cv::Point(static_cast<int>(newNode->point().x * kImageResolution),
 79 |                    static_cast<int>(newNode->point().y * kImageResolution)),
 80 |          cv::Point(static_cast<int>(nearestNode->point().x * kImageResolution),
 81 |                    static_cast<int>(nearestNode->point().y * kImageResolution)),
 82 |          cv::Scalar(0, 255, 0), 10);
 83 | 
 84 |     count++;
 85 |     imshow("RRT", background);
 86 |     cv::waitKey(5);
 87 | 
 88 |     if (sqrt(pow(newNode->point().x - goal_node_->point().x, 2) +
 89 |              pow(newNode->point().y - goal_node_->point().y, 2)) <=
 90 |         step_size_) {
 91 |       std::cout << "The path has been found!" << std::endl;
 92 |       break;
 93 |     }
 94 |   }
 95 | 
 96 |   std::vector<Node*> path;
 97 |   path.push_back(goal_node_);
 98 |   Node* tmp_node = node_list_.back();
 99 |   while (tmp_node->parent() != nullptr) {
100 |     line(
101 |         background,
102 |         cv::Point(static_cast<int>(tmp_node->point().x * kImageResolution),
103 |                   static_cast<int>(tmp_node->point().y * kImageResolution)),
104 |         cv::Point(
105 |             static_cast<int>(tmp_node->parent()->point().x * kImageResolution),
106 |             static_cast<int>(tmp_node->parent()->point().y * kImageResolution)),
107 |         cv::Scalar(255, 0, 255), 10);
108 |     path.push_back(tmp_node);
109 |     tmp_node = tmp_node->parent();
110 |   }
111 | 
112 |   imshow("RRT", background);
113 |   cv::waitKey(0);
114 |   path.push_back(start_node_);
115 |   return path;
116 | }
117 | 
118 | int main(int argc, char* argv[]) {
119 |   // (x, y, r)
120 |   std::vector<std::vector<double>> obstacle_list{{7, 5, 1},  {5, 6, 2}, {5, 8, 2},
121 |                                      {5, 10, 2}, {9, 5, 2}, {11, 5, 2}};
122 | 
123 |   Node* start_node = new Node(2.0, 2.0);
124 |   Node* goal_node = new Node(14.0, 9.0);
125 | 
126 |   RRT rrt(start_node, goal_node, obstacle_list, 0.5, 5);
127 |   rrt.Planning();
128 |   return 0;
129 | }
130 | 


--------------------------------------------------------------------------------
/rrt/rrt.h:
--------------------------------------------------------------------------------
 1 | #ifndef RRT_H_
 2 | #define RRT_H_
 3 | 
 4 | #include <cmath>
 5 | #include <iostream>
 6 | #include <opencv2/opencv.hpp>
 7 | #include <random>
 8 | #include <vector>
 9 | 
10 | struct Point {
11 |   Point(double x, double y) : x(x), y(y) {}
12 |   double x = 0.0;
13 |   double y = 0.0;
14 | };
15 | 
16 | class Node {
17 |  public:
18 |   Node(double x, double y) : point_(x, y), parent_(nullptr), cost_(0.0) {}
19 |   const Point& point() const { return point_; }
20 |   void set_parent(Node* parent) { parent_ = parent; }
21 |   Node* parent() { return parent_; }
22 | 
23 |  private:
24 |   Point point_;
25 |   std::vector<double> path_x_;
26 |   std::vector<double> path_y_;
27 |   Node* parent_ = nullptr;
28 |   double cost_ = 0.0;
29 | };
30 | 
31 | class RRT {
32 |  public:
33 |   RRT(Node* start_node, Node* goal_node,
34 |       const std::vector<std::vector<double>>& obstacle_list,
35 |       double step_size = 1.0, int goal_sample_rate = 5)
36 |       : start_node_(start_node),
37 |         goal_node_(goal_node),
38 |         obstacle_list_(obstacle_list),
39 |         step_size_(step_size),
40 |         goal_sample_rate_(goal_sample_rate),
41 |         goal_gen_(goal_rd_()),
42 |         goal_dis_(std::uniform_int_distribution<int>(0, 100)),
43 |         area_gen_(area_rd_()),
44 |         area_dis_(std::uniform_real_distribution<double>(0, 15)) {}
45 |   Node* GetNearestNode(const std::vector<double>& random_position);
46 |   bool CollisionCheck(Node*);
47 |   std::vector<Node*> Planning();
48 | 
49 |  private:
50 |   Node* start_node_;
51 |   Node* goal_node_;
52 |   std::vector<std::vector<double>> obstacle_list_;
53 |   std::vector<Node*> node_list_;
54 |   double step_size_;
55 | 
56 |   int goal_sample_rate_;
57 | 
58 |   std::random_device goal_rd_;
59 |   std::mt19937 goal_gen_;
60 |   std::uniform_int_distribution<int> goal_dis_;
61 | 
62 |   std::random_device area_rd_;
63 |   std::mt19937 area_gen_;
64 |   std::uniform_real_distribution<double> area_dis_;
65 | };
66 | 
67 | #endif
68 | 


--------------------------------------------------------------------------------