A * 算法C++实现

伪代码

openset = PriorityQueue() // pop out node with least totoal cost (heuriscost + trajcost)
openset.push(start, 0)
closedset = {
   }
trajcost[start] = 0

while not openset.empty():
      current = openset.top()
      
      if current = goal:
         break;
      
      openset.pop()
      closedset.put(current)
      
      for next in neighbors(current):
          if (checkcollison(next)):
              delete next
              continue
          if find in closedset:
              delete next
              continue
          next.trajcost = current.trajcost +  calc_trajcost(current, next)
          if not find in openset:
             next.heuricost = calc_heuriscost(next, goal)
             next.totalcost = next.trajcost + next.heuriscost
             parent[next] = current
             openset.push(next)
          if find in openset and openset[next].trajcost > next.trajcost:
             openset[next].trajcost = next.trajcost
             openset[next].totalcost =  openset[next].trajcost + openset[next].heuriscost

reconstruct(goal)
delete start
delete goal

/*************************************************************************
	> File Name: grid_a_star.cpp
	> Author: Yongyu Chen
	> Mail: yongyu.chen@tum.de
 ************************************************************************/

#include <iostream>
// #include <cmath>
#include <limits>
#include <vector>
#include <queue>
#include <cmath>
#include <algorithm>
#include <matplotlibcpp.h>
#include <unordered_map>

#include <boost/heap/binomial_heap.hpp>

using namespace std;
namespace plt = matplotlibcpp;
/**
 * @brief: Grid base A* shortest path planning
 */ 
class Node2d {
   
public:
    int x; // x index of the grid
    int y; // y index of the grid 
    double sum_cost; // cost, euclidean distance 
    double path_cost; // from start node to the current node
    double h_cost; // from 
    Node2d* p_node; // parent node, or replaced by the 

    // constructor 
    Node2d(int x_, int y_, double sum_cost_ = 0, double path_cost_ = 0, double h_cost_ = 0, 
        Node2d* p_node_ = nullptr): x(x_), y(y_),
        sum_cost(sum_cost_), path_cost(path_cost_), h_cost(h_cost_), p_node(p_node_){
   };
};
int calc_xyindex(double position, double min_pos, double reso) {
   
  return std::round((position - min_pos) / reso);
}
int calc_grid_index(Node2d& node, int xwidth, int ywidth) {
   
  return node.y * xwidth + node.x;  
}
double calc_grid_position(int index, double minp, double reso) {
   
  return index * reso + minp;
}

//  
/**
 * @brief:construct the final path after the algorithm is done 
 */ 
std::vector<std::vector<double>> calc_final_path(Node2d* goal, double minx, double miny, 
                                                    double reso) {
   
    std::vector<double> rx;
    std::vector<double> ry;
    Node2d* node = goal;
    while (node->p_node != nullptr) {
    // while the parent node exists
        node = node->p_node;
        rx.push_back(calc_grid_position(node->x, minx, reso));
        ry.push_back(calc_grid_position(node->y, miny, reso));
    }
    return {
   rx, ry};
}
/**
 * @brief: construct the realtime obstacle map
 * 
 */ 
// obstacle map 是一个二维数组包含0和１ 
std::vector<std::vector<int> > calc_obstacle_map(
      vector<double>& ox, vector<double>& oy, 
      int& xwidth, int