def rename_topic(intopic, inbag, outtopic, outbag): rebag = rosrecord.Rebagger(outbag) for (topic, msg, t) in rosrecord.logplayer(inbag, raw=True): if rospy.is_shutdown(): break if topic == intopic: rebag.add(outtopic, msg, t, raw=True) else: rebag.add(topic, msg, t, raw=True) rebag.close()
def generate_data(): bag = rosrecord.Rebagger("test/migrated_explicit_gen2.bag") m = MigratedExplicit(None, 17, 58.2, "aldfkja", 82) bag.add("migrated_explicit", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/migrated_implicit_gen2.bag") m = MigratedImplicit(None, 34, 16.32, "kljene", MigratedExplicit(None, 17, 58.2, "aldfkja", 82)) bag.add("migrated_implicit", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/migrated_mixed_gen2.bag") m = MigratedMixed( None, MigratedImplicit(None, 34, 16.32, "kljene", MigratedExplicit(None, 17, 58.2, "aldfkja", 82))) bag.add("migrated_mixed", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/partially_migrated_gen2.bag") m = PartiallyMigrated(40, MigratedExplicit(None, 17, 58.2, "aldfkja", 82)) bag.add("partially_migrated", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/renamed_gen2.bag") m = Renamed2(2.17, [8, 2, 5]) bag.add("renamed", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/convergent_gen2.bag") m = Convergent(1.2, 3.4, 5.6, 7.8, SimpleMigrated(11), SimpleMigrated(22), SimpleMigrated(33), SimpleMigrated(44)) bag.add("convergent", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/converged_gen2.bag") m = Converged([1.2, 3.4, 5.6, 7.8], [ SimpleMigrated(11), SimpleMigrated(22), SimpleMigrated(33), SimpleMigrated(44) ]) bag.add("converged", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/constants_gen2.bag") m = Constants(Constants.CONSTANT) bag.add("constants", m, roslib.rostime.Time()) bag.close()
def write_bags_in_ranges(inbags,outdir,ranges): if not os.path.exists(outdir): os.mkdir(outdir) else: if not os.path.isdir(outdir): raise MyException("A file blocks the creations of directory "+outdir) if len(ranges) == 0: return dirs = [] idx = 0 for r in ranges: dirs.append(outdir+"/scene_"+str(idx)) if not os.path.exists(dirs[idx]): os.mkdir(dirs[idx]) else: if not os.path.isdir(dirs[idx]): raise MyException("A file blocks the creations of directory "+outdir) idx += 1 for inbag in inbags: i = 0 r = ranges[i] #if true, reading messages and discarding them until we reach r #otherwise, emitting messages into rebag readingToRange = True for (topic,msg,t) in rosrecord.logplayer(inbag, raw=True): if readingToRange: if t >= r[0]: #open the bag bagfile = dirs[i]+"/"+inbag.split("/").pop() rospy.loginfo("Writing "+bagfile) rebag = rosrecord.Rebagger(bagfile) readingToRange = False #note that I don't use else here-- that way we can quite readingToRange on a message and then emit it if not readingToRange: if t <= r[1]: rebag.add(topic,msg,t,raw=True) else: i = i+1 if i == len(ranges): break r = ranges[i] readingToRange = True
def setUp(self): # Make logfile with bogus messages self.bag = tempfile.NamedTemporaryFile() rebagger = rosrecord.Rebagger(self.bag.name) for i in range(0, row_count): rebagger.add("/diagnostics", make_status_msg(i)) rebagger.close() # Make CSV self.exp = LogExporter(None, self.bag.name) self.exp.process_log() self.exp.finish_logfile() self.filename = self.exp.get_filename('Unit Test') ## Make sparse CSV's self.skip_10 = make_sparse_skip(self.filename, 10) self.length_10 = make_sparse_length(self.filename, 10)
def do_test(self, name, old_msg, new_msg): # Name the bags oldbag = "%s/test/%s_old.bag" % (self.pkg_dir, name) newbag = "%s/test/%s_new.bag" % (self.pkg_dir, name) # Create an old message bag = rosrecord.Rebagger(oldbag) bag.add("topic", old_msg(), roslib.rostime.Time()) bag.close() # Check and migrate res = rosbagmigration.checkbag(oldbag, []) self.assertTrue(res is None or res == [], 'Bag not ready to be migrated') res = rosbagmigration.fixbag(oldbag, newbag, []) self.assertTrue(res, 'Bag not converted successfully') # Pull the first message out of the bag msgs = [msg for msg in rosrecord.logplayer(newbag)] # Reserialize the new message so that floats get screwed up, etc. m = new_msg() buff = StringIO() m.serialize(buff) m.deserialize(buff.getvalue()) #Compare # print "old" # print roslib.message.strify_message(msgs[0][1]) # print "new" # print roslib.message.strify_message(m) # Strifying them helps make the comparison easier until I figure out why the equality operator is failing self.assertTrue( roslib.message.strify_message(msgs[0][1]) == roslib.message.strify_message(m)) # self.assertTrue(msgs[0][1] == m) #Cleanup os.remove(oldbag) os.remove(newbag)
def fix_md5sums(inbags): for b in inbags: print "Trying to migrating file: %s" % b outbag = b + '.tmp' rebag = rosrecord.Rebagger(outbag) try: for i, (topic, msg, t) in enumerate(rosrecord.logplayer(b, raw=True)): rebag.add(topic, msg, t, raw=True) rebag.close() except rosrecord.ROSRecordException, e: print " Migration failed: %s" % (e.message) os.remove(outbag) continue oldnamebase = b + '.old' oldname = oldnamebase i = 1 while os.path.isfile(oldname): i = i + 1 oldname = oldnamebase + str(i) os.rename(b, oldname) os.rename(outbag, b) print " Migration successful. Original stored as: %s" % oldname
def sortbags(inbag, outbag): rebag = rosrecord.Rebagger(outbag) schedule = [] for i, (topic, msg, t) in enumerate(rosrecord.logplayer(inbag, raw=True)): if rospy.is_shutdown(): break schedule.append((t, i)) schedule = [ i for (t,i) in sorted(schedule) ] print schedule stage = {} for i, (topic, msg, t) in enumerate(rosrecord.logplayer(inbag, raw=True)): if rospy.is_shutdown(): break stage[i] = (topic, msg, t) while (schedule != []) and (schedule[0] in stage): (topic, msg, t) = stage[schedule[0]] rebag.add(topic, msg, t, raw=True) del stage[schedule[0]] schedule = schedule[1:] assert schedule == [] assert stage == {} rebag.close()
def fixbags(md5file, inbag, outbag): d = dict() finput = fileinput.input(md5file) for line in finput: sp = line.split() d[sp[1]] = [sp[0], sp[2], sp[3]] rebag = rosrecord.Rebagger(outbag) for i, (topic, msg, t) in enumerate(rosrecord.logplayer(inbag, raw=True)): if rospy.is_shutdown(): break type = msg[0] bytes = msg[1] md5 = msg[2] if md5 in d: if type != d[md5][0]: print 'WARNING: found matching md5, but non-matching name' continue msg = (d[md5][1], msg[1], d[md5][2]) rebag.add(topic, msg, t, raw=True) rebag.close()
def generate_data(): bag = rosrecord.Rebagger("test/migrated_explicit_gen3.bag") m = MigratedExplicit(None, 17, 58.2, "aldfkja", 82) bag.add("migrated_explicit", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/migrated_implicit_gen3.bag") m = MigratedImplicit(None, MigratedExplicit(None, 17, 58.2, "aldfkja", 82), "kljene", 16.32, 34) bag.add("migrated_implicit", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/migrated_mixed_gen3.bag") m = MigratedMixed( None, MigratedImplicit(None, MigratedExplicit(None, 17, 58.2, "aldfkja", 82), "kljene", 16.32, 34), 59) bag.add("migrated_mixed", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/partially_migrated_gen3.bag") m = PartiallyMigrated(40, MigratedExplicit(None, 17, 58.2, "aldfkja", 82), "radasdk") bag.add("partially_migrated", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/renamed_gen3.bag") m = Renamed3(2.17, [8, 2, 5, 1]) bag.add("renamed", m, roslib.rostime.Time()) bag.close() bag = rosrecord.Rebagger("test/converged_gen3.bag") m = Converged([1.2, 3.4, 5.6, 7.8], [ SimpleMigrated(11), SimpleMigrated(22), SimpleMigrated(33), SimpleMigrated(44) ]) bag.add("converged", m, roslib.rostime.Time()) bag.close()
def fastrebag(inbag, outbag): rebag = rosrecord.Rebagger(outbag) for i, (topic, msg, t) in enumerate(rosrecord.logplayer(inbag, raw=True)): rebag.add(topic, msg, t, raw=True) rebag.close()