下面介绍相关程序，包括lidar＿detection＿node.cpp、CMakeLists.txt、start.launch。

1.lidar＿detection＿node.cpp

lidar＿detection＿node.cpp用于处理接收到的激光雷达节点的数据，检测出车辆周围环境中的障碍区域、可通行区域以及未知区域，由主函数、process函数等部分构成。

1）主函数

主函数分为4个部分：初始化ROS并设置节点句柄；配置参数；显示数据；订阅节点消息并回调函数，如下所示。

接下来对主函数的四个部分进一步介绍。

（1）初始化ROS并设置节点句柄。

第一句，用于初始化ROS，节点命名为lidar＿detection＿node，这里需要注意，节点名必须保持唯一。第二句，是为这个进程的节点创建一个句柄nh，用来与ROS进行通信，这里需要注意：第一个创建的句柄会为节点进行初始化。第三句，是创建句柄nh＿private，用于进行参数配置。

这里调用了头文件，如下所示：

ros/ros.h是一个实用的头文件，它引用了ROS系统中大部分常用的头文件，使用它会使得编程很简便。

（2）配置参数。

这里需要配置的参数包括：

高度差阈值；

栅格地图分辨率；

栅格地图x方向跨度；

栅格地图y方向跨度；

栅格地图中车辆x方向位置；

栅格地图中车辆y方向位置；

栅格地图半径；

display布尔参数，用来判断是否需要显示。

这些参数也可以在配置文件launch中设置。

（3）显示数据。

显示数据部分是以数据流形式，输出数据到终端。

（4）订阅节点消息并回调函数。

订阅节点消息部分中，sensor＿msgs：：PointCloud2表明订阅的消息为激光雷达点云数据，点云数据的结构类型为PointCloud2，这在PointCloud2.h头文件中进行了说明，头文件如下所示：

订阅的节点名lidardata，调用函数为process，用来处理订阅的消息。(www.daowen.com)

使用ros：：spin（）语句进入自循环，可以调用函数process。

2）process函数

process函数分为5个部分，下面分别进行介绍。

（1）接收点云数据并转换数据类型。

process函数第一部分程序如下所示，用于接收点云数据并转换数据类型。

程序中sensor＿msgs对应于主函数中订阅节点的消息类型，即激光雷达点云数据类型；pcl：：PointXYZ是最简单的点云格式，它只包括了坐标x、y、z；fromROSMsg函数用于将pointcloud＿msg转化为点云PointXYZ格式的pointcloud。

这里调用了pcl点云的头文件，如下所示：

（2）创建栅格地图grid＿map。

process函数第二部分如下所示，用于创建栅格地图grid＿map。

栅格地图grid＿map包括的参数依次是y、x方向范围，分辨率以及车辆y、x方向坐标。

这里调用了C＋＋中常用的构造参数形式，程序分为三大步，如下所示：

上面第一步设置参数，其中CV：：Mat调用了opencv2/opencv.hpp头文件，如下所示：

上面第二步，对变量进行初始化。第三步，是释放GridMap占用的空间。

（3）计算每个栅格中的最高点和最低点。

process函数的第三部分程序如下，用于计算每个栅格中的最高点和最低点。

其中floor函数调用了库cmath，如下所示：

（4）判断每个栅格属性。

process函数的第四部分，利用高度差原理判断每一个栅格的属性。程序如下：

（5）用opencv绘制栅格地图并显示。

process函数的第五部分程序如下：

这里调用了函数known＿area＿extraction，用于减少栅格地图中的未知栅格，它共有三个参数，第一个参数为栅格地图的属性，第二个参数为经分辨率缩小的栅格地图中的坐标，第三个参数为经分辨率缩小的栅格地图半径。函数known＿area＿extraction程序如下：

2.CMakeLists.txt

CMakeLists.txt用来指定编译规则，具体语句如下：

3.start.launch

start.launch文件可以用于同时启动多个节点程序，也可以用来设置参数。本例中具体语句如下：