Go to the documentation of this file.00001
00002
00003 import rospy
00004 from nav_msgs.msg import Odometry
00005 import numpy
00006 import tf
00007
00008 class OdomPublisher:
00009 def __init__(self):
00010 rospy.init_node("OdometryPublisher", anonymous=True)
00011 self.sub = rospy.Subscriber("~input_odom", Odometry, self.callback)
00012 self.target_frame = rospy.get_param("~target_frame", "ground_truth_odom")
00013 self.intermediate_frame = rospy.get_param("~intermediate_frame", "odom")
00014 self.base_frame = rospy.get_param("~base_frame", "BODY")
00015 self.invert_tf = rospy.get_param("~invert_tf", False)
00016 self.listener = tf.TransformListener()
00017 self.broadcast = tf.TransformBroadcaster()
00018 self.r = rospy.Rate(30)
00019
00020 def execute(self):
00021 while not rospy.is_shutdown():
00022 self.r.sleep()
00023
00024 def callback(self, msg):
00025 try:
00026 (trans,rot) = self.listener.lookupTransform(self.base_frame, self.intermediate_frame, rospy.Time(0))
00027 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00028 rospy.logwarn("failed to solve tf: %s to %s", self.base_frame, self.intermediate_frame)
00029 return
00030
00031 homogeneous_matrix_base_to_intermediate = tf.transformations.quaternion_matrix(rot)
00032 homogeneous_matrix_base_to_intermediate[:3, 3] = numpy.array(trans).reshape(1, 3)
00033 homogeneous_matrix_target_to_base = tf.transformations.quaternion_matrix((msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w))
00034 homogeneous_matrix_target_to_base[:3, 3] = numpy.array((msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z)).reshape(1, 3)
00035 homogeneous_matrix_target_to_intermediate = homogeneous_matrix_target_to_base.dot(homogeneous_matrix_base_to_intermediate)
00036 if self.invert_tf:
00037 inv_homohomogeneous_matrix_target_to_intermediate = numpy.linalg.inv(homogeneous_matrix_target_to_intermediate)
00038 self.broadcast.sendTransform(list(inv_homohomogeneous_matrix_target_to_intermediate[:3, 3]),
00039 list(tf.transformations.quaternion_from_matrix(inv_homohomogeneous_matrix_target_to_intermediate)),
00040 rospy.Time.now(),
00041 self.target_frame, self.intermediate_frame)
00042 else:
00043 self.broadcast.sendTransform(list(homogeneous_matrix_target_to_intermediate[:3, 3]),
00044 list(tf.transformations.quaternion_from_matrix(homogeneous_matrix_target_to_intermediate)),
00045 rospy.Time.now(),
00046 self.intermediate_frame, self.target_frame)
00047
00048 if __name__ == '__main__':
00049 try:
00050 node = OdomPublisher()
00051 node.execute()
00052 except rospy.ROSInterruptException: pass