ROS学习笔记(三)sensor_msgs::LaserScan转pcl::PointCloud

874
0
2020年9月8日 10时57分

文章首发于我的个人博客http://kevinnan.org.cn

一、前言

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

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

二、转换方法

ROS学习笔记(三)sensor_msgs::LaserScan转pcl::PointCloud<T>插图

如上图所示,首先我们需要订阅激光雷达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;
    }
}

最后加入主函数

#include "My_Filter.h"

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

    My_Filter filter;

    ros::spin();

    return 0;
}

四、创建一个点云数据转换的ROS包

首先进入到你自己的ROS工作空间下的src目录,

cd ~/your_workspace/src

然后使用catkin_create_pkg创建一个名为my_pcl的ROS包.

catkin_create_pkg my_pcl pcl_conversions pcl_ros roscpp rospy sensor_msgs laser_geometry tf

修改package.xml文件,添加

<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>

修改CMakeLists.txt文件.下面给出的作为一个参考

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}
)

最后,进入工作空间,编译,运行.

五、结果展示

新建terminal,运行rosrun rviz rviz.在添加PointCloud2就可以观察到转换后的点云

ROS学习笔记(三)sensor_msgs::LaserScan转pcl::PointCloud<T>插图(1)

同时,终端里也打印出转化为pcl::PointCloud类型的点云数据.(详见具体代码)

ROS学习笔记(三)sensor_msgs::LaserScan转pcl::PointCloud<T>插图(2)

六、参考资料

[1] How to turn laser scan to point cloud map

[2] pcl/Overview

发表评论

后才能评论