在ROS中利用pcl库编程(详细）

1 引言

最近在做用激光雷达建图的课题，需要在ROS系统下进行编程，其中涉及到很多对点云数据处理的算法，例如降采样、地面分割等。在点云数据处理上，目前pcl库（Point Cloud Library) 已经有了很好的支持和实现，在ROS编程时可以直接引入利用，但需要做必要的链接和转换，本文针对如何在ROS中实现pcl库的使用问题进行探讨。
参考原文博客：如何在ROS中使用PCL–数据格式（1）
参考ROS Wiki 网站：ROS中的pcl教程

2 方法

与在ROS中新建功能包（package）并实现节点（node）功能一样，可以依照下面的流程：
1、新建工作空间（work space)

$ mkdir -p ~/catkin_ws/src  （catkin_ws修改为你的工作空间的文件名称）

编译工作空间并设置环境变量

$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash
$ echo $ROS_PACKAGE_PATH

在echo返回的路径中找到当前工作空间的目录即为设置成功。
2、新建功能包（package)

$ cd ~/catkin_ws/src
$ catkin_create_pkg pcl_VoxelGrid roscpp pcl_ros pcl_conversions sensor_msgs
$ cd ~/catkin_ws
$ catkin_make
$ . ~/catkin_ws/devel/setup.bash 

功能包的名称和后面指定依赖的库文件根据实际修改，注意其中pcl_ros、和pcl_conversions两项，这是ROS与pcl库的接口。

3、package.xml文件配置
这里以利用VoxelGrid方法对点云数据降采样为例。
创建功能包的命令：

 catkin_create_pkg pcl_VoxelGrid roscpp pcl_ros pcl_conversions sensor_msgs

然后在package.xml文件中添加：

  <build_depend>libpcl-all-dev</build_depend>
  <exec_depend>libpcl-all</exec_depend>

文件完整的内容：

<package format="2">
  <name>pcl_VoxelGrid</name>
  <version>0.0.0</version>
  <description>The pcl_VoxelGrid package</description>
  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>pcl_conversions</build_depend>
  <build_depend>pcl_ros</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_export_depend>pcl_conversions</build_export_depend>
  <build_export_depend>pcl_ros</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_depend>libpcl-all-dev</build_depend>
  <exec_depend>libpcl-all</exec_depend>
  <exec_depend>pcl_conversions</exec_depend>
  <exec_depend>pcl_ros</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <export>
  </export>
</package>

4、编辑cpp文件
在src文件夹下新建文档，并重命名为VoxelGrid_filter.cpp。
复制下面的代码到文件中：

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/filters/voxel_grid.h>

ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;

  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  // Perform the actual filtering
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloudPtr);
  sor.setLeafSize (0.1, 0.1, 0.1);
  sor.filter (cloud_filtered);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 output;
  pcl_conversions::moveFromPCL(cloud_filtered, output);

  // Publish the data
  pub.publish (output);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("velodyne_points", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("filtered_points", 1);

  // Spin
  ros::spin ();
}

代码的主体内容来源于ROS与pcl教程源代码范例。对其中的订阅和发布的话题名称作了修改，订阅的/velodyne_points话题来自激光雷达VLP-16发布。

5、编辑CMakeList.txt文件
在完成源代码编辑后，对CMakeList.txt文件进行修改。
添加两行：

add_executable(VoxelGrid_filter src/VoxelGrid_filter.cpp)
target_link_libraries(VoxelGrid_filter ${catkin_LIBRARIES})

完整的CMakeList.txt文件内容：

cmake_minimum_required(VERSION 2.8.3)
project(pcl_VoxelGrid)

find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(VoxelGrid_filter src/VoxelGrid_filter.cpp)
target_link_libraries(VoxelGrid_filter ${catkin_LIBRARIES})

6、编写launch文件
在src文件夹下新建pcl_VoxelGrid.launch文件，添加下面内容：

<launch>
    <node pkg="pcl_VoxelGrid" type="pcl_VoxelGrid_node" name="pcl_VoxelGrid_node" output="screen"/>
</launch>

7、编译功能包
回到工作空间的根目录进行编译：

$ cd ~/catkin_ws/
$ catkin_make

在devel/lib目录下会生成节点的可执行文件：pcl_VoxelGrid_node
在命令下可用roslaunch命令对节点进行启动（先source，不然可能找不到package）：

$ source devel/setup.bash
$ roslaunch pcl_VoxelGrid pcl_VoxelGrid.launch

在启动激光雷达工作的情况下，节点工作并会发布/filtered_points的话题，可用rivz工具进行可视化。

$ rosrun rviz rviz

打开rviz后点击Add，添加by Topic，选择filtered_points话题，可以看到降采样后的点云。
在CSDN上受益无限，第一次尝试写博客，作为学习心得与大家分享！