如何订阅并保存rosbag中的点云消息？(C++)

最新推荐文章于 2025-04-11 17:26:02 发布

点解质

最新推荐文章于 2025-04-11 17:26:02 发布

阅读量2k

点赞数

CC 4.0 BY-SA版权

分类专栏： ROS

本文链接：https://blog.youkuaiyun.com/yuanshine/article/details/93757813

ROS 专栏收录该内容

3 篇文章

订阅专栏

本文介绍了一种在ROS环境下处理并保存激光雷达点云数据的方法。通过订阅/lidar_topic主题的消息，利用PCL库将接收到的sensor_msgs::PointCloud2类型的数据转换为pcl::PointCloud<pcl::PointXYZI>类型，并将其保存为.pcd文件。此方法适用于需要定期存档或进一步分析点云数据的场景。

#include "pcl/io/pcd_io.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"

static size_t counter = 0;

void SubscribePointCloud(const sensor_msgs::PointCloud2ConstPtr& lidar_message) {
  pcl::PointCloud<pcl::PointXYZI> point_cloud;
  pcl::fromROSMsg(*lidar_message, point_cloud);
  
  counter++;
  std::string file_name = "point_cloud_" + std::to_string(counter) + ".pcd";
  pcl::io::savePCDFile(file_name, point_cloud);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "point_cloud_subscriber");
  ros::NodeHandle node_handle;
  ros::Subscriber point_cloud_sub = 
    node_handle.subscribe("/hdl64/velodyne_points", 1, SubscribePointCloud);
  ros::spin();

  return 0;
}