8 from std_msgs.msg
import ColorRGBA
9 rospy.init_node(
"tf_diff")
11 src1 = rospy.get_param(
"~src1")
12 src2 = rospy.get_param(
"~src2")
15 pub = rospy.Publisher(
"diff_text", OverlayText)
16 while not rospy.is_shutdown():
18 (pos, rot) = listener.lookupTransform(src1, src2, rospy.Time(0))
19 pos_diff = np.linalg.norm(pos)
21 rpy = euler_from_quaternion(rot)
22 rot_diff = np.linalg.norm(rpy)
23 print (pos_diff, rot_diff)
31 msg.font =
"DejaVu Sans Mono" 32 msg.text =
"""%s <-> %s 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)
40 rospy.logerr(
"ignore error")