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为例说明世界坐标系。其中红色轴为世界坐标系的XXX轴，绿色轴为世界坐标系的YYY轴，蓝色轴为世界坐标系的ZZZ轴。快速记忆法：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

那么已知世界坐标系的一点(Wx,Wy)(W_x,W_y)(Wx,Wy)，那在栅格坐标系下的坐标(Mx,My)(M_x,M_y)(Mx,My)为：
{Mx=Wx−origin_xresolutionMy=Wy−origin_yresolution\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的话题中以一维数组进行维护更新，数组的大小为Width×HeightWidth \times HeightWidth×Height。如下图所示,地图数据存储是按照栅格坐标系的x轴进行存储，例如栅格坐标系的某点坐标(x,y)(x,y)(x,y),对应于地图数据为mapData[y×Width+x]mapData[y\times Width+x]mapData[y×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;elseStartFindPath();}

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[mx][my]!=1_mapData[mx][my]!=1,现在对应于一维地图数据改为_mapData[sx∗mapWidth+sy]!=100\_mapData[sx*mapWidth+sy]!=100_mapData[sx∗mapWidth+sy]!=100,注意这里是x×Width+yx\times Width+yx×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×g+b×hf=a\times g+b\times hf=a×g+b×h,distance主要对启发式函数hhh起作用

4.代码运行

启动代码程序

roslaunch grid_map_search astar_demo.launch

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

map.pgm

maze.png

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

(一)路径规划算法---Astar实现自定义的全局路径规划插件
Astar实现自定义的全局路径规划插件文章目录 Astar实现自定义的全局路径规划插件 1.插件功能包的建立 2. 相关步骤 2.1 建立工作空间和环境变量的配置 2.2 建立功能包 2.3 添加源 ...
基于Astar算法的二维栅格地图路径规划和避障MATLAB仿真
目录 1.算法仿真效果 2.MATLAB源码 3.算法概述 4.部分参考文献 1.算法仿真效果 matlab2022a仿真结果如下: 2.MATLAB源码 %********************
路径规划算法学习Day2
路径规划算法学习Day2-栅格法创建环境地图前言一.栅格法 1.原理二.栅格法matlab实现 1.创建地图 2.完整代码 3.所生成地图三.总结前言静态环境中机器人全局路径规划一直是路径 ...
路径规划算法学习Day3
路径规划算法学习Day3-Dijkstra算法实现前言 1.Dijkstra算法 1.1.地图创建 1.2.matlab实现 1.3.20*20地图 1.4.50*50地图前言算法原理:参考路径 ...
AStar路径规划算法
概要 AStar是一种路径规划算法,其思想包含Dijkstra算法和启发式算法.该算法也是通过起点开始,逐步记录能够搜索到的节点,并记录它们离起点的最短距离:同时还会估算搜索到的点离终点的距离. 公式 ...
【ROS-Navigation】—— Astar路径规划算法解析
文章目录前言 1. 导航的相关启动和配置文件 1.1 demo01_gazebo.launch 1.2 nav06_path.launch 1.3 nav04_amcl.launch 1.4 nav ...
python路径规划算法可视化_[大创]一步一步写路径规划并绘图可视化 I new
重新写了之前的I,这是中期答辩我写的桥段,没用上就放到博客里来了.有意向了解的朋友们可以去看看另外几篇详述的.密码是123456 一个简易的室内路径规划基础模型 1.1 选择编程工具编程语言选择Py ...
carsim+simulink联合仿真实现变道包含路径规划算法+mpc轨迹跟踪算法带规划轨迹可视化
carsim+simulink联合仿真实现变道包含路径规划算法+mpc轨迹跟踪算法带规划轨迹可视化可以适用于弯道道路,弯道车道保持,弯道变道 Carsim2020.0 ID:5199664465 ...
基于SSD的自动路径规划算法
目录 1.场景需求 2.路径规划算法简介 2.1 .PRM算法简介 2.2.RRT算法简介 3.基于SSD的自动路径规划算法简介 4.基于SSD的自动路径规划算法详解 4.1.利用外置摄像头获取图像或 ...

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