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);
}这个代码后边会再给大家讲，总之，申明了一个接口，一定要实现就可以了。
按照上述方案，把多个接口都完成，就可以参考我的另一篇文章点击打开链接对程序进行编译。
我完成的代码可以从下面这个网址下载：
把代码复制到工作空间内可以直接编译，如果编译过程中有问题,可以在评论区留言。
关于代码的使用大家可以自己根据代码思考，后续我会再发布相关技术博客。
