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