【C++】A*路径规划算法

#include "ros/ros.h"
#include<iostream>
#include<vector>
#include<algorithm>
#include<cmath>
using namespace std;
//定义地图上的点
struct Point
{
    int x,y;//栅格行列
    Point(int x, int y):x(x),y(y){};//参数列表初始化
    double distance(Point &p)//求距离
    {
        return sqrt((x-p.x)^2 + (y-p.y)^2); //欧几里德距离
    };
};

//定义节点
struct Node
{
    Point point; //栅格点
    double g,h,f;//代价值，f总代价值，g到起点的代价值，h到终点的估计代价
    Node *parent;//父节点指针
    Node(Point point, double g, double h, Node *parent=nullptr):point(point),g(g),h(h),f(g+h),parent(parent){};
};

//自定义Node*的排序规则
struct NodeCompare
{
    bool operator()(Node *n1,Node *n2)
    {
        return (n1->f)<(n2->f);//表示升序排列
    };
};


//基于栅格地图的路径规划-A*算法,返回由Point组成的路径path,输入为地图、起点、终点
vector<Point>AstarPathPlanning(vector<vector<int>>&gridmap,Point &start,Point &goal)
{
    //获取地图参数
    int row=gridmap.size();//行，表示地图的宽
    int col=gridmap[0].size();//列，表示地图的长
    //定义openlist,closelist
    vector<Node *>openlist;//openlist:表示待搜索的节点
    vector<Node *>closelist;//closelist：表示已搜索的节点
    openlist.push_back(new Node(start,start.distance(start),start.distance(goal)));//将起点加入到openlist中初始化
    int count1=1;//统计new的次数    
    //进入循环，开始搜索,搜索到终点则返回路径
    vector<Point> path;
    while (!openlist.empty())//当openlist为空，表示所有可搜索节点已经被搜索，此时循环结束
    {
        //获取当前搜索节点current,即openlist中f最小的节点
        sort(openlist.begin(),openlist.end(),NodeCompare{});//先对openlist排序，这里要自定义排序规则(从小到大排列)
        Node *current=*openlist.begin();//openlist.begin()在排序后即为f最小的元素的迭代器位置
        openlist.erase(openlist.begin());//将current对应的元素从openlist中剔除
        closelist.push_back(current);//将current加入到closelist中
        //对当前搜索节点current进行分类讨论
        //1-current是终点，则返回路径，表示找到路径
        if (current->point.x==goal.x && current->point.y==goal.y)
        {
            while (current != nullptr)//利用父节点，从终点向起点回溯最短路径,因为起点没有副节点，所以起点的时候current的父节点为nullptr空指针
            {
                path.push_back(current->point);
                current=current->parent;
            };
            reverse(path.begin(),path.end());//路径是反的，翻转路径
            //仍然存在一定的问题，需要解决，因为openlist和closelist存储的元素都是new动态分配的内存，需要delete，否则会造成内存的泄漏
            int count2=0;//统计delete的次数
            for(auto o:openlist)
            {
                delete o;
                count2++;
            };
            for(auto c:closelist)
            {
                delete c;
                count2++;
            };  
            cout<<"new的次数:"<<count1<<endl;
            cout<<"delete的次数:"<<count2<<endl;                    
            return path;
        };
        //2-current不是终点，需要讨论其邻近的节点neighbors
        int x=current->point.x;
        int y=current->point.y;
        vector<Point>neighbors={//由current得到8个邻近节点的坐标
            {x-1,y-1},{x-1,y},{x-1,y+1},
            {x,y-1},          {x,y+1},
            {x+1,y-1},{x+1,y},{x+1,y+1}
        };
        for(auto n:neighbors)//遍历所有的邻近节点,每一个邻近节点n必须满足在地图范围内同时不是障碍物
        {
            if ((n.x >= 0 && n.x<row) && (n.y >= 0 && n.y<col)&& gridmap[n.x][n.y]==0)
            {
                //2.1 n在closelist中，表示已经探索过了，此时直接跳过
                bool incloselist=false;
                for(auto c:closelist)
                {
                    if (c->point.x==n.x && c->point.y == n.y)
                    {
                        incloselist=true;
                        break;
                    };
                    
                };
                if (incloselist)
                {
                    continue;
                };
                //2.2 n是否在openlist中进行讨论
                bool inopenlist=false;
                for(auto o:openlist)
                {
                    if (o->point.x==n.x && o->point.y==n.y)
                    {
                        inopenlist=true;//2.2.1 n是否在openlist中,对比f值，更新代价值和父节点parent
                        double g=current->g+n.distance(current->point);
                        //邻近节点n到起点的距离=当前搜索节点current到起点的距离+当前搜索节点current到邻近节点n距离
                        double h=n.distance(goal);//邻近节点n到终点的估计距离代价
                        double f=g+h;
                        if (f<(o->f))
                        {
                            o->f=f;
                            o->parent=current;
                        };
                        
                        break;
                    };
                    
                };
                if (!inopenlist)//2.2.2 n不在openlist中,对比f值，计算代价值，添加到openlist中，下次备选
                {
                    double g=current->g+n.distance(current->point);
                    //邻近节点n到起点的距离=当前搜索节点current到起点的距离+当前搜索节点current到邻近节点n距离
                    double h=n.distance(goal);//邻近节点n到终点的估计距离代价
                    double f=g+h;
                    openlist.push_back(new Node(n,g,h,current));
                    count1++;                    
                };
                
                
            };
            
        };

    };
    //搜索完成没有路径，表示路径规划失败,此时返回空路径
    int count2=0;//统计delete的次数
    for(auto o:openlist)
    {
        delete o;
        count2++;
    };
    for(auto c:closelist)
    {
        delete c;
        count2++;
    };  
    cout<<"new的次数:"<<count1<<endl;
    cout<<"delete的次数:"<<count2<<endl;    
    return path;
    
};

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"exam");
    //定义地图
    vector<vector<int>>gridmap={
        {0,0,1,0,0},
        {0,1,1,1,0},
        {0,0,0,0,0},
        {0,1,1,1,1},
        {0,0,0,0,0}
    };
    //定义起点和终点
    Point start{0,0};
    Point goal{4,4};
    vector<Point> path=AstarPathPlanning(gridmap,start,goal);
    cout<<"最终的路径点数:"<<path.size()<<endl;
    for(auto p:path)
    {
        if (p.x == goal.x && p.x == goal.y)
        {
            cout<<"("<<p.x<<","<<p.y<<")"<<" ";
        }
        else
        {
            cout<<"("<<p.x<<","<<p.y<<")"<<" ";
        };
    };
    cout<<endl;
    return 0;
}

本文章所写的代码是学习【C++】2-手撕A*路径规划算法_哔哩哔哩_bilibili  后手敲下来。

如有侵权，请联系删除！