借鉴本文所述代码简单实现一下BFS，Astar和HybridAstar路径规划算法，用于辅助理解算法原理。
代码在这里，画图用到了matplotlibcpp库，需要先装一下，然后直接在文件目录下执行如下代码即可运行：

mkdir build
cd build
cmake ..
make
./HAstar

1. 场景

这里没考虑朝向

start_pose: 6, 10         // 起始点和朝向
end_pose: 90, 90          // 目标点和朝向
boundary: 0, 100, 0, 100  // 整个场景的边界（这里没有使用）
obstacle: 0, 0, 0, 100, 100, 100, 100, 0  // 整个场景的边界，四个顶点，顺时针组成一个四边形
obstacle: 12, 12, 12, 36, 30, 36, 30, 12  // 障碍物（下面都是），四个顶点，顺时针组成一个四边形
obstacle: 20, 50, 20, 80, 40, 80, 40, 50  
obstacle: 50, 6, 50, 60, 60, 60, 60, 6
obstacle: 60, 70, 60, 95, 85, 95, 85, 70
obstacle: 75, 20, 75, 50, 95, 50, 90, 20

网格大小取0.2（可以选其他值，可能会有不同的效果），原点处网格坐标为（0，0），将场景信息都转化成网格坐标，场景示意图如下图所示，其中红色圆点是起点，绿色圆点是终点：

2. BFS

std::vector<Vec2i> bfs(const Scenario& scenario,const std::unordered_set<Vec2i, Vec2iHash>& obstacles) {// 每个节点的查找方向，左上右下std::vector<Vec2i> neighbors{Vec2i{-1,0}, Vec2i{0,1}, Vec2i{1,0}, Vec2i{0,-1}}; // 每个节点的查找方向，8个方向//vector<Vec2i> neighbors{Vec2i{-1,1}, Vec2i{0, 1}, Vec2i{1, 1},//                           Vec2i{1,0}, Vec2i{1,-1}, Vec2i{0,-1}, //                           Vec2i{-1,-1}, Vec2i{-1,0}};                   std::vector<Vec2i> path; // 路径结果std::queue<Vec2i> open_nodes; // 待check的节点std::unordered_map<std::string, Vec2i> pre_node; // 每个节点的父节点bool isvalid = true;Vec2i start{static_cast<int>(scenario.start_pos.x/RES_GRID), static_cast<int>(scenario.start_pos.y/RES_GRID)};Vec2i goal{static_cast<int>(scenario.end_pos.x/RES_GRID), static_cast<int>(scenario.end_pos.y/RES_GRID)};std::string goal_name = std::to_string(goal.x)+"_" + std::to_string(goal.y);std::string start_name = std::to_string(start.x)+"_" + std::to_string(start.y);open_nodes.push(start);pre_node[start_name] = start;while(!open_nodes.empty()){Vec2i curr_node = open_nodes.front();open_nodes.pop();// 判断是否到达终点if(curr_node.x == goal.x && curr_node.y == goal.y){pre_node[goal_name] = pre_node[std::to_string(curr_node.x)+"_" + std::to_string(curr_node.y)];break;}// 获取相邻的有效节点for(int i = 0; i < 4; i++){isvalid = true;Vec2i neighbor{curr_node.x+neighbors[i].x, curr_node.y+neighbors[i].y};// 检查节点是否已经被遍历过std::string n_name = std::to_string(neighbor.x) + "_" + std::to_string(neighbor.y);if(pre_node.find(n_name) != pre_node.end()){continue;}// 简单的碰撞检测，节点附近一定范围内不能有障碍点 for(int i = -4; i< 5; i++){for(int j = 4; j > -5; j--){Vec2i pt{neighbor.x+i, neighbor.y+j};if(obstacles.find(pt) != obstacles.end()){isvalid = false;break;}}}if(!isvalid) continue;pre_node[n_name] = curr_node;open_nodes.push(neighbor);}}if(pre_node.find(goal_name) == pre_node.end()){std::cout<<"未找到有效路径！"<<std::endl;}else{std::cout<<"找到有效路径！"<<std::endl;Vec2i pt = goal;while(pt.x != start.x || pt.y != start.y){path.emplace_back(pt);pt = pre_node[std::to_string(pt.x) + "_" + std::to_string(pt.y)];   }path.emplace_back(start);}return path;
}

运行结果如下图所示：

3. Astar

std::vector<Vec2i> AStar(const Scenario& scenario,const std::unordered_set<Vec2i, Vec2iHash>& obstacles) {// 每个节点的查找方向，8个方向std::vector<Vec2i> neighbors{Vec2i{-1,1}, Vec2i{0, 1}, Vec2i{1, 1},Vec2i{1,0}, Vec2i{1,-1}, Vec2i{0,-1}, Vec2i{-1,-1}, Vec2i{-1,0}}; std::vector<Vec2i> path; // 路径结果                               // 节点队列，保存节点名和节点总代价（路径代价+启发函数代价）std::priority_queue<std::pair<std::string, double>, std::vector<std::pair<std::string, double>>, cmp> open_nodes_cost;// <节点名，<节点坐标，节点路径代价>>std::unordered_map<std::string, std::pair<Vec2i, double>> open_nodes;// <节点名，<节点坐标，节点路径代价>>std::unordered_map<std::string, Vec2i> close_nodes;// 保存节点的父节点std::unordered_map<std::string, Vec2i> pre_node; bool isvalid = true;Vec2i start{static_cast<int>(scenario.start_pos.x/RES_GRID), static_cast<int>(scenario.start_pos.y/RES_GRID)};Vec2i goal{static_cast<int>(scenario.end_pos.x/RES_GRID), static_cast<int>(scenario.end_pos.y/RES_GRID)};double start_cost = std::sqrt((goal.x - start.x)*(goal.x - start.x) + (goal.y - start.y)*(goal.y - start.y));std::string goal_name = std::to_string(goal.x) + "_" + std::to_string(goal.y);std::string start_name = std::to_string(start.x) + "_" + std::to_string(start.y);open_nodes_cost.emplace(start_name, start_cost);open_nodes.emplace(start_name, std::pair<Vec2i, double>(start, 0));pre_node.emplace(start_name, start);while(!open_nodes_cost.empty()){const std::string curr_name = open_nodes_cost.top().first;open_nodes_cost.pop();Vec2i curr_node = open_nodes[curr_name].first;double curr_path_cost = open_nodes[curr_name].second;// 判断是否到达终点if(curr_node.x == goal.x && curr_node.y == goal.y){pre_node[goal_name] = pre_node[curr_name];break;} // 当前节点已checkclose_nodes.emplace(curr_name, curr_node);// 遍历相邻节点for(int i = 0; i < 8; i++){isvalid = true;Vec2i neighbor{curr_node.x+neighbors[i].x, curr_node.y+neighbors[i].y};std::string neighbor_name = std::to_string(neighbor.x) + "_" + std::to_string(neighbor.y);// 简单的碰撞检测，节点附近一定范围内不能有障碍点 for(int i = -4; i< 5; i++){for(int j = 4; j > -5; j--){Vec2i pt{neighbor.x+i, neighbor.y+j};if(obstacles.find(pt) != obstacles.end()){isvalid = false;break;}}}if(!isvalid) continue;// 如果该点已经check过if(close_nodes.find(neighbor_name) != close_nodes.end()){continue;}// 计算节点代价double neighbor_path_cost = curr_path_cost + std::sqrt(neighbors[i].x*neighbors[i].x+neighbors[i].y*neighbors[i].y);// 启发函数直接用欧式距离double H_cost = std::sqrt((goal.x - neighbor.x)*(goal.x - neighbor.x) + (goal.y - neighbor.y)*(goal.y - neighbor.y));if(open_nodes.find(neighbor_name) == open_nodes.end()){open_nodes.emplace(neighbor_name, std::pair<Vec2i, double>(neighbor, neighbor_path_cost));open_nodes_cost.emplace(neighbor_name, H_cost+neighbor_path_cost);pre_node[neighbor_name] = curr_node;}}}if(pre_node.find(goal_name) == pre_node.end()){std::cout<<"未找到有效路径！"<<std::endl;}else{std::cout<<"找到有效路径！"<<std::endl;Vec2i pt = goal;while(pt.x != start.x || pt.y != start.y){path.emplace_back(pt);pt = pre_node[std::to_string(pt.x) + "_" + std::to_string(pt.y)];   }path.emplace_back(start);}return path;
}

与BFS结果一起显示，如下图所示，红色路径是BFS结果，紫色路径结果是Astar：

4. HybridStar

std::vector<Vec2i> HybridAStar(const Scenario& scenario,const unordered_set<Vec2i, Vec2iHash>& obstacles,double wheelbase, double step_size, double max_steer) {std::shared_ptr<Node> last_node = nullptr;  std::vector<Vec2i> path; // 路径结果                             // 节点队列，保存节点名和节点总代价（路径代价+启发函数代价）std::priority_queue<std::pair<std::string, double>, std::vector<std::pair<std::string, double>>, cmp> open_nodes_cost;// <节点名，节点状态>std::unordered_map<std::string, std::shared_ptr<Node>> open_nodes;// <节点名，节点状态>std::unordered_map<std::string, std::shared_ptr<Node>> close_nodes;Node start(scenario.start_pos.x, scenario.start_pos.y, 0, 0, 0, nullptr);Node goal(scenario.end_pos.x, scenario.end_pos.y, 0, 0, 0, nullptr);Vec2i goal_index{static_cast<int>(goal.x / RES_GRID), static_cast<int>(goal.y / RES_GRID)};double start_cost = std::sqrt((goal.x - start.x)*(goal.x - start.x) + (goal.y - start.y)*(goal.y - start.y)); // 真实坐标距离Vec2i start_index{static_cast<int>(start.x / RES_GRID),static_cast<int>(start.y / RES_GRID)};std::string start_name = std::to_string(start_index.x) +"_" + std::to_string(start_index.y) + "_" + std::to_string(0);std::shared_ptr<Node> start_Node = std::make_shared<Node>();start_Node->x = start.x;start_Node->y = start.y;start_Node->theta = start.theta;start_Node->g = start.g;start_Node->f = start.f;start_Node->parent = nullptr;open_nodes_cost.emplace(start_name, start_cost);open_nodes.emplace(start_name, start_Node);while(!open_nodes_cost.empty()){const std::string curr_name = open_nodes_cost.top().first;open_nodes_cost.pop();auto curr_node = open_nodes[curr_name];// 判断当前节点是否到达终点Vec2i curr_index{static_cast<int>(curr_node->x / RES_GRID), static_cast<int>(curr_node->y / RES_GRID)};if(curr_index.x == goal_index.x && curr_index.y == goal_index.y){last_node = curr_node;break;}close_nodes[curr_name] = curr_node;// 往下扩展，-45°到45°采样5次且只考虑前进for(int i = 0; i < 5; i++){std::shared_ptr<Node> next_Node = std::make_shared<Node>();double next_x = 0;double next_y = 0;double next_theta = 0;double steer = -M_PI/4 + i * (M_PI/8);  // 转向角if(i != 2){ // 转弯double radius = wheelbase / tan(steer); // 转弯半径double delt_theta = step_size / radius; // 航向角偏移next_theta = NormalizeAngle(curr_node->theta - delt_theta); // 转向角正时右转，航向角向负偏移if(radius < 0){ // 左转next_x = curr_node->x + abs(radius)*(cos(curr_node->theta + abs(delt_theta))-cos(curr_node->theta));next_y = curr_node->y + abs(radius)*(sin(curr_node->theta + abs(delt_theta))-sin(curr_node->theta));}else{ // 右转next_x = curr_node->x + abs(radius)*(-cos(curr_node->theta - abs(delt_theta))+cos(curr_node->theta));next_y = curr_node->y + abs(radius)*(sin(-curr_node->theta + abs(delt_theta))+sin(curr_node->theta));}}else{ // 直行next_x = curr_node->x - std::sin(curr_node->theta) * step_size;next_y = curr_node->y + std::cos(curr_node->theta) * step_size;next_theta = curr_node->theta;}next_Node->x = next_x;next_Node->y = next_y;next_Node->theta = next_theta;next_Node->g = curr_node->g + step_size; next_Node->f = next_Node->g + std::sqrt((next_Node->x-goal.x)*(next_Node->x-goal.x) +(next_Node->y-goal.y)*(next_Node->y-goal.y));next_Node->parent = curr_node;Vec2i next_node{static_cast<int>(next_Node->x / RES_GRID),static_cast<int>(next_Node->y / RES_GRID)};std::string next_name = std::to_string(next_node.x) + "_" + std::to_string(next_node.y)+"_"+to_string(i);if (close_nodes.find(next_name) != close_nodes.end()) {continue;}// 简单的碰撞检测，节点附近一定范围内不能有障碍点 bool isvalid_node = true;for(int i = -4; i< 5; i++){for(int j = 4; j > -5; j--){Vec2i pt{next_node.x+i, next_node.y+j};if(obstacles.find(pt) != obstacles.end()){isvalid_node = false;break;}}}if(!isvalid_node) continue;if(open_nodes.find(next_name) == open_nodes.end()){open_nodes.emplace(next_name, next_Node);open_nodes_cost.emplace(next_name, next_Node->f);}}}if(last_node == nullptr){std::cout<<"未找到有效路径！"<<std::endl;}else{std::cout<<"找到有效路径！"<<std::endl;std::shared_ptr<Node> temp_node = last_node;while(temp_node->parent != nullptr){path.emplace_back(Vec2i{static_cast<int>(temp_node->x / RES_GRID), static_cast<int>(temp_node->y / RES_GRID)});temp_node = temp_node->parent;}}return path;
}

与BFS和Astar的结果一起显示，如下图所示，红色路径是BFS结果，紫色路径结果是Astar，棕色路径是HybridAstar，看起来不是很平滑，感兴趣的可以自己调调看：