ros保存topic中点云 从rosbag中提取点云

有时候需要把ros中PointCloud2格式的点云保存成本地ply文件来进一步分析。

整理一个python脚本来完成此任务：

1. 核心脚本 bag_pointcloud_to_plys.py

改编自如下链接的保存图像的脚本
https://gist.github.com/wngreene/835cda68ddd9c5416defce876a4d7dd9：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import os
import argparse

import rosbag
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2

def save_pointcloud_to_ply(filename, pc_msg):
    pc = pc2.read_points(pc_msg, skip_nans=True, field_names=("x", "y", "z"))
    pc_l = list(pc)
    len_points = len(pc_l)

    with open(filename, 'w') as f:
        f.write('ply\n')
        f.write('format ascii 1.0\n')
        element_vertex = 'element vertex %d \n' % len_points
        f.write(element_vertex)
        f.write('property float x\n')
        f.write('property float y\n')
        f.write('property float z\n')
        f.write('element face 0\n')
        f.write('property list uchar int vertex_index\n')
        f.write('end_header\n')

        for p in pc_l:
            f.write('%f %f %f\n' % (p[0],p[1],p[2]))

def main():
    """Extract a folder of pointclouds from a rosbag.
    """
    parser = argparse.ArgumentParser(description="Extract pointclouds from a ROS bag.")
    parser.add_argument("bag_file", help="Input ROS bag.")
    parser.add_argument("output_dir", help="Output directory.")
    parser.add_argument("pointcloud_topic", help="Pointcloud topic.")

    args = parser.parse_args()

    print "Extract pointclouds from %s on topic %s into %s" % (args.bag_file,
                                                            args.pointcloud_topic, args.output_dir)

    bag = rosbag.Bag(args.bag_file, "r")
    count = 0
    for topic, msg, t in bag.read_messages(topics=[args.pointcloud_topic]):
        filename = os.path.join(args.output_dir, "pointcloud%06i.ply" % count)
        save_pointcloud_to_ply(filename, msg)

        print "Wrote pointcloud %i" % count

        count += 1

    bag.close()

    return

if __name__ == '__main__':
    main()

有三个参数，分别如下：

1. bag文件名
2. 保存点云的文件夹名
3. topic名

2. 步骤

1. 首先用如下命令得到bag文件

rosbag recored -O bag_name.bag /topic1_name /topic2_name

Ctrl-C即可中止录制

2. 建立点云保存目录pointclouds

则有文件和目录如下：

- bag_name.bag          // 这是上个步骤录制的rosbag
- bag_pointcloud_to_plys.py
- pointclouds           // 目录，用于存放bag里的图片

3. 运行如下命令：

python bag_pointcloud_to_plys.py bag_name.bag pointclouds /topic1_name

则会在pointclouds目录里有解析出来的ply点云文件

如果需要保存为pcd文件，可按照pcd文件格式来保存即可。
pcd文件格式可参考https://www.jianshu.com/p/10b96e52a996