#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 后手敲下来。
如有侵权，请联系删除！