ROS中从摄像头获取PCL点云数据，并滤波后在Rviz中显示

一、点云简介

    通过测量仪器得到的产品外观表面的点数据集合也称之为点云，通常使用三维坐标测量机所得到的点数量比较少，点与点的间距也比较大，叫稀疏点云；而使用三维激光扫描仪或照相式扫描仪得到的点云，点数量比较大并且比较密集，叫密集点云。

        经过几十年的发展，机器人传感器领域已经发生了巨大的变化：从基于声呐的简单测距功能到现在的视觉传感器和激光扫描仪。由视觉传感器和激光扫描仪提供的大量3D数据已经变得实用起来，像DAPAR无人汽车挑战赛已经可以实用高质量的3D点云来感知世界。近年来，一些3D传感器价格已经普遍为大家接受，像微软公司提供的Xbox360游戏机附带的传感器Kinect，价格在1000元左右。Kinect可以提供3D点云，使得机器人可以看到3D世界。

        那么如何有效处理3D点云数据，PCL（point cloud library）给出了一个很好的解决方案。PCL是一个独立的、大规模开源点云处理软件包，现在已经完全集成到ROS中。PCL被设计用于感知3D世界，可以处理实时感知设备的数据，像激光扫描仪、双目立体视觉系统和ToF（Time of Flight）摄像机。

        下面我们给出一个案例，在ROS中实现从摄像头获取PCL点云数据，并滤波后在Rviz中显示。

二、安装摄像头驱动等基础程序

    经过测试，由于Openni2驱动程序无法对华硕的某型摄像头(ASUSTek)产生的点云消息进行发布，因此，安装Open你驱动程序，命令如下：

apt-get install ros-kinetic-openni-camera ros-kinetic-openni-launch

        安装完，通过如下命令测试

roslaunch openni_launch openni.launch

        注意：如果提示    “USB interface is not supported!”，则通过如下命令继续安装一个驱动，问题即可解决

sudo apt-get install libopenni-dev libopenni-sensor-primesense-dev

三、创建ROS package

catkin_create_pkg my_pcl_tutorial pcl_conversions pcl_ros roscpp sensor_msgs

然后，在package.xml中增加如下代码

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

四、创建源代码文件src/example.cpp，将下面的代码粘贴到里边

#include <ros/ros.h>
// PCL specific includes 。PCL 的相关的头文件
#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中的点云的数据格式
  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);  //设置滤波时创建的体素大小为1cm立方体
  sor.filter (cloud_filtered);//执行滤波处理，存储输出cloud_filtered

  // Convert to ROS data type。// 再将滤波后的点云的数据格式转换为ROS下的数据格式发布出去
  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> ("input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
//创建ROS的发布节点
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

  // Spin
// 回调
  ros::spin ();
}

五、在CMakeLists .txt中增加下面的内容

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

六、编译工作空间

$ cd ～/catkin_ws
$ catkin_make

七、运行roscore

roscore

八、将华硕摄像头连接到USB接口，新的Terminal中运行摄像头驱动及消息发布程序

roslaunch openni_launch openni.launch

九、新的Terminal中运行我们编写的摄像头点云数据降采样程序

rosrun my_pcl_tutorial example input:=/camera/depth/points

十、新的Terminal中运行Rviz,查看滤波前后的点云数据

rosrun rviz rviz

Rviz中，Global Options中的Fixed Frame选择camera_depth_frame；通过Add按钮可以添加camera图像数据以及降采样前后的点云数据到Displays面板中，从而查看运行效果，如下图所示。第一幅为降采样前的点云数据显示，第二幅为降采样后的点云数据显示。