ROS中点云学习（九）：sensor_msgs::LaserScan转sensor_msgs::PointCloud2

转载：四国一

一、前言

首先，为什么要进行数据的转换？举个例子，在ROS中，我们通过订阅Kinect的RGB图像topic就可以获取到图像数据，但当我们要对这些图像进行处理的时候，我们需要用到专门的图像处理库，比如OpenCV．因此，我们需要用到ROS提供的package，cv_brige，将ROS格式的数据转换为OpenCV适用的数据．

回到本文，当我们要对激光雷达的数据进行处理时，我们也需要将ROS获取的雷达数据，即sensor_msgs::LaserScan，转化为PCL点云库可以使用的数据，如pcl::PointCloud．
二、转换方法

如上图所示，首先我们需要订阅激光雷达topic（如/scan）获取到sensor_msgs::LaserScan的message．然后使用ROS提供的laser_geometry包将其转化为sensor_msgs::PointCloud2格式的message．接着将sensor_msgs::PointCloud2的message转换为PCL点云库所需的数据格式．有两种方法，一种方法是使用ROS提供的pcl_conversions包，另一种方法是直接订阅之前转化的PointCloud2的数据，这样可以自动完成两种类型的转换，需要注意的是这种方法需要引入pcl_ros/point_cloud.h．
三、代码示例

首先定义一个pcl点云数据转换类

#include <iostream>

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>


class My_Filter {
     public:
        My_Filter();
    	//订阅 LaserScan　数据，并发布 PointCloud2 点云 
        void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
    
    	//订阅 LaserScan 数据，先转换为 PointCloud2，再转换为 pcl::PointCloud
        void scanCallback_2(const sensor_msgs::LaserScan::ConstPtr& scan);
    	
    	//直接订阅 PointCloud2 然后自动转换为 pcl::PointCloud
        void pclCloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud);

     private:
        ros::NodeHandle node_;
        laser_geometry::LaserProjection projector_;
        tf::TransformListener tfListener_;

        //发布　"PointCloud2"
        ros::Publisher point_cloud_publisher_;

        //订阅 "/scan"
        ros::Subscriber scan_sub_;

        //订阅 "/cloud2" -> "PointCloud2"
        ros::Subscriber pclCloud_sub_;
};

添加类的实现

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

Main

#include "My_Filter.h"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_pcl_node");

    My_Filter filter;

    ros::spin();

    return 0;
}

编译

catkin_create_pkg my_pcl pcl_conversions pcl_ros roscpp rospy sensor_msgs laser_geometry tf

Cmakelist

cmake_minimum_required(VERSION 2.8.3)
project(my_pcl)

find_package(catkin REQUIRED COMPONENTS
  laser_geometry
  pcl_conversions
  pcl_ros
  roscpp
  rospy
  sensor_msgs
  tf
)

find_package(PCL 1.7 REQUIRED)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

add_executable(${PROJECT_NAME}_node src/My_Filter.cpp src/my_pcl_node.cpp)

target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
  ${PCL_LIBRARIES}
)

雷达数据(LaserScan)使用北洋雷达发布，参考：ROS melodic通过hokuyo_node驱动从USB读取URG-04LX-UG01雷达数据

效果