Python解析rosbag数据

前言

使用Python的rosbags库从rosbag1文件中提取点云和图片数据。

导入及函数准备

难点主要是解析msg的data数据，注意以下几点：

1. 所有的点是一个铺平的ndarray，每个元素代表一个字节的数据
2. 每msg.point_step个字节合起来代表一个点
3. 在一个点/周期内，存放着该点的所有msg.fields的数据
4. 每个field占几个字节，都在msg.fields里有说明

import struct
from pathlib import Path

import numpy as np
import pandas as pd
from PIL import Image
from rosbags.highlevel import AnyReader


def generate_pcd_header(
    fields,
    sizes,
    types,
    n_points
):
    headers = (
        "# .PCD v0.7 - Point Cloud Data file format\n"
        "VERSION 0.7\n"
        f"FIELDS {
     ' '.join(fields)}\n"
        f"SIZE {
     ' '.join(map(str, sizes))}\n"
        f"TYPE {
     ' '.join(types)}\n"
        f"COUNT {
     ' '.join(['1'] * len(fields))}\n"
        f"WIDTH {
     n_points}\n"
        "HEIGHT 1\n"
        "VIEWPOINT 0 0 0 1 0 0 0\n"
        f"POINTS {
     n_points}\n"
        "DATA binary\n"
    )

    return headers

def parse_msg_pcd(
    msg,
    needed_fields = ('x', 'y', 'z', 'intensity')
):
    datatype_byte_sizes =