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

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

代码下载链接: 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$轴，绿色轴为世界坐标系的$Y$轴，蓝色轴为世界坐标系的$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)$，那在栅格坐标系下的坐标$(M_x,M_y)$为：
$$\{\begin{array}{l}{M}_x=\frac{W_x-origin\_x}{resolution}\\ M_y=\frac{W_y-origin\_y}{resolution}\end{array}$$

2.5二维数据存储

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

3.代码实践

3.1 ros节点函数

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

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

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

2. 通过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<<"\033[0;31m[E] : Please set the valid goal point\033[0m"<<std::endl;
   }
   void GoalPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
      {
          goalMapPoint.push_back(WorldToMap(msg->pose.position.x,msg->pose.position.y));
          if (goalMapPoint[0][0]==-1&&goalMapPoint[0][1]==-1)
              std::cout<<"\033[0;30m[Kamerider E] : Please set the valid goal point\033[0m"<<std::endl;
          else
              StartFindPath();
      }
   

   std::vector<int> WorldToMap(double wx,double wy)
   {
       std::vector<int> 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<width && my<height)
       {
           v.push_back(my);
           v.push_back(mx);
           return v;
       }
   }
   

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

   void StartFindPath()
   { 
       //获得栅格坐标系下的起点 
       int xStart=startMapPoint[0][0];
       int yStart=startMapPoint[0][1];
       std::cout<<"start:"<<xStart<<","<<yStart<<std::endl;
       
       //获得栅格坐标系下的终点
       int xStop=goalMapPoint[0][0];
       int yStop=goalMapPoint[0][1];
       std::cout<<"goal:"<<xStop<<","<<yStop<<std::endl;
       
       //构建Astar的对象
       ASTAR::CAstar astar(xStart, yStart, xStop, yStop, weight_a, weight_b,ASTAR::CAstar::PathType::NOFINDPATHPOINT,distance);
       astar.InitMap(mapData,width,height);
       astarPath = astar.PathPoint(); //Astar算法的核心函数
       if (astar.m_noPathFlag == ASTAR::CAstar::PathType::NOFINDPATHPOINT)
       {
           std::cout << "A星算法没能找到路径!!!" << std::endl;
       }
       else
       {
           PublishPath();
       } 
   }
   
   

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

      //发布Astar路径轨迹
      void PublishPath()
      {
          nav_msgs::Path astarPathTopic;    //Astar路径的话题名
          for(int i=0;i<astarPath.size();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::vector<double>MapToWorld(double my,double mx)
      {
         std::vector<double> 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函数与上文的博客差别不大，主要改动以下两个方面

1. 添加了初始化地图的函数，目的是将一维地图数据、地图的宽、地图的高作为参数的形式传入，当然在初始化函数中移除地图数据。

   /***
   *@函数功能   初始化一维地图数据
   ------------------------------------------------
   *@参数       _mapData  地图数据
   *@参数       _width    地图的宽
   *@参数       _height   地图的高
   ------------------------------------------------
   */
   void ASTAR::CAstar::InitMap(std::vector<int> _mapData,int _width,int _height)
   {
   	m_mapData.swap(_mapData);
   	m_width=_width;
   	m_height=_height;
   }
   

2.在Astar扩展节点的ExpandArray函数中，由于之前二维地图为$\_mapData[mx][my]!=1$,现在对应于一维地图数据改为$\_mapData[sx\ast mapWidth+sy]!=100$,注意这里是$x\times Width+y$,是由于该算法工作在图片坐标系中，而栅格坐标系与图片坐标系x与y相反。

3.3 launch部分参数

<launch>
  <arg name="distance" default="euclidean"/> <!--　euclidean  manhattan-->
  <arg name="weight_a" default="1.0"/>      
  <arg name="weight_b" default="1.0"/>
  
  <arg name="map_file" default="$(find grid_map_search)/maps/map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)">
    <param name="frame_id" value="/map"/>
  </node>
 
  <node name="astar_node" pkg="grid_map_search" type="astar_node" output="screen">
    <param name="heuristic/distance" value="$(arg distance)"/>
    <param name="weight/a" value="$(arg weight_a)"/>
    <param name="weight/b" value="$(arg weight_b)"/>
  </node>
  <node pkg="tf" type="static_transform_publisher" name="odom_combined_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
  
 
 <node name="rviz" pkg="rviz" type="rviz" args="-d $(find grid_map_search)/rviz/astar.rviz"/>
  
</launch>

其中参数

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

weight_a：表示Astar的权重a值

weight_b：表示Astar的权重b值

即$f=a\times g+b\times h$,distance主要对启发式函数$h$起作用

4.代码运行

启动代码程序

roslaunch grid_map_search astar_demo.launch

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

map.pgm

maze.png