tf_filter_bag.py

#!/usr/bin/python
import rospy
import rosbag
import os
import sys
import argparse

def remove_tf(inbag,outbag,frame_ids):
  rospy.loginfo('   Processing input bagfile: %s', inbag)
  rospy.loginfo('  Writing to output bagfile: %s', outbag)
  rospy.loginfo('         Removing frame_ids: %s', ' '.join(frame_ids))

  outbag = rosbag.Bag(outbag,'w')
  for topic, msg, t in rosbag.Bag(inbag,'r').read_messages():
      if topic == "/tf":
          new_transforms = []
          for transform in msg.transforms:
              if transform.child_frame_id not in frame_ids:
                  new_transforms.append(transform)
          msg.transforms = new_transforms
      outbag.write(topic, msg, t)
  rospy.loginfo('Closing output bagfile and exit...')
  outbag.close();

if __name__ == "__main__":
  parser = argparse.ArgumentParser(
      description='removes all transforms from the /tf topic that contain one of the given frame_ids in the header as the child.')
  parser.add_argument('-i', metavar='INPUT_BAGFILE', required=True, help='input bagfile')
  parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile')
  parser.add_argument('-f', metavar='FRAME_ID', required=True, help='frame_id(s) of the transforms to remove from the /tf topic', nargs='+')
  args = parser.parse_args()

  try:
    remove_tf(args.i,args.o,args.f)
  except Exception, e:
    import traceback
    traceback.print_exc()