本文主要对上述工作空间中的grid_path_search、rviz_plugins、waypoint_generator进行介绍，其中rviz_plugins和waypoint_generator为RVIZ插件，为了更加方便的在rviz中提供指定的三维坐标点，并不是本文的研究重点。下面对三维点云地图中Astar算法的实现的功能包grid_path_search进行介绍。
其中文件说明如下
include：存放Astar算法的头文件
launch：Astar算法的启动文件
pcd：存放点云地图的文件夹
rviz：RVIZ中的配置文件
src：Astar算法的头文件以及Astar对外的ros接口文件
其中红色框为本文基于原作者自己修改的头文件代码，包含自己所理解的中文注释。绿色框为源代码所拥有的头文件(源代码还提供了JPS算法，大家有兴趣可以将它和Astar进行比较。)
其中红色框为本文基于原作者自己修改的源文件代码，包含中文注释。绿色框为源代码所拥有的源文件
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
世界坐标系：明确一点世界坐标系的原点就是点云地图的原点，且点云地图是反应真实世界的，默认单位为m。
栅格坐标系：名称自定义的，嘻嘻。目的是为了方便Astar算法搜索周围邻居，而在三维点云中，各个点之间并不会像图片那样可以方便的查找自己周围的邻居，它们是无序排列的。因此采用将点云空间进行栅格化，如果其中一个栅格有三维点，则在该栅格中置1，否则置0。
两大坐标系如下图所示，其中
O
1
X
1
Y
1
O_1X_1Y_1
O1X1Y1为世界坐标系，
O
2
X
2
Y
2
O_2X_2Y_2
O2X2Y2为栅格坐标系，其中在程序代码中，
O
1
O_1
O1处在整个栅格的中心位置，
O
2
O_2
O2处在
O
1
O_1
O1的左上角位置。
O
2
A
O_2A
O2A为整个点云栅格的宽，
O
2
B
O_2B
O2B为整个点云栅格的长，
O
2
C
O_2C
O2C为整个点云栅格的高。
该程序代码中，点云栅格地图数据的存储也是一维数据，按照先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]
data[x×Length×Hight+y×Hight+x]。
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
Wx=Mx×resolution+origin_xWy=My×resolution+origin_yWz=Mz×resolution+origin_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)
min(max(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)=⎩⎨⎧XabX∈[a,b]X<aX>b
Vector3i AstarPathFinder::coord2gridIndex(const Vector3d & pt)
{
Vector3i idx;
/*
0<=(pt(0) - gl_xl) * inv_resolution)<=GLX_SIZE - 1
0<=(pt(1) - gl_yl) * inv_resolution)<=GLY_SIZE - 1
0<=(pt(2) - gl_zl) * inv_resolution)<=GLZ_SIZE - 1
*/
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 Width×Length×Hight。具体栅格坐标系下某点坐标转化到一维数组中的位置，见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路径的生成也是基于世界坐标系的。
启动
roslaunch grid_path_search my_demo.launch
稍等片刻，就会显示rviz信息，如下图所示。可以改变AllMap下的Spheres进行点云数据不同风格的显示。点击3D Nav Goal(或者点击键盘中的"g"键) 进行目标点的设置，首先通过鼠标左键进行XY平面位置的确定，此时按住鼠标左键不要松，在点击鼠标右键进行上下拖动，进行Z位置的确定，同时松开鼠标左键和鼠标右键，即可显示Astar的三维路径。
注意：
<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进行起始点的设置，不过为了偷懒，并没有写，感兴趣的可以尝试一下。
此外，非常感谢源代码对我的帮助，从中学到了很多有用的知识。