实例介绍
【实例简介】
【实例截图】
【实例截图】
【核心代码】
///////////////////////////////////////////////////////////
// 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);
}
好例子网口号：伸出你的我的手 — 分享！
小贴士
感谢您为本站写下的评论，您的评论对其它用户来说具有重要的参考价值，所以请认真填写。
- 类似“顶”、“沙发”之类没有营养的文字，对勤劳贡献的楼主来说是令人沮丧的反馈信息。
- 相信您也不想看到一排文字/表情墙，所以请不要反馈意义不大的重复字符，也请尽量不要纯表情的回复。
- 提问之前请再仔细看一遍楼主的说明，或许是您遗漏了。
- 请勿到处挖坑绊人、招贴广告。既占空间让人厌烦，又没人会搭理，于人于己都无利。
关于好例子网
本站旨在为广大IT学习爱好者提供一个非营利性互相学习交流分享平台。本站所有资源都可以被免费获取学习研究。本站资源来自网友分享，对搜索内容的合法性不具有预见性、识别性、控制性，仅供学习研究，请务必在下载后24小时内给予删除，不得用于其他任何用途，否则后果自负。基于互联网的特殊性，平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查；无论平台是否已进行审查，用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场，基于网友分享，根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定，若资源存在侵权或相关问题请联系本站客服人员，点此联系我们。关于更多版权及免责申明参见 版权及免责申明
网友评论
我要评论