(一)路径规划算法---Astar与C++可视化在RVIZ的二维栅格地图

Astar与C++可视化在RVIZ的二维栅格地图中

文章目录

Astar与C++可视化在RVIZ的二维栅格地图中
- 1.功能包介绍
- 2.二维栅格地图以及相关坐标系说明
- - 2.1 世界坐标系
  - 2.2 栅格坐标系
  - 2.3 图像坐标系
  - 2.4 栅格坐标系与世界坐标系的转化
  - 2.5二维数据存储
- 3.代码实践
- - 3.1 ros节点函数
  - 3.2 Astar函数
  - 3.3 launch部分参数
- 4.代码运行

代码下载链接: https://gitee.com/tanggujie/astar

1.功能包介绍

本文主要对grid_map_search功能包进行介绍，该包主要在栅格地图中对Astar算法以C++的形式显示，并以RVIZ进行可视化显示。

其中文件夹主要说明如下

include：存放Astar算法的头文件

launch：Astar算法的启动文件

maps：存放栅格地图的文件夹，里面提供两组地图数据，供大家使用和调试

rviz：RVIZ中的配置文件

src：Astar算法的头文件以及Astar对外的ros接口文件

2.二维栅格地图以及相关坐标系说明 2.1 世界坐标系

首先世界坐标系起点是机器人建图的起点，单位为m，表示机器人处在现实中哪个位置，位置原点可以通过RVIZ中的Axes进行查看。这里以maps文件夹中maze.png为例说明世界坐标系。其中红色轴为世界坐标系的 X X X轴，绿色轴为世界坐标系的 Y Y Y轴，蓝色轴为世界坐标系的 Z Z Z轴。快速记忆法：RGB------>XYZ,即颜色的三要素对应于坐标系的三轴。

2.2 栅格坐标系

栅格坐标系是本人所取，实在不知道怎么定义这个坐标系的名字。有人取为map坐标系，但是与TF树中的map坐标系相混合，两者并不是一个东西。暂且表示为栅格坐标系，单位为像素。注意该坐标系并不能直接在rviz中进行显示。ROS中将该坐标系的原点定义为图片的左下角，x轴向右，y轴向上。这里以maps文件夹中maze.png为例进行说明。

2.3 图像坐标系

图像坐标系延续图像中默认的坐标系，即X轴向右，Y轴向下，单位为像素。该坐标系在代码的章节重点介绍。下图为三大坐标系的示意图，注意图像坐标系与栅格坐标系原点重合。

2.4 栅格坐标系与世界坐标系的转化

由maze.yaml文件可知

image: maze.png
resolution: 0.05
origin: [-1.0, -2.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

maze.png的图片相关参数为

resolution：分辨率为0.05，表示单位m与单位pt(像素)之间的转换关系。

origin[x,y, θ theta θ]：表示栅格坐标系与世界坐标系的x、y、角度偏差。其中x、y单位均为m

那么已知世界坐标系的一点 ( W x , W y ) (W_x,W_y) (Wx,Wy)，那在栅格坐标系下的坐标 ( M x , M y ) (M_x,M_y) (Mx,My)为：
{ M x = W x − o r i g i n _ x r e s o l u t i o n M y = W y − o r i g i n _ y r e s o l u t i o n left{ begin{array}{l} {M_x} = frac{{{W_x} - origin_x}}{{resolution}}\ {M_y} = frac{{{W_y} - origin_y}}{{resolution}} end{array} right. {Mx=resolutionWx−origin_xMy=resolutionWy−origin_y

2.5二维数据存储

首先明确一点的是，二维栅格地图数据在ROS的话题中以一维数组进行维护更新，数组的大小为 W i d t h × H e i g h t Width times Height Width×Height。如下图所示,地图数据存储是按照栅格坐标系的x轴进行存储，例如栅格坐标系的某点坐标 ( x , y ) (x,y) (x,y),对应于地图数据为 m a p D a t a [ y × W i d t h + x ] mapData[ytimes Width+x] mapData[y×Width+x]。其中存储0表示自由可通行区域、100表示障碍物

3.代码实践 3.1 ros节点函数

AstarNode.cpp为Astar算法实现的对外显示函数，具体变量与函数说明见代码所示。执行流程为

首先通过MapCallback()函数读取栅格地图的基本信息，如上述的分辨率、起始点等，同时存储一维地图数据

//获得地图数据 0自由通行区域，100障碍物
for (int i=0;i
    for(int j=0;j
        mapData[i*width+j]=int (msg->data[i*width+j]);
    }
}

通过rviz中的2D Pose Estimate按钮和2D Nav Goal设置世界坐标系的起始点和结束点，再通过世界—>栅格可得到栅格坐标系的起始点和结束点。由于栅格坐标系与图片坐标系X和Y相反，因此WorldToMap先存储my,再存储mx。

//初始位置的回调函数
void InitPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & msg)
{  
    startMapPoint.push_back(WorldToMap(msg->pose.pose.position.x,msg->pose.pose.position.y));
    if (startMapPoint[0][0]==-1&&startMapPoint[0][1]==-1)
        std::cout<<"33[0;31m[E] : Please set the valid goal point33[0m"<
       goalMapPoint.push_back(WorldToMap(msg->pose.position.x,msg->pose.position.y));
       if (goalMapPoint[0][0]==-1&&goalMapPoint[0][1]==-1)
           std::cout<<"33[0;30m[Kamerider E] : Please set the valid goal point33[0m"< std::vector WorldToMap(double wx,double wy)
{
    std::vector v;
    if (wx<(1.0*origin_x) || wy<(1.0*origin_y))
    {
        v.push_back(-1);
        v.push_back(-1);
        return v;
    }
    int mx=int((1.0*(wx-origin_x))/resolution);
    int my=int((1.0*(wy-origin_y))/resolution);
    if (mx
        v.push_back(my);
        v.push_back(mx);
        return v;
    }
}

调用StartFindPath()函数，将栅格起始点、结束点、地图数据、Astar相关参数传入，最后得出图像坐标系下Astar的路径坐标点。

void StartFindPath()
{ 
    //获得栅格坐标系下的起点 
    int xStart=startMapPoint[0][0];
    int yStart=startMapPoint[0][1];
    std::cout<<"start:"<
        std::cout << "A星算法没能找到路径!!!" << std::endl;
    }
    else
    {
        PublishPath();
    } 
}

通过PublishPath()函数发布路径点，注意该路径点在世界坐标系下，注意MapToWorld()函数形参值。

//发布Astar路径轨迹
void PublishPath()
{
    nav_msgs::Path astarPathTopic;    //Astar路径的话题名
    for(int i=0;i   
        geometry_msgs::PoseStamped pathPose;
        pathPose.pose.position.x=MapToWorld(astarPath[i].first,astarPath[i].second)[0];
        pathPose.pose.position.y=MapToWorld(astarPath[i].first,astarPath[i].second)[1];
        pathPose.pose.position.z=0;

        pathPose.pose.orientation.x = 0.0;
        pathPose.pose.orientation.y = 0.0;
        pathPose.pose.orientation.z = 0.0;
        pathPose.pose.orientation.w = 1.0;
        astarPathTopic.header.stamp=ros::Time::now();
        astarPathTopic.header.frame_id="odom";
        astarPathTopic.poses.push_back(pathPose); 
    }   
    astarPathPub.publish(astarPathTopic);
}

std::vectorMapToWorld(double my,double mx)
{
   std::vector v;
   if(mx>width || my>height)
   {
       v.push_back(-1);
       v.push_back(-1);
       return v;
   }
   double wx=(mx*resolution+origin_x);
   double wy=(my*resolution+origin_y);
   if (wx>origin_x&&wy>origin_y)
   {
       v.push_back(wx);
       v.push_back(wy);
       return v;
   }
}

3.2 Astar函数

Astar函数与上文的博客差别不大，主要改动以下两个方面

添加了初始化地图的函数，目的是将一维地图数据、地图的宽、地图的高作为参数的形式传入，当然在初始化函数中移除地图数据。
```
void ASTAR::CAstar::InitMap(std::vector _mapData,int _width,int _height)
{
	m_mapData.swap(_mapData);
	m_width=_width;
	m_height=_height;
}
```

2.在Astar扩展节点的ExpandArray函数中，由于之前二维地图为 _ m a p D a t a [ m x ] [ m y ] ! = 1 _mapData[mx][my]!=1 _mapData[mx][my]!=1,现在对应于一维地图数据改为 _ m a p D a t a [ s x ∗ m a p W i d t h + s y ] ! = 100 _mapData[sx*mapWidth+sy]!=100 _mapData[sx∗mapWidth+sy]!=100,注意这里是 x × W i d t h + y xtimes Width+y x×Width+y,是由于该算法工作在图片坐标系中，而栅格坐标系与图片坐标系x与y相反。

3.3 launch部分参数

其中参数

distance：表示启发函数选择，目前代码提供欧式距离和曼哈顿距离

weight_a：表示Astar的权重a值

weight_b：表示Astar的权重b值

即 f = a × g + b × h f=atimes g+btimes h f=a×g+b×h,distance主要对启发式函数 h h h起作用

4.代码运行

启动代码程序

roslaunch grid_map_search astar_demo.launch

点击rviz中2D Pose Estimate按钮和2D Nav Goal设置起始点和结束点，其中起始点设置为红色方块、结束点设置蓝色方块。稍等片刻，会在rviz中显示绿色的Astar路径。效果如下

map.pgm

maze.png

(一)路径规划算法---Astar与C++可视化在RVIZ的二维栅格地图

C/C++/C#相关栏目本月热门文章