tf_diff_text.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 import tf
5 from tf.transformations import *
6 import numpy as np
7 from jsk_rviz_plugins.msg import OverlayText
8 from std_msgs.msg import ColorRGBA
9 rospy.init_node("tf_diff")
10 
11 src1 = rospy.get_param("~src1")
12 src2 = rospy.get_param("~src2")
14 r = rospy.Rate(1)
15 pub = rospy.Publisher("diff_text", OverlayText)
16 while not rospy.is_shutdown():
17  try:
18  (pos, rot) = listener.lookupTransform(src1, src2, rospy.Time(0))
19  pos_diff = np.linalg.norm(pos)
20  # quaternion to rpy
21  rpy = euler_from_quaternion(rot)
22  rot_diff = np.linalg.norm(rpy)
23  print (pos_diff, rot_diff)
24  msg = OverlayText()
25  msg.width = 1000
26  msg.height = 200
27  msg.left = 10
28  msg.top = 10
29  msg.text_size = 20
30  msg.line_width = 2
31  msg.font = "DejaVu Sans Mono"
32  msg.text = """%s <-> %s
33 pos: %f
34 rot: %f
35  """ % (src1, src2, pos_diff, rot_diff)
36  msg.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
37  msg.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.0)
38  pub.publish(msg)
39  except:
40  rospy.logerr("ignore error")
41  finally:
42  r.sleep()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18