如何在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群中