如何在ROS中使用PCL—資料格式
在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之間的轉換
那麼如何在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> 型別