将原始影像、IMU、LiDAR数据转换成ROS Bag文件

Jun 15,2022   8774 words   32 min

Tags: SLAM

在之前的这篇博客中,我们重点实现的是将ROS Bag包拆解成各种各样的数据。但在实际应用中,我们往往可能还有“相反”的需求——将各种各样的数据组合成ROS Bag文件。这篇博客也就是在这样的背景下诞生的。目前我手边有一些多传感器数据(相机、IMU、LiDAR),需要将它们转化成ROS Bag的形式。之前零零散散写过一些脚本,但都“散落在各地”,一时之间难以找到了。为了以后更方便使用,这篇博客就把这些脚本汇总一下。为了便于查找,本篇博客的所有代码还是放到了这个Github项目中。

其实将各种各样的数据转换成ROS Bag的核心思路都是一样的。首先是读取数据,然后调用ROS的Python接口构造Topic与Message,最后还是调用ROS接口,将这些Message保存下来。当然,根据需求,可以转换到不同的Message类型,一些常用的Message类型可见这篇博客

1.将影像序列转换成Bag文件

按照常规流程,我们可以用OpenCV读取影像,然后调用ROS的CvBridge实现类型转换,最后调用函数写入ROS Bag,核心代码如下,对应Github项目中的genBagFromImg.py文件。

# coding=utf-8
import rosbag
import sys
import os
import numpy as np
import cv2
from cv_bridge import CvBridge
import rospy


def findFiles(root_dir, filter_type, reverse=False):
    """
    在指定目录查找指定类型文件 -> paths, names, files
    :param root_dir: 查找目录
    :param filter_type: 文件类型
    :param reverse: 是否返回倒序文件列表,默认为False
    :return: 路径、名称、文件全路径
    """

    separator = os.path.sep
    paths = []
    names = []
    files = []
    for parent, dirname, filenames in os.walk(root_dir):
        for filename in filenames:
            if filename.endswith(filter_type):
                paths.append(parent + separator)
                names.append(filename)
    for i in range(paths.__len__()):
        files.append(paths[i] + names[i])
    print(names.__len__().__str__() + " files have been found.")
    
    paths = np.array(paths)
    names = np.array(names)
    files = np.array(files)

    index = np.argsort(files)

    paths = paths[index]
    names = names[index]
    files = files[index]

    paths = list(paths)
    names = list(names)
    files = list(files)
    
    if reverse:
        paths.reverse()
        names.reverse()
        files.reverse()
    return paths, names, files

if __name__ == '__main__':
    img_dir = sys.argv[1]   # 影像所在文件夹路径
    img_type = sys.argv[2]  # 影像类型
    topic_name = sys.argv[3]    # Topic名称
    bag_path = sys.argv[4]  # 输出Bag路径

    bag_out = rosbag.Bag(bag_path,'w')

    cb = CvBridge()

    paths, names, files = findFiles(img_dir,img_type)
    for i in range(len(files)):
        print(i,'/',len(files))

        frame_img = cv2.imread(files[i])
        timestamp = int(names[i].split(".")[0])/10e8
        print(timestamp)

        ros_ts = rospy.rostime.Time.from_sec(timestamp)
        ros_img = cb.cv2_to_imgmsg(frame_img,encoding='bgr8')
        ros_img.header.stamp = ros_ts
        bag_out.write(topic_name,ros_img,ros_ts)
    
    bag_out.close()

如下图所示,我们有一系列影像,影像名称即为其时间戳。 运行上述代码,我们就可以获得生成的ROS Bag文件,可以利用rosbag info查看信息,如下。 这样,便完成了将影像序列转换成Bag的功能。如果有多个影像序列呢?比如双目影像序列。其实也非常简单。只要在上面的代码中再加一个Topic即可。

2.将IMU数据转换成Bag文件

类似的,将IMU数据转换成ROS Bag代码如下,对应Github项目中的genBagFromIMU.py文件。

# coding=utf-8
import rosbag
import sys
import os
import numpy as np
from sensor_msgs.msg import Imu
import rospy
from geometry_msgs.msg import Vector3


def findFiles(root_dir, filter_type, reverse=False):
    """
    在指定目录查找指定类型文件 -> paths, names, files
    :param root_dir: 查找目录
    :param filter_type: 文件类型
    :param reverse: 是否返回倒序文件列表,默认为False
    :return: 路径、名称、文件全路径
    """

    separator = os.path.sep
    paths = []
    names = []
    files = []
    for parent, dirname, filenames in os.walk(root_dir):
        for filename in filenames:
            if filename.endswith(filter_type):
                paths.append(parent + separator)
                names.append(filename)
    for i in range(paths.__len__()):
        files.append(paths[i] + names[i])
    print(names.__len__().__str__() + " files have been found.")
    
    paths = np.array(paths)
    names = np.array(names)
    files = np.array(files)

    index = np.argsort(files)

    paths = paths[index]
    names = names[index]
    files = files[index]

    paths = list(paths)
    names = list(names)
    files = list(files)
    
    if reverse:
        paths.reverse()
        names.reverse()
        files.reverse()
    return paths, names, files

def readIMU(imu_path):
    timestamps = []
    wxs = []
    wys = []
    wzs = []
    axs = []
    ays = []
    azs = []
    fin = open(imu_path, 'r')
    fin.readline()
    line = fin.readline().strip()
    while line:
        parts = line.split(",")
        ts = float(parts[0])/10e8
        wx = float(parts[1])
        wy = float(parts[2])
        wz = float(parts[3])
        ax = float(parts[4])
        ay = float(parts[5])
        az = float(parts[6])
        timestamps.append(ts)

        wxs.append(wx)
        wys.append(wy)
        wzs.append(wz)
        axs.append(ax)
        ays.append(ay)
        azs.append(az)
        line = fin.readline().strip()
    return timestamps, wxs, wys, wzs, axs, ays, azs


if __name__ == '__main__':
    imu_path = sys.argv[1]  # IMU数据文件路径
    imu_topic_name = sys.argv[2]    # Topic名称
    bag_path = sys.argv[3]  # 输出Bag路径

    bag_out = rosbag.Bag(bag_path,'w')

    imu_ts, wxs, wys, wzs, axs, ays, azs = readIMU(imu_path)
    imu_msg = Imu()
    angular_v = Vector3()
    linear_a = Vector3()

    for i in range(len(imu_ts)):
        imu_ts_ros = rospy.rostime.Time.from_sec(imu_ts[i])
        imu_msg.header.stamp = imu_ts_ros
        
        angular_v.x = wxs[i]
        angular_v.y = wys[i]
        angular_v.z = wzs[i]

        linear_a.x = axs[i]
        linear_a.y = ays[i]
        linear_a.z = azs[i]

        imu_msg.angular_velocity = angular_v
        imu_msg.linear_acceleration = linear_a

        bag_out.write(imu_topic_name, imu_msg, imu_ts_ros)
        print('imu:',i,'/',len(imu_ts))
    
    bag_out.close()

这里我们的原始IMU数据以EuRoC格式存储(如果不清楚具体格式参考这篇博客),转换完成以后,可以查看Bag文件信息,如下。

3.将相机和IMU数据合并到一个Bag文件

有了上面的基础,其实将多源数据合并到一个Bag文件也就比较简单了,只要分别把各自的流程走一遍即可,如下,对应Github项目中的genBagFromImgAndIMU.py文件。

# coding=utf-8
import rosbag
import sys
import os
import numpy as np
import cv2
from sensor_msgs.msg import Image, Imu
from cv_bridge import CvBridge
import rospy
from geometry_msgs.msg import Vector3


def findFiles(root_dir, filter_type, reverse=False):
    """
    在指定目录查找指定类型文件 -> paths, names, files
    :param root_dir: 查找目录
    :param filter_type: 文件类型
    :param reverse: 是否返回倒序文件列表,默认为False
    :return: 路径、名称、文件全路径
    """

    separator = os.path.sep
    paths = []
    names = []
    files = []
    for parent, dirname, filenames in os.walk(root_dir):
        for filename in filenames:
            if filename.endswith(filter_type):
                paths.append(parent + separator)
                names.append(filename)
    for i in range(paths.__len__()):
        files.append(paths[i] + names[i])
    print(names.__len__().__str__() + " files have been found.")
    
    paths = np.array(paths)
    names = np.array(names)
    files = np.array(files)

    index = np.argsort(files)

    paths = paths[index]
    names = names[index]
    files = files[index]

    paths = list(paths)
    names = list(names)
    files = list(files)
    
    if reverse:
        paths.reverse()
        names.reverse()
        files.reverse()
    return paths, names, files

def readIMU(imu_path):
    timestamps = []
    wxs = []
    wys = []
    wzs = []
    axs = []
    ays = []
    azs = []
    fin = open(imu_path, 'r')
    fin.readline()
    line = fin.readline().strip()
    while line:
        parts = line.split(",")
        ts = float(parts[0])/10e8
        wx = float(parts[1])
        wy = float(parts[2])
        wz = float(parts[3])
        ax = float(parts[4])
        ay = float(parts[5])
        az = float(parts[6])
        timestamps.append(ts)

        wxs.append(wx)
        wys.append(wy)
        wzs.append(wz)
        axs.append(ax)
        ays.append(ay)
        azs.append(az)
        line = fin.readline().strip()
    return timestamps, wxs, wys, wzs, axs, ays, azs


if __name__ == '__main__':
    img_dir = sys.argv[1]   # 影像所在文件夹路径
    img_type = sys.argv[2]  # 影像文件类型
    img_topic_name = sys.argv[3]    # 影像Topic名称
    imu_path = sys.argv[4]  # IMU文件路径
    imu_topic_name = sys.argv[5]    # IMU Topic名称
    bag_path = sys.argv[6]  # Bag文件输出路径

    bag_out = rosbag.Bag(bag_path,'w')

    # 先处理IMU数据
    imu_ts, wxs, wys, wzs, axs, ays, azs = readIMU(imu_path)
    imu_msg = Imu()
    angular_v = Vector3()
    linear_a = Vector3()

    for i in range(len(imu_ts)):
        imu_ts_ros = rospy.rostime.Time.from_sec(imu_ts[i])
        imu_msg.header.stamp = imu_ts_ros
        
        angular_v.x = wxs[i]
        angular_v.y = wys[i]
        angular_v.z = wzs[i]

        linear_a.x = axs[i]
        linear_a.y = ays[i]
        linear_a.z = azs[i]

        imu_msg.angular_velocity = angular_v
        imu_msg.linear_acceleration = linear_a

        bag_out.write(imu_topic_name, imu_msg, imu_ts_ros)
        print('imu:',i,'/',len(imu_ts))

    # 再处理影像数据
    paths, names, files = findFiles(img_dir,img_type)
    cb = CvBridge()
    
    for i in range(len(files)):
        print('image:',i,'/',len(files))

        frame_img = cv2.imread(files[i])
        timestamp = int(names[i].split(".")[0])/10e8
        print(timestamp)

        ros_ts = rospy.rostime.Time.from_sec(timestamp)
        ros_img = cb.cv2_to_imgmsg(frame_img,encoding='bgr8')
        ros_img.header.stamp = ros_ts
        bag_out.write(img_topic_name,ros_img,ros_ts)
    
    bag_out.close()

运行上述代码以后,生成的Bag文件信息如下。 可以看到,这个Bag文件里已经有两个Topic了。当然,当转换完数据以后,你可能还有其它的需求,比如改变Message的帧率等等,这就可以使用之前写好的很多脚本了,点击查看

4.将LiDAR数据转换成Bag文件

将LiDAR数据转换成Bag文件当然可以用上面提到的方法,自己先解析再转换。但对于比如Velodyne这种常用的传感器,ROS官方其实已经写好了相关的转换功能,我们直接使用即可。 这里我们原始的LiDAR数据以pcap格式存储。核心步骤为,我们用ROS提供的程序解析并发布原始数据,然后,我们利用rosbag record录制发布的Topic,即可实现数据的转换。在进入正题之前,不妨先学习一下如何利用ROS可视化pcap数据,如果这个会了,录制包自然是十分简单的事情。

4.1 ROS读取PCAP文件并用Rviz可视化
4.1.1 环境配置

输入如下指令,依次安装libpcap和ROS中专门用于读取Velodyne数据的包。

sudo apt-get install libpcap-dev
sudo apt-get install ros-kinetic-velodyne*

安装好以后,环境配置就完成了。如果是Ubuntu 18.04,那么应该是ros-melodic-velodyne*

4.1.2 ROS读取PCAP数据

输入如下命令(每条命令都需要打开一个新终端),正常情况下就可以读取PCAP数据了。

roslaunch velodyne_pointcloud VLP16_points.launch
rosrun velodyne_driver velodyne_node _model:=VLP16 _pcap:=vlp16.pcap _read_once:=true _pcap_time:=true

正常情况下,这个节点会发布一个叫做/velodyne_points的Topic,消息类型为sensor_msgs/PointCloud2,如下图所示。 关于这个消息类型,在之前的这篇博客中介绍过了。这样我们就成功利用ROS读取了PCAP中的LiDAR数据。接下来的操作就很简单了。

4.1.3 Rviz可视化点云

我们打开Rviz,订阅/velodyne_points这个Topic理论上就可以显示点云,如下图所示。 但你可能会遇到错误,如下图所示。 说是map坐标系和velodyne坐标系之间的变换不存在。这个错误如果用Rviz比较多的话应该可以感觉到是全局坐标系名称有问题。简单来说就是,打开Rviz,默认的global frame叫做map,而我们数据用的坐标系名称是velodyne。但问题恰恰在于,我们并没有定义mapvelodyne之间的变换,所以显示不成功。解决办法也非常简单,那就是把全局坐标系名称改为velodyne即可。这样就不存在坐标系之间的变换了。改完之后,就可以正常可视化轨迹,如下所示。

4.2 ROS读取PCAP文件并转换成Bag格式

有了上面的基础,转换成Bag就十分简单了,主要步骤都是一样的,不同之处在于这里不再是用Rviz可视化,而是用rosbag工具直接录制/velodyne_points这个Topic(当然你在录制的时候用Rviz可视化一下也完全没问题),所以在终端输入如下命令即可(每条命令都需要打开一个新终端)。

# 启动转换节点
roslaunch velodyne_pointcloud VLP16_points.launch
# 准备录制
rosbag record -O vlp16.bag /velodyne_points
# 读取PCAP数据
rosrun velodyne_driver velodyne_node _model:=VLP16 _pcap:=vlp16.pcap _read_once:=true _pcap_time:=true
# 可视化点云(可选)
rviz

通过这种方式,就可以将PCAP转换成Bag文件了,我们可以查看Bag文件的信息,如下。 可以看到,这样我们就成功地将pcap文件转换成了ROS Bag文件。不过这样会个潜在的问题,那就是数据帧的时间戳并非采集时刻的,而是录制时候的。如果说你对于LiDAR数据的时间戳无所谓,不需要做数据对齐,那还好。如果有以上需求,就需要解决这个问题。这个问题理论上说可以通过上面命令里的_pcap_time:=true参数解决,但是在我测试的数据上没有效果。如果有什么更好的解决办法,欢迎在评论区交流。

本文作者原创,未经许可不得转载,谢谢配合

返回顶部