A*算法是一种Greedy算法,ROS中的navigation导航包中的global_planner中就能找到。
今天,我们从头入手,自己移植并完成关于A*算法在ROS上的移植和实现。
首先来说一下我们最后想实现的目的:
我们首先画一个200*200像素的“交通路段”
在开始我们的算法之前,首先感谢网友[一路向北]的开源A*算法,他MFC界面的源代码网址如下:
我移植好的代码地址如下:
A*算法的具体实现我们并不关心,我们的目的提取它的核心算法,然后加入ROS相关的接口,并进行封装。
首先,算法移植前,我们一定要关注A*的核心类:
先看“一路向北”的原始代码,在这里由于源代码太长,我只列出封装的类:
class AStartFindPath
{
public:
AStartFindPath();
virtual ~AStartFindPath(){};
int GetPos(int &x,int &y);
void FindDestinnation(OpenList* open,CloseList* close);
OpenList* FindMinInOpen(OpenList* open);
bool Insert2OpenList(OpenList* , int x, int y);
bool IsInOpenList(OpenList*, int x, int y);
bool IsInCloseList(OpenList*, int x, int y);
void IsChangeParent(OpenList*, int x, int y);
bool IsAviable(OpenList* , int x, int y);
unsigned int DistanceManhattan(int d_x, int d_y, int x, int y);
unsigned int steps;
int startpoint_x;
int startpoint_y;
int endpoint_x;
int endpoint_y;
int m_height,m_width;
double m_resolution;
//Lists
OpenList* openlist;
CloseList* closelist ;
int x,y,des_x,des_y;
char Thrs;
};
可以看到,这个AStarFindPath类有很多函数,但最主要的还是这个:
void FindDestinnation(OpenList* open,CloseList* close);
我们针对这个类做移植。
首先,我们可以新建一个接口类,比如AStarInterface,一端调用AStarFindPath的函数,另一端连接ROS的标准协议。
我们也可以采用更为简单的方法,就是直接在AStarFindPath中添加ROS相关代码:
下面的代码为我修改后的代码:大家对照之前的代码,看加了一些什么内容。
class AStartFindPath
{
public:
ros::NodeHandle n;
Node **m_node;
AStartFindPath();
virtual ~AStartFindPath(){};
int GetPos(int &x,int &y);
void FindDestinnation(OpenList* open,CloseList* close);
OpenList* FindMinInOpen(OpenList* open);
bool Insert2OpenList(OpenList* , int x, int y);
bool IsInOpenList(OpenList*, int x, int y);
bool IsInCloseList(OpenList*, int x, int y);
void IsChangeParent(OpenList*, int x, int y);
bool IsAviable(OpenList* , int x, int y);
unsigned int DistanceManhattan(int d_x, int d_y, int x, int y);
/*以下是加入的ROS接口代码*/
void map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
void start_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
void end_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
ros::Subscriber map_sub;
ros::Subscriber start_sub;
ros::Subscriber end_sub;
ros::Publisher nav_plan;
//TF Scalar Listener
tf::TransformListener AGV_transform_listener;
tf::StampedTransform AGV_transform;
private:
unsigned int steps;
int startpoint_x;
int startpoint_y;
int endpoint_x;
int endpoint_y;
int m_height,m_width;
double m_resolution;
//Lists
OpenList* openlist;
CloseList* closelist ;
int x,y,des_x,des_y;
char Thrs;
ros::Publisher map_pub;
};
我在一个部分前加了注释,表明那个类是我移植后加入的ROS相关,其中添加了3个函数:
void map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
void start_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
void end_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
这种类型的函数通常被成为回调函数,回调函数是ROS收信息的接口;比如map_Callback就是收地图信息的函数,start_Callback是收到用户目标点激发的函数等等。
另外,在类的末尾,我加入了一个这个东西:ros::Publisher map_pub;这个Publisher叫消息发送器,消息发送器是ROS发信息的接口。现在我们在原来的类中加入了ROS的消息收发接口。
既然我们定义好了接口,那就要对其实现。首先是地图的收取接口
void AStartFindPath::map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
nav_msgs::OccupancyGrid m_map;
m_height=msg->info.height;
m_width=msg->info.width;
m_resolution=msg->info.resolution;
m_map.info.height=msg->info.height;
m_map.info.width=msg->info.width;
m_map.info.resolution=msg->info.resolution;
if(m_node!=NULL)
{
for(int i=0;i<m_height;i++)delete [] m_node[i];
delete [] m_node;
m_node=NULL;
}
m_node=new Node*[m_height];
for(int i=0;i {
m_node[i]=new Node[m_width];
for(int j=0;j<m_width;j++)
{
m_node[i][j].location_x = j;
m_node[i][j].location_y =i;
m_node[i][j].parent = NULL;
m_node[i][j].gray_val=msg->data[(i)*m_width+j];
if(msg->data[(i)*m_width+j]!=0)m_node[i][j].flag = WALL;
else m_node[i][j].flag = VIABLE;
}
}
m_map.data.resize(m_height*m_width);
for(int i=0;i<m_height;i++)
for(int j=0;j<m_width;j++)
m_map.data[i*m_width +j]=m_node[i][j].gray_val;
map_pub.publish(m_map);
printf("收到地图:长:%d Pixels----宽 %d Pixels\n",m_height,m_width);
}
这个代码后边会再给大家讲,总之,申明了一个接口,一定要实现就可以了。
按照上述方案,把多个接口都完成,就可以参考我的另一篇文章点击打开链接对程序进行编译。
我完成的代码可以从下面这个网址下载:
把代码复制到工作空间内可以直接编译,如果编译过程中有问题,可以在评论区留言。
关于代码的使用大家可以自己根据代码思考,后续我会再发布相关技术博客。