解决问题:速腾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;
}