如何在ROS中使用PCL（2）

記錄關於我們運行roslaunch openni_launch openni.launch  命令時生成的話題以及這些話題的數據類型便於后期的處理，只有知道它們的數據結構，才能很好的對數據進行處理，我們觀察到使用rostopic list的所有話題的列表,當然其中也有一些不經常使用的話題類型，比如下面這些話題是我們經常使用的
/camera/depth/image
/camera/depth/image_raw
/camera/depth/points
/camera/ir/image_raw
/camera/rgb/image_color
/camera/rgb/image_raw

發布的話題：

image_raw (sensor_msgs/Image) ： 未處理的原始圖像

使用命令查看sensor_msgs/Image的數據

camera_info (sensor_msgs/CameraInfo)：包含了相機標定配置以及相關數據

 

介紹幾個ROS節點運行的幾種工具。他們的作用是ROS格式點雲或包與點雲數據（PCD）文件格式之間的相互轉換。

（1）bag_to_pcd

用法：rosrun pcl_ros bag_to_pcd <input_file.bag> <topic> <output_directory>

讀取一個包文件，保存所有ROS點雲消息在指定的PCD文件中。

（2）convert_pcd_to_image

用法：rosrun pcl_ros convert_pcd_to_image <cloud.pcd>

加載一個PCD文件，將其作為ROS圖像消息每秒中發布五次。

(3) convert_pointcloud_to_image

用法：rosrun pcl_ros convert_pointcloud_to_image input:=/my_cloud output:=/my_image

 查看圖像：rosrun image_view image_view image:=/my_image

訂閱一個ROS的點雲的話題並以圖像的信息發布出去。
（4）pcd_to_pointcloud

用法：rosrun pcl_ros pcd_to_pointcloud <file.pcd> [ <interval> ]

• <file.pcd> is the (required) file name to read.

• <interval> is the (optional) number of seconds to sleep between messages. If <interval> is zero or not specified the message is published once.

加載一個PCD文件，發布一次或多次作為ROS點雲消息
（5）pointcloud_to_pcd

例如： rosrun pcl_ros pointcloud_to_pcd input:=/velodyne/pointcloud2

訂閱一個ROS的話題和保存為點雲PCD文件。每個消息被保存到一個單獨的文件，名稱是由一個可自定義的前綴參數，ROS時間的消息，和以PCD擴展的文件。

那么我們使用一個簡單的例子來實現在ROS中進行平面的分割，同時注意到使用的數據轉換的使用

/**************************************************************************
關於使用pcl/PointCloud<T>的舉例應用。這一類型的數據格式是PCL庫中定義的一種數據格式
這里面使用了兩次數據轉換從 sensor_msgs/PointCloud2 到 pcl/PointCloud<T> 和 
                       從 pcl::ModelCoefficients 到 pcl_msgs::ModelCoefficients.
************************************************************************/
#include <iostream>
//ROS
#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/io/pcd_io.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::fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T>&);  
  //pcl::io::savePCDFileASCII("test_pcd.pcd",cloud);

  // 把提取出來的內點形成的平面模型的參數發布出去
  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 ();
}

在這里我們的input就是要訂閱的話題/camera/depth/points

我們在rosrun 的時候注明input：=/camera/depth/points的這樣就可以使用kienct發布的點雲數據，同時你也可以指定點雲的數據