def test_fromTf(self):
transformer = Transformer(True, rospy.Duration(10.0))
m = TransformStamped()
m.header.frame_id = 'wim'
m.child_frame_id = 'james'
m.transform.translation.x = 2.71828183
m.transform.rotation.w = 1.0
transformer.setTransform(m)
b = pm.fromTf(transformer.lookupTransform('wim', 'james', rospy.Time(0)))
def test_fromTf(self):
transformer = Transformer(True, rospy.Duration(10.0))
m = TransformStamped()
m.header.frame_id = 'wim'
m.child_frame_id = 'james'
m.transform.translation.x = 2.71828183
m.transform.rotation.w = 1.0
transformer.setTransform(m)
b = pm.fromTf(
transformer.lookupTransform('wim', 'james', rospy.Time(0)))
def get_vertices(pose, size, shape_type):
tf = Transformer(True, rospy.Duration(10.0))
m = TransformStamped()
m.header.frame_id = 'object'
m.transform.rotation.w = 1
keys = []
if shape_type=='box':
for mx in POM:
for my in POM:
for mz in POM:
key = 'frame%s%s%s'%(F[mx], F[my], F[mz])
m.child_frame_id = key
keys.append(key)
m.transform.translation.x = size[0]/2.0*mx
m.transform.translation.y = size[1]/2.0*my
m.transform.translation.z = size[2]/2.0*mz
tf.setTransform(m)
else:
return None
m.transform.translation.x = pose.position.x
m.transform.translation.y = pose.position.y
m.transform.translation.z = pose.position.z
m.transform.rotation.x = pose.orientation.x
m.transform.rotation.y = pose.orientation.y
m.transform.rotation.z = pose.orientation.z
m.transform.rotation.w = pose.orientation.w
m.header.frame_id = 'map'
m.child_frame_id = 'object'
tf.setTransform(m)
vertices = []
for key in keys:
(off,rpy) = tf.lookupTransform('map', key, rospy.Time(0))
vertices.append(off)
return vertices
def get_vertices(pose, size, shape_type):
tf = Transformer(True, rospy.Duration(10.0))
m = TransformStamped()
m.header.frame_id = 'object'
m.transform.rotation.w = 1
keys = []
if shape_type == 'box':
for mx in POM:
for my in POM:
for mz in POM:
key = 'frame%s%s%s' % (F[mx], F[my], F[mz])
m.child_frame_id = key
keys.append(key)
m.transform.translation.x = size[0] / 2.0 * mx
m.transform.translation.y = size[1] / 2.0 * my
m.transform.translation.z = size[2] / 2.0 * mz
tf.setTransform(m)
else:
return None
m.transform.translation.x = pose.position.x
m.transform.translation.y = pose.position.y
m.transform.translation.z = pose.position.z
m.transform.rotation.x = pose.orientation.x
m.transform.rotation.y = pose.orientation.y
m.transform.rotation.z = pose.orientation.z
m.transform.rotation.w = pose.orientation.w
m.header.frame_id = 'map'
m.child_frame_id = 'object'
tf.setTransform(m)
vertices = []
for key in keys:
(off, rpy) = tf.lookupTransform('map', key, rospy.Time(0))
vertices.append(off)
return vertices
def process(bagfile):
# Load bag
print "\nOpening bag file [%s]" % bagfile
bag = rosbag.Bag(bagfile)
hz = 100
# Get dictionary of topics
# topics = list(set([c.topic for c in bag._get_connections() if c.topic.startswith("/cf") or c.topic.]))
topics = list(set([c.topic for c in bag._get_connections() if c.topic != "/diagnostics" and c.topic.rfind("rosout")+c.topic.rfind("parameter_") == -2]))
if topics == []:
rospy.logwarn('[%s] has no topics, skipping')
return
start, finish = printInfo(bag, topics=topics)
dur = finish-start
rostf = Transformer(True, dur) #buffer
duration = dur.to_sec()
# deal with TF
# treat each tf frame as a topic
# tfdict = {}
# for tfs in [m[1] for m in bag.read_messages("/tf")]:
# #from->to
# for t in tfs.transforms:
# tFrom = t.header.frame_id
# tTo = t.child_frame_id
# if not tfdict.has_key(tFrom):
# tfdict[tFrom] = {}
# if not tfdict[tFrom].has_key(tTo):
# tfdict[tFrom][tTo] = []
# tfdict[tFrom][tTo].append(t)
for tfs in bag.read_messages("/tf"):
for t in tfs[1].transforms:
t.header.stamp -= start
if (t.header.stamp < rospy.Duration(0)):
rospy.logwarn('%s - %s [%f]' % (t.header.frame_id, t.child_frame_id, t.header.stamp.to_sec()))
continue
rostf.setTransform(t) #cache all tf info into messages to a transformer object
# tFrom = t.header.frame_id
# tTo = t.child_frame_id
# if not tfdict.has_key(tTo):
# tfdict[tTo] = {}
# if not tfdict[tTo].has_key(tFrom):
# tfdict[tTo][tFrom] = []
# tfdict[tTo][tFrom].append(t)
# # Check
# for f in tfdict.keys():
# fs = tfdict[f].keys()
# if (len(fs)>1):
# rospy.logerr("Warning: Frame [%s] has multiple parent frames: %s", f, fs)
# for f in sorted(tfdict.keys()):
# for fs in sorted(tfdict[f].keys()):
# print fs, "->", f
# print "___"
# Print existing frames
# get [(child_frame, parent_frame), ...] for all known transforms in bag
framestr = [(k[k.rfind(' ')+1:],k[0:k.find(' ')]) for k in rostf.allFramesAsString().replace('Frame ','').split('.\n') if k]
for parent, child in framestr:
if parent != "NO_PARENT":
print parent, "->", child
# List of transforms we are interested in using
FRAMES = [("/world", "/cf_gt"), ("/world", "/goal"), ("/cf_gt2d", "/goal")] # from->to pairs
# For TF we interpolate at equal intervals
timelist = np.arange(0, duration, 1./hz)
# container for all transforms
frames_pq = {"time": timelist}
for parent, child in FRAMES:
# Get TF info at given HZ
pq = []
for ti in timelist:
t = rospy.Time(secs=ti)
if rostf.canTransform(parent, child, t):
p, q = rostf.lookupTransform(parent, child, t)
pq.append([p[0],p[1],p[2],q[0],q[1],q[2],q[3]])
else:
#rospy.logwarn("Could not get transform %s -> %s at time %fs", parent, child, t.to_sec())
pq.append([0,0,0, 0,0,0,1])
frames_pq[parent[1:].replace('/','_')+"__"+child[1:].replace('/','_')] = np.array(pq) # matlab friendly
# # PLot xyz
# import matplotlib.pyplot as plt
# # Create the plot
# plt.plot(timelist, frames_pq["world-cf_gt"][:, 0:3]) # FRAME, time step, dimension
# plt.plot(timelist, frames_pq["world-goal"][ :, 0:3]) # FRAME, time step, dimension
# plt.show()
# Collect all data
data = {t: {} for t in topics if t != '/tf' and not t.endswith('image_raw') and not t.endswith('camera_info')}
# Prepare all the dictionaries
for t in data.keys():
print t
name, msg, t = bag.read_messages(topics=t).next()
data[name] = {slot: [] for slot in msg.__slots__ if slot != "header"}
data[name]["time"] = []
# print "USED DATA: \n",
[k +": "+str(data[k].keys()) for k in data.keys()]
# Fill all dictionaries
for name, msg, t in bag.read_messages(topics=data.keys()):
# They all have a time stamp
data[name]["time"].append((t-start).to_sec())
# Append data for each slot
for slot in data[name].keys():
if slot == "time":
continue
contents = msg.__getattribute__(slot)
data[name][slot].append(contents)
# Convert all to NP arrays
mdata = {}
for name in data.keys():
mname = name[name.rfind('/')+1:] # matlab friendly name
mdata[mname] = {}
for slot in data[name].keys():
mdata[mname][slot] = np.array(data[name][slot])
# add TF data
mdata['tf'] = frames_pq
# Save as MAT
sio.savemat(bagfile[:-4]+'.mat', {'bag'+bagfile[:-4].replace('-','_'):mdata}, oned_as='column')
return
