Skip to content

Instantly share code, notes, and snippets.

Last active December 17, 2015 03:49
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
What would you like to do?
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.
<node name="bag_play" pkg="rosbag" type="play"
args="--clock /home/ubuntu/hoveringDown.bag">
<!-- 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">
#!/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.
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
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' = 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