读取激光雷达点云bin文件并使用rosbag保存

读取激光雷达点云bin文件并保存为rosbag

Apollo Scape数据是以bin文件来储存点云的，从kitti上找到了将kitti数据转到rosbag的工具包，但不想拘泥与kitti的数据形式，所以就自己写了一个小工具。
先附上Apollo Scape的网址：
http://apolloscape.auto/tracking.htm
以及kitti的工具包：
https://github.com/tomas789/kitti2bag
下面开始正文 ：

数据准备

apollo的数据是以bin文件的形式存储在不同的frame文件夹下面的，我们将每个bin文件的地址找到保存在一个文档里，方便之后在ros生成消息时找到。

    cur_path = os.path.dirname(os.path.realpath(__file__))
    yaml_path = os.path.join(cur_path, 'config.yaml')

    with open(yaml_path, 'r', encoding='utf-8') as f:
        config = f.read()
        config = yaml.load(config)

    result_path = config['result_path']
    list_path = result_path + 'list.txt'
    if os.path.exists(list_path):
        os.remove(list_path)
    if not os.path.exists(result_path):
        raise Exception('ERROR: NO EXIST RESULT PATH!')

    dataset_path = ''
    dataset_list = []
    num_datalist = 0
    if config['train'] == False and config['data_class_name'] == 'apollo':
        dataset_path = 'detection_test/'
        dataset_path = config['root_path'] + dataset_path
        if not os.path.exists(dataset_path):
            raise Exception('ERROR: NO EXIST DATASET PATH!')
        with open(list_path, 'w+') as f:
            for pcd_id in os.listdir(dataset_path):
                pcd_path = os.path.join(dataset_path, pcd_id)
                for frame_id in os.listdir(pcd_path):
                    frame_path = os.path.join(pcd_path, frame_id)
                    for bin_id in os.listdir(frame_path):
                        bin_path = os.path.join(frame_path, bin_id)
                        f.writelines(bin_path + '\n')
                        dataset_list.append(bin_path)
                        num_datalist += 1

创建bag包，别忘记close：

bag = rosbag.Bag(f=result_path + 'dataset.bag', mode='w')

python下的ros使用

之前用的比较多的开发环境是c++，这次开始学习python下的ros，有一些不同的。发布消息和初始化节点：

def publisher(dataset_list, num):
    pub = rospy.Publisher('bin_to_bag', PointCloud2, queue_size=10)
    rospy.init_node('publisher', anonymous=True)

    header = std_msgs.msg.Header()
    header.frame_id = 'tobag'
    header.stamp = rospy.get_rostime()
    fields = [PointField('x', 0, PointField.FLOAT32, 1),
              PointField('y', 4, PointField.FLOAT32, 1),
              PointField('z', 8, PointField.FLOAT32, 1),
              PointField('intensity', 12, PointField.FLOAT32, 1)]
    with progressbar.ProgressBar(max_value=num) as bar:
        for i in range(num):
            cloud = read_bin_file(dataset_list[i])
            msg_point_cloud = point_cloud2.create_cloud(header=header,
                                                        fields=fields,
                                                        points=cloud)
            pub.publish(msg_point_cloud)
            bag.write('bin_to_bag', msg_point_cloud)
            bar.update(i)

这里的PointCloud2是sensor_msg.msg中的消息类型。要生成这个消息类型可以用到point_cloud2中的create_cloud方法，要有header，PointField，points。header 和c++一样，附了一个frame_id和stamp。PointField是要给出点云特征，因为需要用到强度，所以这里定义了4个特征：x，y，z，intensity。如果不需要强度，可以直接用create_cloud_xyz32。
读bin文件生成一帧点云：

def read_bin_file(path):
    cloud_np = np.fromfile(path, dtype=np.float32, count=-1).reshape([-1, 4])
    size = np.size(cloud_np, 0)
    cloud = []
    for i in range(size):
        point = [cloud_np[i, 0], cloud_np[i, 1],
                 cloud_np[i, 2], cloud_np[i, 3]]
        cloud.append(point)
    return cloud

效果