Created
September 24, 2019 10:22
-
-
Save marcoesposito1988/d6964fb85f9f8c35326489def444e7f6 to your computer and use it in GitHub Desktop.
Make /tf_static usable in rosbags
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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