sensor_msgs::laserscan 與pointcloud2、pointcloud 的轉換

 sensor_msgs::laserscan 與pointcloud2、pointcloud 的轉換 

 

#include "My_Filter.h"

My_Filter::My_Filter(){
    //scan_sub_ = node_.subscribe<sensor_msgs::LaserScan> ("/scan", 100, &My_Filter::scanCallback, this);

    //訂閱　"/scan"
    scan_sub_ = node_.subscribe<sensor_msgs::LaserScan> ("/scan", 100, &My_Filter::scanCallback_2, this);

    //發布LaserScan轉換為PointCloud2的后的數據
    point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2> ("/cloud2", 100, false);

    //此處的tf是 laser_geometry 要用到的
    tfListener_.setExtrapolationLimit(ros::Duration(0.1));

    //前面提到的通過訂閱PointCloud2，直接轉化為pcl::PointCloud的方式
    pclCloud_sub_ = node_.subscribe<pcl::PointCloud<pcl::PointXYZ> >("/cloud2", 10, &My_Filter::pclCloudCallback, this);

}


void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    sensor_msgs::PointCloud2 cloud;
    projector_.transformLaserScanToPointCloud("laser", *scan, cloud, tfListener_);
    point_cloud_publisher_.publish(cloud);
}


void My_Filter::scanCallback_2(const sensor_msgs::LaserScan::ConstPtr& scan){
    sensor_msgs::PointCloud2 cloud;

    /*laser_geometry包中函數，將 sensor_msgs::LaserScan 轉換為 sensor_msgs::PointCloud2 */
    //普通轉換
    //projector_.projectLaser(*scan, cloud);        
    //使用tf的轉換
    projector_.transformLaserScanToPointCloud("laser", *scan, cloud, tfListener_);

    int row_step = cloud.row_step;
    int height = cloud.height;

    /*將 sensor_msgs::PointCloud2 轉換為　pcl::PointCloud<T> */
    //注意要用fromROSMsg函數需要引入pcl_versions（見頭文件定義）
    pcl::PointCloud<pcl::PointXYZ> rawCloud;
    pcl::fromROSMsg(cloud, rawCloud);

    for(size_t i = 0; i < rawCloud.points.size(); i++){
        std::cout<<rawCloud.points[i].x<<"\t"<<rawCloud.points[i].y<<"\t"<<rawCloud.points[i].z<<std::endl;
    }

    point_cloud_publisher_.publish(cloud);


}

void My_Filter::pclCloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud){
    for(size_t i = 0; i < cloud->points.size(); i++){
        std::cout<<"direct_trans: "<<cloud->points[i].x<<"\t"<<cloud->points[i].y<<"\t"<<cloud->points[i].z<<std::endl;
    }
}

 

參考

古月的博客

https://www.guyuehome.com/12913