Skip to content

Instantly share code, notes, and snippets.

@nicolov
Last active December 17, 2015 03:49
Show Gist options
  • Save nicolov/ad7f24ebdf62c00c8119 to your computer and use it in GitHub Desktop.
Save nicolov/ad7f24ebdf62c00c8119 to your computer and use it in GitHub Desktop.
Convert SFly UAV datasets to a ROS bag
<!--
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>
#!/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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment