def load_from_bag(filename, selected_frames): cam = None framecounter = 0 afs = {} for topic, msg in rosrecord.logplayer(filename): if rospy.is_shutdown(): break if topic == "/videre/cal_params" and not cam: cam = camera.VidereCamera(msg.data) if cam and topic == "/videre/images": print "frame", framecounter if framecounter in selected_frames: def imgAdapted(msg_img): return Image.fromstring("L", (msg_img.width, msg_img.height), msg_img.data) imgR = imgAdapted(msg.images[0]) imgL = imgAdapted(msg.images[1]) afs[framecounter] = SparseStereoFrame(imgL, imgR) if framecounter == max(selected_frames): break framecounter += 1 return (cam, afs)
def xtest_smoke_bag(self): import rosrecord import visualize class imgAdapted: def __init__(self, i): self.i = i self.size = (i.width, i.height) def tostring(self): return self.i.data cam = None filename = "/u/prdata/videre-bags/loop1-mono.bag" filename = "/u/prdata/videre-bags/vo1.bag" framecounter = 0 for topic, msg in rosrecord.logplayer(filename): print framecounter if rospy.is_shutdown(): break #print topic,msg if topic == "/videre/cal_params" and not cam: cam = camera.VidereCamera(msg.data) vo = VisualOdometer(cam) if cam and topic == "/videre/images": if -1 <= framecounter and framecounter < 360: imgR = imgAdapted(msg.images[0]) imgL = imgAdapted(msg.images[1]) af = SparseStereoFrame(imgL, imgR) pose = vo.handle_frame(af) visualize.viz(vo, af) framecounter += 1 print "distance from start:", vo.pose.distance() vo.summarize_timers()
def do_test_renamed(self, N): tmp_rule_files = [ 'migrated_explicit_rules.py', 'migrated_mixed_rules.py', 'migrated_addsub_rules.py', 'renamed_rules.py' ] rule_files = ["%s/test/%s" % (self.pkg_dir, r) for r in tmp_rule_files] inbag = "%s/test/renamed_gen%d.bag" % (self.pkg_dir, N) outbag = "%s/test/renamed_gen%d.fixed.bag" % (self.pkg_dir, N) mm = rosbagmigration.MessageMigrator(rule_files, False) res = rosbagmigration.checkbag(mm, inbag) self.assertTrue(not False in [m[1] == [] for m in res], 'Bag not ready to be migrated') res = rosbagmigration.fixbag(mm, inbag, outbag) self.assertTrue(res, 'Bag not converted successfully') msgs = [msg for msg in rosrecord.logplayer(outbag)] self.assertTrue(len(msgs) > 0) self.assertEqual(msgs[0][1]._type, 'test_rosbagmigration/Renamed4', 'Type name is wrong') self.assertEqual(msgs[0][1].foo, struct.unpack('<d', struct.pack('<d', 2.17))[0]) self.assertEqual(msgs[0][1].bar, (8, 2, 5, 1))
def do_test_migrated_mixed(self, N): tmp_rule_files = [ 'migrated_explicit_rules.py', 'migrated_mixed_rules.py', 'migrated_addsub_rules.py' ] rule_files = ["%s/test/%s" % (self.pkg_dir, r) for r in tmp_rule_files] inbag = "%s/test/migrated_mixed_gen%d.bag" % (self.pkg_dir, N) outbag = "%s/test/migrated_mixed_gen%d.fixed.bag" % (self.pkg_dir, N) mm = rosbagmigration.MessageMigrator(rule_files, False) res = rosbagmigration.checkbag(mm, inbag) self.assertTrue(not False in [m[1] == [] for m in res], 'Bag not ready to be migrated') res = rosbagmigration.fixbag(mm, inbag, outbag) self.assertTrue(res, 'Bag not converted successfully') msgs = [msg for msg in rosrecord.logplayer(outbag)] self.assertTrue(len(msgs) > 0) self.assertEqual(msgs[0][1].field1.field4.afield2, struct.unpack('<f', struct.pack('<f', 58.2))[0]) self.assertEqual(msgs[0][1].field1.field4.combo_field3, "aldfkja 17") self.assertEqual(msgs[0][1].field1.field4.afield4, 82) self.assertEqual(msgs[0][1].field1.field1, 34) self.assertEqual(msgs[0][1].field1.field2, struct.unpack('<f', struct.pack('<f', 16.32))[0]) self.assertEqual(msgs[0][1].field1.field3, "kljene") self.assertEqual(msgs[0][1].field2, 59)
def bag_reader(file_name, topics): f = open(file_name) tdict = {} for t in topics: tdict[t] = True for r in rosrecord.logplayer(f): if tdict.has_key(r[0]): yield r f.close()
def __init__(self, sourcename): if os.path.isdir(sourcename): self.next = self.next_from_dir self.dc = yaml.load( open("%s/sequence_parameters" % sourcename).read()) self.cam = camera.DictCamera(self.dc) self.f = int(self.dc['FirstFrame']) else: self.gen = rosrecord.logplayer(sourcename) self.next = self.next_from_bag self.sourcename = sourcename
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 main(argv): people_tracker = PeopleTracker() if len(argv) > 0 and argv[0] == "-bag": people_tracker.usebag = True # Use a bag file if people_tracker.usebag: import rosrecord #filename = "/wg/stor2/prdata/videre-bags/people-color-close-single-2.bag" filename = "/wg/stor2/prdata/videre-bags/loop1-mono.bag" #filename = "/wg/stor2/prdata/videre-bags/face2.bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass num_frames = 0 start_frame = 4700 end_frame = 4900 for topic, msg in rosrecord.logplayer(filename): if topic == '/videre/cal_params': people_tracker.params(msg) elif topic == '/videre/images': if num_frames >= start_frame and num_frames < end_frame: people_tracker.frame(msg) num_frames += 1 else: pass # Use new ROS messages, and output the 3D position of the face in the camera frame. else: print "Non-bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass people_tracker.pub = rospy.Publisher('head_controller/track_point', PointStamped) rospy.init_node('videre_face_tracker', anonymous=True) rospy.TopicSub('/head_controller/track_point', PointStamped, people_tracker.point_stamped) rospy.TopicSub('/videre/images', ImageArray, people_tracker.frame) rospy.TopicSub('/videre/cal_params', String, people_tracker.params) rospy.spin()
def mtrace_plot(argv): if len(argv) < 2: usage() return 1 rospy.init_node("mtrace_plot", anonymous=True) bagfn = argv[1] samples = [] for (topic, msg, t) in rosrecord.logplayer(bagfn, raw=False): samples += msg.samples print len(samples) callback(samples)
def main(argv) : people_tracker = PeopleTracker() if len(argv)>0 and argv[0]=="-bag": people_tracker.usebag = True # Use a bag file if people_tracker.usebag : import rosrecord #filename = "/wg/stor2/prdata/videre-bags/people-color-close-single-2.bag" filename = "/wg/stor2/prdata/videre-bags/loop1-mono.bag" #filename = "/wg/stor2/prdata/videre-bags/face2.bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass num_frames = 0 start_frame = 4700 end_frame = 4900 for topic, msg in rosrecord.logplayer(filename): if topic == '/videre/cal_params': people_tracker.params(msg) elif topic == '/videre/images': if num_frames >= start_frame and num_frames < end_frame: people_tracker.frame(msg) num_frames += 1 else : pass # Use new ROS messages, and output the 3D position of the face in the camera frame. else : print "Non-bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass people_tracker.pub = rospy.Publisher('head_controller/track_point',PointStamped) rospy.init_node('videre_face_tracker', anonymous=True) rospy.TopicSub('/head_controller/track_point',PointStamped,people_tracker.point_stamped) rospy.TopicSub('/videre/images',ImageArray,people_tracker.frame) rospy.TopicSub('/videre/cal_params',String,people_tracker.params) rospy.spin()
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 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 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 do_test_constants_rules(self, N): tmp_rule_files = ['constants.bmr'] rule_files = ["%s/test/%s" % (self.pkg_dir, r) for r in tmp_rule_files] inbag = "%s/test/constants_gen%d.bag" % (self.pkg_dir, N) outbag = "%s/test/constants_gen%d.fixed.bag" % (self.pkg_dir, N) mm = rosbagmigration.MessageMigrator(rule_files, False) res = rosbagmigration.checkbag(mm, inbag) self.assertTrue(not False in [m[1] == [] for m in res], 'Bag not ready to be migrated') res = rosbagmigration.fixbag(mm, inbag, outbag) self.assertTrue(res, 'Bag not converted successfully') msgs = [msg for msg in rosrecord.logplayer(outbag)] self.assertTrue(len(msgs) > 0) self.assertEqual(msgs[0][1]._type, 'test_rosbagmigration/Constants', 'Type name is wrong') self.assertEqual(msgs[0][1].value, msgs[0][1].CONSTANT)
def do_test_convergent(self, N): tmp_rule_files = [ 'migrated_explicit_rules.py', 'migrated_mixed_rules.py', 'migrated_addsub_rules.py', 'renamed_rules.py', 'simple_migrated_rules.py', 'converged_rules.py' ] rule_files = ["%s/test/%s" % (self.pkg_dir, r) for r in tmp_rule_files] inbag = "%s/test/convergent_gen%d.bag" % (self.pkg_dir, N) outbag = "%s/test/convergent_gen%d.fixed.bag" % (self.pkg_dir, N) mm = rosbagmigration.MessageMigrator(rule_files, False) res = rosbagmigration.checkbag(mm, inbag) self.assertTrue(not False in [m[1] == [] for m in res], 'Bag not ready to be migrated') res = rosbagmigration.fixbag(mm, inbag, outbag) self.assertTrue(res, 'Bag not converted successfully') msgs = [msg for msg in rosrecord.logplayer(outbag)] self.assertTrue(len(msgs) > 0) self.assertEqual(msgs[0][1]._type, 'test_rosbagmigration/Converged', 'Type name is wrong') self.assertEqual(msgs[0][1].field1[0], struct.unpack('<f', struct.pack('<f', 1.2))[0]) self.assertEqual(msgs[0][1].field1[1], struct.unpack('<f', struct.pack('<f', 3.4))[0]) self.assertEqual(msgs[0][1].field1[2], struct.unpack('<f', struct.pack('<f', 5.6))[0]) self.assertEqual(msgs[0][1].field1[3], struct.unpack('<f', struct.pack('<f', 7.8))[0]) self.assertEqual(msgs[0][1].field2[0].data, 11) self.assertEqual(msgs[0][1].field2[1].data, 22) self.assertEqual(msgs[0][1].field2[2].data, 33) self.assertEqual(msgs[0][1].field2[3].data, 44)
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 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 test_addsub(self): tmp_rule_files = [ 'migrated_explicit_rules.py', 'migrated_mixed_rules.py', 'migrated_addsub_rules.py' ] rule_files = ["%s/test/%s" % (self.pkg_dir, r) for r in tmp_rule_files] inbag = "%s/test/migrated_addsub_gen1.bag" % (self.pkg_dir, ) outbag = "%s/test/migrated_addsub_gen1.fixed.bag" % (self.pkg_dir, ) mm = rosbagmigration.MessageMigrator(rule_files, False) res = rosbagmigration.checkbag(mm, inbag) self.assertTrue(not False in [m[1] == [] for m in res], 'Bag not ready to be migrated') res = rosbagmigration.fixbag(mm, inbag, outbag) self.assertTrue(res, 'Bag not converted successfully') msgs = [msg for msg in rosrecord.logplayer(outbag)] self.assertTrue(len(msgs) > 0) self.assertEqual(msgs[0][1].field1.field1, 42) self.assertEqual(msgs[0][1].field2.field1, 42)
from optparse import OptionParser if __name__ == '__main__': parser = OptionParser(usage="usage: savemsg.py [-b <bagfile] type") parser.add_option( "-b", "--bagfiles", action="store", dest="bagfile", default=None, help="Save message from a bagfile rather than system definition") (options, args) = parser.parse_args() if len(args) == 1: if options.bagfile is None: sys_class = roslib.message.get_message_class(args[0]) if sys_class is None: print >> sys.stderr, "Could not find message %s." % args[0] else: print "[%s]:" % args[0] print sys_class._full_text else: for i, (topic, msg, t) in enumerate( rosrecord.logplayer(options.bagfile, raw=True)): if (msg[0] == args[0]): print "[%s]:" % args[0] print msg[4]._full_text exit(0) print >> sys.stderr, "Could not find message %s in bag %s." % ( args[0], options.bagfile)
def find_gaps(inbags): deltas={} deltas["/wide_stereo/raw_stereo_muxed"]=roslib.rostime.Duration.from_seconds(0.5) deltas["/narrow_stereo/raw_stereo_muxed"]=roslib.rostime.Duration.from_seconds(0.5) deltas["/tilt_scan_muxed"]=roslib.rostime.Duration.from_seconds(0.5) deltas["/base_scan_muxed"]=roslib.rostime.Duration.from_seconds(0.5) deltas["/mechanism_state_muxed"]=roslib.rostime.Duration.from_seconds(0.5) deltas["/laser_tilt_controller/laser_scanner_signal"]=roslib.rostime.Duration.from_seconds(2.0) deltas["/odom_muxed"]=roslib.rostime.Duration.from_seconds(0.5) deltas["/tf_message_muxed"]=roslib.rostime.Duration.from_seconds(0.5) long_duration = roslib.rostime.Duration.from_seconds(5) epsilon=roslib.rostime.Duration.from_seconds(0.001) beginningOfTime=roslib.rostime.Time.from_seconds(0) gaps = {} #assumes that no topic appears in more than one inbag prevtime = {} for inbag in inbags: for i,(topic,msg,t) in enumerate(rosrecord.logplayer(inbag, raw=True)): #print (i,topic,t) #print roslib.rostime.Duration.from_seconds(0.5) if not topic in deltas: rospy.loginfo("No delta defined for topic "+topic) raise MyException("Grr") delta = deltas[topic] #if this topic has appeared before if topic in prevtime: #print "\t"+str(t-prevtime[topic]) #check if t-prevtime[topic] > delta: rospy.loginfo(topic+": Found gap of "+str(roslib.rostime.Duration.to_seconds(t-prevtime[topic]))+" seconds") gaps[topic].append((prevtime[topic]+epsilon, t-epsilon)) else: #this is the first instance of this topic, everything up until now is a gap gaps[topic] = [] gaps[topic].append((beginningOfTime,t-epsilon)) #Record this time for the next iteration prevtime[topic]=t #Find the end of time endOfTime = beginningOfTime for topic in prevtime: rospy.loginfo(topic+" ended at "+str(roslib.rostime.Time.to_seconds(prevtime[topic]))) if prevtime[topic] > endOfTime: endOfTime = prevtime[topic] #All time after the ending is a gap for topic in prevtime: assert prevtime[topic] <= endOfTime if prevtime[topic] < endOfTime - long_duration: rospy.logerr(topic+" ended "+str(roslib.rostime.Duration.to_seconds(endOfTime-prevtime[topic]))+" seconds before the final topic") gaps[topic].append((prevtime[topic],endOfTime+epsilon)) #Find the beginning of time beginningOfTime = endOfTime for topic in gaps: (ignore, first) = gaps[topic][0] if first < beginningOfTime: beginningOfTime = first #Back-patch the gaps to use this tighter beginningOfTime rather than 0 for topic in gaps: (ignore, first) = gaps[topic][0] assert first >= beginningOfTime gaps[topic][0] = (beginningOfTime - epsilon, first) if first - long_duration > beginningOfTime: rospy.logerr(topic+" began "+str(roslib.rostime.Duration.to_seconds(first-beginningOfTime))+" seconds after the first topic") return gaps
def process_bag(stats, update, bagfile): for (topic, msg, t) in rosrecord.logplayer(bagfile, raw=False): update(stats, topic, msg)
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()
while (current_file != ""): seen_topic1 = 0 seen_topic2 = 0 starting_sum0 = 0 starting_sum1 = 0 seen_20 = False seen_100_after_20 = False max_position = 0.0 current_file = current_file.strip() text_file = open("%s.txt" % current_file, "w") print "opening %s.txt" % current_file #text_file.write("#position, velocity, force_finger0, force_finger1, time, min_finger0, max_finger0, min_finger1, max_finger1, mean_finger0, mean_finger1, mean_fingers, force_fingers, min_fingers, max_fingers\n") closing_gripper = 2 #0 = contacted, 1 = closing, 2 = opening for topic, data, t in rosrecord.logplayer(current_file): if (topic == "/pressure/r_gripper_motor"): seen_topic1 = 1 data0 = data.data0 data1 = data.data1 if starting_sum0 == 0 and starting_sum1 == 0: last_time = t elif (topic == "/pr2_gripper_controller/state"): seen_topic2 = 1 actual_velocity = data.joint_velocity actual_position = data.joint_position commanded_effort = data.joint_commanded_effort if (seen_topic1 + seen_topic2 == 2 and t > last_time + delta_time): #print "t %f, last %f, del %f" %(t.to_seconds(), last_time.to_seconds(), delta_time.to_seconds()) last_time = t
def main(argv) : people_tracker = PeopleTracker() if len(argv)>0 and argv[0]=="-bag": people_tracker.usebag = True # if len(argv)>1 and argv[1]=="-visualize": # people_tracker.visualize = True # Use a bag file if people_tracker.usebag : import rosrecord #filename = "/wg/stor2/prdata/videre-bags/people-color-close-single-2.bag" filename = "/wg/stor2/prdata/videre-bags/loop1-mono.bag" #filename = "/wg/stor2/prdata/videre-bags/face2.bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass # if people_tracker.visualize: # people_tracker.pub = rospy.Publisher('/std_msgs/full_cloud',PointCloud) # rospy.init_node('videre_face_tracker',anonymous=True) num_frames = 0 start_frame = 4700 end_frame = 5000 for topic, msg in rosrecord.logplayer(filename): if topic == '/videre/cal_params': people_tracker.params(msg) elif topic == '/videre/images': if num_frames >= start_frame and num_frames < end_frame: people_tracker.frame(msg) if people_tracker.visualize: s = raw_input("Press any key in this window to proceed to the next frame.") elif num_frames >= end_frame: break num_frames += 1 else : pass # Use new ROS messages, and output the 3D position of the face in the camera frame. else : print "Non-bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass #people_tracker.pub = rospy.Publisher('head_controller/track_point',PointStamped) people_tracker.pub = rospy.Publisher('/stereo_face_feature_tracker/position_measurement',PositionMeasurement) rospy.init_node('videre_face_tracker', anonymous=True) #rospy.TopicSub('/head_controller/track_point',PointStamped,people_tracker.point_stamped) rospy.TopicSub('/videre/images',ImageArray,people_tracker.frame) rospy.TopicSub('/videre/cal_params',String,people_tracker.params) rospy.spin()
import rosrecord import visualize def imgAdapted(im): return Image.fromstring({'mono8' : 'L', 'rgb24' : 'RGB'}[im.colorspace], (im.width, im.height), im.data).convert("L") ds = {} for filename in sys.argv[1:]: cam = None prev_frame = None framecounter = 0 all_ds = [] sos = numpy.array([.0, .0, .0]) for topic, msg in rosrecord.logplayer(filename): if rospy.is_shutdown(): break if topic.endswith("videre/cal_params") and not cam: cam = camera.VidereCamera(msg.data) vo = VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD()) if cam and topic.endswith("videre/images"): imgR = imgAdapted(msg.images[0]) imgL = imgAdapted(msg.images[1]) assert msg.images[0].label == "right_rectified" assert msg.images[1].label == "left_rectified" frame = SparseStereoFrame(imgL, imgR)
return array.array('B', [ min(x,255) for x in d ]).tostring() #return array.array('f', [ float(x) for x in d ]).tostring() def msg2im(msg): """ Take an image_msgs::Image and return a PIL image """ if len(msg.uint8_data.data) == 0 and len(msg.int16_data.data) == 0: return None else: if msg.depth == 'uint8': ma = msg.uint8_data image_data = ma.data else: ma = msg.int16_data image_data = int16_str(ma.data) dim = dict([ (d.label,d.size) for d in ma.layout.dim ]) mode = { ('uint8',1) : "L", ('uint8',3) : "RGB", ('int16',1) : "L" }[msg.depth, dim['channel']] (w,h) = (dim['width'], dim['height']) return Image.fromstring(mode, (w,h), image_data) counter = 0 for topic, msg, t in rosrecord.logplayer(sys.argv[1]): if rospy.is_shutdown(): break if topic.endswith("stereo/raw_stereo"): for (mi,c) in [ (msg.left_image, 'L'), (msg.right_image, 'R'), (msg.disparity_image, 'D')]: im = msg2im(mi) if im: ext = { 'L':'png', 'RGB':'png', 'F':'tiff' }[im.mode] im.save("%06d%s.%s" % (counter, c, ext)) counter += 1
import rospy import sys import rosrecord filename = sys.argv[1] framecounter = 1 print "Starting Playback" f = filename keys = set() headers = [] headers_file = open("joint_names.txt", "w") pos_file = open("joint_pos.txt", "w") for topic, msg, t in rosrecord.logplayer(f): print topic if rospy.is_shutdown(): break if topic == "/grabber/calibration_data": print framecounter joint_names = [x.name for x in msg.mechanism_state.joint_states] joint_pos = [x.position for x in msg.mechanism_state.joint_states] if (len(headers) == 0): headers = joint_names headers_file.writelines([x + "\n" for x in headers]) headers_file.writelines(['\n'])
def process_log(self): for (topic, msg, t) in rosrecord.logplayer(self.logfile, raw=False): self._update(topic, msg)
import votools as VO class FeatureDetectorMine: def name(self): return self.__class__.__name__ def __init__(self): pass def detect(self, frame, target_points): quality_level = 0.01 min_distance = 10.0 return VO.harris(frame.rawdata, frame.size[0], frame.size[1], 1000, quality_level, min_distance) for topic, msg, t in rosrecord.logplayer(filename): if rospy.is_shutdown(): break if topic.endswith("videre/cal_params") and not cam: print msg.data cam = camera.VidereCamera(msg.data) (Fx, Fy, Tx, Clx, Crx, Cy) = cam.params Tx /= (7.30 / 7.12) cam = camera.Camera((Fx, Fy, Tx, Clx, Crx, Cy)) vos = [ VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), sba = None), VisualOdometer(cam, feature_detector = FeatureDetectorMine(), inlier_error_threshold = 1.0, descriptor_scheme = DescriptorSchemeSAD(), sba = (3,10,10)), #VisualOdometer(cam, feature_detector = FeatureDetectorFast(), descriptor_scheme = DescriptorSchemeSAD(), sba = (3,8,10)),
import roslib roslib.load_manifest('hai_sandbox') import rospy import rosrecord import sys f = open(sys.argv[1]) i = 0 for topic, message, time in rosrecord.logplayer(f): i = i + 1 print topic, time if i > 10: break f.close() ## # In this bag, give me messages from these topics # @param file_name # @param topics def bag_reader(file_name, topics): f = open(file_name) tdict = {} for t in topics: tdict[t] = True for r in rosrecord.logplayer(f): if tdict.has_key(r[0]): yield r f.close()
def main(argv): people_tracker = PeopleTracker() if len(argv) > 0 and argv[0] == "-bag": people_tracker.usebag = True # if len(argv)>1 and argv[1]=="-visualize": # people_tracker.visualize = True # Use a bag file if people_tracker.usebag: import rosrecord #filename = "/wg/stor2/prdata/videre-bags/people-color-close-single-2.bag" filename = "/wg/stor2/prdata/videre-bags/loop1-mono.bag" #filename = "/wg/stor2/prdata/videre-bags/face2.bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass # if people_tracker.visualize: # people_tracker.pub = rospy.Publisher('/std_msgs/full_cloud',PointCloud) # rospy.init_node('videre_face_tracker',anonymous=True) num_frames = 0 start_frame = 4700 end_frame = 5000 for topic, msg in rosrecord.logplayer(filename): if topic == '/videre/cal_params': people_tracker.params(msg) elif topic == '/videre/images': if num_frames >= start_frame and num_frames < end_frame: people_tracker.frame(msg) if people_tracker.visualize: s = raw_input( "Press any key in this window to proceed to the next frame." ) elif num_frames >= end_frame: break num_frames += 1 else: pass # Use new ROS messages, and output the 3D position of the face in the camera frame. else: print "Non-bag" if SAVE_PICS: try: os.mkdir("/tmp/tiff/") except: pass #people_tracker.pub = rospy.Publisher('head_controller/track_point',PointStamped) people_tracker.pub = rospy.Publisher( '/stereo_face_feature_tracker/position_measurement', PositionMeasurement) rospy.init_node('videre_face_tracker', anonymous=True) #rospy.TopicSub('/head_controller/track_point',PointStamped,people_tracker.point_stamped) rospy.TopicSub('/videre/images', ImageArray, people_tracker.frame) rospy.TopicSub('/videre/cal_params', String, people_tracker.params) rospy.spin()
import roslib; roslib.load_manifest('hai_sandbox') import rospy import rosrecord import sys f = open(sys.argv[1]) i = 0 for topic, message, time in rosrecord.logplayer(f): i = i + 1 print topic, time if i > 10: break f.close() ## # In this bag, give me messages from these topics # @param file_name # @param topics def bag_reader(file_name, topics): f = open(file_name) tdict = {} for t in topics: tdict[t] = True for r in rosrecord.logplayer(f): if tdict.has_key(r[0]): yield r f.close()