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];    //设置timestamppoint.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;
}