室外低速自动导航车的设计(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

 

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

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

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

点击打开链接

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

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

你可能感兴趣的:(ROS,室外自动导航车)