tf_diff_text.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import tf
00005 from tf.transformations import *
00006 import numpy as np
00007 from jsk_rviz_plugins.msg import OverlayText
00008 from std_msgs.msg import ColorRGBA
00009 rospy.init_node("tf_diff")
00010 
00011 src1 = rospy.get_param("~src1")
00012 src2 = rospy.get_param("~src2")
00013 listener = tf.TransformListener()
00014 r = rospy.Rate(1)
00015 pub = rospy.Publisher("diff_text", OverlayText)
00016 while not rospy.is_shutdown():
00017     try:
00018         (pos, rot) = listener.lookupTransform(src1, src2, rospy.Time(0))
00019         pos_diff = np.linalg.norm(pos)
00020         # quaternion to rpy
00021         rpy = euler_from_quaternion(rot)
00022         rot_diff = np.linalg.norm(rpy)
00023         print (pos_diff, rot_diff)
00024         msg = OverlayText()
00025         msg.width = 1000
00026         msg.height = 200
00027         msg.left = 10
00028         msg.top = 10
00029         msg.text_size = 20
00030         msg.line_width = 2
00031         msg.font = "DejaVu Sans Mono"
00032         msg.text = """%s <-> %s
00033 pos: %f
00034 rot: %f
00035         """ % (src1, src2, pos_diff, rot_diff)
00036         msg.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
00037         msg.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.0)
00038         pub.publish(msg)
00039     except:
00040         rospy.logerr("ignore error")
00041     finally:
00042         r.sleep()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03