Astar与C++可视化在RVIZ的三维点云地图
文章目录
源代码下载链接:
https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots
本文代码下载链接:
https://gitee.com/tanggujie/astar
1.功能包介绍
本文主要对上述工作空间中的 grid_path_search、rviz_plugins、waypoint_generator进行介绍，其中 rviz_plugins和 waypoint_generator为RVIZ插件，为了更加方便的在rviz中提供指定的 三维坐标点，并不是本文的研究重点。下面对三维点云地图中Astar算法的实现的功能包 grid_path_search进行介绍。
; 2.算法功能包的组成与介绍
2.1文件系统组成
其中文件说明如下
include：存放Astar算法的头文件
launch：Astar算法的启动文件
pcd：存放点云地图的文件夹
rviz：RVIZ中的配置文件
src：Astar算法的头文件以及Astar对外的ros接口文件
; 2.2 头文件说明
其中红色框为本文基于原作者自己修改的头文件代码，包含自己所理解的中文注释。绿色框为源代码所拥有的头文件(源代码还提供了JPS算法，大家有兴趣可以将它和Astar进行比较。)
2.3 源文件说明
其中红色框为本文基于原作者自己修改的源文件代码，包含中文注释。绿色框为源代码所拥有的源文件
Astar.cpp:Astar算法的本体函数
astar_demo.cppAstar算法对外的ros接口函数
complex_map.cpp：提供的固定点云地图文件，点云地图以pcd的方式进行存储。其中pcd文件是通过源代码中的 random_complex_generator.cpp进行产生(注意：该random_complex_generator.cpp并不会自己产生点云文件，需要自己添加保存点云的代码，并且random_complex_generator.cpp每次会随机产生点云地图,调试代码并不方便)
查看保存好的点云，指令如下,其中 fc为背景颜色，ax为坐标轴的大小
pcl_viewer map.pcd -fc 255,255,255 -ax 1
3.相关坐标系说明
3.1 坐标系
世界坐标系：明确一点世界坐标系的原点就是点云地图的原点，且点云地图是反应真实世界的，默认单位为m。
栅格坐标系：名称自定义的，嘻嘻。目的是为了方便Astar算法搜索周围邻居，而在三维点云中，各个点之间并不会像图片那样可以方便的查找自己周围的邻居，它们是无序排列的。因此采用将点云空间进行栅格化，如果其中一个栅格有三维点，则在该栅格中置1，否则置0。
两大坐标系如下图所示，其中O 1 X 1 Y 1 O_1X_1Y_1 O 1 X 1 Y 1 为世界坐标系，O 2 X 2 Y 2 O_2X_2Y_2 O 2 X 2 Y 2 为栅格坐标系，其中在程序代码中，O 1 O_1 O 1 处在整个栅格的中心位置，O 2 O_2 O 2 处在O 1 O_1 O 1 的左上角位置。O 2 A O_2A O 2 A为整个点云栅格的宽，O 2 B O_2B O 2 B为整个点云栅格的长，O 2 C O_2C O 2 C为整个点云栅格的高。
; 3.2 点云数据存储
该程序代码中，点云栅格地图数据的存储也是一维数据，按照先Z后Y再X的方向进行存储。例如栅格地图中某点坐标为( x , y , z ) (x,y,z)(x ,y ,z )（其中x,y,z均为整数），对应于一维点云数据为d a t a [ x × L e n g t h × H i g h t + y × H i g h t + x ] data[x\times Length\times Hight+y\times Hight+x]d a t a [x ×L e n g t h ×H i g h t +y ×H i g h t +x ]。
4.重要代码说明
1.程序通过先读取点云地图进行Astar地图初始化，在程序设计中将该点的世界坐标系与栅格坐标系绑定起来，方便后续路径查找。
for (int i=0;i<GLX_SIZE;i++)
{
GridNodeMap[i].resize(GLY_SIZE);
for (int j=0;j<GLY_SIZE;j++)
{
GridNodeMap[i][j].resize(GLZ_SIZE);
for (int k=0;k<GLZ_SIZE;k++)
{
Vector3i tmpIdx(i,j,k);
Vector3d pos = gridIndex2coord(tmpIdx);
GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos);
}
}
}
栅格转世界坐标系函数gridIndex2coord()
W x = M x × r e s o l u t i o n + o r i g i n _ x W y = M y × r e s o l u t i o n + o r i g i n _ y W z = M z × r e s o l u t i o n + o r i g i n _ z W_x=M_x\times resolution+origin_x \ W_y=M_y\times resolution+origin_y \ W_z=M_z\times resolution+origin_z W x =M x ×r e s o l u t i o n +o r i g i n _x W y =M y ×r e s o l u t i o n +o r i g i n _y W z =M z ×r e s o l u t i o n +o r i g i n _z
Vector3d AstarPathFinder::gridIndex2coord(const Vector3i & index)
{
Vector3d pt;
pt(0) = ((double)index(0) + 0.5) * resolution + gl_xl;
pt(1) = ((double)index(1) + 0.5) * resolution + gl_yl;
pt(2) = ((double)index(2) + 0.5) * resolution + gl_zl;
return pt;
}
世界转栅格坐标系函数coord2gridIndex()
其中m i n ( m a x ( X , a ) , b ) min(max(X,a),b)m i n (m a x (X ,a ),b )表示
f ( X ) = { X X ∈ [ a , b ] a X < a b X > b f(X) = \left{ \begin{array}{l} X &\text X \in [a,b]\ a &\text X < a\ b &\text X > b \end{array} \right.f (X )=⎩⎨⎧X a b X ∈[a ,b ]X b
Vector3i AstarPathFinder::coord2gridIndex(const Vector3d & pt)
{
Vector3i idx;
idx << min( max( int( (pt(0) - gl_xl) * inv_resolution), 0), GLX_SIZE - 1),
min( max( int( (pt(1) - gl_yl) * inv_resolution), 0), GLY_SIZE - 1),
min( max( int( (pt(2) - gl_zl) * inv_resolution), 0), GLZ_SIZE - 1);
return idx;
}
2.Astar算法的障碍物判断为isOccupied()函数，三维栅格点云数据以一维数组 data进行存储，数组大小为W i d t h × L e n g t h × H i g h t Width \times Length \times Hight W i d t h ×L e n g t h ×H i g h t。 具体栅格坐标系下某点坐标转化到一维数组中的位置，见3.2节点云数据的存储。
inline bool AstarPathFinder::isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const
{
return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE &&
(data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] == 1));
}
3.其他均是一些Astar算法的本体函数，与一些接口函数的调用，本文就不赘述了。具体可参考代码中的注释。
3.1 Astar算法中的启发式函数h h h 的计算是对真实距离的估计，因此h h h 的计算基于世界坐标系。
3.2 Astar邻居的索引是在栅格坐标系中的，因为三维点云数据无序，需要进行栅格化。
3.3 Astar路径的生成是通过rviz中的Marker功能实现的，而Marker所需的坐标点是基于世界坐标系的，因此，Astar路径的生成也是基于世界坐标系的。
5.代码运行情况
启动
roslaunch grid_path_search my_demo.launch
稍等片刻，就会显示rviz信息，如下图所示。可以改变AllMap下的Spheres进行点云数据不同风格的显示。 点击3D Nav Goal(或者点击键盘中的”g”键) 进行目标点的设置，首先通过鼠标左键进行XY平面位置的确定，此时按住鼠标左键不要松，在点击鼠标右键进行上下拖动，进行Z位置的确定，同时松开鼠标左键和鼠标右键，即可显示Astar的三维路径。
注意：
- 在该算法演示中，并没有设置Astar的起点，若想改变Astar的起点位置，需要在my_demo.launch中
<arg name="start_x" default=" 0.0"/>
<arg name="start_y" default=" 0.0"/>
<arg name="start_z" default=" 1.0"/>
进行起始点X 、 Y 、 Z X、Y、Z X 、Y 、Z的设置。其实也可以利用3D Nav Goal进行起始点的设置，不过为了偷懒，并没有写，感兴趣的可以尝试一下。
- 该显示函数提供了更新功能，即不需要关闭程序，在此指定一个新的坐标点，算法会重新生成一条新的Astar路径点。
此外，非常感谢源代码对我的帮助，从中学到了很多有用的知识。
Original: https://blog.csdn.net/qq_45369294/article/details/124537628
Author: 月夕花晨TS
Title: (一) 路径规划算法—Astar与C++可视化在RVIZ的三维点云地图
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/598041/
转载文章受原作者版权保护。转载请注明原作者出处！