在做Lidar与IMU数据之间的标定时,出现了数据无法读取的问题
主要是代码里读取lidar数据是需要下面的语句
// add lidar msg
sensor_msgs::PointCloud2ConstPtr lidar_msg = m.instantiate<sensor_msgs::PointCloud2>();
查看bag文件的数据类型
rosbag info XXX.bag
可以明显看出,用的是自带的sensor_msgs/PointCloud2类型的读取函数,因此这里需要对数据格式进行一个转换
如图可以看出两种数据类型的区别
进行转换需要首先分析这两种数据类型的存储格式
livox存储格式看这
Livox defined msg format is :
CustomPoint format:
代码部分:
点云部分
for (int i = 0; i < point_num; i++)
{
cloud.points[i].x = msg.points.at(i).x;
cloud.points[i].y = msg.points.at(i).y;
cloud.points[i].z = msg.points.at(i).z;
cloud.points[i].intensity = msg.points.at(i).reflectivity;
}
其他信息:
ros_msg.header.stamp = msg.header.stamp;
ros_msg.header.frame_id = "rslidar";
ros_msg.header.seq = 0;
ros_msg.height = 1;
ros_msg.width = cloud.width;
ros_msg.point_step = sizeof(pcl::PointXYZI);
ros_msg.row_step =
static_cast<uint32_t>(sizeof(pcl::PointXYZI) * ros_msg.width);
ros_msg.is_dense = false;
livoxbag.write("/horizontal_laser_3d", ros_msg.header.stamp, ros_msg);
main函数:
int main(int argc, char* argv[])
{
std::string bag_path = "/home/u/ws_livox_pc2/src/data/livox.bag";
livoxbag.open(bag_path, rosbag::bagmode::Write);
ros::init(argc, argv, "livox2pointcloud2");
ros::NodeHandle nh;
ros::Subscriber imu_sub = nh.subscribe("/livox/imu", 100, &imuCallback);
ros::Subscriber livox_sub = nh.subscribe("/livox/lidar", 100, &laserCallback);
ros::spin();
return 0;
}
这段代码这里是输入一个新的包进行数据写入
然后打开一个新终端
roscore
输入
./XXX
再打开一个新的终端
rosbag play XXX.bag
就可以实现类型的转换啦