结果展示

这里对于图形化界面进行一个简单的介绍：

绿色：已搜索（待遍历）节点；
浅灰色：搜索过程中的最小F数值点；
红色：起点；
绿色：终点；
黄色：最终的生成路径；
黑色：障碍物（不可走坐标）；

A_star类

源码如下所示：

class A_star
{
    public:
        A_star() {};
        ~A_star();
        void Astar_algorithm();
        bool judge_env_legal(const Point&);
        int calcu_Hn(const Point&);
        int calcu_Gn(const Point&);
        int calcu_cost(const Point&, const Point&);
        void set_start_end_obs_map(Graph&);
        void add_openset(Point* p) { openset.push_back(p); };
        void add_fatherset(Point* p) { fatherset.push_back(p); };
        void delete_openset(const int& idx) {openset.erase(openset.begin() + idx);};
        void search_env_block(Point*);
        void calcu_type_selection(int&, const int&, const int&);
        void show_para_below(Point*);
        bool is_alread_set(std::vector<Point*>&, Point*);
        void get_trajectory(const Point*);
        UserPara get_astar_userpara();
        Point* find_min_Fn(int& idx);

    private:
        int cross_cost = 14;
        int plane_cost = 10;
        std::vector<Point*> openset;
        std::vector<Point*> fatherset;
        //std::vector<Point*> trajectory;
        std::vector<Point*> obstacleset;        
        Point start;
        Point end;
        UserPara user_trajectory;
        const int env_nums = 8;
        //int offset_x[4] = {0, 1, 0, -1};
        //int offset_y[4] = {-1, 0, 1, 0};
        int offset_x[8] = {-1, 0, 1, 1, 1, 0, -1, -1};
        int offset_y[8] = { -1, -1, -1, 0, 1, 1, 1, 0};

};

这个就是A_star类定义，下面是Astar_algorithm函数主体：

void A_star::Astar_algorithm()
{
    int min_Fn_idx(0);
    Point* min_Fn_p = new Point(start.x, start.y);

    memcpy(min_Fn_p, &start, sizeof(start));
    add_openset(min_Fn_p);

    while (!openset.empty()) {
        Graph::show_map(user_trajectory);
        min_Fn_p = find_min_Fn(min_Fn_idx);
        show_para_below(min_Fn_p);
        if (*min_Fn_p == end) { break; }
        search_env_block(min_Fn_p);

        add_fatherset(min_Fn_p);
        delete_openset(min_Fn_idx);

        user_trajectory.map[min_Fn_p->x][min_Fn_p->y] = PASS_SEARCH;
    }

    get_trajectory(min_Fn_p);
}

可以看到，这个这个函数主体就是这篇文章里的伪代码的具体实现：[Astar_algorithm01]A*算法伪代码以及思路

那么接下来就是算法探索邻域的方法了，从父节点往外走，一共有8个格点可以走，搜索节点的方法也是通过数组来进行索引的，具体示意图如下：

获得了搜索坐标点之后我们需要判断搜索的点是不是合法的，是不是超过了地图界限，如果是的话，需要delete掉申请的堆里的内存。那么如果搜索到的是正常的节点的时候就需要进行操作了。

首先是计算该节点的新G值，计算方法就是父节点的G加上父节点到该搜索节点的cost。

接下来就需要判断该坐标的节点是否已经遍历过且存到了fatherset里面或者是否是障碍物，如果是的话同样需要delete掉堆内的内存，如果不是的话就是可以更新探索的节点了。

如果该搜索节点已经在了openset里，那就说明这个点已经搜索过了，那么之后当该节点的新G值小于原来就有的G值的时候就需要进行G值，F值的更新否则不做操作，或者如果该搜索节点没有在openset里面，就只需要添加G值，F值然后把该节点加入openset即可。

void A_star::search_env_block(Point* p)
{
    int temp_cost(INT_MAX);

    for (int i = 0; i < env_nums; i++) {

        Point* search_p = new Point((*p).x + offset_x[i], (*p).y + offset_y[i]);

        if (judge_env_legal(*search_p)) {

            temp_cost = (*p).Gn + calcu_cost(*p, *search_p);

            if (is_alread_set(obstacleset, search_p) || is_alread_set(fatherset, search_p)) {
                delete search_p;
                continue;
            }
            else {
                user_trajectory.map[search_p->x][search_p->y] = SEARCH;
                if (!is_alread_set(openset, search_p)) {
                    search_p->Gn = temp_cost;
                    search_p->Fn = search_p->Gn + calcu_Hn(*search_p);
                    search_p->pre_point = p;
                    add_openset(search_p);
                }else if(search_p->Gn > temp_cost){
                    search_p->Gn = temp_cost;
                    search_p->Fn = search_p->Gn + calcu_Hn(*search_p);
                    search_p->pre_point = p;
                }

            }
        }
        else {delete search_p;}
    }

}