Skip to content

Instantly share code, notes, and snippets.

@marcoesposito1988
Created September 24, 2019 10:22
Show Gist options
  • Save marcoesposito1988/d6964fb85f9f8c35326489def444e7f6 to your computer and use it in GitHub Desktop.
Save marcoesposito1988/d6964fb85f9f8c35326489def444e7f6 to your computer and use it in GitHub Desktop.
Make /tf_static usable in rosbags
import rospy
import rosbag
inpath = '/path/to/original/bag.bag'
outpath = '/path/to/output/bag.bag'
trigger = lambda topic, msg, _: topic == '/tf' and msg.transforms[0].header.frame_id == '/world'
inbag = rosbag.Bag(inpath)
tf_static_messages = list(inbag.read_messages(('/tf_static')))
with rosbag.Bag(outpath, 'w') as outbag:
for topic, msg, t in inbag:
if trigger(topic, msg, t):
for tsm in tf_static_messages:
for tsmtr in tsm.message.transforms:
tsmtr.header.stamp = t
outbag.write('/tf', tsm.message, t-rospy.Duration(0.1))
outbag.write(topic, msg, t)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment