tf_bag_to_tum.py

#!/usr/bin/python
import argparse
import rosbag
import tf_bag
import tf2_py

parser = argparse.ArgumentParser()
parser.add_argument("filepath", type=str)
args = parser.parse_args()

frame1_id = "map"
frame2_id = "base_link"
trigger_orig_frame = "odom"

class BagWrapper(rosbag.Bag):
    def __iter__(self):
        return self.read_messages(topics=["/tf", "/static_tf"])

class BagTfTransformer(tf_bag.BagTfTransformer):
    pass

bag = BagWrapper(args.filepath)
bag_transformer = BagTfTransformer(bag)

gen = bag_transformer.lookupTransformWhenTransformUpdates(frame1_id, frame2_id, trigger_orig_frame)

end = False
while not end:
    try:
        for t, (trans, rot) in gen:
            print("{} {} {} {} {} {} {} {}".format(t.to_sec(), trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3]))
        end = True
    except tf2_py.ExtrapolationException as e:
        pass # print(e)