def print_pose(self):
try:
tfu.wait('map', 'base_footprint', self.tflistener, 1.)
m_T_b = tfu.transform('map', 'base_footprint', self.tflistener)
t, q = tfu.matrix_as_tf(m_T_b)
print m_T_b
print 't', t, 'q', q
except Exception, e:
print e
def __init__(self):
self.tflistener = tf.TransformListener()
tfu.wait('map', 'base_footprint', self.tflistener, 4.)