Livox LiDAR点云数据类型的转化——livox_ros_driver/CustomMsg到sensor_msgs/PointCloud2

在做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

就可以实现类型的转换啦