PCL_ROS的使用

1.数据格式
在ROS中点云的数据类型
在ROS中表示点云的数据结构有：
sensor_msgs::PointCloud
sensor_msgs::PointCloud2
pcl::PointCloud< T >
关于PCL在ros的数据的结构，具体的介绍可查 看 wiki.ros.org/pcl/Overview
关于sensor_msgs::PointCloud2 和 pcl::PointCloud 之间的转换使用pcl::fromROSMsg 和 pcl::toROSMsg
sensor_msgs::PointCloud 和 sensor_msgs::PointCloud2之间的转换
使用sensor_msgs::convertPointCloud2ToPointCloud 和sensor_msgs::convertPointCloudToPointCloud2.
那么如何在ROS中使用PCL呢？
新建工作空间workspace

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

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

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

在echo返回的路径中找到当前工作空间的目录即为设置成功。
新建功能包（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 

在建立的包下的CMakeLists.txt文件下添加依赖项

在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>

在src文件夹下新建VoxelGrid_filter.cpp文件

#include 
// PCL specific includes
#include 
#include 
#include 
#include 

#include 

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 ();
}

在 CMakeLists.txt 文件中添加:

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

catkin_make之后生成可执行文件，运行以下命令

roslaunch openni_launch openni.launch    这是打开kinect发布的命令
rosrun ros_slam example input:=/camera/depth/points      //运行我们生成的文件

代码的主体内容来源于http://wiki.ros.org/pcl/Tutorials/hydro?action=AttachFile&do=view&target=example_voxelgrid.cpp。对其中的订阅和发布的话题名称作了修改，订阅的/velodyne_points话题来自激光雷达VLP-16发布。
编辑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})

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

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

编译功能包
回到工作空间的根目录利用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 -f velodyne

打开rviz后点击Add，添加by Topic，选择filtered_points话题，可以看到降采样后的点云