hlrc / client / python / tools / dump_relative_gazetargets.py @ 6a233066
History | View | Annotate | Download (3.097 KB)
1 | 6a233066 | Simon Schulz | #!/usr/bin/env python
|
---|---|---|---|
2 | import roslib |
||
3 | import rospy |
||
4 | import math |
||
5 | import tf |
||
6 | import errno |
||
7 | from humotion.msg import * |
||
8 | |||
9 | """
|
||
10 | small code snippet to dump humotion relative gaze targets
|
||
11 | combined with the current gaze direction from tf
|
||
12 | """
|
||
13 | humotion_gaze_topic = "/flobi/humotion/gaze/target"
|
||
14 | tf_topic_base = "BASE_LINK"
|
||
15 | tf_topic_gaze_l = "EYES_LEFT_UD_VIRTUAL_LINK"
|
||
16 | tf_topic_gaze_r = "EYES_RIGHT_UD_VIRTUAL_LINK"
|
||
17 | |||
18 | |||
19 | def fetch_gaze_from_tf(timestamp): |
||
20 | try:
|
||
21 | #fetch right eye
|
||
22 | (trans,rot) = listener.lookupTransform(tf_topic_base, tf_topic_gaze_r, timestamp) |
||
23 | euler = tf.transformations.euler_from_quaternion(rot, axes='sxyz')
|
||
24 | pan_r = -euler[2]
|
||
25 | tilt_r = -euler[1]
|
||
26 | #fetch left eye
|
||
27 | (trans,rot) = listener.lookupTransform(tf_topic_base, tf_topic_gaze_l, timestamp) |
||
28 | euler = tf.transformations.euler_from_quaternion(rot, axes='sxyz')
|
||
29 | pan_l = -euler[2]
|
||
30 | tilt_l = -euler[1]
|
||
31 | #combine to overall gaze direction:
|
||
32 | pan = (pan_l + pan_r) / 2.0 * 180.0 / math.pi |
||
33 | tilt = (tilt_l + tilt_r) / 2.0 * 180.0 / math.pi |
||
34 | return (pan, tilt)
|
||
35 | |||
36 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
|
||
37 | print("> WARNING: no transformation %s-->%s at %s found" % (tf_topic_base, tf_topic_gaze_l, timestamp))
|
||
38 | return (0.0, 0.0) |
||
39 | |||
40 | def incoming_humotion_gaze_target(msg): |
||
41 | print("> incoming gaze @%f" % (msg.gaze_timestamp.to_sec()))
|
||
42 | |||
43 | if (msg.gaze_type == gaze.GAZETYPE_RELATIVE):
|
||
44 | #find out the overall gaze state using TF:
|
||
45 | (tf_pan, tf_tilt) = fetch_gaze_from_tf(msg.gaze_timestamp) |
||
46 | #relative targets
|
||
47 | rel_pan = msg.pan |
||
48 | rel_tilt = msg.tilt |
||
49 | #combined:
|
||
50 | target_pan = tf_pan + rel_pan |
||
51 | target_tilt = tf_tilt + rel_tilt |
||
52 | #current position:
|
||
53 | now = rospy.get_rostime() |
||
54 | (now_pan, now_tilt) = fetch_gaze_from_tf(rospy.Time(0))
|
||
55 | now_pan |
||
56 | print("%f %f %f %f %f %f %f %f #DUMP" % (
|
||
57 | now.to_sec(), |
||
58 | now_pan, |
||
59 | now_tilt, |
||
60 | msg.gaze_timestamp.to_sec(), |
||
61 | rel_pan, |
||
62 | rel_tilt, |
||
63 | target_pan, |
||
64 | target_tilt |
||
65 | )) |
||
66 | #print("pan = %f, tilt=%f" % (pan* 180.0 / math.pi,tilt* 180.0 / math.pi))
|
||
67 | |||
68 | else:
|
||
69 | print("> NOT dumping absolute targets...")
|
||
70 | |||
71 | |||
72 | |||
73 | |||
74 | if __name__ == '__main__': |
||
75 | new_transform = False
|
||
76 | rospy.init_node('dump_relative_gazetarget')
|
||
77 | |||
78 | #humotion gaze subscriber
|
||
79 | print("> setting up subscriber on %s" % (humotion_gaze_topic))
|
||
80 | rospy.Subscriber(humotion_gaze_topic, gaze, incoming_humotion_gaze_target) |
||
81 | |||
82 | |||
83 | #tf listener
|
||
84 | listener = tf.TransformListener() |
||
85 | |||
86 | print("> waiting for transform...")
|
||
87 | try:
|
||
88 | listener.waitForTransform(tf_topic_base, tf_topic_gaze_r, rospy.Time(), rospy.Duration(4.0))
|
||
89 | except tf.Exception:
|
||
90 | print("> ERROR: timed out while waiting for transform %s->%s to become available\n" % (tf_topic_base, tf_topic_gaze_r))
|
||
91 | exit(errno.ETIMEDOUT)
|
||
92 | print("> first transform arrived, will start now...")
|
||
93 | |||
94 | rate = rospy.Rate(50.0)
|
||
95 | |||
96 | while not rospy.is_shutdown(): |
||
97 | |||
98 | rate.sleep() |