使用优先队列完成了一个Astar搜索算法的c++实现，有时间可以完整的完成一遍

Posted 2020-12-21

#include<queue>
#include<vector>
#include<unordered_map>
using namespace std;
struct Node {
    int x;
    int y;
    double cost;
    int parent;

    Node(int ix, int iy, int icost, int iparent)
        : x(ix), y(iy), cost(icost), parent(iparent) {
        }
    
    bool operator < (const Node& b) {
        return cost < b.cost;
    }
};



class AstarPlanner {
    public:
        void generateObstacleMap();
        vector<tuple<int, int, double>> getMotions();
        void verifyGrid(int x, int y);
        vector<pair<int, int>> aStarPlan(int sx, int sy, int tx, int ty) {
            auto start_node = Node(0, 0, 0, -1);
            auto target_node = Node(10, 10, 0, -1);

            priority_queue<tuple<double, int>> priority_cost_queue;
            unordered_map<int, Node> open_list;
            unordered_map<int, Node> closed_list;

            priority_cost_queue.push({0.0, 0});

            while (!priority_cost_queue.empty()) {
                 
                auto c_index = get<1>(priority_cost_queue.top());
                auto c_cost = get<0>(priority_cost_queue.top());
                auto cur_node = open_list[c_index];
                if (cur_node.x == target_node.x && cur_node.y == target_node.y) {
                    target_node.parent = cur_node.parent;
                    target_node.cost = cur_node.cost;
                    break;
                }


                priority_cost_queue.pop();
                open_list.erase(c_index);
                closed_list.insert(pair<int, Node>(c_index, cur_node));

                // expansion cur_node
                auto motions = getMotions();
                for (auto motion : motions_) {
                    auto node = Node(cur_node.x + get<0>(motion), cur_node.y + 
                                    get<1>(motion), cur_node.cost + get<2>(motion), c_index);
                    int node_index = getNodeIndex(node);

                    auto ptr_close = closed_list.find(node_index);
                    if (ptr_close != closed_list.end()) {
                        continue;
                    }

                    auto ptr_open = open_list.find(node_index);
                    if (ptr_open != open_list.end()) {
                        ptr_open->second.cost = node.cost;
                        priority_cost_queue.push({node.cost, node_index});
                    }
                    else if (ptr_open->second.cost > node.cost) {
                        open_list[node_index] = node;
                        priority_cost_queue.push({node.cost, node_index});
                    }
                    
                }
            }

            vector<pair<int, int>> path;
            path.emplace_back(target_node.x, target_node.y);
            auto parent_id = target_node.parent;
            while (1) {
                path.emplace_back(closed_list[parent_id].x, closed_list[parent_id].y);
                if (closed_list[parent_id].x == start_node.x && closed_list[parent_id].y == start_node.y) {
                    break;
                }
                parent_id = closed_list[parent_id].parent;
            } 

            reverse(path.begin(), path.end());
            return path;
        }

        int getNodeIndex(Node node);
    private:
        vector<vector<bool>> obstacle_map_;
        int x_wideth_;
        int y_wideth_;
        vector<tuple<int, int, double>> motions_;

};