def process_bags(outfilename, inbags):
outbag = rosbag.Bag(outfilename, "w")
pm = SedsMessage() # store previous message for dx computation
cm = SedsMessage() # current seds message
zero = rospy.rostime.Duration(0)
# open all the trajectory bags
for (i,bagname) in enumerate(inbags):
rospy.loginfo("Processing bag %d: %s" % (i,bagname))
# need a new listener for each bag -- otherwise TF_OLD_DATA warnings
bag = rosbag.Bag(bagname)
first = True # set pm on first step -- compute pm.dx on every other step
# read all the bag messages and process /tf messages
for topic, msg, t in bag.read_messages():
if topic == "/wam/cartesian_coordinates":
cm.x = npa(msg.position)
cm.t = t
cm.index = i
if first:
pm.x = cm.x
pm.index = cm.index
pm.t = cm.t
first = False
# compute dx
pm.dx = cm.x - pm.x
# compute dt
pm.dt = cm.t - pm.t;
# Write out the seds bag if time has changed.
# Some tf frames change at a much faster rate and
# so listener reprocesses the same transform
# lookup even though latest common time remains
# the same. If we did not check pm.dt != zero,
# we'd end up with a lot of duplicate entries.
# We also check whether pm.dx is zero. This choice
# is the result of a fair amount of
# experimentation to try to extract the actual
# motions from a bagfile that probably contains a
# lot of the robot just sitting there (e.g. before
# the teaching trajectory actually starts). I
# experimented a fair bit with the best way to
# extract the useful subtrajectory out of the bag
# and this criterion is the best in terms of
# simplicity and quality.
if (pm.dt != zero and not iszero(pm.dx)):
rospy.logdebug("Message time: %s" % str(pm.t))
outbag.write('seds/trajectories', pm)
# alt. set t parameter to orginal bag
# time? would need to process bags in
# order to avoid time travel
pm.x = cm.x
pm.index = cm.index
pm.t = cm.t
# end of current bag -- write out last entry with dx = 0
cm.dx = cm.x - pm.x
outbag.write('seds/trajectories', cm)
bag.close()
rospy.loginfo("Writing bag: %s" % outfilename)
outbag.close() # very important to close a rosbag after writing
def process_bags(outfilename, inbags, source_fid, target_fid):
outbag = rosbag.Bag(outfilename, "w")
pm = SedsMessage() # store previous message for dx computation
cm = SedsMessage() # current seds message
zero = rostime.Duration(0)
s1 = String()
s1.data = source_fid
outbag.write('seds/source_fid', s1)
rospy.loginfo("Writing seds/source_fid " + s1.data)
s2 = String()
s2.data = target_fid
outbag.write('seds/target_fid', s2)
rospy.loginfo("Writing seds/target_fid " + s2.data)
# open all the trajectory bags
for (i,bagname) in enumerate(inbags):
rospy.loginfo("Processing bag %d: %s" % (i,bagname))
# need a new listener for each bag -- otherwise TF_OLD_DATA warnings
listener = BagListener()
bag = rosbag.Bag(bagname)
first = True # set pm on first step -- compute pm.dx on every other step
tf_cnt=0
tf_match_cnt=0
nonzero_cnt=0
# read all the bag messages and process /tf messages
for topic, msg, t in bag.read_messages():
#only process /tf messages
if topic[-2:] == "tf":# and (cfid == target_fid or cfid == source_fid):
# loads the transforms
listener.load(msg)
tf_cnt+=1
try:
#Can we transform between these?
#et = listener.lookupTransform(source_fid, target_fid, rostime.Time(0))
#since source and target may be at very different rates, we need to time travel
#so get times with respect to a constant (the torso)
unmoving_fid="torso_lift_link"
source_time=listener.getLatestCommonTime(source_fid,unmoving_fid)
target_time=listener.getLatestCommonTime(target_fid,unmoving_fid)
et = listener.lookupTransformFull(source_fid,source_time,target_fid,target_time,unmoving_fid)
#et = listener.lookupTransformFull(target_fid,target_time,source_fid,source_time,unmoving_fid)
tf_match_cnt+=1
# get the current x
cm.x = et[0][:] + euler_from_quaternion(et[1][:])
cm.index = i
cm.t = max(source_time,target_time)
if first:
pm.x = cm.x
pm.index = cm.index
pm.t = cm.t
first = False
# compute dx
pm.dx = npa(cm.x) - npa(pm.x)
# compute dt
pm.dt = cm.t - pm.t;
# Write out the seds bag if time has changed.
# Some tf frames change at a much faster rate and
# so listener reprocesses the same transform
# lookup even though latest common time remains
# the same. If we did not check pm.dt != zero,
# we'd end up with a lot of duplicate entries.
# We also check whether pm.dx is zero. This choice
# is the result of a fair amount of
# experimentation to try to extract the actual
# motions from a bagfile that probably contains a
# lot of the robot just sitting there (e.g. before
# the teaching trajectory actually starts). I
# experimented a fair bit with the best way to
# extract the useful subtrajectory out of the bag
# and this criterion is the best in terms of
# simplicity and quality.
if (pm.dt != zero) and not iszero(pm.dx):
#rospy.loginfo("Message time: %s" % str(pm.t))
#print "At time " + str(min(source_time,target_time))
#print "-Translation " + str(pm.x[0:3])
outbag.write('seds/trajectories', pm)
nonzero_cnt+=1
# alt. set t parameter to orginal bag
# time? would need to process bags in
# order to avoid time travel
pm.x = cm.x
#pm.index = cm.index
pm.t = cm.t
except tf.Exception, error:
rospy.logdebug("%s %s %s %s" % (error,topic,msg,t))
#rospy.loginfo("TF Exception %s" % error)
# end of current bag -- write out last entry with dx = 0
cm.dx = npa(cm.x) - npa(pm.x)
outbag.write('seds/trajectories', cm)
#print "Found " + str(tf_cnt) + "tfs, " + str(tf_match_cnt) + " match, and " + str(nonzero_cnt) + " are nonzero."
rospy.loginfo("Processed %s, found transforms for %s and %s are nonzero" % (str(tf_cnt), str(tf_match_cnt), str(nonzero_cnt)))
bag.close()
[0.478417,-0.222597,-0.099429],
[0.532435,-0.075026,-0.247842],
[0.531750,-0.071448,-0.256943],
[0.529323,-0.077282,-0.250892]]
for (i,s) in enumerate(starts):
x = s
px = None
cnt = 0
while x != px and cnt < 1000:
# compute the next step
dx = list(ds(x).dx)
# record x,dx in SedsMessage
sm.x = x
sm.dx = dx
sm.t = rospy.Time(0)
sm.dt = rospy.Duration(0.2)
sm.index = i
outbag.write('seds/trajectories', sm)
rospy.logdebug(x)
x = list(npa(x) + npa(dx))
cnt += 1
outbag.close()
def process_bags(outfilename, inbags, source_fid, target_fid):
outbag = rosbag.Bag(outfilename, "w")
pm = SedsMessage() # store previous message for dx computation
cm = SedsMessage() # current seds message
zero = rostime.Duration(0)
s1 = String()
s1.data = source_fid
outbag.write('seds/source_fid', s1)
rospy.loginfo("Writing seds/source_fid " + s1.data)
s2 = String()
s2.data = target_fid
outbag.write('seds/target_fid', s2)
rospy.loginfo("Writing seds/target_fid " + s2.data)
# open all the trajectory bags
for (i,bagname) in enumerate(inbags):
rospy.loginfo("Processing bag %d: %s" % (i,bagname))
# need a new listener for each bag -- otherwise TF_OLD_DATA warnings
listener = BagListener()
bag = rosbag.Bag(bagname)
first = True # set pm on first step -- compute pm.dx on every other step
# read all the bag messages and process /tf messages
for topic, msg, t in bag.read_messages():
if topic[-2:] == "tf":
# loads the transforms
listener.load(msg)
# computes the coordinates
try:
et = listener.lookupTransform(source_fid, target_fid, rostime.Time(0))
t = listener.getLatestCommonTime(source_fid, target_fid)
# fill in the seds message
cm.x = et[0][:] # pos (x,y,z)
cm.index = i
cm.t = t
if first:
pm.x = cm.x
pm.index = cm.index
pm.t = cm.t
first = False
# compute dx
pm.dx = npa(cm.x) - npa(pm.x)
# compute dt
pm.dt = cm.t - pm.t;
# Write out the seds bag if time has changed.
# Some tf frames change at a much faster rate and
# so listener reprocesses the same transform
# lookup even though latest common time remains
# the same. If we did not check pm.dt != zero,
# we'd end up with a lot of duplicate entries.
# We also check whether pm.dx is zero. This choice
# is the result of a fair amount of
# experimentation to try to extract the actual
# motions from a bagfile that probably contains a
# lot of the robot just sitting there (e.g. before
# the teaching trajectory actually starts). I
# experimented a fair bit with the best way to
# extract the useful subtrajectory out of the bag
# and this criterion is the best in terms of
# simplicity and quality.
if (pm.dt != zero and not iszero(pm.dx)):
rospy.logdebug("Message time: %s" % str(pm.t))
outbag.write('seds/trajectories', pm)
# alt. set t parameter to orginal bag
# time? would need to process bags in
# order to avoid time travel
pm.x = cm.x
pm.index = cm.index
pm.t = cm.t
except LookupException, error:
# sometimes not enough info is recorded to complete the transform lookup
rospy.logdebug("%s %s %s %s" % (error,topic,msg,t))
except ConnectivityException, error:
# sometimes the perceptual information drops out
rospy.loginfo("ConnectivityException %s" % error)
rospy.logdebug("%s %s %s %s" % (error,topic,msg,t))
