-
-
Save daoran/fd7279376b6fcb1251094359c2c9de1f to your computer and use it in GitHub Desktop.
This file contains hidden or 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
| #!/usr/bin/env python | |
| import sys | |
| import roslib; | |
| import rospy | |
| import rosbag | |
| from rospy import rostime | |
| import argparse | |
| import os | |
| # Original: | |
| # https://github.com/udacity/self-driving-car/blob/master/image-localization/community-code/roboauto/scripts/bagmerge.py | |
| def parse_args(): | |
| parser = argparse.ArgumentParser( | |
| prog = 'bagmerge.py', | |
| description='Merges two bagfiles.') | |
| parser.add_argument('-o', type=str, help='name of the output file', | |
| default = None, metavar = "output_file") | |
| parser.add_argument('-t', type=str, help='topics which should be merged to the main bag', | |
| default = None, metavar = "topics") | |
| parser.add_argument('-p', type=str, help='optional prefix for the second bag', | |
| default = '', metavar = "secondary_prefix") | |
| parser.add_argument('-i', help='reindex bagfile', | |
| default = False, action="store_true") | |
| parser.add_argument('main_bagfile', type=str, help='path to a bagfile, which will be the main bagfile') | |
| parser.add_argument('bagfile', type=str, help='path to a bagfile which should be merged to the main bagfile') | |
| args = parser.parse_args() | |
| return args | |
| def get_next(bag_iter, reindex = False, | |
| main_start_time = None, start_time = None, | |
| topics = None): | |
| try: | |
| result = bag_iter.next() | |
| if topics != None: | |
| while not result[0] in topics: | |
| result = bag_iter.next() | |
| if reindex: | |
| return (result[0], result[1], | |
| result[2] - start_time + main_start_time) | |
| return result | |
| except StopIteration: | |
| return None | |
| def merge_bag(main_bagfile, bagfile, outfile = None, topics = None, | |
| reindex = True, prefix = ''): | |
| #get min and max time in bagfile | |
| main_limits = get_limits(main_bagfile) | |
| limits = get_limits(bagfile) | |
| #check output file | |
| if outfile == None: | |
| pattern = main_bagfile + "_merged_%i.bag" | |
| outfile = main_bagfile + "_merged.bag" | |
| index = 0 | |
| while (os.path.exists(outfile)): | |
| outfile = pattern%index | |
| index += 1 | |
| #output some information | |
| print "merge bag %s in %s"%(bagfile, main_bagfile) | |
| print "topics filter: ", topics | |
| print "writing to %s."%outfile | |
| if prefix: | |
| print "secondary prefix: %s"%prefix | |
| #merge bagfile | |
| outbag = rosbag.Bag(outfile, 'w') | |
| main_bag = rosbag.Bag(main_bagfile).__iter__() | |
| bag = rosbag.Bag(bagfile).__iter__() | |
| main_next = get_next(main_bag) | |
| next = get_next(bag, reindex, main_limits[0], limits[0], topics) | |
| try: | |
| while main_next != None or next != None: | |
| if main_next == None: | |
| outbag.write(prefix + next[0], next[1], next[2]) | |
| next = get_next(bag, reindex, main_limits[0], limits[0], topics) | |
| elif next == None: | |
| outbag.write(main_next[0], main_next[1], main_next[2]) | |
| main_next = get_next(main_bag) | |
| elif next[2] < main_next[2]: | |
| outbag.write(prefix + next[0], next[1], next[2]) | |
| next = get_next(bag, reindex, main_limits[0], limits[0], topics) | |
| else: | |
| outbag.write(main_next[0], main_next[1], main_next[2]) | |
| main_next = get_next(main_bag) | |
| finally: | |
| outbag.close() | |
| def get_limits(bagfile): | |
| print "Determine start and end index of %s..."%bagfile | |
| end_time = None | |
| start_time = None | |
| for topic, msg, t in rosbag.Bag(bagfile).read_messages(): | |
| if start_time == None or t < start_time: | |
| start_time = t | |
| if end_time == None or t > end_time: | |
| end_time = t | |
| return (start_time, end_time) | |
| if __name__ == "__main__": | |
| args = parse_args() | |
| print args | |
| if args.t != None: | |
| args.t = args.t.split(',') | |
| merge_bag(args.main_bagfile, | |
| args.bagfile, | |
| outfile = args.o, | |
| topics = args.t, | |
| reindex = args.i, | |
| prefix = args.p) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment