def get_transformation(frame_from='/base_link',
frame_to='/local_map',
time_from=None,
time_to=None,
static_frame=None,
tf_listener=None,
tf_ros=None):
if tf_listener is None:
tf_listener = TransformListener()
if tf_ros is None:
tf_ros = TransformerROS()
try:
if time_from is None or time_to is None:
# tf_listener.waitForTransform(frame_from, frame_to, rospy.Time(), rospy.Duration(1.0))
(trans, rot) = tf_listener.lookupTransform(frame_to, frame_from,
rospy.Time(0))
else:
# tf_listener.waitForTransformFull(frame_from, time_from, frame_to, time_to, static_frame, rospy.Duration(1.0))
(trans,
rot) = tf_listener.lookupTransformFull(frame_to, time_to,
frame_from, time_from,
static_frame)
except (LookupException, ConnectivityException, ExtrapolationException):
rospy.logerr("exception, from %s to %s frame may not have setup!",
frame_from, frame_to)
return None, None, None, None
# pose.pose.orientation.w = 1.0 # Neutral orientation
# tf_pose = self.tf_listener_.transformPose("/world", pose)
# R_local = quaternion_matrix(tf_pose.pose.orientation)
T = tf_ros.fromTranslationRotation(trans, rot)
euler = euler_from_quaternion(rot)
# print "Position of the pose in the local map:"
# print trans, euler
return T, trans, rot, euler
