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 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 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 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 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 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()
