Convert SFly UAV datasets to a ROS bag

launch_file.launch

<!--
Some packages do not handle compressed images correctly. We use the image_transport
package to republished the JPEGs stored in the bags as Image messages. This is an
example launch file that does just that.
-->

<launch>
    <node name="bag_play" pkg="rosbag" type="play"
          args="--clock /home/ubuntu/hoveringDown.bag">
    </node>

	<!-- republish the jpeg images as a proper (uncompressed) image
		 on the fly -->
    <node name="image_republish" pkg="image_transport" type="republish"
    	  args="compressed in:=/camera/image out:=/camera2">
    </node>

</launch>

sfly_2_rosbag.py

#!/usr/bin/env python

"""
A script to convert the UAV datasets in [1] to a ROS bag containing the Vicon
ground truth, the IMU measurements, and the (undistorted) images. Run it in the
folder created when unzipping each archive.

[1] https://sites.google.com/site/gimheelee/home/mavdataset
"""

import os, glob

import pandas as pd
import numpy as np

import rospy, rosbag
from sensor_msgs.msg import Imu, CameraInfo
from geometry_msgs.msg import PoseStamped, TransformStamped
from sensor_msgs.msg import CompressedImage
from tf.transformations import *
from tf.msg import tfMessage

ROOT_FOLDER = '.'

dataset_name = os.path.split(os.getcwd())[-1]
outbag_fname = "{}.bag".format(dataset_name)

imu_fname = glob.glob(os.path.join(ROOT_FOLDER, 'imu*.txt'))[0]
imus = pd.read_csv(imu_fname, header=None, delim_whitespace=True, skiprows=4)
imus.columns = ['timestamp', 'acc_x', 'acc_y', 'acc_z', 'ang_x', 'ang_y', 'ang_z', 'r', 'p', 'y', 'mag_x', 'mag_y', 'mag_z']

vicon_fname = glob.glob(os.path.join(ROOT_FOLDER, 'vicon*.txt'))[0]
vicon = pd.read_csv(vicon_fname, header=None, delim_whitespace=True, skiprows=4)
vicon.columns = ['timestamp', 'x', 'y', 'z', 'r_r', 'r_p', 'r_y', 'ang_x', 'ang_y', 'ang_z']

imgs_fname = glob.glob(os.path.join(ROOT_FOLDER, 'imagesUndistort*.txt'))[0]
imgs = pd.read_csv(imgs_fname, header=None, delim_whitespace=True, skiprows=3)
imgs.columns = ['timestamp', 'file_name']
imgs['file_path'] = imgs.file_name.apply(lambda x: os.path.join(glob.glob('imagesUndistort*/')[0], x))

with rosbag.Bag(outbag_fname, 'w') as outbag:
    for i, imu_row in imus.iterrows():
        imu_msg = Imu()
        imu_msg.header.stamp = rospy.Time.from_sec(imu_row.timestamp)
        imu_msg.linear_acceleration.x = imu_row.acc_x
        imu_msg.linear_acceleration.y = imu_row.acc_y
        imu_msg.linear_acceleration.z = imu_row.acc_z
        imu_msg.angular_velocity.x = imu_row.ang_x
        imu_msg.angular_velocity.y = imu_row.ang_y
        imu_msg.angular_velocity.z = imu_row.ang_z
        outbag.write('/imu', imu_msg, imu_msg.header.stamp)

    for i, vicon_row in vicon.iterrows():
        q = quaternion_from_matrix(
            euler_matrix(vicon_row.r_r, vicon_row.r_p, vicon_row.r_y))

        pose_msg = PoseStamped()
        pose_msg.header.stamp = rospy.Time.from_sec(vicon_row.timestamp)
        pose_msg.pose.position.x = vicon_row.x
        pose_msg.pose.position.y = vicon_row.y
        pose_msg.pose.position.z = vicon_row.z
        pose_msg.pose.orientation.x = q[0]
        pose_msg.pose.orientation.y = q[1]
        pose_msg.pose.orientation.z = q[2]
        pose_msg.pose.orientation.w = q[3]
        outbag.write('/vicon', pose_msg, pose_msg.header.stamp)

        transf = TransformStamped()
        transf.header.stamp = rospy.Time.from_sec(vicon_row.timestamp)
        transf.header.frame_id = 'world'
        transf.child_frame_id = 'b'
        transf.transform.translation.x = vicon_row.x
        transf.transform.translation.y = vicon_row.y
        transf.transform.translation.z = vicon_row.z
        transf.transform.rotation.x = q[0]
        transf.transform.rotation.y = q[1]
        transf.transform.rotation.z = q[2]
        transf.transform.rotation.w = q[3]
        tf_msg = tfMessage()
        tf_msg.transforms = [transf]
        outbag.write('/tf', tf_msg, transf.header.stamp)

    for i, img_row in imgs.iterrows():
        img_msg = CompressedImage()
        img_msg.header.stamp = rospy.Time.from_sec(img_row.timestamp)
        img_msg.format = 'jpeg'
        img_msg.data = open(img_row.file_path).read()
        outbag.write('/camera/image/compressed', img_msg, img_msg.header.stamp)