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

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

/***********************************************************
关于使用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 ();
}

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