如何在ROS中使用PCL—数据格式（1）

在ROS中点云的数据类型

在ROS中表示点云的数据结构有： sensor_msgs::PointCloud      sensor_msgs::PointCloud2     pcl::PointCloud<T>

关于PCL在ros的数据的结构，具体的介绍可查 看            wiki.ros.org/pcl/Overview

关于sensor_msgs::PointCloud2   和  pcl::PointCloud<T>之间的转换使用pcl::fromROSMsg 和 pcl::toROSMsg 

sensor_msgs::PointCloud   和   sensor_msgs::PointCloud2之间的转换

使用sensor_msgs::convertPointCloud2ToPointCloud 和sensor_msgs::convertPointCloudToPointCloud2.

那么如何在ROS中使用PCL呢？
（1）在建立的包下的CMakeLists.txt文件下添加依赖项

在package.xml文件里添加：

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

在src文件夹下新建.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>

ros::Publisher pub;

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;

 
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
  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})

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

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

运行RVIZ可视化以下，添加了程序发布的点云的话题既可以显示。同时也可以使用PCL自带的显示的函数可视化（这里不再一一赘述）

$ rosrun rviz rviz
在RVIZ中显示的点云的数据格式sensor_msgs::PointCloud2;

那么如果我们想实现对获取的点云的数据的滤波的处理，这里就是进行一个简单的体素网格采样的实验

同样在src文件夹下新建.cpp文件，然后我们的程序如下。也就是要在回调函数中实现对获取的点云的滤波的处理，但是我们要特别注意每个程序中的点云的数据格式以及我们是如何使用函数实现对ROS与PCL 的转化的。

程序如下

/***********************************************************
关于使用sensor_msgs/PointCloud2，
***********************************************************/

#include <ros/ros.h>
// 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& input)  //特别注意的是这里面形参的数据格式
{
 // 声明存储原始数据与滤波后的数据的点云的 格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;    //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); 
  pcl::PCLPointCloud2 cloud_filtered;     //存储滤波后的数据格式

  // 转化为PCL中的点云的数据格式
  pcl_conversions::toPCL(*input, *cloud);

  // 进行一个滤波处理
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;   //实例化滤波
  sor.setInputCloud (cloudPtr);     //设置输入的滤波
  sor.setLeafSize (0.1, 0.1, 0.1);   //设置体素网格的大小
  sor.filter (cloud_filtered);      //存储滤波后的点云

  // 再将滤波后的点云的数据格式转换为ROS 下的数据格式发布出去
  sensor_msgs::PointCloud2 output;   //声明的输出的点云的格式
  pcl_conversions::fromPCL(cloud_filtered, output);    //第一个参数是输入，后面的是输出

  //发布命令
  pub.publish (output);
}

int
main (int argc, char** argv)
{
  // 初始化 ROS节点
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;   //声明节点的名称

  // 为接受点云数据创建一个订阅节点
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

  //创建ROS的发布节点
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

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

看一下结果如图，这是在RVIZ中显示的结果，当然也可以使用PCL库实现可视化（注意我们在rviz中显示的点云的数据格式都是sensor_msgs::PointCloud2

要区别pcl::PCLPointCloud2  这是PCL点云库中定义的一种的数据格式，在RVIZ中不可显示，）

/**************************************************************************
关于使用pcl/PointCloud<T>的举例应用。这一类型的数据格式是PCL库中定义的一种数据格式
这里面使用了两次数据转换从

                      sensor_msgs/PointCloud2       到        pcl/PointCloud<T>
                     pcl::ModelCoefficients                到         pcl_msgs::ModelCoefficients.

代码

#include <ros/ros.h>
// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//关于平面分割的头文件
#include <pcl/sample_consensus/model_types.h>   //分割模型的头文件
#include <pcl/sample_consensus/method_types.h>   //采样一致性的方法
#include <pcl/segmentation/sac_segmentation.h>  //ransac分割法

ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // 将点云格式为sensor_msgs/PointCloud2 格式转为 pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);   //关键的一句数据的转换

  pcl::ModelCoefficients coefficients;   //申明模型的参数
  pcl::PointIndices inliers;             //申明存储模型的内点的索引
  // 创建一个分割方法
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // 这一句可以选择最优化参数的因子
  seg.setOptimizeCoefficients (true);
  // 以下都是强制性的需要设置的
  seg.setModelType (pcl::SACMODEL_PLANE);   //平面模型
  seg.setMethodType (pcl::SAC_RANSAC);    //分割平面模型所使用的分割方法
  seg.setDistanceThreshold (0.01);        //设置最小的阀值距离

  seg.setInputCloud (cloud.makeShared ());   //设置输入的点云
  seg.segment (inliers, coefficients);       //cloud.makeShared() 创建一个 boost shared_ptr
  
  // 把提取出来的内点形成的平面模型的参数发布出去
  pcl_msgs::ModelCoefficients ros_coefficients;
  pcl_conversions::fromPCL(coefficients, ros_coefficients);
  pub.publish (ros_coefficients);
}

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 ("input", 1, cloud_cb);

  // Create a ROS publisher for the output model coefficients
  pub = nh.advertise<pcl_msgs::ModelCoefficients> ("output", 1);

  // Spin
  ros::spin ();
}

提取点云中平面的参数并且发布出去

 PCL对ROS的接口的总结

比如： pcl::toROSMsg(*cloud,output);

实现的功能是将pcl里面的pcl::PointCloud<pcl::PointXYZ>   cloud   转换成ros里面的sensor_msgs::PointCloud2   output 这个类型。

PCL对ROS的接口提供PCL数据结构的转换，通过通过ROS提供的以消息为基础的转换系统系统。这有一系列的转换函数提供用来转换原始的PCL数据类型成消息型。一些最有用常用的的message类型列举在下面。

std_msgs：Header:这不是真的消息类型，但是用在Ros消息里面的每一个部分。它包含了消息被发送的时间和序列号和框名。PCL等于pcl::Header类型

sensor_msgs::PointCloud2:这是最重要的类型。这个消息通常是用来转换pcl::PointCloud类型的，pcl::PCLPointCloud2这个类型也很重要，因为前面版本的可能被废除。

pcl_msgs::PointIndices:这个类型存储属于点云里面的点的下标，在pcl里面等于pcl::PointIndices

pcl_msgs::PolygonMesh这个类型包括消息需要描述多边形网眼，就是顶点和边，在pcl里面等于pcl::PolygonMesh

pcl_msgs::Vertices：这个类型包含了一系列的顶点作为一个数组的下标，来描述一个多边形。在pcl里面等于pcl::Vertices

pcl_msgs::ModelCoefficients:这存储了一个模型的不同的系数，比如描述一个平面需要4个系数。在PCL里面等于pcl::ModelCoefficients

上面的数据可以从PCL转成ROS里面的PCL。所有的函数有一个类似的特征，意味着一旦我们知道这样去转换一个类型，我们就能学会转换其他的类型。下面的函数是在pcl_conversions命名空间里面提供的函数

下面的函数是在pcl_conversions命名空间里面提供的函数

void	copyImageMetaData (const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
void	copyPCLImageMetaData (const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
void	copyPCLPointCloud2MetaData (const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
void	copyPointCloud2MetaData (const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
void	fromPCL (const pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
void	fromPCL (const std::vector< pcl::PCLPointField > &pcl_pfs, std::vector< sensor_msgs::PointField > &pfs)
void	fromPCL (const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
void	moveFromPCL (pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
void	moveFromPCL (pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2)
void	moveToPCL (sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
void	moveToPCL (sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)
void	moveToPCL (pcl_msgs::ModelCoefficients &mc, pcl::ModelCoefficients &pcl_mc)
void	toPCL (const sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
void	toPCL (const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2)

总结出来就是

void fromPCL(const <PCL Type> &, <ROS Message type> &);
void moveFromPCL(<PCL Type> &, <ROS Message type> &);
void toPCL(const <ROS Message type> &, <PCL Type> &);
void moveToPCL(<ROS Message type> &, <PCL Type> &);

PCL类型必须被替换成先前指定的PCL类型和ROS里面相应的类型。sensor_msgs::PointCloud2有一个特定的函数集去执行转换

void toROSMsg(const pcl::PointCloud<T> &, sensor_msgs::PointCloud2 &);                     转换为ROS的点云sensor_msgs::PointCloud2类型
void fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T>&);                 转为PCL中的pcl::PointCloud<T>类型
void moveFromROSMsg(sensor_msgs::PointCloud2 &, pcl::PointCloud<T> &);               转换为pcl::PointCloud<T> 类型

**************

有兴趣者可以关注微信公众号或者加入QQ群中