ROS第三梯：ROS+C++实现速腾Bag包的解析

解决问题：速腾Bag包利用bag_to_pcd生成的pcd文件字段名称存在问题，多了几个异常的"_"，导致强度属性无法在Intensity中显示。

解决方案：利用sensor_msgs库进行数据读取和转换成sensor_msgs::PointCloud格式，再通过遍历该数据结构获得坐标和属性信息，具体代码如下：

/*
依赖库：
roscpp
rosbag
std_msgs
sensor_msgs
*/
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <string>
#include <stdlib.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <fstream>

rosbag::Bag bag;
bag.open(path, rosbag::bagmode::Read); //open *.bag
//set topics which you want to read
std::vector<std::string> topics; 
 //这个是我指定要读取的消息topic
topics.push_back(std::string("/bp1/rslidar_points"));
//read defined topics
rosbag::View view(bag, rosbag::TopicQuery(topics));; 
for (rosbag::MessageInstance const m : view) 
{
        sensor_msgs::PointCloud2::ConstPtr input = m.instantiate<sensor_msgs::PointCloud2>();
        if (input == NULL) {
            continue;
        }

        sensor_msgs::PointCloud clouddata;
        sensor_msgs::convertPointCloud2ToPointCloud(*input, clouddata);

        for(int i = 0;i<clouddata.points.size();i++)
        {
            PtType point;
            point.x = clouddata.points[i].x;
            if(isnan(point.x)) continue;
            point.y = clouddata.points[i].y;
            point.z = clouddata.points[i].z;
            //设置反射强度
            point.intensity = clouddata.channels[0].values[i];  
            //设置ring  
            point.ring = clouddata.channels[1].values[i];    
            //设置timestamp
            point.timestamp = clouddata.channels[2].values[i];    
            std::cout<<i<<" "<<point.x<<" "<<point.y<<" "<<point.z<<" "<<point.intensity
                    <<" "<<point.ring<<" "<<std::to_string(clouddata.channels[2].values[i])<<std::endl;
}