ROS学习笔记（三）sensor_msgs::LaserScan转pcl::PointCloud＜T＞
本文介绍了如何在ROS中将激光雷达（LiDAR）数据转换为点云数据。主要步骤包括：
数据转换需求：在ROS中，LaserScan数据格式需要通过lasergeometry包转换为sensormsgs::PointCloud2，再通过pcl_conversions包转换为PCL点云格式。
方法选择：可以使用lasergeometry包提供的API进行转换，或者直接订阅PointCloud2数据并自动完成转换（需引入pclros/point_cloud.h）。
代码实现：提供了一个名为My.Filter的类，展示了如何订阅LaserScan数据，发布转换后的PointCloud2数据，并直接订阅并转换为PCL点云。
ROS包创建：建议使用catkincreatepkg创建ROS包，并修改package.xml和CMakeLists.txt以引入必要的PCL依赖项。
结果展示：通过RViz可以观察到转换后的点云数据，并在终端中打印出点云信息。
摘要涵盖了数据转换的必要性、方法选择、代码实现和结果展示，帮助读者快速掌握激光雷达数据处理的基本流程。
一、前言
首先，为什么要进行数据的转换？例如，在ROS框架中，通过订阅Kinect的RGB图像topic，我们可以获取图像数据。然而，当需要对这些图像进行处理时，我们需要依赖专门的图像处理库，如OpenCV。为此，我们必须利用ROS提供的cv_bridge包，将ROS格式的数据转换为OpenCV兼容的数据格式。
在本文中，处理激光雷达数据时，我们不仅需要处理激光雷达数据本身，还需要将ROS系统获取的激光雷达数据，即sensor_msgs::LaserScan，转换为PCL能够处理的数据格式，如pcl::PointCloud<T>。
二、转换方法
如上图所示，首先，我们需要订阅特定的LIDAR topic（如/scan），以接收sensor_msgs::LaserScan类型的原始消息。接着，我们利用ROS提供的laser_geometry包，将接收到的sensor_msgs::LaserScan消息转换为sensor_msgs::PointCloud2格式的数据。这一步骤确保了数据格式的一致性，为后续处理奠定了基础。随后，我们将sensor_msgs::PointCloud2格式的消息进一步转换为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}
)最后，进入工作空间，编译，运行．
五、结果展示
启动终端，执行命令rosrun rviz rviz。当添加PointCloud2时，即可观察到转换后的点云数据。
此外，在终端窗口中也会输出并生成相应的pcl::PointCloud类型点云数据。具体生成方式可参考附录中的代码实现。
六、参考资料
[1] How to turn laser scan to point cloud map
[2] pcl/Overview