PCL库的学习笔记1：怎么在ROS系统中使用PCL

官方学习资料包括：

1. 怎么在ROS系统中使用PCL
2. ROS中PCL的总体介绍
3. PCL库的官方教程和进阶

怎么在ROS系统中使用PCL

1.1. 创建包

在工作空间的src文件夹下创建新库，PCL的主要依赖库名字是pcl_conversions pcl_ros而不是pcl

catkin_create_pkg my_pcl_tutorial pcl_conversions pcl_ros roscpp sensor_msgs 

1.2 修改package.xml文件

有些依赖不能创建的时候直接指定？

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

注：CMakeList.txt文件是当你写完cpp文件（即创建节点）的时候用的，看第三步

2. 创建代码骨架

在包里面的scr文件夹内创建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>

//先声明发布器，因为回调函数里面会用到
ros::Publisher pub;

// const 可以保护input的内容
// sensor_msgs::PointCloud2ConstPtr用来创建常量点云的指针
// sensor_msgs::PointCloud2 用来创建普通的点云容器
// 点云的数据类型还有很多，但是常用这个，参考第4步
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Create a container for the data.
  sensor_msgs::PointCloud2 output;

  // Do data processing here...
  output = *input;

  // 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
  // 创建订阅器的时候是和PCL没有关系的，只和ROS有关系，回调函数才需要涉及PCL
  // 这里的Input可以改成自己想订阅的topic,比如"/rslidar_points"
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  //创建发布器的时候和PCL有关系，因为要指定发布出去的信息格式
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

  // Spin
  ros::spin ();//ros::spin()用于调用所有可触发的回调函数,将进入循环,不会返回,类似于在循环里反               复调用spinOnce()	   
}

3. 修改CMakeLists.txt

修改包里的CMakeLIsts.txt，指定编译的可执行文件和依赖库位置
add_executable(example src/example.cpp)
target_link_libraries(example ${catkin_LIBRARIES})

4.点云在ROS里面的数据类型

主要有3种，第一种是ROS的信息格式，第二第三种是PCL的数据格式，常用第一种和第二种，第三种是PCL为了兼容？ROS的

sensor_msgs::PointCloud2 
pcl::PointCloud<T>
pcl::PCLPointCloud2 

4.1以pcl::PCLPointCloud2为数据类型写代码

主要包含ROS转PCL，处理完在转成ROS

这一节用PCL官方教程为例，展示了如何把点云数据缩小，PCL官方教如何导入PCL数据，处理PCL数据以及导出PCL数据，但是因为在ROS里面订阅和发布的都是ROS信息，不需要导入和保存了，所以其实只需要中间如何处理PCL数据的代码就可以，但同时需要把ROS信息转成PCL信息才能作中间的处理

以example.cpp的代码为骨架进行修改如下

#include <pcl/filters/voxel_grid.h> 
//新的函数要导入新的头文件，但是是属于PCL库的所以刚开始创建的依赖库够了

...

void  cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // Container for original & filtered data
  // PCL库只能处理PCL类型，所以需要把发布的ROS信息转成PCL
  // 在这里先定义被处理的PCL变量
  // 先定义一个PCL的全新地址的点云指针* cloud
  // 然后以这个指针定义一个常量指针，用来处理，以防原来的指针被修改 cloudPtr
  // 在这里不直接定义一个常量指针，是因为如果定义了常量指针，就不能把ROS的信息保存成PCL了
  // 定义一个输出的PCL变量 cloud_filtered
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;

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

  // Perform the actual filtering
  // 对数据进行过滤，这里的代码直接抄PCL库的官方文献（教程）
  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
  // 再把PCL格式转成ROS格式才能发布
  // 这里的fromPCL可以换成 moveFromPCL，因为fromPCL是又复制了一份，比较低效
  // 但是作者说这里只能from了，因为原来的输入是常量不能更改？为什么是常量呢，这里不是看输出吗，输出是个变量吧？
  sensor_msgs::PointCloud2 output;
  pcl_conversions::fromPCL(cloud_filtered, output);

  // Publish the data
  // 发布信息
  pub.publish (output);
}

修改完后保存，进入工作空间，运行catkin_make就会自动编译了
编译后他就是一个节点，我们在CMake里面命名好的节点

# 记得之前先开一个终端运行roscore
rosrun my_pcl_tutorial example input:=/narrow_stereo_textured/points2
#这个input可以改成你的lidar收集到的topic名称，在cpp文件里面修改，如果是订阅topic就只要运行
rosrun my_pcl_tutorial example 

然后运行rviz显示output这个topic就好

4.2 pcl/PointCloud格式的处理

包括把ros转成pcl/PointCloud，处理PCL，最后发布消息的时候是转成pcl_msgs::ModelCoefficients，而不是ros，所以说输出的内容不一定是ros格式

参考下列代码修改example.cpp文件并回到工作空间路径重新catkin_make

   1 #include <pcl/sample_consensus/model_types.h>
   2 #include <pcl/sample_consensus/method_types.h>
   3 #include <pcl/segmentation/sac_segmentation.h>
   4 
   5 ...
   6 
   7 void 
   8 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
   9 {
  10   // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  11   pcl::PointCloud<pcl::PointXYZ> cloud;
  12   pcl::fromROSMsg (*input, cloud);
  13 
  14   pcl::ModelCoefficients coefficients;
  15   pcl::PointIndices inliers;
  16   // Create the segmentation object
  17   pcl::SACSegmentation<pcl::PointXYZ> seg;
  18   // Optional
  19   seg.setOptimizeCoefficients (true);
  20   // Mandatory
  21   seg.setModelType (pcl::SACMODEL_PLANE);
  22   seg.setMethodType (pcl::SAC_RANSAC);
  23   seg.setDistanceThreshold (0.01);
  24 
  25   seg.setInputCloud (cloud.makeShared ());
  26   seg.segment (inliers, coefficients);
  27 
  28   // Publish the model coefficients
  29   pcl_msgs::ModelCoefficients ros_coefficients;
  30   pcl_conversions::fromPCL(coefficients, ros_coefficients);
  31   pub.publish (ros_coefficients);
  32 }