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