室外低速自动导航车的设计（8）——A*算法在ROS上的移植

A*算法是一种Greedy算法，ROS中的navigation导航包中的global_planner中就能找到。

今天，我们从头入手，自己移植并完成关于A*算法在ROS上的移植和实现。

首先来说一下我们最后想实现的目的：

我们首先画一个200*200像素的“交通路段”，如下图，将其保存为BMP图片文件（当然，大家可以自己画，白色为道路，黑色为障碍）。

 

现在，我们以图片的最中央为起点，实现点击任意一个白色位置，即道路上任意一点，让程序自动规划出路线，并显示出来（类似下图）。

 

 

在开始我们的算法之前，首先感谢网友[一路向北]的开源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;idata[(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

 

      这个代码后边会再给大家讲，总之，申明了一个接口，一定要实现就可以了。

按照上述方案，把多个接口都完成，就可以参考我的另一篇文章点击打开链接对程序进行编译。

我完成的代码可以从下面这个网址下载：

点击打开链接

把代码复制到工作空间内可以直接编译，如果编译过程中有问题,可以在评论区留言。

关于代码的使用大家可以自己根据代码思考，后续我会再发布相关技术博客。