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

月夕花晨TS

已于 2022-05-02 10:52:40 修改

阅读量5.8k

点赞数 14

分类专栏：路径规划人工智能文章标签：人工智能

于 2022-04-30 20:55:06 首次发布

本文链接：https://blog.csdn.net/qq_45369294/article/details/124518384

版权

路径规划同时被 2 个专栏收录

10 篇文章 33 订阅

订阅专栏

人工智能

10 篇文章 14 订阅

订阅专栏

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)$ 为：
$\left\{ \begin{array}{l} {M_x} = \frac{{{W_x} - origin\_x}}{{resolution}}\\ {M_y} = \frac{{{W_y} - origin\_y}}{{resolution}} \end{array} \right.$

2.5二维数据存储

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

3.代码实践

3.1 ros节点函数

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

首先通过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]);
    }
}

通过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;
    }
}

调用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();
    } 
}

通过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函数与上文的博客差别不大，主要改动以下两个方面

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

/***
*@函数功能   初始化一维地图数据
------------------------------------------------
*@参数       _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*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

月夕花晨TS

关注

14
点赞
踩
141

收藏

觉得还不错? 一键收藏
13
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫

专栏目录