基本信息

---

下载

   友情提示：（无需注册或充值，赞助后即可获取资源下载链接）

---

     嘿，亲！知识可是无价之宝呢，但咱这精心整理的资料也耗费了不少心血呀。小小地破费一下，绝对物超所值哦！如有下载和支付问题，请联系我们QQ(微信同号)：813200300

本次赞助数额为： 2 元　

支付宝支付

微信支付

   源码介绍

---

///////////////////////////////////////////////////////////
//			ROS用A*算法  最优寻路算法
//			算法是一种静态路网中求解最短路最有效的算法
//				1）公式表示为： f(n)=g(n) h(n),
//				2） 加入最优路径修正
//						如果某个相邻的方格已经在 open list 中，则检查这条路径是否更优，
//						也就是说经由当前方格 ( 我们选中的方格 ) 到达那个方格是否具有更小的 G 值。
//						如果没有，不做任何操作。
//												感谢原作者一路向南           重写：张德雨伞
//															2016,12,23
/////////////////////////////////////////////////////////

#include "astar_global_planner.h"

Node::Node()
{
	flag=0;
	value_h= 0;
	value_g= 0;
	value_f =  0;
	parent= NULL;
}

void AddNode2Open(OpenList* openlist, Node* node)
{
	if(openlist ==NULL)
	{
		cout<<"no data in openlist!"<<endl;
		return;
	}
	if(node->flag!=STARTPOINT)
	{
		node->flag= INOPEN;
	}
	OpenList* temp =  new OpenList;
	temp->next=NULL;
	temp->opennode = node;
	while(openlist->next != NULL)
	{
		if(node->value_f < openlist->next->opennode->value_f)
		{
			OpenList* tempadd= openlist->next;
			temp->next= tempadd;
			openlist->next = temp;
			break;
		}
		else
			openlist= openlist->next;
	}
	openlist->next = temp;
}
// openlist 此处必须为指针的引用
void AddNode2Close(CloseList* close, OpenList* &open)
{
	if(open==NULL)
	{
		cout<<"no data in openlist!"<<endl;
		return;
	}
	if(open->opennode->flag != STARTPOINT)
		open->opennode->flag =INCLOSE;

	if(close->closenode == NULL)
	{	
		close->closenode = open->opennode;
		OpenList* tempopen=open;
		open=open->next;
		//open->opennode=NULL;
	//	open->next=NULL;
		delete tempopen;
		return;
	}
	while(close->next!= NULL)
		close= close->next;

	CloseList* temp= new CloseList;
	temp->closenode = open->opennode;
	temp->next=NULL;
	close->next= temp;

	OpenList* tempopen=open;
	open=open->next;
	delete tempopen;
}
AStartFindPath::AStartFindPath()
{
	steps=0;
	startpoint_x = -1;startpoint_y = -1;endpoint_y = -1; endpoint_x = -1;x=0;y=0;
	m_resolution=1;
	Thrs=65;
	m_node=NULL;
	m_width=0;m_height=0;
	openlist = NULL;
	closelist= NULL;
	//const OpenList* HEADOPEN= openlist;
	//const CloseList* HEADCLOSE= closelist;
	map_pub = n.advertise<nav_msgs::OccupancyGrid>("astar_map", 1,true);
	map_sub = n.subscribe<nav_msgs::OccupancyGrid>("env_map",1,&AStartFindPath::map_Callback,this);
	start_sub = n.subscribe<geometry_msgs::PoseStamped>("my_point",1,&AStartFindPath::start_Callback,this);
	end_sub = n.subscribe<geometry_msgs::PoseStamped>("move_base_simple/goal",1,&AStartFindPath::end_Callback,this);
	nav_plan = n.advertise<nav_msgs::Path>("astar_path", 1);
}
void AStartFindPath::FindDestinnation(OpenList* open,CloseList* close)
{

     int i=0;
	//printf("%d %d\n",open->opennode->location_x,open->opennode->location_y);
	printf("开始计算路径！\n");
	Insert2OpenList(open,startpoint_y,startpoint_x);
	AddNode2Close(close,open);
	while(!Insert2OpenList(open, open->opennode->location_y, open->opennode->location_x))
	{
		//printf("%d %d\n",open->opennode->location_x,open->opennode->location_y);
		i  ;
		AddNode2Close(close,open);
		if(open==NULL||i>30000)
		{
			printf("找不到出口！\n");
			return;
		}
	}
	printf("计算路径成功！！\n");
 Node *tempnode= &m_node[endpoint_y][endpoint_x];

  nav_msgs::Path plan;
   i=0;


  while(tempnode->parent->flag!=STARTPOINT)
	{
		tempnode=tempnode->parent;
		i  ;
	}

  plan.poses.resize(i);
  plan.header.frame_id="map";
  Node *tempnode2= &m_node[endpoint_y][endpoint_x];

  while(tempnode2->parent->flag!=STARTPOINT)
  	{
  		tempnode2=tempnode2->parent;
  		plan.poses[--i].pose.position.x=tempnode2->location_x*m_resolution;
  		plan.poses[i].pose.position.y=tempnode2->location_y*m_resolution;
  	}
  nav_plan.publish(plan);
}
// 在openlist中找到最小的 f值  节点
OpenList* AStartFindPath:: FindMinInOpen(OpenList* open)
{
	return open;
}
bool AStartFindPath::Insert2OpenList(OpenList* open,int center_x, int center_y)
{
	int i=0;
	//while()
	//int counts
	//static int counts=0;
	//counts  ;
	for(; i<8 ; i  )
	{
		int new_x=center_x   direction[i][0];
		int new_y=center_y  direction[i][1];

		if(new_x>=0 && new_y>=0 && new_x<m_height &&
			new_y<m_width &&
			IsAviable(open, new_x, new_y))// 0
		{
			
			if(	m_node[new_x][new_y].flag==DESTINATION)
			{
				m_node[new_x][new_y].parent = &m_node[center_x][center_y];
				return true;
			}
			m_node[new_x][new_y].flag =INOPEN;
			m_node[new_x][new_y].parent = &m_node[center_x][center_y];
			m_node[new_x][new_y].value_h = 
				DistanceManhattan(endpoint_x, endpoint_y, new_x,new_y);//曼哈顿距离

			if(0==i || 2==i||5==i||7==i)
				m_node[new_x][new_y].value_g = m_node[center_x][center_y].value_g 14;
			else
				m_node[new_x][new_y].value_g = m_node[center_x][center_y].value_g 10;

			m_node[new_x][new_y].value_f = m_node[new_x][new_y].value_g m_node[new_x][new_y].value_h;

			AddNode2Open(open, &m_node[new_x][new_y]);// 加入到 openlist中
		}
	}
	IsChangeParent(open, center_x,  center_y);
	//if(counts>1000)
	//	return true;
	//else
	return false;
}
// 是否有更好的路径
void AStartFindPath::IsChangeParent(OpenList* open,int center_x, int center_y)
{
	int i=0;
	for(; i<8 ; i  )
	{
		int new_x=center_x   direction[i][0];
		int new_y=center_y  direction[i][1];
		if(new_x>=0 && new_y>=0 && new_x<m_height &&
			new_y<m_width &&
			IsInOpenList(open, new_x, new_y))// 0
		{

			if(0==i|| 2==i|| 5==i|| 7==i)
			{
				if(m_node[new_x][new_y].value_g >  m_node[center_x][center_y].value_g 14)
				{
					m_node[new_x][new_y].parent = &m_node[center_x][center_y];
					m_node[new_x][new_y].value_g =   m_node[center_x][center_y].value_g 14;
				}
			}
			else
			{
				if(m_node[new_x][new_y].value_g >   m_node[center_x][center_y].value_g 10)
				{
					m_node[new_x][new_y].parent = &m_node[center_x][center_y];
					m_node[new_x][new_y].value_g =   m_node[center_x][center_y].value_g 10;
				}
			}
		}
	}
}

bool AStartFindPath::IsAviable(OpenList* open, int x, int y)
{
	if(IsInOpenList( open, x, y))
		return false;
	if(IsInCloseList( open, x, y))
		return false;
	if(m_node[x][y].flag == WALL )
		return false;
	else 
		return true;
}
bool AStartFindPath::IsInOpenList(OpenList* openlist, int x,int y)
{
	if(m_node[x][y].flag == INOPEN)
		return true;
	else 
		return false;
}

bool AStartFindPath::IsInCloseList(OpenList* openlist, int x,int y)
{
	if(m_node[x][y].flag == INCLOSE|| m_node[x][y].flag==STARTPOINT)
		return true;
	else 
		return false;
}
unsigned int AStartFindPath::DistanceManhattan(int d_x, int d_y, int x, int y)
{
	unsigned int temp=((d_x - x)>0?(d_x - x):-(d_x - x)   (d_y-y)>0?(d_y-y):-(d_y-y))*DISTANCE;
	return temp;
}
//初始化 node
int AStartFindPath::GetPos(int &x,int &y)
{
	//获取当前位置

		try
			{
				AGV_transform_listener.lookupTransform("/map", "/base_link",ros::Time(0), AGV_transform);
			}
			catch (tf::TransformException ex)
			{
				printf("ERROR: %s",ex.what());
					ros::Duration(1.0).sleep();
		      		return 1;
		    	}
			x=AGV_transform.getOrigin().x()/m_resolution;
			y=AGV_transform.getOrigin().y()/m_resolution;
			return 0;
}
void AStartFindPath::end_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{

	des_x=msg->pose.position.x/m_resolution;
	des_y=msg->pose.position.y/m_resolution;
	printf("目标像素位置：%d---- %d灰度 %d\n",m_node[des_y][des_x].location_x,m_node[des_y][des_x].location_y,m_node[des_y][des_x].gray_val);
	GetPos(x,y);
	printf("当前像素位置：%d---- %d,灰度 %d\n",m_node[y][x].location_x,m_node[y][x].location_y,m_node[y][x].gray_val);

	if(m_node[y][x].flag==WALL||m_node[des_y][des_x].flag==WALL)
	{
		printf("我擦你丫往墙上开毛啊！！！\n");
    		return;
	}

	printf("重载节点！！\n");
	if(openlist!=NULL){delete  openlist; openlist=NULL;}
	if(closelist!=NULL){delete closelist; closelist=NULL;}
	openlist = new OpenList;
	closelist= new CloseList;
	closelist->closenode=NULL;
	for(int i=0;i<m_height ;i  )
			for(int j=0;j<m_width;j  )
				if(m_node[i][j].flag!=WALL) m_node[i][j].flag=VIABLE;
	printf("重载节点完成！！\n");

	m_node[y][x].flag = STARTPOINT;
	openlist->next=NULL;
	openlist->opennode= &m_node[y][x];
	startpoint_x= x;
	startpoint_y=y;
	m_node[des_y][des_x].flag = DESTINATION;
	endpoint_x= des_x;
	endpoint_y=des_y;

	FindDestinnation(openlist,closelist);
}
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_height ;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);
}
void  AStartFindPath::start_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{

}
int main(int argc, char** argv)
{
	ros::init(argc, argv, "astar_planner");
	AStartFindPath planner;
	const OpenList* HEADOPEN= planner.openlist;
	const CloseList* HEADCLOSE= planner.closelist;
	ros::AsyncSpinner spinner(2);
	spinner.start();
	ros::waitForShutdown();
	return 0;
	//findpath.InitNodeMap( planner.map_occupy,openlist);
	//findpath.FindDestinnation(openlist,closelist, planner.map_occupy);

}