├── .gitignore ├── A_Star ├── Astar.cpp ├── Astar.h └── main.cpp ├── B-spline ├── BSpline.cpp ├── BSpline.h └── main.cpp ├── Bezier ├── BezierCurve.cpp ├── BezierCurve.h └── main.cpp ├── CMakeLists.txt ├── Dijkstra ├── Dijkstra.cpp ├── Dijkstra.h └── main.cpp ├── README.assets ├── dijkstra_demo.gif ├── rrt_connect-16832995765544.gif ├── rrt_connect-16832995877726.gif ├── rrt_connect1.gif └── rrt_star_demo.gif ├── README.md ├── Rapidly-exploring_Random_Tree ├── RRT.cpp ├── RRT.h └── main.cpp ├── Rapidly-exploring_Random_Tree_Star ├── RRT_Star.cpp ├── RRT_Star.h └── main.cpp ├── Rapidly-exploring_Random_Tree_connect ├── RRT_connect.cpp ├── RRT_connect.h └── main.cpp ├── gif ├── astar.gif ├── b_spline_demo.gif ├── bezier_demo.gif ├── dijkstra_demo.gif ├── rrt_connect.gif ├── rrt_demo.gif └── rrt_star_demo.gif └── matplotlibcpp.h /.gitignore: -------------------------------------------------------------------------------- 1 | /.vscode 2 | /build -------------------------------------------------------------------------------- /A_Star/Astar.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Astar.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-16 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "Astar.h" 13 | 14 | int gifindex = 0; 15 | Astar::Node::Node(double x, double y, float cost, double parentIndex) : x(x), y(y), cost(cost), parent_index(parentIndex) {} 16 | 17 | Astar::Astar(double resolution, double robotRadius) : resolution(resolution), robot_radius(robotRadius) {} 18 | 19 | /** 20 | * @brief 设置障碍物以及边缘 21 | * 22 | */ 23 | void Astar::setObstale(vector &ox, vector &oy) 24 | { 25 | for (double i = -10; i < 60; i++) 26 | { 27 | ox.push_back(i); 28 | oy.push_back(-10.0); 29 | } 30 | for (double i = -10; i < 60; i++) 31 | { 32 | ox.push_back(60.0); 33 | oy.push_back(i); 34 | } 35 | for (double i = -10; i < 61; i++) 36 | { 37 | ox.push_back(i); 38 | oy.push_back(60.0); 39 | } 40 | for (double i = -10; i < 61; i++) 41 | { 42 | ox.push_back(-10.0); 43 | oy.push_back(i); 44 | } 45 | for (double i = -10; i < 10; i++) 46 | { 47 | ox.push_back(i); 48 | oy.push_back(10); 49 | } 50 | for (double i = 0; i < 30; i++) 51 | { 52 | ox.push_back(40.0 - i); 53 | oy.push_back(30); 54 | } 55 | for (double i = 0; i < 15; i++) 56 | { 57 | ox.push_back(60.0 - i); 58 | oy.push_back(30); 59 | } 60 | } 61 | 62 | /** 63 | * @brief 得到障碍物信息图，有障碍物的地方标记为true，没有标记为false 64 | * @param ox 障碍物x坐标集合 65 | * @param oy 障碍物y坐标集合 66 | */ 67 | void Astar::calObstacleMap(const vector &ox, const vector &oy) 68 | { 69 | min_x = round(*min_element(ox.begin(), ox.end())); 70 | min_y = round(*min_element(oy.begin(), oy.end())); 71 | max_x = round(*max_element(ox.begin(), ox.end())); 72 | max_y = round(*max_element(oy.begin(), oy.end())); 73 | 74 | cout << "min_x:" << min_x << " min_y:" << min_y << " max_x:" << max_x << " max_y:" << max_y << endl; 75 | 76 | x_width = round((max_x - min_x) / resolution); 77 | y_width = round((max_y - min_y) / resolution); 78 | cout << "x_width:" << x_width << " y_width:" << y_width << endl; 79 | 80 | obstacle_map = vector>(x_width, vector(y_width, false)); 81 | 82 | for (double i = 0; i < x_width; i++) 83 | { 84 | double x = calPosition(i, min_x); 85 | for (double j = 0; j < y_width; j++) 86 | { 87 | double y = calPosition(j, min_y); 88 | for (double k = 0; k < ox.size(); k++) 89 | { 90 | double d = sqrt(pow(ox[k] - x, 2) + pow(oy[k] - y, 2)); 91 | if (d <= robot_radius) 92 | { 93 | obstacle_map[i][j] = true; 94 | break; 95 | } 96 | } 97 | } 98 | } 99 | } 100 | 101 | /** 102 | * @brief 计算栅格在地图中的位置 103 | * @param index 104 | * @param minp 105 | * @return 106 | */ 107 | double Astar::calPosition(double index, double minp) 108 | { 109 | double pos = index * resolution + minp; 110 | return pos; 111 | } 112 | 113 | /** 114 | * @brief 标记移动代价 115 | * @return 116 | */ 117 | void Astar::getMotionModel() 118 | { 119 | // x,y,cost 120 | motion = {{1, 0, 1}, 121 | {0, 1, 1}, 122 | {-1, 0, 1}, 123 | {0, -1, 1}, 124 | {-1, -1, sqrt(2)}, 125 | {-1, 1, sqrt(2)}, 126 | {1, -1, sqrt(2)}, 127 | {1, 1, sqrt(2)}}; 128 | } 129 | 130 | /** 131 | * @brief 计算起点终点的栅格索引 132 | * @param position 133 | * @param minp 134 | * @return 135 | */ 136 | double Astar::calXyIndex(double position, double minp) 137 | { 138 | return round((position - minp) / resolution); 139 | } 140 | 141 | /** 142 | * @brief 计算栅格索引 143 | * @param node 144 | * @return 145 | */ 146 | double Astar::calIndex(Astar::Node *node) 147 | { 148 | // cout<x<<","<y<y - min_y) * x_width + (node->x - min_x); 150 | } 151 | 152 | /** 153 | * @brief 判断节点是否有效，即是否超出边界和碰到障碍物 154 | * @param node 155 | * @return 156 | */ 157 | bool Astar::verifyNode(Astar::Node *node) 158 | { 159 | double px = calPosition(node->x, min_x); 160 | double py = calPosition(node->y, min_y); 161 | // 超出边界 162 | if (px < min_x || py < min_y || px >= max_x || py >= max_y) 163 | return false; 164 | // 遇到障碍物 165 | if (obstacle_map[node->x][node->y]) 166 | return false; 167 | return true; 168 | } 169 | 170 | /** 171 | * @brief 根据parent_index，倒推出路径 172 | * @param goal_node 173 | * @param closed_set 174 | * @return 175 | */ 176 | pair, vector> Astar::calFinalPath(Astar::Node *goal_node, map closed_set) 177 | { 178 | vector rx, ry; 179 | rx.push_back(calPosition(goal_node->x, min_x)); 180 | ry.push_back(calPosition(goal_node->y, min_y)); 181 | 182 | double parent_index = goal_node->parent_index; 183 | 184 | while (parent_index != -1) 185 | { 186 | Node *node = closed_set[parent_index]; 187 | rx.push_back(calPosition(node->x, min_x)); 188 | ry.push_back(calPosition(node->y, min_y)); 189 | parent_index = node->parent_index; 190 | } 191 | return {rx, ry}; 192 | } 193 | 194 | /** 195 | * @brief 规划 196 | * @param start 起点 197 | * @param goal 终点 198 | * @return 规划后的路径 199 | */ 200 | pair, vector> Astar::planning(const vector start, const vector goal) 201 | { 202 | double sx = start[0], sy = start[1]; 203 | double gx = goal[0], gy = goal[1]; 204 | Node *start_node = new Node(calXyIndex(sx, min_x), calXyIndex(sy, min_y), 0.0, -1); 205 | Node *goal_node = new Node(calXyIndex(gx, min_x), calXyIndex(gy, min_y), 0.0, -1); 206 | 207 | map open_set, closed_set; 208 | // 将起点加入到open set 209 | open_set[calIndex(start_node)] = start_node; 210 | 211 | Node *current = nullptr; 212 | while (true) 213 | { 214 | double cur_id = numeric_limits::max(); 215 | double cost = numeric_limits::max(); 216 | // 计算代价最小的节点,与dijkstra代码维一不同的地方，即启发函数的计算不同，其他都一样 217 | for (auto it = open_set.begin(); it != open_set.end(); it++) 218 | { 219 | double now_cost = it->second->cost + calHeuristic(goal_node, it->second); 220 | if (now_cost < cost) 221 | { 222 | cost = now_cost; 223 | cur_id = it->first; 224 | } 225 | } 226 | current = open_set[cur_id]; 227 | 228 | plotGraph(current); // 画图 229 | 230 | // 若找到了目标结点，则退出循环 231 | if (abs(current->x - goal_node->x) < EPS && abs(current->y - goal_node->y) < EPS) 232 | { 233 | cout << "Find goal" << endl; 234 | goal_node->parent_index = current->parent_index; 235 | goal_node->cost = current->cost; 236 | break; 237 | } 238 | 239 | // 从open set中去除 240 | auto iter = open_set.find(cur_id); 241 | open_set.erase(iter); 242 | // 将其加入到closed set 243 | closed_set[cur_id] = current; 244 | 245 | // 根据motion，扩展搜索网络 246 | for (vector move : motion) 247 | { 248 | // cout<x + move[0], current->y + move[1], current->cost + move[2], cur_id); 251 | node->cost += calHeuristic(goal_node, node); 252 | double n_id = calIndex(node); 253 | // 如果已经在closed_set中了 254 | if (closed_set.find(n_id) != closed_set.end()) 255 | continue; 256 | // 如果超出边界或者碰到障碍物了 257 | if (!verifyNode(node)) 258 | continue; 259 | // 如果open set中没有这个节点 260 | if (open_set.find(n_id) == open_set.end()) 261 | { 262 | open_set[n_id] = node; 263 | } 264 | // 如果open set中已经存在这个节点,则更新cost 265 | else 266 | { 267 | if (open_set[n_id]->cost >= node->cost) 268 | { 269 | open_set[n_id] = node; 270 | } 271 | } 272 | } 273 | } 274 | return calFinalPath(goal_node, closed_set); 275 | } 276 | /** 277 | * @brief 启发函数计算，与dijkstra不同 278 | * 279 | * @param n1 280 | * @param n2 281 | * @return double 282 | */ 283 | double Astar::calHeuristic(Node *n1, Node *n2) 284 | { 285 | double w = 1.0; // 启发函数权重 286 | double d = w * sqrt(pow(n1->x - n2->x, 2) + pow(n1->y - n1->x, 2)); 287 | return d; 288 | } 289 | 290 | /** 291 | * @brief 画图 292 | * @param current 293 | */ 294 | void Astar::plotGraph(Astar::Node *current) 295 | { 296 | plt::plot(vector{calPosition(current->x, min_x)}, vector{calPosition(current->y, min_y)}, "xc"); 297 | // // 将每一帧保存为单独的文件 298 | // stringstream filename; 299 | // filename << "./frame_" << gifindex++ << ".png"; 300 | // plt::save(filename.str()); 301 | plt::pause(0.0000001); 302 | } 303 | 304 | /** 305 | * @brief 设置坐标 306 | * 307 | * @param st 起点 308 | * @param go 目标 309 | * @param ox 障碍物x 310 | * @param oy 障碍物y 311 | */ 312 | void Astar::set(const vector &st, const vector &go, const vector &ox, const vector &oy) 313 | { 314 | Astar::st = st; 315 | Astar::go = go; 316 | Astar::ox = ox; 317 | Astar::oy = oy; 318 | } 319 | -------------------------------------------------------------------------------- /A_Star/Astar.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Astar.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-10 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef Astar_H 13 | #define Astar_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "../matplotlibcpp.h" 24 | 25 | using namespace std; 26 | using namespace Eigen; 27 | namespace plt = matplotlibcpp; 28 | #define EPS 1e-4 29 | #define PI 3.14159265354 30 | 31 | //extern int gifindex; 32 | class Astar 33 | { 34 | public: 35 | struct Node 36 | { 37 | double x; 38 | double y; 39 | float cost; 40 | // Node* p_node; 41 | double parent_index; 42 | 43 | Node(double x, double y, float cost, double parentIndex); 44 | }; 45 | 46 | private: 47 | double resolution; // 栅格大小 48 | double robot_radius; 49 | double min_x, min_y, max_x, max_y; // 地图范围 50 | double x_width, y_width; // 长宽 51 | vector> obstacle_map; // 障碍物地图 52 | vector> motion; // 障碍物地图 53 | vector st, go; 54 | vector ox, oy; 55 | 56 | public: 57 | Astar(double resolution, double robotRadius); 58 | void setObstale(vector &ox, vector &oy); 59 | void calObstacleMap(const vector &ox, const vector &oy); 60 | 61 | double calPosition(double index, double minp); 62 | 63 | void getMotionModel(); 64 | 65 | double calXyIndex(double position, double minp); 66 | 67 | double calIndex(Node *node); 68 | 69 | bool verifyNode(Node *node); 70 | 71 | pair, vector> calFinalPath(Node *goal_node, map closed_set); 72 | 73 | pair, vector> planning(vector start, vector goal); 74 | double calHeuristic(Node *n1, Node *n2); 75 | 76 | void plotGraph(Node *current); 77 | 78 | void set(const vector &st, const vector &go, const vector &ox, const vector &oy); 79 | }; 80 | 81 | #endif // ASTAR_H 82 | -------------------------------------------------------------------------------- /A_Star/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 主函数 5 | * @version 0.1 6 | * @date 2023-04-10 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "Astar.h" 13 | 14 | int main() 15 | { 16 | vector start{-5, -5}, goal{50, 50}; 17 | double grid_size = 2.0; 18 | double robot_radius = 1.0; 19 | 20 | vector ox; 21 | vector oy; 22 | 23 | Astar astar(grid_size, robot_radius); 24 | astar.setObstale(ox, oy); // 设置障碍物信息 25 | astar.set(start, goal, ox, oy); // 设置起点，目标点，障碍物的(x,y) 26 | astar.calObstacleMap(ox, oy); // 在地图上生成障碍物 27 | astar.getMotionModel(); // 创建移动代价 28 | 29 | // 绘制地图 30 | plt::plot(ox, oy, ".k"); 31 | plt::plot(vector{start[0]}, vector{start[1]}, "ob"); 32 | plt::plot(vector{goal[0]}, vector{goal[1]}, "or"); 33 | plt::grid(true); 34 | 35 | // 规划路径 36 | pair, vector> xy = astar.planning(start, goal); 37 | // 绘制路径 38 | plt::plot(xy.first, xy.second, "-r"); 39 | 40 | // stringstream filename; 41 | // filename << "./frame_" << gifindex << ".png"; 42 | // plt::save(filename.str()); 43 | 44 | // // 合成 GIF 图片 45 | // const char *gif_filename = "./astar_demo.gif"; 46 | // stringstream cmd; 47 | // cmd << "convert -delay 2 -loop 0 ./frame_*.png " << gif_filename; 48 | // system(cmd.str().c_str()); 49 | // cout << "Saving result to " << gif_filename << std::endl; 50 | // plt::show(); 51 | // //删除png图片 52 | // system("rm *.png"); 53 | // return 0; 54 | 55 | // 保存图片 56 | const char *filename = "./astar_demo.png"; 57 | cout << "Saving result to " << filename << std::endl; 58 | plt::save(filename); 59 | plt::show(); 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /B-spline/BSpline.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file BSpline.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-22 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "BSpline.h" 13 | 14 | /** 15 | * @brief 基函数定义 16 | * @param i 17 | * @param k B样条阶数k 18 | * @param u 自变量 19 | * @param knots 节点向量 array([u0,u1,u2,...,u_n+k],shape=[1,n+k+1]. 20 | */ 21 | double basic(int i, int k, double u, vector node_vector) 22 | { 23 | // 0次B样条（1阶B样条） 24 | double Bik_u; 25 | if (k == 1) 26 | { 27 | if (u >= node_vector[i] && u < node_vector[i + 1]) 28 | { 29 | Bik_u = 1; 30 | } 31 | else 32 | { 33 | Bik_u = 0; 34 | } 35 | } 36 | else 37 | { 38 | // 公式中的两个分母 39 | double denominator_1 = node_vector[i + k - 1] - node_vector[i]; 40 | double denominator_2 = node_vector[i + k] - node_vector[i + 1]; 41 | // # 如果遇到分母为 0的情况： 42 | // # 1. 如果此时分子也为0，约定这一项整体为0； 43 | // # 2. 如果此时分子不为0，则约定分母为1 。 44 | if (denominator_1 == 0) 45 | denominator_1 = 1; 46 | if (denominator_2 == 0) 47 | denominator_2 = 1; 48 | Bik_u = (u - node_vector[i]) / denominator_1 * basic(i, k - 1, u, node_vector) + (node_vector[i + k] - u) / denominator_2 * 49 | basic(i + 1, k - 1, u, node_vector); 50 | } 51 | return Bik_u; 52 | } 53 | 54 | /** 55 | * @brief 准均匀B样条的节点向量计算 56 | * 首末值定义为 0 和 1 57 | * @param n 控制点n个 58 | * @param k B样条阶数k， k阶B样条，k-1次曲线. 59 | * @return vector 60 | */ 61 | vector QuasiUniform(int n, int k) 62 | { 63 | int numKnots = n + k; // 节点个数 64 | vector knots(numKnots); 65 | 66 | for (int i = 0; i < numKnots; i++) 67 | { 68 | if (i < k) 69 | knots[i] = 0.0f; // 前k个节点向量为0 70 | else if (i >= n) 71 | knots[i] = 1.0f; // 后k个节点向量为1 72 | else 73 | { 74 | // 计算均匀参数值 75 | knots[i] += knots[i - 1] + (double)1 / (n - k + 1); 76 | } 77 | } 78 | 79 | return knots; 80 | } 81 | 82 | /** 83 | * @brief 均匀B样条的节点向量计算 84 | * 85 | * @param n 控制点n个 86 | * @param k B样条阶数k， k阶B样条，k-1次曲线 87 | * @return vector 88 | */ 89 | vector Uniform(int n, int k) 90 | { 91 | int numKnots = n + k; 92 | vector knots(numKnots); 93 | 94 | for (int i = 0; i < numKnots; i++) 95 | { 96 | knots[i] = (double)i / (double)(numKnots - 1); 97 | } 98 | 99 | return knots; 100 | } -------------------------------------------------------------------------------- /B-spline/BSpline.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file BSpline.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-23 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef BSPLINE_H 13 | #define BSPLINE_H 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | using namespace std; 20 | using namespace Eigen; 21 | 22 | double basic(int i, int k, double u, vector knots); 23 | 24 | vector QuasiUniform(int n, int k); 25 | 26 | vector Uniform(int n, int k); 27 | 28 | #endif // BSPLINE_H 29 | -------------------------------------------------------------------------------- /B-spline/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-23 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "BSpline.h" 13 | #include "../matplotlibcpp.h" 14 | namespace plt = matplotlibcpp; 15 | 16 | int main() 17 | { 18 | // 控制点 19 | vector points{Vector2d(0, 0), Vector2d(1, 1), Vector2d(2, 1), Vector2d(3, 1), Vector2d(4, 2)}; 20 | 21 | // 生成控制点 22 | vector x(points.size()), y(points.size()); 23 | for (int i = 0; i < points.size(); ++i) 24 | { 25 | x[i] = points[i][0]; 26 | y[i] = points[i][1]; 27 | } 28 | 29 | // 生成曲线点 30 | vector x_, y_; 31 | int n = points.size(); // 控制点个数 32 | int k = 3; // k阶、k-1次B样条 33 | Vector2d p_u(0.0, 0.0); 34 | vector b(n); 35 | 36 | int flag; 37 | cout << "请选择：1. 均匀B样条 2.准均匀B样条 0. 退出 " << endl; 38 | cin >> flag; 39 | vector knots; 40 | switch (flag) 41 | { 42 | case 1: // 均匀B样条 43 | knots = Uniform(n, k); 44 | for (double u = (double)(k - 1) / (n + k); u < (double)(n + 1) / (n + k); u += 0.005) 45 | { 46 | plt::clf(); 47 | for (int i = 0; i < n; i++) 48 | { 49 | b[i] = basic(i, k, u, knots); 50 | // cout << b[i] << endl; 51 | } 52 | for (int i = 0; i < points.size(); i++) 53 | { 54 | p_u += points[i] * b[i]; 55 | } 56 | x_.push_back(p_u[0]); 57 | y_.push_back(p_u[1]); 58 | // 画图 59 | // plt::xlim(0,1); 60 | plt::plot(x_, y_, "r"); 61 | plt::plot(x, y); 62 | plt::pause(0.01); 63 | p_u = Vector2d(0, 0); 64 | } 65 | break; 66 | case 2: // 准均匀B样条 67 | knots = QuasiUniform(n, k); 68 | for (double u = 0; u < 1; u += 0.005) 69 | { 70 | plt::clf(); 71 | for (int i = 0; i < n; i++) 72 | { 73 | b[i] = basic(i, k, u, knots); 74 | // cout << b[i] << endl; 75 | } 76 | for (int i = 0; i < points.size(); i++) 77 | { 78 | p_u += points[i] * b[i]; 79 | // cout<(n - i + 1) / static_cast(i); 25 | } 26 | return result; 27 | } 28 | 29 | /** 30 | * 计算n阶贝塞尔曲线上的点 31 | * @param points 控制点 32 | * @param t 参数t 33 | * @return n阶贝塞尔曲线上的点 34 | */ 35 | Vector2d bezier(const vector &points, double t) 36 | { 37 | 38 | int n = points.size() - 1; 39 | Vector2d result; 40 | for (int i = 0; i <= n; ++i) 41 | { 42 | result += combination(n, i) * pow(t, i) * pow(1 - t, n - i) * points[i]; 43 | } 44 | return result; 45 | } -------------------------------------------------------------------------------- /Bezier/BezierCurve.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file BezierCurve.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-21 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef BEZIERCURVE_H 13 | #define BEZIERCURVE_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | using namespace std; 21 | using namespace Eigen; 22 | 23 | double combination(int n, int k); 24 | 25 | Vector2d bezier(const vector& points, double t); 26 | 27 | 28 | #endif // BEZIERCURVE_H 29 | -------------------------------------------------------------------------------- /Bezier/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-21 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "BezierCurve.h" 13 | #include "../matplotlibcpp.h" 14 | namespace plt = matplotlibcpp; 15 | 16 | int main() 17 | { 18 | // 控制点 19 | vector points{Vector2d(0, 0), Vector2d(1, 1), Vector2d(2.5, 1.5), Vector2d(3, 1), Vector2d(4, 2)}; 20 | 21 | // 生成控制点 22 | cout << "Plotting..." << endl; 23 | vector x(points.size()), y(points.size()); 24 | for (int i = 0; i < points.size(); ++i) 25 | { 26 | x[i] = points[i][0]; 27 | y[i] = points[i][1]; 28 | } 29 | 30 | // 生成贝塞尔曲线 31 | const int n_points = 100; 32 | vector curve(n_points); 33 | vector x_curve, y_curve; 34 | for (int i = 0; i < n_points; ++i) 35 | { 36 | plt::clf(); 37 | double t = static_cast(i) / n_points; 38 | curve[i] = bezier(points, t); 39 | x_curve.emplace_back(curve[i][0]); 40 | y_curve.emplace_back(curve[i][1]); 41 | // 绘制控制点和曲线 42 | plt::plot(x_curve, y_curve, "r"); 43 | plt::plot(x, y); 44 | // // 将每一帧保存为单独的文件 45 | // stringstream filename; 46 | // filename << "./frame_" << i << ".png"; 47 | // plt::save(filename.str()); 48 | plt::pause(0.01); 49 | } 50 | // // 合成 GIF 图片 51 | // const char *gif_filename = "./bezier_demo.gif"; 52 | // stringstream cmd; 53 | // cmd << "convert -delay 10 -loop 0 ./frame_*.png " << gif_filename; 54 | // system(cmd.str().c_str()); 55 | // cout << "Saving result to " << gif_filename << std::endl; 56 | // plt::show(); 57 | // //删除png图片 58 | // system("rm *.png"); 59 | // return 0; 60 | 61 | 62 | // save figure 63 | const char* filename = "./bezier_demo.png"; 64 | cout << "Saving result to " << filename << std::endl; 65 | plt::save(filename); 66 | plt::show(); 67 | return 0; 68 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.21) 2 | project(path_planning) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | 7 | # include(GNUInstallDirs) 8 | set(PACKAGE_NAME path_planning) 9 | 10 | # output 11 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) 12 | 13 | 14 | # Library target 15 | add_library(path_planning INTERFACE) 16 | 17 | # TODO: Use `Development.Embed` component when requiring cmake >= 3.18 18 | find_package(Python COMPONENTS Interpreter Development NumPy REQUIRED) 19 | target_link_libraries(path_planning INTERFACE 20 | Python::Python 21 | Python::Module 22 | Python::NumPy 23 | ) 24 | install( 25 | TARGETS path_planning 26 | EXPORT install_targets 27 | ) 28 | 29 | 30 | find_package(Eigen3 REQUIRED) 31 | include_directories(${EIGEN3_INCLUDE_DIR}) 32 | 33 | 34 | # bezier 35 | add_executable(bezier_demo Bezier/main.cpp Bezier/BezierCurve.cpp) 36 | target_link_libraries(bezier_demo PRIVATE path_planning) 37 | 38 | # B-spline 39 | add_executable(b_spline_demo B-spline/main.cpp B-spline/BSpline.cpp) 40 | target_link_libraries(b_spline_demo PRIVATE path_planning) 41 | 42 | 43 | # RRT 44 | add_executable(rrt_demo Rapidly-exploring_Random_Tree/main.cpp Rapidly-exploring_Random_Tree/RRT.cpp) 45 | target_link_libraries(rrt_demo PRIVATE path_planning) 46 | 47 | 48 | # RRT_connect 49 | add_executable(rrt_connect_demo Rapidly-exploring_Random_Tree_connect/main.cpp Rapidly-exploring_Random_Tree_connect/RRT_connect.cpp) 50 | target_link_libraries(rrt_connect_demo path_planning) 51 | 52 | # RRT_star 53 | add_executable(rrt_star_demo Rapidly-exploring_Random_Tree_Star/main.cpp Rapidly-exploring_Random_Tree_Star/RRT_Star.cpp Rapidly-exploring_Random_Tree/RRT.cpp) 54 | target_link_libraries(rrt_star_demo path_planning) 55 | 56 | # Dijkstra 57 | add_executable(dijkstra_demo Dijkstra/main.cpp Dijkstra/Dijkstra.cpp) 58 | target_link_libraries(dijkstra_demo path_planning) 59 | 60 | # astar 61 | add_executable(astar_demo A_Star/Astar.cpp A_Star/main.cpp) 62 | target_link_libraries(astar_demo path_planning) -------------------------------------------------------------------------------- /Dijkstra/Dijkstra.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Dijkstra.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-16 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "Dijkstra.h" 13 | 14 | //int gifindex = 0; 15 | 16 | Dijkstra::Node::Node(double x, double y, float cost, double parentIndex) : x(x), y(y), cost(cost), parent_index(parentIndex) {} 17 | 18 | Dijkstra::Dijkstra(double resolution, double robotRadius) : resolution(resolution), robot_radius(robotRadius) {} 19 | 20 | /** 21 | * @brief 设置障碍物以及边缘 22 | * 23 | */ 24 | void Dijkstra::setObstale(vector &ox, vector &oy) 25 | { 26 | for (double i = -10; i < 60; i++) 27 | { 28 | ox.push_back(i); 29 | oy.push_back(-10.0); 30 | } 31 | for (double i = -10; i < 60; i++) 32 | { 33 | ox.push_back(60.0); 34 | oy.push_back(i); 35 | } 36 | for (double i = -10; i < 61; i++) 37 | { 38 | ox.push_back(i); 39 | oy.push_back(60.0); 40 | } 41 | for (double i = -10; i < 61; i++) 42 | { 43 | ox.push_back(-10.0); 44 | oy.push_back(i); 45 | } 46 | for (double i = -10; i < 10; i++) 47 | { 48 | ox.push_back(i); 49 | oy.push_back(10); 50 | } 51 | for (double i = 0; i < 30; i++) 52 | { 53 | ox.push_back(40.0 - i); 54 | oy.push_back(30); 55 | } 56 | for (double i = 0; i < 15; i++) 57 | { 58 | ox.push_back(60.0 - i); 59 | oy.push_back(30); 60 | } 61 | } 62 | 63 | /** 64 | * @brief 得到障碍物信息图，有障碍物的地方标记为true，没有标记为false 65 | * @param ox 障碍物x坐标集合 66 | * @param oy 障碍物y坐标集合 67 | */ 68 | void Dijkstra::calObstacleMap(const vector &ox, const vector &oy) 69 | { 70 | min_x = round(*min_element(ox.begin(), ox.end())); 71 | min_y = round(*min_element(oy.begin(), oy.end())); 72 | max_x = round(*max_element(ox.begin(), ox.end())); 73 | max_y = round(*max_element(oy.begin(), oy.end())); 74 | 75 | cout << "min_x:" << min_x << " min_y:" << min_y << " max_x:" << max_x << " max_y:" << max_y << endl; 76 | 77 | x_width = round((max_x - min_x) / resolution); 78 | y_width = round((max_y - min_y) / resolution); 79 | cout << "x_width:" << x_width << " y_width:" << y_width << endl; 80 | 81 | obstacle_map = vector>(x_width, vector(y_width, false)); 82 | 83 | for (double i = 0; i < x_width; i++) 84 | { 85 | double x = calPosition(i, min_x); 86 | for (double j = 0; j < y_width; j++) 87 | { 88 | double y = calPosition(j, min_y); 89 | for (double k = 0; k < ox.size(); k++) 90 | { 91 | double d = sqrt(pow(ox[k] - x, 2) + pow(oy[k] - y, 2)); 92 | if (d <= robot_radius) 93 | { 94 | obstacle_map[i][j] = true; 95 | break; 96 | } 97 | } 98 | } 99 | } 100 | } 101 | 102 | /** 103 | * @brief 计算栅格在地图中的位置 104 | * @param index 105 | * @param minp 106 | * @return 107 | */ 108 | double Dijkstra::calPosition(double index, double minp) 109 | { 110 | double pos = index * resolution + minp; 111 | return pos; 112 | } 113 | 114 | /** 115 | * @brief 标记移动代价 116 | * @return 117 | */ 118 | void Dijkstra::getMotionModel() 119 | { 120 | // x,y,cost 121 | motion = {{1, 0, 1}, 122 | {0, 1, 1}, 123 | {-1, 0, 1}, 124 | {0, -1, 1}, 125 | {-1, -1, sqrt(2)}, 126 | {-1, 1, sqrt(2)}, 127 | {1, -1, sqrt(2)}, 128 | {1, 1, sqrt(2)}}; 129 | } 130 | 131 | /** 132 | * @brief 计算起点终点的栅格索引 133 | * @param position 134 | * @param minp 135 | * @return 136 | */ 137 | double Dijkstra::calXyIndex(double position, double minp) 138 | { 139 | return round((position - minp) / resolution); 140 | } 141 | 142 | /** 143 | * @brief 计算栅格索引 144 | * @param node 145 | * @return 146 | */ 147 | double Dijkstra::calIndex(Dijkstra::Node *node) 148 | { 149 | // cout<x<<","<y<y - min_y) * x_width + (node->x - min_x); 151 | } 152 | 153 | /** 154 | * @brief 判断节点是否有效，即是否超出边界和碰到障碍物 155 | * @param node 156 | * @return 157 | */ 158 | bool Dijkstra::verifyNode(Dijkstra::Node *node) 159 | { 160 | double px = calPosition(node->x, min_x); 161 | double py = calPosition(node->y, min_y); 162 | // 超出边界 163 | if (px < min_x || py < min_y || px >= max_x || py >= max_y) 164 | return false; 165 | // 遇到障碍物 166 | if (obstacle_map[node->x][node->y]) 167 | return false; 168 | return true; 169 | } 170 | 171 | /** 172 | * @brief 根据parent_index，倒推出路径 173 | * @param goal_node 174 | * @param closed_set 175 | * @return 176 | */ 177 | pair, vector> Dijkstra::calFinalPath(Dijkstra::Node *goal_node, map closed_set) 178 | { 179 | vector rx, ry; 180 | rx.push_back(calPosition(goal_node->x, min_x)); 181 | ry.push_back(calPosition(goal_node->y, min_y)); 182 | 183 | double parent_index = goal_node->parent_index; 184 | 185 | while (parent_index != -1) 186 | { 187 | Node *node = closed_set[parent_index]; 188 | rx.push_back(calPosition(node->x, min_x)); 189 | ry.push_back(calPosition(node->y, min_y)); 190 | parent_index = node->parent_index; 191 | } 192 | return {rx, ry}; 193 | } 194 | 195 | /** 196 | * @brief 规划 197 | * @param start 起点 198 | * @param goal 终点 199 | * @return 规划后的路径 200 | */ 201 | pair, vector> Dijkstra::planning(const vector start, const vector goal) 202 | { 203 | double sx = start[0], sy = start[1]; 204 | double gx = goal[0], gy = goal[1]; 205 | Node *start_node = new Node(calXyIndex(sx, min_x), calXyIndex(sy, min_y), 0.0, -1); 206 | Node *goal_node = new Node(calXyIndex(gx, min_x), calXyIndex(gy, min_y), 0.0, -1); 207 | 208 | map open_set, closed_set; 209 | // 将起点加入到open set 210 | open_set[calIndex(start_node)] = start_node; 211 | 212 | Node *current = nullptr; 213 | while (true) 214 | { 215 | double cur_id = numeric_limits::max(); 216 | double cost = numeric_limits::max(); 217 | // 计算open_set中代价最小的节点（此处可以用优先队列优化） 218 | for (auto it = open_set.begin(); it != open_set.end(); it++) 219 | { 220 | if (it->second->cost < cost) 221 | { 222 | cost = it->second->cost; 223 | cur_id = it->first; // index 224 | } 225 | } 226 | current = open_set[cur_id]; 227 | 228 | plotGraph(current); // 画图 229 | 230 | // 若找到了目标结点，则退出循环 231 | if (abs(current->x - goal_node->x) < EPS && abs(current->y - goal_node->y) < EPS) 232 | { 233 | cout << "Find goal" << endl; 234 | goal_node->parent_index = current->parent_index; 235 | goal_node->cost = current->cost; 236 | break; 237 | } 238 | 239 | // 从open set中去除 240 | auto iter = open_set.find(cur_id); 241 | open_set.erase(iter); 242 | // 将其加入到closed set 243 | closed_set[cur_id] = current; 244 | 245 | // 根据motion，扩展搜索网络 246 | for (vector move : motion) 247 | { 248 | // cout<x + move[0], current->y + move[1], current->cost + move[2], cur_id); 251 | double n_id = calIndex(node); 252 | // 如果已经在closed_set中了 253 | if (closed_set.find(n_id) != closed_set.end()) 254 | continue; 255 | // 如果超出边界或者碰到障碍物了 256 | if (!verifyNode(node)) 257 | continue; 258 | // 如果open set中没有这个节点 259 | if (open_set.find(n_id) == open_set.end()) 260 | { 261 | open_set[n_id] = node; 262 | } 263 | // 如果open set中已经存在这个节点,则更新cost 264 | else 265 | { 266 | if (open_set[n_id]->cost >= node->cost) 267 | { 268 | open_set[n_id] = node; 269 | } 270 | } 271 | } 272 | } 273 | return calFinalPath(goal_node, closed_set); 274 | } 275 | 276 | /** 277 | * @brief 画图 278 | * @param current 279 | */ 280 | void Dijkstra::plotGraph(Dijkstra::Node *current) 281 | { 282 | plt::plot(vector{calPosition(current->x, min_x)}, vector{calPosition(current->y, min_y)}, "xc"); 283 | // // 将每一帧保存为单独的文件 284 | // stringstream filename; 285 | // filename << "./frame_" << gifindex++ << ".png"; 286 | // plt::save(filename.str()); 287 | plt::pause(0.0000001); 288 | } 289 | 290 | /** 291 | * @brief 设置坐标 292 | * 293 | * @param st 起点 294 | * @param go 目标 295 | * @param ox 障碍物x 296 | * @param oy 障碍物y 297 | */ 298 | void Dijkstra::set(const vector &st, const vector &go, const vector &ox, const vector &oy) 299 | { 300 | Dijkstra::st = st; 301 | Dijkstra::go = go; 302 | Dijkstra::ox = ox; 303 | Dijkstra::oy = oy; 304 | } 305 | -------------------------------------------------------------------------------- /Dijkstra/Dijkstra.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Dijkstra.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-10 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef DIJKSTRA_H 13 | #define DIJKSTRA_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "../matplotlibcpp.h" 24 | namespace plt = matplotlibcpp; 25 | using namespace std; 26 | using namespace Eigen; 27 | 28 | #define EPS 1e-4 29 | #define PI 3.14159265354 30 | 31 | //extern int gifindex; 32 | 33 | class Dijkstra 34 | { 35 | public: 36 | struct Node 37 | { 38 | double x; 39 | double y; 40 | float cost; 41 | // Node* p_node; 42 | double parent_index; 43 | 44 | Node(double x, double y, float cost, double parentIndex); 45 | }; 46 | 47 | private: 48 | double resolution; // 栅格大小 49 | double robot_radius; 50 | double min_x, min_y, max_x, max_y; // 地图范围 51 | double x_width, y_width; // 长宽 52 | vector> obstacle_map; // 障碍物地图 53 | vector> motion; // 障碍物地图 54 | vector st, go; 55 | vector ox, oy; 56 | 57 | public: 58 | Dijkstra(double resolution, double robotRadius); 59 | void setObstale(vector &ox, vector &oy); 60 | void calObstacleMap(const vector &ox, const vector &oy); 61 | 62 | double calPosition(double index, double minp); 63 | 64 | void getMotionModel(); 65 | 66 | double calXyIndex(double position, double minp); 67 | 68 | double calIndex(Node *node); 69 | 70 | bool verifyNode(Node *node); 71 | 72 | pair, vector> calFinalPath(Node *goal_node, map closed_set); 73 | 74 | pair, vector> planning(vector start, vector goal); 75 | 76 | void plotGraph(Node *current); 77 | 78 | void set(const vector &st, const vector &go, const vector &ox, const vector &oy); 79 | }; 80 | 81 | #endif // DIJKSTRA_H 82 | -------------------------------------------------------------------------------- /Dijkstra/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 主函数 5 | * @version 0.1 6 | * @date 2023-04-10 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "Dijkstra.h" 13 | 14 | int main() 15 | { 16 | vector start{-5, -5}, goal{50, 50}; 17 | double grid_size = 2.0; 18 | double robot_radius = 1.0; 19 | 20 | vector ox; 21 | vector oy; 22 | 23 | Dijkstra dijkstra(grid_size, robot_radius); 24 | dijkstra.setObstale(ox, oy); // 设置障碍物信息 25 | dijkstra.set(start, goal, ox, oy); // 设置起点，目标点，障碍物的(x,y) 26 | dijkstra.calObstacleMap(ox, oy); // 在地图上生成障碍物 27 | dijkstra.getMotionModel(); // 创建移动代价 28 | 29 | // 绘制地图 30 | plt::plot(ox, oy, ".k"); 31 | plt::plot(vector{start[0]}, vector{start[1]}, "ob"); 32 | plt::plot(vector{goal[0]}, vector{goal[1]}, "or"); 33 | plt::grid(true); 34 | 35 | // 规划路径 36 | pair, vector> xy = dijkstra.planning(start, goal); 37 | // 绘制路径 38 | plt::plot(xy.first, xy.second, "-r"); 39 | 40 | // // 合成 GIF 图片 41 | // stringstream filename; 42 | // filename << "./frame_" << gifindex << ".png"; 43 | // plt::save(filename.str()); 44 | // const char *gif_filename = "./dijkstra_demo.gif"; 45 | // stringstream cmd; 46 | // cmd << "convert -delay 3 -loop 0 ./frame_*.png " << gif_filename; 47 | // system(cmd.str().c_str()); 48 | // cout << "Saving result to " << gif_filename << std::endl; 49 | // plt::show(); 50 | // //删除png图片 51 | // system("rm *.png"); 52 | // return 0; 53 | 54 | // 保存图片 55 | const char *filename = "./dijkstra_demo.png"; 56 | cout << "Saving result to " << filename << std::endl; 57 | plt::save(filename); 58 | plt::show(); 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /README.assets/dijkstra_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/README.assets/dijkstra_demo.gif -------------------------------------------------------------------------------- /README.assets/rrt_connect-16832995765544.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/README.assets/rrt_connect-16832995765544.gif -------------------------------------------------------------------------------- /README.assets/rrt_connect-16832995877726.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/README.assets/rrt_connect-16832995877726.gif -------------------------------------------------------------------------------- /README.assets/rrt_connect1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/README.assets/rrt_connect1.gif -------------------------------------------------------------------------------- /README.assets/rrt_star_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/README.assets/rrt_star_demo.gif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 简介 2 | 3 | 自动驾驶常用路径规划算法C++实现 4 | 5 | 6 | 7 | # 项目依赖 8 | 9 | 推荐在Ubuntu 18.04/20.04 环境下运行 10 | 11 | - **cmake** 12 | 13 | 在Ubuntu中安装cmake： 14 | 15 | ``` 16 | sudo apt install cmake 17 | ``` 18 | 19 | - **Eigen** 20 | 21 | 在Ubuntu中安装Eigen： 22 | 23 | ``` 24 | sudo apt-get install libeigen3-dev 25 | ``` 26 | 27 | - **python3** 28 | 29 | 30 | 31 | # 编译 32 | 33 | 在当前目录下输入： 34 | 35 | ```shell 36 | mkdir build 37 | cd build 38 | cmake ../ 39 | make 40 | ``` 41 | 42 | 43 | 44 | # Path_planning 45 | 46 | ## Dijkstra 47 | 48 | 49 | 50 | ![dijkstra_demo](README.assets/dijkstra_demo.gif) 51 | 52 | ## A star 53 | 54 | ![astar](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/astar-168329807331712.gif) 55 | 56 | ## RRT 57 | 58 | 59 | 60 | ![rrt_demo](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/rrt_demo-168329807983514.gif) 61 | 62 | 63 | 64 | ## RRT connect 65 | 66 | ![rrt_connect](README.assets/rrt_connect-16832995877726.gif) 67 | 68 | ## RRT star 69 | 70 | ![rrt_star_demo](README.assets/rrt_star_demo.gif) 71 | 72 | 73 | 74 | ## Bezier 75 | 76 | ![bezier_demo](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/bezier_demo.gif) 77 | 78 | 79 | 80 | ## B spline 81 | 82 | ![b_spline_demo](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/b_spline_demo.gif) 83 | 84 | 85 | 86 | 87 | 88 | # 参考 89 | 90 | [PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics#pythonrobotics) 91 | 92 | [chhRobotics_CPP](https://github.com/CHH3213/chhRobotics_CPP) 93 | 94 | -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree/RRT.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RRT.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-23 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "RRT.h" 13 | 14 | //int gifindex = 0; 15 | 16 | Node::Node(double x, double y) : x(x), y(y), parent(NULL), cost(0) {} 17 | 18 | /** 19 | * @brief Construct 20 | * 21 | * @param obstacleList 障碍物位置列表 [[x,y,size],...] 22 | * @param randArea 采样区域 x,y ∈ [min,max]; 23 | * @param playArea 约束随机树的范围 [xmin,xmax,ymin,ymax] 24 | * @param robotRadius 机器人半径 25 | * @param expandDis 扩展的步长 26 | * @param goalSampleRate 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点 27 | * @param maxIter 最大采样点数 28 | */ 29 | RRT::RRT(const vector> &obstacleList, 30 | const vector &randArea, const vector &playArea, double robotRadius, double expandDis, 31 | double goalSampleRate, int maxIter) : obstacle_list(obstacleList), rand_area(randArea), 32 | play_area(playArea), robot_radius(robotRadius), expand_dis(expandDis), 33 | goal_sample_rate(goalSampleRate), max_iter(maxIter) {} 34 | 35 | /** 36 | * @brief 计算两点间的距离和方位角 37 | * 38 | * @param from_node 39 | * @param to_node 40 | * @return vector 41 | */ 42 | vector RRT::calDistanceAngle(Node *from_node, Node *to_node) 43 | { 44 | double dx = to_node->x - from_node->x; 45 | double dy = to_node->y - from_node->y; 46 | double d = sqrt(pow(dx, 2) + pow(dy, 2)); 47 | double theta = atan2(dy, dx); 48 | return {d, theta}; 49 | } 50 | 51 | /** 52 | * @brief 判断是否有障碍物 53 | * 54 | * @param node 55 | * @return true 56 | * @return false 57 | */ 58 | bool RRT::obstacleFree(Node *node) 59 | { 60 | for (vector obs : obstacle_list) 61 | { 62 | for (int i = 0; i < node->path_x.size(); i++) 63 | { 64 | double x = node->path_x[i]; 65 | double y = node->path_y[i]; 66 | if (pow(obs[0] - x, 2) + pow(obs[1] - y, 2) <= pow(obs[2] + robot_radius, 2)) 67 | return false; // collision 68 | } 69 | } 70 | return true; 71 | } 72 | 73 | /** 74 | * @brief 判断是否在可行区域里面 75 | * @param node 76 | * @return 77 | */ 78 | bool RRT::isInsidePlayArea(Node *node) 79 | { 80 | if (node->x < play_area[0] || node->x > play_area[1] || node->y < play_area[2] || node->y > play_area[3]) 81 | return false; 82 | return true; 83 | } 84 | 85 | /** 86 | * @brief 计算最近的节点 87 | * @param node_list 节点列表 88 | * @param rnd_node 随机采样的节点 89 | * @return 最近的节点索引 90 | */ 91 | int RRT::getNearestNodeIndex(vector node_list, Node *rnd_node) 92 | { 93 | int min_index = -1; 94 | double d = numeric_limits::max(); 95 | for (int i = 0; i < node_list.size(); i++) 96 | { 97 | Node *node = node_list[i]; 98 | double dist = pow(node->x - rnd_node->x, 2) + pow(node->y - rnd_node->y, 2); 99 | if (d > dist) 100 | { 101 | d = dist; 102 | min_index = i; 103 | } 104 | } 105 | return min_index; 106 | } 107 | 108 | /** 109 | * @brief 随机采样以(100-goal_sample_rate%的概率随机生长，(goal_sample_rate)%的概率朝向目标点生长 110 | * @return 生成的节点 111 | */ 112 | Node *RRT::sampleFree() 113 | { 114 | Node *rnd = nullptr; 115 | if (rand() % (100) > goal_sample_rate) 116 | { 117 | double x_rand = rand() / double(RAND_MAX) * (rand_area[1] - rand_area[0]) + rand_area[0]; 118 | double y_rand = rand() / double(RAND_MAX) * (rand_area[1] - rand_area[0]) + rand_area[0]; 119 | rnd = new Node(x_rand, y_rand); 120 | } 121 | else 122 | { 123 | rnd = new Node(end->x, end->y); 124 | } 125 | return rnd; 126 | } 127 | 128 | void RRT::setBegin(Node *begin) 129 | { 130 | RRT::begin = begin; 131 | } 132 | 133 | void RRT::setEnd(Node *end) 134 | { 135 | RRT::end = end; 136 | } 137 | 138 | /** 139 | * @brief 计算(x,y)离目标点的距离 140 | * @param x 141 | * @param y 142 | * @return 143 | */ 144 | double RRT::calDistToGoal(double x, double y) 145 | { 146 | double dx = x - end->x; 147 | double dy = y - end->y; 148 | return sqrt(pow(dx, 2) + pow(dy, 2)); 149 | } 150 | 151 | /** 152 | * @brief 生成路径 153 | * @param goal_ind 154 | * @return 155 | */ 156 | pair, vector> RRT::generateFinalCourse(double goal_ind) 157 | { 158 | vector x_, y_; 159 | x_.push_back(end->x); 160 | y_.push_back(end->y); 161 | Node *node = node_list[goal_ind]; 162 | while (node->parent != nullptr) 163 | { 164 | x_.push_back(node->x); 165 | y_.push_back(node->y); 166 | node = node->parent; 167 | // cout<x<<","<y<x); 170 | y_.push_back(node->y); 171 | return {x_, y_}; 172 | } 173 | 174 | /** 175 | * @brief 连线方向扩展固定步长查找x_new 176 | * @param from_node x_near 177 | * @param to_node x_rand 178 | * @param extend_length 扩展步长u. Defaults to float("inf"). 179 | * @return 180 | */ 181 | Node *RRT::steer(Node *from_node, Node *to_node, double extend_length) 182 | { 183 | // 利用反正切计算角度, 然后利用角度和步长计算新坐标 184 | vector dist_angle = calDistanceAngle(from_node, to_node); 185 | 186 | double new_x, new_y; 187 | if (extend_length >= dist_angle[0]) 188 | { 189 | new_x = to_node->x; 190 | new_y = to_node->y; 191 | } 192 | else 193 | { 194 | new_x = from_node->x + extend_length * cos(dist_angle[1]); 195 | new_y = from_node->y + extend_length * sin(dist_angle[1]); 196 | } 197 | 198 | Node *new_node = new Node(new_x, new_y); 199 | new_node->path_x.push_back(from_node->x); 200 | new_node->path_y.push_back(from_node->y); 201 | new_node->path_x.push_back(new_node->x); 202 | new_node->path_y.push_back(new_node->y); 203 | 204 | new_node->parent = from_node; 205 | // cout<x<<","<y<, vector> RRT::planning() 215 | { 216 | node_list.push_back(begin); // 将起点作为根节点x_{init}，加入到随机树的节点集合中。 217 | for (int i = 0; i < max_iter; i++) 218 | { 219 | // 从可行区域内随机选取一个节点x_{rand} 220 | Node *rnd_node = sampleFree(); 221 | 222 | // 已生成的树中利用欧氏距离判断距离x_{rand}最近的点x_{near}。 223 | int nearest_ind = getNearestNodeIndex(node_list, rnd_node); 224 | Node *nearest_node = node_list[nearest_ind]; 225 | 226 | // 从x_{near}与x_{rand}的连线方向上扩展固定步长u，得到新节点 x_{new} 227 | Node *new_node = steer(nearest_node, rnd_node, expand_dis); 228 | 229 | // 如果在可行区域内，且x_{near}与x_{new}之间无障碍物 230 | if (isInsidePlayArea(new_node) && obstacleFree(new_node)) 231 | { 232 | node_list.push_back(new_node); 233 | } 234 | // cout<x, node_list[node_list.size() - 1]->y) <= expand_dis) 237 | { 238 | Node *final_node = steer(node_list[node_list.size() - 1], end, expand_dis); 239 | if (obstacleFree(final_node)) 240 | { 241 | cout << "reaches the goal!" << endl; 242 | // return {node_list[node_list.size()-1]->path_x,node_list[node_list.size()-1]->path_y}; 243 | return generateFinalCourse(node_list.size() - 1); 244 | } 245 | } 246 | // cout<x<<","<y< x_t, y_t; 262 | for (double i = 0.; i <= 2 * PI; i += 0.01) 263 | { 264 | x_t.push_back(x + size * cos(i)); 265 | y_t.push_back(y + size * sin(i)); 266 | } 267 | plt::plot(x_t, y_t, color); 268 | } 269 | 270 | /** 271 | * 画出搜索过程的图 272 | * @param node 273 | */ 274 | void RRT::draw(Node *node) 275 | { 276 | plt::clf(); 277 | // 画随机点 278 | if (node) 279 | { 280 | plt::plot(vector{node->x}, vector{node->y}, "^k"); 281 | if (robot_radius > 0) 282 | { 283 | plotCircle(node->x, node->y, robot_radius, "-r"); 284 | } 285 | } 286 | 287 | // 画已生成的树 288 | for (Node *node1 : node_list) 289 | { 290 | if (node1->parent) 291 | { 292 | plt::plot(node1->path_x, node1->path_y, "-g"); 293 | } 294 | } 295 | // 画障碍物 296 | for (vector ob : obstacle_list) 297 | { 298 | plotCircle(ob[0], ob[1], ob[2]); 299 | } 300 | 301 | plt::plot(vector{play_area[0], play_area[1], play_area[1], play_area[0], play_area[0]}, vector{play_area[2], play_area[2], play_area[3], play_area[3], play_area[2]}, "k-"); 302 | 303 | // 画出起点和目标点 304 | plt::plot(vector{begin->x}, vector{begin->y}, "xr"); 305 | plt::plot(vector{end->x}, vector{end->y}, "xr"); 306 | plt::axis("equal"); 307 | plt::grid(true); 308 | plt::xlim(play_area[0] - 1, play_area[1] + 1); 309 | plt::ylim(play_area[2] - 1, play_area[3] + 1); 310 | 311 | // // 将每一帧保存为单独的文件 312 | // stringstream filename; 313 | // filename << "./frame_" << gifindex++ << ".png"; 314 | // plt::save(filename.str()); 315 | 316 | plt::pause(0.01); 317 | } 318 | -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree/RRT.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RRT.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-23 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | #ifndef RRT_H 12 | #define RRT_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "../matplotlibcpp.h" 23 | namespace plt = matplotlibcpp; 24 | 25 | using namespace std; 26 | using namespace Eigen; 27 | 28 | #define PI 3.14159265354 29 | 30 | //extern int gifindex; 31 | 32 | class Node 33 | { 34 | public: 35 | double x, y; // 节点坐标 36 | vector path_x = {}, path_y = {}; // 路径，作为画图的数据 37 | Node *parent; 38 | double cost; 39 | 40 | public: 41 | Node(double x, double y); 42 | }; 43 | 44 | class RRT 45 | { 46 | public: 47 | vector> obstacle_list; // 障碍物位置列表 [[x,y,size],...] 48 | vector rand_area, play_area; // 采样区域 x,y ∈ [min,max];约束随机树的范围 [xmin,xmax,ymin,ymax] 49 | double robot_radius; // 机器人半径 50 | double expand_dis; // 扩展的步长 51 | double goal_sample_rate; // 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点 52 | vector node_list; 53 | Node *begin; // 起始点 54 | Node *end; // 目标点 55 | 56 | int max_iter; //最大采样点数 57 | 58 | public: 59 | 60 | RRT(const vector> &obstacleList, 61 | const vector &randArea, const vector &playArea, double robotRadius, double expandDis, 62 | double goalSampleRate, int maxIter); 63 | 64 | vector calDistanceAngle(Node *from_node, Node *to_node); // 计算两个节点间的距离和方位角 65 | 66 | bool obstacleFree(Node *node); // 判断是否有障碍物 67 | 68 | bool isInsidePlayArea(Node *node); // 判断是否在可行区域里面 69 | 70 | int getNearestNodeIndex(vector node_list, Node *rnd_node); // 计算最近的节点 71 | 72 | Node *sampleFree(); // 采样生成节点 73 | 74 | double calDistToGoal(double x, double y); // 计算(x,y)离目标点的距离 75 | 76 | pair, vector> generateFinalCourse(double goal_ind); // 生成路径，画图 77 | 78 | Node *steer(Node *from_node, Node *to_node, double extend_length = numeric_limits::max()); // 连线方向扩展固定步长查找x_new 79 | 80 | pair, vector> planning(); 81 | 82 | void setBegin(Node *begin); 83 | 84 | void setEnd(Node *End); 85 | 86 | void plotCircle(double x, double y, double size, string color = "b"); // 画圆 87 | void draw(Node *node = nullptr); 88 | }; 89 | 90 | #endif // RRT_H 91 | -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-23 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | #include "RRT.h" 12 | 13 | int main() 14 | { 15 | vector> obstacle_list{ 16 | {5, 5, 1}, 17 | {3, 6, 2}, 18 | {3, 8, 2}, 19 | {3, 10, 2}, 20 | {7, 5, 2}, 21 | {9, 5, 2}, 22 | {8, 10, 1}, 23 | {6, 12, 1}}; 24 | Node *begin = new Node(0.0, 0.0); 25 | Node *end = new Node(6.0, 10.0); 26 | vector rnd_area{-2, 15}; // 采样区域 x,y ∈ [min,max]; 27 | vector play_area{-2, 12, 0, 14}; // 约束随机树的范围 [xmin,xmax,ymin,ymax] 28 | double radius = 0.8; // 机器人半径 29 | double expand_dis = 2; // 扩展的步长 30 | double goal_sample_rate = 5; // 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点 31 | int max_iter = 500; // 最大采样点数 32 | RRT rrt(obstacle_list, rnd_area, play_area, radius, expand_dis, goal_sample_rate, max_iter); 33 | rrt.setBegin(begin); 34 | rrt.setEnd(end); 35 | 36 | pair, vector> traj = rrt.planning(); 37 | 38 | plt::plot(traj.first, traj.second, "r"); 39 | 40 | // // 合成 GIF 图片 41 | // stringstream filename; 42 | // filename << "./frame_" << gifindex << ".png"; 43 | // plt::save(filename.str()); 44 | 45 | // const char *gif_filename = "./rrt_demo.gif"; 46 | // stringstream cmd; 47 | // cmd << "convert -delay 10 -loop 0 ./frame_*.png " << gif_filename; 48 | // system(cmd.str().c_str()); 49 | // cout << "Saving result to " << gif_filename << std::endl; 50 | // plt::show(); 51 | // //删除png图片 52 | // system("rm *.png"); 53 | // return 0; 54 | 55 | const char *filename = "./rrt_demo.png"; 56 | cout << "Saving result to " << filename << std::endl; 57 | plt::save(filename); 58 | plt::show(); 59 | 60 | return 0; 61 | } -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree_Star/RRT_Star.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RRT_Star.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-24 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "RRT_Star.h" 13 | 14 | 15 | /** 16 | * @brief Construct a new rrt star::rrt star object 17 | * 18 | * @param obstacleList 障碍物位置列表 [[x,y,size],...] 19 | * @param randArea 采样区域 x,y ∈ [min,max]; 20 | * @param playArea 约束随机树的范围 [xmin,xmax,ymin,ymax] 21 | * @param robotRadius 机器人半径 22 | * @param expandDis 扩展的步长 23 | * @param goalSampleRate 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点 24 | * @param maxIter 最大采样点数 25 | * @param connectCircleDist rewire的探索半径 26 | * @param searchUntilMaxIter 27 | */ 28 | RRT_Star::RRT_Star(const vector> &obstacleList, const vector &randArea, 29 | const vector &playArea, double robotRadius, double expandDis, double goalSampleRate, 30 | int maxIter, double connectCircleDist, bool searchUntilMaxIter) : RRT(obstacleList, randArea, 31 | playArea, robotRadius, 32 | expandDis, goalSampleRate, 33 | maxIter), 34 | connect_circle_dist( 35 | connectCircleDist), 36 | search_until_max_iter(searchUntilMaxIter) {} 37 | 38 | pair, vector> RRT_Star::planning() 39 | { 40 | node_list.push_back(begin); // 将起点作为根节点x_{init}，加入到随机树的节点集合中。 41 | for (int i = 0; i < max_iter; i++) 42 | { 43 | // 从可行区域内随机选取一个节点x_{rand} 44 | Node *rnd_node = sampleFree(); 45 | cout << "随机树节点个数：" << node_list.size() << endl; 46 | // 已生成的树中利用欧氏距离判断距离x_{rand}最近的点x_{near}。 47 | int nearest_ind = getNearestNodeIndex(node_list, rnd_node); 48 | Node *nearest_node = node_list[nearest_ind]; 49 | // 从x_{near}与x_{rand}的连线方向上扩展固定步长u，得到新节点 x_{new} 50 | Node *new_node = steer(nearest_node, rnd_node, expand_dis); 51 | // 计算代价,欧氏距离 52 | new_node->cost = nearest_node->cost + sqrt(pow(new_node->x - nearest_node->x, 2) + pow(new_node->y - nearest_node->y, 2)); 53 | 54 | // 如果在可行区域内，且x_{near}与x_{new}之间无障碍物 55 | if (isInsidePlayArea(new_node) && obstacleFree(new_node)) 56 | { 57 | vector near_ind = findNearInds(new_node); // 找到x_new的邻近节点 58 | Node *node_with_updated_parent = chooseParent(new_node, near_ind); // 重新选择父节点 59 | // 如果父节点更新了(非空） 60 | if (node_with_updated_parent) 61 | { 62 | // 重布线 63 | rewire(node_with_updated_parent, near_ind); 64 | node_list.push_back(node_with_updated_parent); 65 | } 66 | else 67 | { 68 | node_list.push_back(new_node); 69 | } 70 | } 71 | 72 | draw(rnd_node); 73 | 74 | if ((!search_until_max_iter) && new_node) 75 | { // reaches goal 76 | int last_index = findBestGoalInd(); 77 | if (last_index != -1) 78 | { 79 | cout << "reaches the goal!" << endl; 80 | return generateFinalCourse(last_index); 81 | } 82 | } 83 | } 84 | cout << "达到最大回合数" << endl; 85 | int last_index = findBestGoalInd(); 86 | if (last_index != -1) 87 | return generateFinalCourse(last_index); 88 | return {}; 89 | } 90 | 91 | /** 92 | * @brief 计算周围一定半径内的所有邻近节点 93 | * @param new_node 94 | * @return 所有邻近节点索引 95 | */ 96 | vector RRT_Star::findNearInds(Node *new_node) 97 | { 98 | int nnode = node_list.size() + 1; 99 | vector inds; 100 | double r = connect_circle_dist * sqrt(log(nnode) / nnode); 101 | for (int i = 0; i < node_list.size(); i++) 102 | { 103 | Node *n_ = node_list[i]; 104 | if (pow(n_->x - new_node->x, 2) + pow(n_->y - new_node->y, 2) < r * r) 105 | { 106 | inds.push_back(i); 107 | } 108 | } 109 | return inds; 110 | } 111 | 112 | /** 113 | * @brief 更新以paren_node为父节点到起点的所有cost 114 | * 115 | * @param parent_node 116 | */ 117 | void RRT_Star::propagateCostToLeaves(Node *parent_node) 118 | { 119 | for (Node *node : node_list) 120 | { 121 | if (node->parent == parent_node) 122 | { 123 | node->cost = calcNewCost(parent_node, node); 124 | propagateCostToLeaves(node); 125 | } 126 | } 127 | } 128 | 129 | /** 130 | * @brief 计算代价 131 | * @param from_node 132 | * @param to_node 133 | * @return 134 | */ 135 | double RRT_Star::calcNewCost(Node *from_node, Node *to_node) 136 | { 137 | vector da = calDistanceAngle(from_node, to_node); 138 | return from_node->cost + da[0]; 139 | } 140 | 141 | /** 142 | * @brief rewire 143 | * @param new_node 144 | * @param near_inds 145 | */ 146 | void RRT_Star::rewire(Node *new_node, vector near_inds) 147 | { 148 | for (int i : near_inds) 149 | { 150 | Node *near_node = node_list[i]; 151 | Node *edge_node = steer(new_node, near_node); 152 | if (!edge_node) 153 | continue; 154 | edge_node->cost = calcNewCost(new_node, near_node); 155 | 156 | if (obstacleFree(edge_node) && near_node->cost > edge_node->cost) 157 | { 158 | near_node->x = edge_node->x; 159 | near_node->y = edge_node->y; 160 | near_node->cost = edge_node->cost; 161 | near_node->path_x = edge_node->path_x; 162 | near_node->path_y = edge_node->path_y; 163 | near_node->parent = edge_node->parent; 164 | propagateCostToLeaves(new_node); 165 | } 166 | } 167 | } 168 | 169 | /** 170 | * @brief 计算离目标点的最佳索引 171 | * @return 172 | */ 173 | int RRT_Star::findBestGoalInd() 174 | { 175 | vector goal_inds, safe_goal_inds; 176 | for (int i = 0; i < node_list.size(); i++) 177 | { 178 | Node *node = node_list[i]; 179 | double dist = calDistToGoal(node->x, node->y); 180 | if (dist <= expand_dis) 181 | { 182 | goal_inds.push_back(i); 183 | } 184 | } 185 | 186 | for (int goal_ind : goal_inds) 187 | { 188 | Node *t_node = steer(node_list[goal_ind], end); 189 | if (obstacleFree(t_node)) 190 | { 191 | safe_goal_inds.push_back(goal_ind); 192 | } 193 | } 194 | if (safe_goal_inds.empty()) 195 | return -1; 196 | double min_cost = numeric_limits::max(); 197 | int safe_ind = -1; 198 | for (int ind : safe_goal_inds) 199 | { 200 | if (min_cost > node_list[ind]->cost) 201 | { 202 | min_cost = node_list[ind]->cost; 203 | safe_ind = ind; 204 | } 205 | } 206 | return safe_ind; 207 | } 208 | 209 | /** 210 | * @brief 在新产生的节点 $x_{new}$ 附近以定义的半径范围$r$内寻找所有的近邻节点 $X_{near}$， 211 | 作为替换 $x_{new}$ 原始父节点 $x_{near}$ 的备选 212 | 我们需要依次计算起点到每个近邻节点 $X_{near}$ 的路径代价加上近邻节点 $X_{near}$ 到 $x_{new}$ 的路径代价， 213 | 取路径代价最小的近邻节点$x_{min}$作为 $x_{new}$ 新的父节点 214 | * @param new_node 215 | * @param near_inds 216 | * @return 217 | */ 218 | Node *RRT_Star::chooseParent(Node *new_node, vector near_inds) 219 | { 220 | if (near_inds.empty()) 221 | return nullptr; 222 | vector costs; 223 | for (int i : near_inds) 224 | { 225 | Node *near_node = node_list[i]; 226 | Node *t_node = steer(near_node, new_node); 227 | if (t_node && obstacleFree(t_node)) 228 | { 229 | costs.push_back(calcNewCost(near_node, new_node)); 230 | } 231 | else 232 | { 233 | costs.push_back(numeric_limits::max()); // the cost of collision node 234 | } 235 | } 236 | double min_cost = *min_element(costs.begin(), costs.end()); 237 | 238 | if (min_cost == numeric_limits::max()) 239 | { 240 | cout << "There is no good path.(min_cost is inf)" << endl; 241 | return nullptr; 242 | } 243 | int min_ind = near_inds[min_element(costs.begin(), costs.end()) - costs.begin()]; 244 | 245 | Node *determine_node = steer(node_list[min_ind], new_node); 246 | determine_node->cost = min_cost; 247 | return determine_node; 248 | } 249 | -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree_Star/RRT_Star.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RRT_Star.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-24 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef RRT_STAR_H 13 | #define RRT_STAR_H 14 | 15 | #include "../Rapidly-exploring_Random_Tree/RRT.h" 16 | 17 | 18 | class RRT_Star : public RRT 19 | { 20 | public: 21 | double connect_circle_dist; // rewire的探索半径 22 | bool search_until_max_iter; // 达到最大步长后是否停止搜索 23 | 24 | RRT_Star(const vector> &obstacleList, const vector &randArea, const vector &playArea, 25 | double robotRadius, double expandDis, double goalSampleRate, int maxIter, double connectCircleDist, 26 | bool searchUntilMaxIter); 27 | 28 | pair, vector> planning(); 29 | 30 | vector findNearInds(Node *new_node); // 找出邻近节点集合 31 | 32 | void propagateCostToLeaves(Node *parent_node); 33 | 34 | double calcNewCost(Node *from_node, Node *to_node); 35 | 36 | void rewire(Node *new_node, vector near_inds); 37 | 38 | int findBestGoalInd(); 39 | 40 | Node *chooseParent(Node *new_node, vector near_inds); 41 | }; 42 | 43 | #endif // RRT_STAR_H 44 | -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree_Star/main.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by chh3213 on 2022/11/27. 3 | // 4 | #include "RRT_Star.h" 5 | 6 | int main() 7 | { 8 | vector> obstacle_list{ 9 | {5, 5, 1}, 10 | {3, 6, 2}, 11 | {3, 8, 2}, 12 | {3, 10, 2}, 13 | {7, 5, 2}, 14 | {9, 5, 2}, 15 | {8, 10, 1}, 16 | {6, 12, 1}}; 17 | Node *begin = new Node(0.0, 0.0); 18 | Node *end = new Node(6.0, 10.0); 19 | vector rnd_area{-2, 15}; 20 | vector play_area{-2, 12, 0, 14}; 21 | double radius = 0.5; 22 | double expand_dis = 1; // 扩展的步长 23 | double goal_sample_rate = 20; // 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点 24 | int max_iter = 500; 25 | double connect_circle_dist = 5.0; 26 | bool search_until_max_iter = false; 27 | 28 | RRT_Star rrt(obstacle_list, rnd_area, play_area, radius, expand_dis, goal_sample_rate, max_iter, connect_circle_dist, search_until_max_iter); 29 | rrt.setBegin(begin); 30 | rrt.setEnd(end); 31 | 32 | pair, vector> traj = rrt.planning(); 33 | 34 | plt::plot(traj.first, traj.second, "r"); 35 | 36 | // // 合成 GIF 图片 37 | // stringstream filename; 38 | // filename << "./frame_" << gifindex << ".png"; 39 | // plt::save(filename.str()); 40 | 41 | // const char *gif_filename = "./rrt_star_demo.gif"; 42 | // stringstream cmd; 43 | // cmd << "convert -delay 10 -loop 0 ./frame_*.png " << gif_filename; 44 | // system(cmd.str().c_str()); 45 | // cout << "Saving result to " << gif_filename << std::endl; 46 | // plt::show(); 47 | // //删除png图片 48 | // system("rm *.png"); 49 | // return 0; 50 | 51 | // save figure 52 | const char *filename = "./rrt_star_demo.png"; 53 | cout << "Saving result to " << filename << std::endl; 54 | plt::save(filename); 55 | plt::show(); 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree_connect/RRT_connect.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RRT_connect.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-24 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "RRT_connect.h" 13 | 14 | Node::Node(double x, double y) : x(x), y(y), parent(NULL) {} 15 | 16 | //int gifindex = 0; 17 | 18 | /** 19 | * @brief Construct 20 | * 21 | * @param obstacleList 障碍物位置列表 [[x,y,size],...] 22 | * @param randArea 采样区域 x,y ∈ [min,max]; 23 | * @param playArea 约束随机树的范围 [xmin,xmax,ymin,ymax] 24 | * @param robotRadius 机器人半径 25 | * @param expandDis 扩展的步长 26 | * @param goalSampleRate 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点 27 | * @param maxIter 最大采样点数 28 | */ 29 | RRTConnect::RRTConnect(const vector> &obstacleList, 30 | const vector &randArea, const vector &playArea, double robotRadius, double expandDis, 31 | double goalSampleRate, int maxIter) : obstacle_list(obstacleList), rand_area(randArea), 32 | play_area(playArea), robot_radius(robotRadius), expand_dis(expandDis), 33 | goal_sample_rate(goalSampleRate), max_iter(maxIter) {} 34 | 35 | /** 36 | * @brief 计算两个节点间的距离和方位角 37 | * @param from_node 38 | * @param to_node 39 | * @return 40 | */ 41 | vector RRTConnect::calDistanceAngle(Node *from_node, Node *to_node) 42 | { 43 | double dx = to_node->x - from_node->x; 44 | double dy = to_node->y - from_node->y; 45 | double d = sqrt(pow(dx, 2) + pow(dy, 2)); 46 | double theta = atan2(dy, dx); 47 | return {d, theta}; 48 | } 49 | 50 | /** 51 | * @brief 判断是否有障碍物 52 | * @param node 节点坐标 53 | * @return 54 | */ 55 | bool RRTConnect::obstacleFree(Node *node) 56 | { 57 | for (vector obs : obstacle_list) 58 | { 59 | for (int i = 0; i < node->path_x.size(); i++) 60 | { 61 | double x = node->path_x[i]; 62 | double y = node->path_y[i]; 63 | if (pow(obs[0] - x, 2) + pow(obs[1] - y, 2) <= pow(obs[2] + robot_radius, 2)) 64 | return false; // collision 65 | } 66 | } 67 | return true; // safe 68 | } 69 | 70 | /** 71 | * @brief 判断是否在可行区域里面 72 | * @param node 73 | * @return 74 | */ 75 | bool RRTConnect::isInsidePlayArea(Node *node) 76 | { 77 | if (node->x < play_area[0] || node->x > play_area[1] || node->y < play_area[2] || node->y > play_area[3]) 78 | return false; 79 | return true; 80 | } 81 | 82 | /** 83 | * @brief 计算最近的节点 84 | * @param node_list 节点列表 85 | * @param rnd_node 随机采样的节点 86 | * @return 最近的节点索引 87 | */ 88 | int RRTConnect::getNearestNodeIndex(vector node_list, Node *rnd_node) 89 | { 90 | int min_index = -1; 91 | double d = numeric_limits::max(); 92 | for (int i = 0; i < node_list.size(); i++) 93 | { 94 | Node *node = node_list[i]; 95 | double dist = pow(node->x - rnd_node->x, 2) + pow(node->y - rnd_node->y, 2); 96 | if (d > dist) 97 | { 98 | d = dist; 99 | min_index = i; 100 | } 101 | } 102 | return min_index; 103 | } 104 | 105 | /** 106 | * @brief 以（100-goal_sample_rate）%的概率随机生长，(goal_sample_rate)%的概率朝向目标点生长 107 | * @return 生成的节点 108 | */ 109 | Node *RRTConnect::sampleFree() 110 | { 111 | Node *rnd = nullptr; 112 | if (rand() % (100) > goal_sample_rate) 113 | { 114 | 115 | double x_rand = rand() / double(RAND_MAX) * (rand_area[1] - rand_area[0]) + rand_area[0]; 116 | double y_rand = rand() / double(RAND_MAX) * (rand_area[1] - rand_area[0]) + rand_area[0]; 117 | // cout<x, end->y); 123 | } 124 | return rnd; 125 | } 126 | 127 | void RRTConnect::setBegin(Node *begin) 128 | { 129 | RRTConnect::begin = begin; 130 | } 131 | 132 | void RRTConnect::setEnd(Node *end) 133 | { 134 | RRTConnect::end = end; 135 | } 136 | 137 | /** 138 | * @brief 计算(x,y)离目标点的距离 139 | * @param x 140 | * @param y 141 | * @return 142 | */ 143 | double RRTConnect::calDistToGoal(double x, double y) 144 | { 145 | double dx = x - end->x; 146 | double dy = y - end->y; 147 | return sqrt(pow(dx, 2) + pow(dy, 2)); 148 | } 149 | 150 | /** 151 | * @brief 生成路径 152 | * @param goal_ind 153 | * @return 154 | */ 155 | pair, vector> RRTConnect::generateFinalCourse() 156 | { 157 | vector x_, y_, x1, y1, x2, y2; 158 | 159 | Node *node = node_list_1[node_list_1.size() - 1]; 160 | while (node->parent != nullptr) 161 | { 162 | x1.push_back(node->x); 163 | y1.push_back(node->y); 164 | node = node->parent; 165 | // cout<x<<","<y<x); 168 | y1.push_back(node->y); 169 | 170 | node = node_list_2[node_list_2.size() - 1]; 171 | while (node->parent != nullptr) 172 | { 173 | x2.push_back(node->x); 174 | y2.push_back(node->y); 175 | node = node->parent; 176 | // cout<x<<","<y<x); 179 | y2.push_back(node->y); 180 | 181 | for (int i = x1.size() - 1; i >= 0; i--) 182 | { 183 | x_.push_back(x1[i]); 184 | y_.push_back(y1[i]); 185 | } 186 | for (int i = 0; i < x2.size(); i++) 187 | { 188 | x_.push_back(x2[i]); 189 | y_.push_back(y2[i]); 190 | } 191 | 192 | return {x_, y_}; 193 | } 194 | 195 | /** 196 | * @brief 连线方向扩展固定步长查找x_new 197 | * @param from_node x_near 198 | * @param to_node x_rand 199 | * @param extend_length 扩展步长u. Defaults to float("inf"). 200 | * @return 201 | */ 202 | Node *RRTConnect::steer(Node *from_node, Node *to_node, double extend_length) 203 | { 204 | // 利用反正切计算角度, 然后利用角度和步长计算新坐标 205 | vector dist_angle = calDistanceAngle(from_node, to_node); 206 | double new_x, new_y; 207 | if (extend_length >= dist_angle[0]) 208 | { 209 | new_x = to_node->x; 210 | new_y = to_node->y; 211 | } 212 | else 213 | { 214 | new_x = from_node->x + extend_length * cos(dist_angle[1]); 215 | new_y = from_node->y + extend_length * sin(dist_angle[1]); 216 | } 217 | 218 | Node *new_node = new Node(new_x, new_y); 219 | new_node->path_x.push_back(from_node->x); 220 | new_node->path_y.push_back(from_node->y); 221 | new_node->path_x.push_back(new_node->x); 222 | new_node->path_y.push_back(new_node->y); 223 | 224 | new_node->parent = from_node; 225 | return new_node; 226 | } 227 | 228 | /** 229 | * @brief rrt path planning,两边同时进行搜索 230 | * @return 轨迹数据 231 | */ 232 | pair, vector> RRTConnect::planning() 233 | { 234 | node_list_1.push_back(begin); // 将起点作为根节点x_{init}，加入到第一棵随机树的节点集合中。 235 | node_list_2.push_back(end); // 将终点作为根节点x_{init}，加入到第二棵随机树的节点集合中。 236 | for (int i = 0; i < max_iter; i++) 237 | { 238 | // 从可行区域内随机选取一个节点x_{rand} 239 | Node *rnd_node = sampleFree(); 240 | 241 | // 已生成的树中利用欧氏距离判断距离x_{rand}最近的点x_{near}。 242 | int nearest_ind = getNearestNodeIndex(node_list_1, rnd_node); 243 | Node *nearest_node = node_list_1[nearest_ind]; 244 | // 从x_{near}与x_{rand}的连线方向上扩展固定步长u，得到新节点 x_{new} 245 | Node *new_node = steer(nearest_node, rnd_node, expand_dis); 246 | 247 | // 第一棵树，如果在可行区域内，且q_{near}与q_{new}之间无障碍物 248 | if (isInsidePlayArea(new_node) && obstacleFree(new_node)) 249 | { 250 | // 将x_new加入到集合中 251 | node_list_1.push_back(new_node); 252 | // 扩展完第一棵树的新节点x_{𝑛𝑒𝑤}后，以这个新的目标点x_{𝑛𝑒𝑤}作为第二棵树扩展的x_rand。 253 | nearest_ind = getNearestNodeIndex(node_list_2, new_node); 254 | nearest_node = node_list_2[nearest_ind]; 255 | // 从x_{near}与x_{rand}的连线方向上扩展固定步长u，得到新节点 x_{new2} 256 | Node *new_node_2 = steer(nearest_node, new_node, expand_dis); 257 | // 第二棵树,如果在可行区域内，且x_{near}与x_{new2}之间无障碍物 258 | if (isInsidePlayArea(new_node_2) && obstacleFree(new_node_2)) 259 | { 260 | // 将x_new2加入第二棵树 261 | node_list_2.push_back(new_node_2); 262 | // 接下来，第二棵树继续往第一棵树的x_{new}方向扩展，直到扩展失败（碰到障碍物）或者𝑞′𝑛𝑒𝑤=𝑞𝑛𝑒𝑤表示与第一棵树相连 263 | while (true) 264 | { 265 | Node *new_node_2_ = steer(new_node_2, new_node, expand_dis); 266 | if (obstacleFree(new_node_2_)) 267 | { 268 | node_list_2.push_back(new_node_2_); 269 | new_node_2 = new_node_2_; 270 | } 271 | else 272 | break; 273 | // 当$𝑞′_{𝑛𝑒𝑤}=𝑞_{𝑛𝑒𝑤}$时，表示与第一棵树相连，算法结束 274 | if (abs(new_node_2->x - new_node->x) < 0.00001 && abs(new_node_2->y - new_node->y) < 0.00001) 275 | { 276 | cout << "reaches the goal!" << endl; 277 | return generateFinalCourse(); 278 | } 279 | } 280 | } 281 | } 282 | // # 考虑两棵树的平衡性，即两棵树的节点数的多少，交换次序选择“小”的那棵树进行扩展。 283 | if (node_list_1.size() > node_list_2.size()) 284 | { 285 | swap(node_list_1, node_list_2); 286 | } 287 | draw(rnd_node, new_node); 288 | } 289 | return {}; 290 | } 291 | 292 | /** 293 | * 画圆 294 | * @param x 295 | * @param y 296 | * @param size 297 | * @param color 298 | */ 299 | void RRTConnect::plotCircle(double x, double y, double size, string color) 300 | { 301 | vector x_t, y_t; 302 | for (double i = 0.; i <= 2 * PI; i += 0.01) 303 | { 304 | x_t.push_back(x + size * cos(i)); 305 | y_t.push_back(y + size * sin(i)); 306 | } 307 | plt::plot(x_t, y_t, color); 308 | } 309 | 310 | /** 311 | * 画出搜索过程的图 312 | * @param node 313 | */ 314 | void RRTConnect::draw(Node *node1, Node *node2) 315 | { 316 | plt::clf(); 317 | // 画随机点 318 | if (node1) 319 | { 320 | plt::plot(vector{node1->x}, vector{node1->y}, "^k"); 321 | if (robot_radius > 0) 322 | { 323 | plotCircle(node1->x, node1->y, robot_radius, "-r"); 324 | } 325 | } 326 | if (node2) 327 | { 328 | plt::plot(vector{node2->x}, vector{node2->y}, "^k"); 329 | if (robot_radius > 0) 330 | { 331 | plotCircle(node2->x, node2->y, robot_radius, "-r"); 332 | } 333 | } 334 | 335 | // 画已生成的树 336 | for (Node *n1 : node_list_1) 337 | { 338 | if (n1->parent) 339 | { 340 | plt::plot(n1->path_x, n1->path_y, "-g"); 341 | } 342 | } 343 | // 画已生成的树 344 | for (Node *n2 : node_list_2) 345 | { 346 | if (n2->parent) 347 | { 348 | plt::plot(n2->path_x, n2->path_y, "-g"); 349 | } 350 | } 351 | // 画障碍物 352 | for (vector ob : obstacle_list) 353 | { 354 | plotCircle(ob[0], ob[1], ob[2]); 355 | } 356 | 357 | plt::plot(vector{play_area[0], play_area[1], play_area[1], play_area[0], play_area[0]}, vector{play_area[2], play_area[2], play_area[3], play_area[3], play_area[2]}, "k-"); 358 | 359 | // 画出起点和目标点 360 | plt::plot(vector{begin->x}, vector{begin->y}, "xr"); 361 | plt::plot(vector{end->x}, vector{end->y}, "xr"); 362 | plt::axis("equal"); 363 | plt::grid(true); 364 | plt::xlim(play_area[0] - 1, play_area[1] + 1); 365 | plt::ylim(play_area[2] - 1, play_area[3] + 1); 366 | // // 将每一帧保存为单独的文件 367 | // stringstream filename; 368 | // filename << "./frame_" << gifindex++ << ".png"; 369 | // plt::save(filename.str()); 370 | plt::pause(0.01); 371 | } 372 | -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree_connect/RRT_connect.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RRT_connect.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-24 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef RRT_CONNECT_H 13 | #define RRT_CONNECT_H 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "../matplotlibcpp.h" 24 | namespace plt = matplotlibcpp; 25 | 26 | using namespace std; 27 | using namespace Eigen; 28 | 29 | #define PI 3.14159265354 30 | 31 | //extern int gifindex; 32 | 33 | class Node 34 | { 35 | public: 36 | double x, y; // 节点坐标 37 | vector path_x = {}, path_y = {}; // 路径，作为画图的数据 38 | Node *parent; 39 | double cost; 40 | 41 | public: 42 | Node(double x, double y); 43 | }; 44 | 45 | class RRTConnect 46 | { 47 | public: 48 | vector> obstacle_list; // 障碍物位置列表 [[x,y,size],...] 49 | vector rand_area, play_area; // 采样区域 x,y ∈ [min,max];约束随机树的范围 [xmin,xmax,ymin,ymax] 50 | 51 | double robot_radius; // 机器人半径 52 | double expand_dis; // 扩展的步长 53 | double goal_sample_rate; // 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点 54 | vector node_list_1, node_list_2; // 与RRT不同的地方 55 | Node *begin; // 根节点 56 | Node *end; // 终节点 57 | 58 | int max_iter; 59 | 60 | public: 61 | RRTConnect(const vector> &obstacleList, 62 | const vector &randArea, const vector &playArea, double robotRadius, double expandDis, 63 | double goalSampleRate, int maxIter); 64 | 65 | vector calDistanceAngle(Node *from_node, Node *to_node); // 计算两个节点间的距离和方位角 66 | 67 | bool obstacleFree(Node *node); // 判断是否有障碍物 68 | 69 | bool isInsidePlayArea(Node *node); // 判断是否在可行区域里面 70 | 71 | int getNearestNodeIndex(vector node_list, Node *rnd_node); // 计算最近的节点 72 | 73 | Node *sampleFree(); // 采样生成节点 74 | 75 | double calDistToGoal(double x, double y); // 计算(x,y)离目标点的距离 76 | 77 | pair, vector> generateFinalCourse(); // 生成路径，与RRT不同的地方 78 | 79 | Node *steer(Node *from_node, Node *to_node, double extend_length = numeric_limits::max()); // 连线方向扩展固定步长查找x_new 80 | 81 | pair, vector> planning(); // 与RRT不同的地方 82 | 83 | void setBegin(Node *begin); 84 | 85 | void setEnd(Node *end); 86 | 87 | void plotCircle(double x, double y, double size, string color = "b"); // 画圆 88 | void draw(Node *node1 = nullptr, Node *node2 = nullptr); 89 | }; 90 | 91 | #endif // RRT_CONNECT_H 92 | -------------------------------------------------------------------------------- /Rapidly-exploring_Random_Tree_connect/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-04-24 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "RRT_connect.h" 13 | 14 | int main() 15 | { 16 | vector> obstacle_list{ 17 | {5, 5, 1}, 18 | {3, 6, 2}, 19 | {3, 8, 2}, 20 | {3, 10, 2}, 21 | {7, 5, 2}, 22 | {9, 5, 2}, 23 | {8, 10, 1}, 24 | {6, 12, 1}}; 25 | Node *begin = new Node(0.0, 0.0); 26 | Node *end = new Node(6.0, 10.0); 27 | vector rnd_area{-2, 15}; // 采样区域 x,y ∈ [min,max]; 28 | vector play_area{-2, 12, 0, 14}; // 约束随机树的范围 [xmin,xmax,ymin,ymax] 29 | double radius = 0.8; // 机器人半径 30 | double expand_dis = 2; // 扩展的步长 31 | double goal_sample_rate = 5; // 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点 32 | int max_iter = 500; // 最大采样点数 33 | RRTConnect rrt(obstacle_list, rnd_area, play_area, radius, expand_dis, goal_sample_rate, max_iter); 34 | rrt.setBegin(begin); 35 | rrt.setEnd(end); 36 | 37 | pair, vector> traj = rrt.planning(); 38 | 39 | plt::plot(traj.first, traj.second, "r"); 40 | 41 | // // 合成 GIF 图片 42 | // stringstream filename; 43 | // filename << "./frame_" << gifindex << ".png"; 44 | // plt::save(filename.str()); 45 | 46 | // const char *gif_filename = "./rrt_connect.gif"; 47 | // stringstream cmd; 48 | // cmd << "convert -delay 10 -loop 0 ./frame_*.png " << gif_filename; 49 | // system(cmd.str().c_str()); 50 | // cout << "Saving result to " << gif_filename << std::endl; 51 | // plt::show(); 52 | // //删除png图片 53 | // system("rm *.png"); 54 | // return 0; 55 | 56 | const char *filename = "./rrt_connect_demo.png"; 57 | cout << "Saving result to " << filename << std::endl; 58 | plt::save(filename); 59 | plt::show(); 60 | 61 | return 0; 62 | } -------------------------------------------------------------------------------- /gif/astar.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/gif/astar.gif -------------------------------------------------------------------------------- /gif/b_spline_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/gif/b_spline_demo.gif -------------------------------------------------------------------------------- /gif/bezier_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/gif/bezier_demo.gif -------------------------------------------------------------------------------- /gif/dijkstra_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/gif/dijkstra_demo.gif -------------------------------------------------------------------------------- /gif/rrt_connect.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/gif/rrt_connect.gif -------------------------------------------------------------------------------- /gif/rrt_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/gif/rrt_demo.gif -------------------------------------------------------------------------------- /gif/rrt_star_demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/path_planning/83627373069fe4d84dae0fde2de125dcde909a16/gif/rrt_star_demo.gif -------------------------------------------------------------------------------- /matplotlibcpp.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Python headers must be included before any system headers, since 4 | // they define _POSIX_C_SOURCE 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include // requires c++11 support 15 | #include 16 | #include // std::stod 17 | 18 | #ifndef WITHOUT_NUMPY 19 | # define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION 20 | # include 21 | 22 | # ifdef WITH_OPENCV 23 | # include 24 | # endif // WITH_OPENCV 25 | 26 | /* 27 | * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so 28 | * define the ones we need here. 29 | */ 30 | # if CV_MAJOR_VERSION > 3 31 | # define CV_BGR2RGB cv::COLOR_BGR2RGB 32 | # define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA 33 | # endif 34 | #endif // WITHOUT_NUMPY 35 | 36 | #if PY_MAJOR_VERSION >= 3 37 | # define PyString_FromString PyUnicode_FromString 38 | # define PyInt_FromLong PyLong_FromLong 39 | # define PyString_FromString PyUnicode_FromString 40 | #endif 41 | 42 | 43 | namespace matplotlibcpp { 44 | namespace detail { 45 | 46 | static std::string s_backend; 47 | 48 | struct _interpreter { 49 | PyObject* s_python_function_arrow; 50 | PyObject *s_python_function_show; 51 | PyObject *s_python_function_close; 52 | PyObject *s_python_function_draw; 53 | PyObject *s_python_function_pause; 54 | PyObject *s_python_function_save; 55 | PyObject *s_python_function_figure; 56 | PyObject *s_python_function_fignum_exists; 57 | PyObject *s_python_function_plot; 58 | PyObject *s_python_function_quiver; 59 | PyObject* s_python_function_contour; 60 | PyObject *s_python_function_semilogx; 61 | PyObject *s_python_function_semilogy; 62 | PyObject *s_python_function_loglog; 63 | PyObject *s_python_function_fill; 64 | PyObject *s_python_function_fill_between; 65 | PyObject *s_python_function_hist; 66 | PyObject *s_python_function_imshow; 67 | PyObject *s_python_function_scatter; 68 | PyObject *s_python_function_boxplot; 69 | PyObject *s_python_function_subplot; 70 | PyObject *s_python_function_subplot2grid; 71 | PyObject *s_python_function_legend; 72 | PyObject *s_python_function_xlim; 73 | PyObject *s_python_function_ion; 74 | PyObject *s_python_function_ginput; 75 | PyObject *s_python_function_ylim; 76 | PyObject *s_python_function_title; 77 | PyObject *s_python_function_axis; 78 | PyObject *s_python_function_axhline; 79 | PyObject *s_python_function_axvline; 80 | PyObject *s_python_function_axvspan; 81 | PyObject *s_python_function_xlabel; 82 | PyObject *s_python_function_ylabel; 83 | PyObject *s_python_function_gca; 84 | PyObject *s_python_function_xticks; 85 | PyObject *s_python_function_yticks; 86 | PyObject* s_python_function_margins; 87 | PyObject *s_python_function_tick_params; 88 | PyObject *s_python_function_grid; 89 | PyObject* s_python_function_cla; 90 | PyObject *s_python_function_clf; 91 | PyObject *s_python_function_errorbar; 92 | PyObject *s_python_function_annotate; 93 | PyObject *s_python_function_tight_layout; 94 | PyObject *s_python_colormap; 95 | PyObject *s_python_empty_tuple; 96 | PyObject *s_python_function_stem; 97 | PyObject *s_python_function_xkcd; 98 | PyObject *s_python_function_text; 99 | PyObject *s_python_function_suptitle; 100 | PyObject *s_python_function_bar; 101 | PyObject *s_python_function_barh; 102 | PyObject *s_python_function_colorbar; 103 | PyObject *s_python_function_subplots_adjust; 104 | PyObject *s_python_function_rcparams; 105 | PyObject *s_python_function_spy; 106 | 107 | /* For now, _interpreter is implemented as a singleton since its currently not possible to have 108 | multiple independent embedded python interpreters without patching the python source code 109 | or starting a separate process for each. [1] 110 | Furthermore, many python objects expect that they are destructed in the same thread as they 111 | were constructed. [2] So for advanced usage, a `kill()` function is provided so that library 112 | users can manually ensure that the interpreter is constructed and destroyed within the 113 | same thread. 114 | 115 | 1: http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program 116 | 2: https://github.com/lava/matplotlib-cpp/pull/202#issue-436220256 117 | */ 118 | 119 | static _interpreter& get() { 120 | return interkeeper(false); 121 | } 122 | 123 | static _interpreter& kill() { 124 | return interkeeper(true); 125 | } 126 | 127 | // Stores the actual singleton object referenced by `get()` and `kill()`. 128 | static _interpreter& interkeeper(bool should_kill) { 129 | static _interpreter ctx; 130 | if (should_kill) 131 | ctx.~_interpreter(); 132 | return ctx; 133 | } 134 | 135 | PyObject* safe_import(PyObject* module, std::string fname) { 136 | PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); 137 | 138 | if (!fn) 139 | throw std::runtime_error(std::string("Couldn't find required function: ") + fname); 140 | 141 | if (!PyFunction_Check(fn)) 142 | throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); 143 | 144 | return fn; 145 | } 146 | 147 | private: 148 | 149 | #ifndef WITHOUT_NUMPY 150 | # if PY_MAJOR_VERSION >= 3 151 | 152 | void *import_numpy() { 153 | import_array(); // initialize C-API 154 | return NULL; 155 | } 156 | 157 | # else 158 | 159 | void import_numpy() { 160 | import_array(); // initialize C-API 161 | } 162 | 163 | # endif 164 | #endif 165 | 166 | _interpreter() { 167 | 168 | // optional but recommended 169 | #if PY_MAJOR_VERSION >= 3 170 | wchar_t name[] = L"plotting"; 171 | #else 172 | char name[] = "plotting"; 173 | #endif 174 | Py_SetProgramName(name); 175 | Py_Initialize(); 176 | 177 | wchar_t const *dummy_args[] = {L"Python", NULL}; // const is needed because literals must not be modified 178 | wchar_t const **argv = dummy_args; 179 | int argc = sizeof(dummy_args)/sizeof(dummy_args[0])-1; 180 | 181 | #if PY_MAJOR_VERSION >= 3 182 | PySys_SetArgv(argc, const_cast(argv)); 183 | #else 184 | PySys_SetArgv(argc, (char **)(argv)); 185 | #endif 186 | 187 | #ifndef WITHOUT_NUMPY 188 | import_numpy(); // initialize numpy C-API 189 | #endif 190 | 191 | PyObject* matplotlibname = PyString_FromString("matplotlib"); 192 | PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); 193 | PyObject* cmname = PyString_FromString("matplotlib.cm"); 194 | PyObject* pylabname = PyString_FromString("pylab"); 195 | if (!pyplotname || !pylabname || !matplotlibname || !cmname) { 196 | throw std::runtime_error("couldnt create string"); 197 | } 198 | 199 | PyObject* matplotlib = PyImport_Import(matplotlibname); 200 | 201 | Py_DECREF(matplotlibname); 202 | if (!matplotlib) { 203 | PyErr_Print(); 204 | throw std::runtime_error("Error loading module matplotlib!"); 205 | } 206 | 207 | // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, 208 | // or matplotlib.backends is imported for the first time 209 | if (!s_backend.empty()) { 210 | PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); 211 | } 212 | 213 | 214 | 215 | PyObject* pymod = PyImport_Import(pyplotname); 216 | Py_DECREF(pyplotname); 217 | if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } 218 | 219 | s_python_colormap = PyImport_Import(cmname); 220 | Py_DECREF(cmname); 221 | if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } 222 | 223 | PyObject* pylabmod = PyImport_Import(pylabname); 224 | Py_DECREF(pylabname); 225 | if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } 226 | 227 | s_python_function_arrow = safe_import(pymod, "arrow"); 228 | s_python_function_show = safe_import(pymod, "show"); 229 | s_python_function_close = safe_import(pymod, "close"); 230 | s_python_function_draw = safe_import(pymod, "draw"); 231 | s_python_function_pause = safe_import(pymod, "pause"); 232 | s_python_function_figure = safe_import(pymod, "figure"); 233 | s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); 234 | s_python_function_plot = safe_import(pymod, "plot"); 235 | s_python_function_quiver = safe_import(pymod, "quiver"); 236 | s_python_function_contour = safe_import(pymod, "contour"); 237 | s_python_function_semilogx = safe_import(pymod, "semilogx"); 238 | s_python_function_semilogy = safe_import(pymod, "semilogy"); 239 | s_python_function_loglog = safe_import(pymod, "loglog"); 240 | s_python_function_fill = safe_import(pymod, "fill"); 241 | s_python_function_fill_between = safe_import(pymod, "fill_between"); 242 | s_python_function_hist = safe_import(pymod,"hist"); 243 | s_python_function_scatter = safe_import(pymod,"scatter"); 244 | s_python_function_boxplot = safe_import(pymod,"boxplot"); 245 | s_python_function_subplot = safe_import(pymod, "subplot"); 246 | s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); 247 | s_python_function_legend = safe_import(pymod, "legend"); 248 | s_python_function_xlim = safe_import(pymod, "xlim"); 249 | s_python_function_ylim = safe_import(pymod, "ylim"); 250 | s_python_function_title = safe_import(pymod, "title"); 251 | s_python_function_axis = safe_import(pymod, "axis"); 252 | s_python_function_axhline = safe_import(pymod, "axhline"); 253 | s_python_function_axvline = safe_import(pymod, "axvline"); 254 | s_python_function_axvspan = safe_import(pymod, "axvspan"); 255 | s_python_function_xlabel = safe_import(pymod, "xlabel"); 256 | s_python_function_ylabel = safe_import(pymod, "ylabel"); 257 | s_python_function_gca = safe_import(pymod, "gca"); 258 | s_python_function_xticks = safe_import(pymod, "xticks"); 259 | s_python_function_yticks = safe_import(pymod, "yticks"); 260 | s_python_function_margins = safe_import(pymod, "margins"); 261 | s_python_function_tick_params = safe_import(pymod, "tick_params"); 262 | s_python_function_grid = safe_import(pymod, "grid"); 263 | s_python_function_ion = safe_import(pymod, "ion"); 264 | s_python_function_ginput = safe_import(pymod, "ginput"); 265 | s_python_function_save = safe_import(pylabmod, "savefig"); 266 | s_python_function_annotate = safe_import(pymod,"annotate"); 267 | s_python_function_cla = safe_import(pymod, "cla"); 268 | s_python_function_clf = safe_import(pymod, "clf"); 269 | s_python_function_errorbar = safe_import(pymod, "errorbar"); 270 | s_python_function_tight_layout = safe_import(pymod, "tight_layout"); 271 | s_python_function_stem = safe_import(pymod, "stem"); 272 | s_python_function_xkcd = safe_import(pymod, "xkcd"); 273 | s_python_function_text = safe_import(pymod, "text"); 274 | s_python_function_suptitle = safe_import(pymod, "suptitle"); 275 | s_python_function_bar = safe_import(pymod,"bar"); 276 | s_python_function_barh = safe_import(pymod, "barh"); 277 | s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); 278 | s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); 279 | s_python_function_rcparams = PyObject_GetAttrString(pymod, "rcParams"); 280 | s_python_function_spy = PyObject_GetAttrString(pymod, "spy"); 281 | #ifndef WITHOUT_NUMPY 282 | s_python_function_imshow = safe_import(pymod, "imshow"); 283 | #endif 284 | s_python_empty_tuple = PyTuple_New(0); 285 | } 286 | 287 | ~_interpreter() { 288 | Py_Finalize(); 289 | } 290 | }; 291 | 292 | } // end namespace detail 293 | 294 | /// Select the backend 295 | /// 296 | /// **NOTE:** This must be called before the first plot command to have 297 | /// any effect. 298 | /// 299 | /// Mainly useful to select the non-interactive 'Agg' backend when running 300 | /// matplotlibcpp in headless mode, for example on a machine with no display. 301 | /// 302 | /// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use 303 | inline void backend(const std::string& name) 304 | { 305 | detail::s_backend = name; 306 | } 307 | 308 | inline bool annotate(std::string annotation, double x, double y) 309 | { 310 | detail::_interpreter::get(); 311 | 312 | PyObject * xy = PyTuple_New(2); 313 | PyObject * str = PyString_FromString(annotation.c_str()); 314 | 315 | PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); 316 | PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); 317 | 318 | PyObject* kwargs = PyDict_New(); 319 | PyDict_SetItemString(kwargs, "xy", xy); 320 | 321 | PyObject* args = PyTuple_New(1); 322 | PyTuple_SetItem(args, 0, str); 323 | 324 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); 325 | 326 | Py_DECREF(args); 327 | Py_DECREF(kwargs); 328 | 329 | if(res) Py_DECREF(res); 330 | 331 | return res; 332 | } 333 | 334 | namespace detail { 335 | 336 | #ifndef WITHOUT_NUMPY 337 | // Type selector for numpy array conversion 338 | template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default 339 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; 340 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; 341 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; 342 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; 343 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; 344 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; 345 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; 346 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; 347 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; 348 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; 349 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; 350 | 351 | // Sanity checks; comment them out or change the numpy type below if you're compiling on 352 | // a platform where they don't apply 353 | static_assert(sizeof(long long) == 8); 354 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; 355 | static_assert(sizeof(unsigned long long) == 8); 356 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; 357 | 358 | template 359 | PyObject* get_array(const std::vector& v) 360 | { 361 | npy_intp vsize = v.size(); 362 | NPY_TYPES type = select_npy_type::type; 363 | if (type == NPY_NOTYPE) { 364 | size_t memsize = v.size()*sizeof(double); 365 | double* dp = static_cast(::malloc(memsize)); 366 | for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); 370 | return varray; 371 | } 372 | 373 | PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); 374 | return varray; 375 | } 376 | 377 | 378 | template 379 | PyObject* get_2darray(const std::vector<::std::vector>& v) 380 | { 381 | if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); 382 | 383 | npy_intp vsize[2] = {static_cast(v.size()), 384 | static_cast(v[0].size())}; 385 | 386 | PyArrayObject *varray = 387 | (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); 388 | 389 | double *vd_begin = static_cast(PyArray_DATA(varray)); 390 | 391 | for (const ::std::vector &v_row : v) { 392 | if (v_row.size() != static_cast(vsize[1])) 393 | throw std::runtime_error("Missmatched array size"); 394 | std::copy(v_row.begin(), v_row.end(), vd_begin); 395 | vd_begin += vsize[1]; 396 | } 397 | 398 | return reinterpret_cast(varray); 399 | } 400 | 401 | #else // fallback if we don't have numpy: copy every element of the given vector 402 | 403 | template 404 | PyObject* get_array(const std::vector& v) 405 | { 406 | PyObject* list = PyList_New(v.size()); 407 | for(size_t i = 0; i < v.size(); ++i) { 408 | PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); 409 | } 410 | return list; 411 | } 412 | 413 | #endif // WITHOUT_NUMPY 414 | 415 | // sometimes, for labels and such, we need string arrays 416 | inline PyObject * get_array(const std::vector& strings) 417 | { 418 | PyObject* list = PyList_New(strings.size()); 419 | for (std::size_t i = 0; i < strings.size(); ++i) { 420 | PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); 421 | } 422 | return list; 423 | } 424 | 425 | // not all matplotlib need 2d arrays, some prefer lists of lists 426 | template 427 | PyObject* get_listlist(const std::vector>& ll) 428 | { 429 | PyObject* listlist = PyList_New(ll.size()); 430 | for (std::size_t i = 0; i < ll.size(); ++i) { 431 | PyList_SetItem(listlist, i, get_array(ll[i])); 432 | } 433 | return listlist; 434 | } 435 | 436 | } // namespace detail 437 | 438 | /// Plot a line through the given x and y data points.. 439 | /// 440 | /// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html 441 | template 442 | bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) 443 | { 444 | assert(x.size() == y.size()); 445 | 446 | detail::_interpreter::get(); 447 | 448 | // using numpy arrays 449 | PyObject* xarray = detail::get_array(x); 450 | PyObject* yarray = detail::get_array(y); 451 | 452 | // construct positional args 453 | PyObject* args = PyTuple_New(2); 454 | PyTuple_SetItem(args, 0, xarray); 455 | PyTuple_SetItem(args, 1, yarray); 456 | 457 | // construct keyword args 458 | PyObject* kwargs = PyDict_New(); 459 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 460 | { 461 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 462 | } 463 | 464 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); 465 | 466 | Py_DECREF(args); 467 | Py_DECREF(kwargs); 468 | if(res) Py_DECREF(res); 469 | 470 | return res; 471 | } 472 | 473 | // TODO - it should be possible to make this work by implementing 474 | // a non-numpy alternative for `detail::get_2darray()`. 475 | #ifndef WITHOUT_NUMPY 476 | template 477 | void plot_surface(const std::vector<::std::vector> &x, 478 | const std::vector<::std::vector> &y, 479 | const std::vector<::std::vector> &z, 480 | const std::map &keywords = 481 | std::map(), 482 | const long fig_number=0) 483 | { 484 | detail::_interpreter::get(); 485 | 486 | // We lazily load the modules here the first time this function is called 487 | // because I'm not sure that we can assume "matplotlib installed" implies 488 | // "mpl_toolkits installed" on all platforms, and we don't want to require 489 | // it for people who don't need 3d plots. 490 | static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; 491 | if (!mpl_toolkitsmod) { 492 | detail::_interpreter::get(); 493 | 494 | PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); 495 | PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); 496 | if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } 497 | 498 | mpl_toolkitsmod = PyImport_Import(mpl_toolkits); 499 | Py_DECREF(mpl_toolkits); 500 | if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } 501 | 502 | axis3dmod = PyImport_Import(axis3d); 503 | Py_DECREF(axis3d); 504 | if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } 505 | } 506 | 507 | assert(x.size() == y.size()); 508 | assert(y.size() == z.size()); 509 | 510 | // using numpy arrays 511 | PyObject *xarray = detail::get_2darray(x); 512 | PyObject *yarray = detail::get_2darray(y); 513 | PyObject *zarray = detail::get_2darray(z); 514 | 515 | // construct positional args 516 | PyObject *args = PyTuple_New(3); 517 | PyTuple_SetItem(args, 0, xarray); 518 | PyTuple_SetItem(args, 1, yarray); 519 | PyTuple_SetItem(args, 2, zarray); 520 | 521 | // Build up the kw args. 522 | PyObject *kwargs = PyDict_New(); 523 | PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); 524 | PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); 525 | 526 | PyObject *python_colormap_coolwarm = PyObject_GetAttrString( 527 | detail::_interpreter::get().s_python_colormap, "coolwarm"); 528 | 529 | PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); 530 | 531 | for (std::map::const_iterator it = keywords.begin(); 532 | it != keywords.end(); ++it) { 533 | if (it->first == "linewidth" || it->first == "alpha") { 534 | PyDict_SetItemString(kwargs, it->first.c_str(), 535 | PyFloat_FromDouble(std::stod(it->second))); 536 | } else { 537 | PyDict_SetItemString(kwargs, it->first.c_str(), 538 | PyString_FromString(it->second.c_str())); 539 | } 540 | } 541 | 542 | PyObject *fig_args = PyTuple_New(1); 543 | PyObject* fig = nullptr; 544 | PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number)); 545 | PyObject *fig_exists = 546 | PyObject_CallObject( 547 | detail::_interpreter::get().s_python_function_fignum_exists, fig_args); 548 | if (!PyObject_IsTrue(fig_exists)) { 549 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 550 | detail::_interpreter::get().s_python_empty_tuple); 551 | } else { 552 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 553 | fig_args); 554 | } 555 | Py_DECREF(fig_exists); 556 | if (!fig) throw std::runtime_error("Call to figure() failed."); 557 | 558 | PyObject *gca_kwargs = PyDict_New(); 559 | PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); 560 | 561 | PyObject *gca = PyObject_GetAttrString(fig, "gca"); 562 | if (!gca) throw std::runtime_error("No gca"); 563 | Py_INCREF(gca); 564 | PyObject *axis = PyObject_Call( 565 | gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); 566 | 567 | if (!axis) throw std::runtime_error("No axis"); 568 | Py_INCREF(axis); 569 | 570 | Py_DECREF(gca); 571 | Py_DECREF(gca_kwargs); 572 | 573 | PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); 574 | if (!plot_surface) throw std::runtime_error("No surface"); 575 | Py_INCREF(plot_surface); 576 | PyObject *res = PyObject_Call(plot_surface, args, kwargs); 577 | if (!res) throw std::runtime_error("failed surface"); 578 | Py_DECREF(plot_surface); 579 | 580 | Py_DECREF(axis); 581 | Py_DECREF(args); 582 | Py_DECREF(kwargs); 583 | if (res) Py_DECREF(res); 584 | } 585 | 586 | template 587 | void contour(const std::vector<::std::vector> &x, 588 | const std::vector<::std::vector> &y, 589 | const std::vector<::std::vector> &z, 590 | const std::map &keywords = {}) 591 | { 592 | detail::_interpreter::get(); 593 | 594 | // using numpy arrays 595 | PyObject *xarray = detail::get_2darray(x); 596 | PyObject *yarray = detail::get_2darray(y); 597 | PyObject *zarray = detail::get_2darray(z); 598 | 599 | // construct positional args 600 | PyObject *args = PyTuple_New(3); 601 | PyTuple_SetItem(args, 0, xarray); 602 | PyTuple_SetItem(args, 1, yarray); 603 | PyTuple_SetItem(args, 2, zarray); 604 | 605 | // Build up the kw args. 606 | PyObject *kwargs = PyDict_New(); 607 | 608 | PyObject *python_colormap_coolwarm = PyObject_GetAttrString( 609 | detail::_interpreter::get().s_python_colormap, "coolwarm"); 610 | 611 | PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); 612 | 613 | for (std::map::const_iterator it = keywords.begin(); 614 | it != keywords.end(); ++it) { 615 | PyDict_SetItemString(kwargs, it->first.c_str(), 616 | PyString_FromString(it->second.c_str())); 617 | } 618 | 619 | PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_contour, args, kwargs); 620 | if (!res) 621 | throw std::runtime_error("failed contour"); 622 | 623 | Py_DECREF(args); 624 | Py_DECREF(kwargs); 625 | if (res) Py_DECREF(res); 626 | } 627 | 628 | template 629 | void spy(const std::vector<::std::vector> &x, 630 | const double markersize = -1, // -1 for default matplotlib size 631 | const std::map &keywords = {}) 632 | { 633 | detail::_interpreter::get(); 634 | 635 | PyObject *xarray = detail::get_2darray(x); 636 | 637 | PyObject *kwargs = PyDict_New(); 638 | if (markersize != -1) { 639 | PyDict_SetItemString(kwargs, "markersize", PyFloat_FromDouble(markersize)); 640 | } 641 | for (std::map::const_iterator it = keywords.begin(); 642 | it != keywords.end(); ++it) { 643 | PyDict_SetItemString(kwargs, it->first.c_str(), 644 | PyString_FromString(it->second.c_str())); 645 | } 646 | 647 | PyObject *plot_args = PyTuple_New(1); 648 | PyTuple_SetItem(plot_args, 0, xarray); 649 | 650 | PyObject *res = PyObject_Call( 651 | detail::_interpreter::get().s_python_function_spy, plot_args, kwargs); 652 | 653 | Py_DECREF(plot_args); 654 | Py_DECREF(kwargs); 655 | if (res) Py_DECREF(res); 656 | } 657 | #endif // WITHOUT_NUMPY 658 | 659 | template 660 | void plot3(const std::vector &x, 661 | const std::vector &y, 662 | const std::vector &z, 663 | const std::map &keywords = 664 | std::map(), 665 | const long fig_number=0) 666 | { 667 | detail::_interpreter::get(); 668 | 669 | // Same as with plot_surface: We lazily load the modules here the first time 670 | // this function is called because I'm not sure that we can assume "matplotlib 671 | // installed" implies "mpl_toolkits installed" on all platforms, and we don't 672 | // want to require it for people who don't need 3d plots. 673 | static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; 674 | if (!mpl_toolkitsmod) { 675 | detail::_interpreter::get(); 676 | 677 | PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); 678 | PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); 679 | if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } 680 | 681 | mpl_toolkitsmod = PyImport_Import(mpl_toolkits); 682 | Py_DECREF(mpl_toolkits); 683 | if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } 684 | 685 | axis3dmod = PyImport_Import(axis3d); 686 | Py_DECREF(axis3d); 687 | if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } 688 | } 689 | 690 | assert(x.size() == y.size()); 691 | assert(y.size() == z.size()); 692 | 693 | PyObject *xarray = detail::get_array(x); 694 | PyObject *yarray = detail::get_array(y); 695 | PyObject *zarray = detail::get_array(z); 696 | 697 | // construct positional args 698 | PyObject *args = PyTuple_New(3); 699 | PyTuple_SetItem(args, 0, xarray); 700 | PyTuple_SetItem(args, 1, yarray); 701 | PyTuple_SetItem(args, 2, zarray); 702 | 703 | // Build up the kw args. 704 | PyObject *kwargs = PyDict_New(); 705 | 706 | for (std::map::const_iterator it = keywords.begin(); 707 | it != keywords.end(); ++it) { 708 | PyDict_SetItemString(kwargs, it->first.c_str(), 709 | PyString_FromString(it->second.c_str())); 710 | } 711 | 712 | PyObject *fig_args = PyTuple_New(1); 713 | PyObject* fig = nullptr; 714 | PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number)); 715 | PyObject *fig_exists = 716 | PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, fig_args); 717 | if (!PyObject_IsTrue(fig_exists)) { 718 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 719 | detail::_interpreter::get().s_python_empty_tuple); 720 | } else { 721 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 722 | fig_args); 723 | } 724 | if (!fig) throw std::runtime_error("Call to figure() failed."); 725 | 726 | PyObject *gca_kwargs = PyDict_New(); 727 | PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); 728 | 729 | PyObject *gca = PyObject_GetAttrString(fig, "gca"); 730 | if (!gca) throw std::runtime_error("No gca"); 731 | Py_INCREF(gca); 732 | PyObject *axis = PyObject_Call( 733 | gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); 734 | 735 | if (!axis) throw std::runtime_error("No axis"); 736 | Py_INCREF(axis); 737 | 738 | Py_DECREF(gca); 739 | Py_DECREF(gca_kwargs); 740 | 741 | PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); 742 | if (!plot3) throw std::runtime_error("No 3D line plot"); 743 | Py_INCREF(plot3); 744 | PyObject *res = PyObject_Call(plot3, args, kwargs); 745 | if (!res) throw std::runtime_error("Failed 3D line plot"); 746 | Py_DECREF(plot3); 747 | 748 | Py_DECREF(axis); 749 | Py_DECREF(args); 750 | Py_DECREF(kwargs); 751 | if (res) Py_DECREF(res); 752 | } 753 | 754 | template 755 | bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) 756 | { 757 | assert(x.size() == y.size()); 758 | 759 | detail::_interpreter::get(); 760 | 761 | // using numpy arrays 762 | PyObject* xarray = detail::get_array(x); 763 | PyObject* yarray = detail::get_array(y); 764 | 765 | // construct positional args 766 | PyObject* args = PyTuple_New(2); 767 | PyTuple_SetItem(args, 0, xarray); 768 | PyTuple_SetItem(args, 1, yarray); 769 | 770 | // construct keyword args 771 | PyObject* kwargs = PyDict_New(); 772 | for (std::map::const_iterator it = 773 | keywords.begin(); it != keywords.end(); ++it) { 774 | PyDict_SetItemString(kwargs, it->first.c_str(), 775 | PyString_FromString(it->second.c_str())); 776 | } 777 | 778 | PyObject* res = PyObject_Call( 779 | detail::_interpreter::get().s_python_function_stem, args, kwargs); 780 | 781 | Py_DECREF(args); 782 | Py_DECREF(kwargs); 783 | if (res) 784 | Py_DECREF(res); 785 | 786 | return res; 787 | } 788 | 789 | template< typename Numeric > 790 | bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) 791 | { 792 | assert(x.size() == y.size()); 793 | 794 | detail::_interpreter::get(); 795 | 796 | // using numpy arrays 797 | PyObject* xarray = detail::get_array(x); 798 | PyObject* yarray = detail::get_array(y); 799 | 800 | // construct positional args 801 | PyObject* args = PyTuple_New(2); 802 | PyTuple_SetItem(args, 0, xarray); 803 | PyTuple_SetItem(args, 1, yarray); 804 | 805 | // construct keyword args 806 | PyObject* kwargs = PyDict_New(); 807 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 808 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 809 | } 810 | 811 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); 812 | 813 | Py_DECREF(args); 814 | Py_DECREF(kwargs); 815 | 816 | if (res) Py_DECREF(res); 817 | 818 | return res; 819 | } 820 | 821 | template< typename Numeric > 822 | bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) 823 | { 824 | assert(x.size() == y1.size()); 825 | assert(x.size() == y2.size()); 826 | 827 | detail::_interpreter::get(); 828 | 829 | // using numpy arrays 830 | PyObject* xarray = detail::get_array(x); 831 | PyObject* y1array = detail::get_array(y1); 832 | PyObject* y2array = detail::get_array(y2); 833 | 834 | // construct positional args 835 | PyObject* args = PyTuple_New(3); 836 | PyTuple_SetItem(args, 0, xarray); 837 | PyTuple_SetItem(args, 1, y1array); 838 | PyTuple_SetItem(args, 2, y2array); 839 | 840 | // construct keyword args 841 | PyObject* kwargs = PyDict_New(); 842 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { 843 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 844 | } 845 | 846 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); 847 | 848 | Py_DECREF(args); 849 | Py_DECREF(kwargs); 850 | if(res) Py_DECREF(res); 851 | 852 | return res; 853 | } 854 | 855 | template 856 | bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", 857 | const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { 858 | PyObject* obj_x = PyFloat_FromDouble(x); 859 | PyObject* obj_y = PyFloat_FromDouble(y); 860 | PyObject* obj_end_x = PyFloat_FromDouble(end_x); 861 | PyObject* obj_end_y = PyFloat_FromDouble(end_y); 862 | 863 | PyObject* kwargs = PyDict_New(); 864 | PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); 865 | PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); 866 | PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); 867 | PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); 868 | 869 | PyObject* plot_args = PyTuple_New(4); 870 | PyTuple_SetItem(plot_args, 0, obj_x); 871 | PyTuple_SetItem(plot_args, 1, obj_y); 872 | PyTuple_SetItem(plot_args, 2, obj_end_x); 873 | PyTuple_SetItem(plot_args, 3, obj_end_y); 874 | 875 | PyObject* res = 876 | PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); 877 | 878 | Py_DECREF(plot_args); 879 | Py_DECREF(kwargs); 880 | if (res) 881 | Py_DECREF(res); 882 | 883 | return res; 884 | } 885 | 886 | template< typename Numeric> 887 | bool hist(const std::vector& y, long bins=10,std::string color="b", 888 | double alpha=1.0, bool cumulative=false) 889 | { 890 | detail::_interpreter::get(); 891 | 892 | PyObject* yarray = detail::get_array(y); 893 | 894 | PyObject* kwargs = PyDict_New(); 895 | PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); 896 | PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); 897 | PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); 898 | PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); 899 | 900 | PyObject* plot_args = PyTuple_New(1); 901 | 902 | PyTuple_SetItem(plot_args, 0, yarray); 903 | 904 | 905 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); 906 | 907 | 908 | Py_DECREF(plot_args); 909 | Py_DECREF(kwargs); 910 | if(res) Py_DECREF(res); 911 | 912 | return res; 913 | } 914 | 915 | #ifndef WITHOUT_NUMPY 916 | namespace detail { 917 | 918 | inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map