您的位置:首页 > 房产 > 家装 > ROS第三梯:ROS+C++实现速腾Bag包的解析

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

2024/10/6 2:21:58 来源:https://blog.csdn.net/qq625924821/article/details/142106048  浏览:    关键词: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;
}

版权声明:

本网仅为发布的内容提供存储空间,不对发表、转载的内容提供任何形式的保证。凡本网注明“来源:XXX网络”的作品,均转载自其它媒体,著作权归作者所有,商业转载请联系作者获得授权,非商业转载请注明出处。

我们尊重并感谢每一位作者,均已注明文章来源和作者。如因作品内容、版权或其它问题,请及时与我们联系,联系邮箱:809451989@qq.com,投稿邮箱:809451989@qq.com