hlrc / client / python / tools / dump_relative_gazetargets.py @ 4bd3e852
History | View | Annotate | Download (4.916 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 | c1e27eb0 | Simon Schulz | import array |
9 | 6a233066 | Simon Schulz | |
10 | """
|
||
11 | small code snippet to dump humotion relative gaze targets
|
||
12 | combined with the current gaze direction from tf
|
||
13 | """
|
||
14 | 4bd3e852 | Simon Schulz | USE_MEKA_TOPICS=True
|
15 | c1e27eb0 | Simon Schulz | |
16 | 6a233066 | Simon Schulz | |
17 | c1e27eb0 | Simon Schulz | #joint targets
|
18 | if USE_MEKA_TOPICS:
|
||
19 | 4bd3e852 | Simon Schulz | humotion_gaze_topic = "/flobi/humotion/gaze/target"
|
20 | tf_topic_base = "BASE_LINK"
|
||
21 | tf_topic_gaze_l = "EYES_LEFT_UD_VIRTUAL_LINK"
|
||
22 | tf_topic_gaze_r = "EYES_RIGHT_UD_VIRTUAL_LINK"
|
||
23 | c1e27eb0 | Simon Schulz | from trajectory.msg import * |
24 | topic_joint_target = "/meka_roscontrol/head_position_trajectory_controller/command"
|
||
25 | else:
|
||
26 | 4bd3e852 | Simon Schulz | humotion_gaze_topic = "/flobi/humotion/gaze/target"
|
27 | tf_topic_base = "BASE_LINK"
|
||
28 | tf_topic_gaze_l = "EYES_LEFT_UD_VIRTUAL_LINK"
|
||
29 | tf_topic_gaze_r = "EYES_RIGHT_UD_VIRTUAL_LINK"
|
||
30 | c1e27eb0 | Simon Schulz | from xsc3_server.msg import * |
31 | topic_joint_target = "/xsc3/custom_joint_state"
|
||
32 | |||
33 | |||
34 | target_neck_pan = 0.0
|
||
35 | target_neck_tilt = 0.0
|
||
36 | target_eyes_tilt = 0.0
|
||
37 | target_eye_l_pan = 0.0
|
||
38 | target_eye_r_pan = 0.0
|
||
39 | 6a233066 | Simon Schulz | |
40 | def fetch_gaze_from_tf(timestamp): |
||
41 | try:
|
||
42 | #fetch right eye
|
||
43 | (trans,rot) = listener.lookupTransform(tf_topic_base, tf_topic_gaze_r, timestamp) |
||
44 | euler = tf.transformations.euler_from_quaternion(rot, axes='sxyz')
|
||
45 | pan_r = -euler[2]
|
||
46 | tilt_r = -euler[1]
|
||
47 | #fetch left eye
|
||
48 | (trans,rot) = listener.lookupTransform(tf_topic_base, tf_topic_gaze_l, timestamp) |
||
49 | euler = tf.transformations.euler_from_quaternion(rot, axes='sxyz')
|
||
50 | pan_l = -euler[2]
|
||
51 | tilt_l = -euler[1]
|
||
52 | #combine to overall gaze direction:
|
||
53 | pan = (pan_l + pan_r) / 2.0 * 180.0 / math.pi |
||
54 | tilt = (tilt_l + tilt_r) / 2.0 * 180.0 / math.pi |
||
55 | return (pan, tilt)
|
||
56 | |||
57 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
|
||
58 | print("> WARNING: no transformation %s-->%s at %s found" % (tf_topic_base, tf_topic_gaze_l, timestamp))
|
||
59 | return (0.0, 0.0) |
||
60 | |||
61 | c1e27eb0 | Simon Schulz | |
62 | def incoming_target_position_meka(msg): |
||
63 | print("> incoming target")
|
||
64 | print msg
|
||
65 | |||
66 | |||
67 | def incoming_target_position_flobi(msg): |
||
68 | global target_neck_pan, target_neck_tilt, target_eyes_tilt, target_eye_l_pan, target_eye_r_pan
|
||
69 | #python 2.x interprets int8_t as string, reconvert this to an array:
|
||
70 | ids = list(array.array("B", msg.id)) |
||
71 | id = ids[0]
|
||
72 | #print("> incoming target for joint %d" % (id))
|
||
73 | if (id == 0x01): |
||
74 | target_neck_pan = msg.target_position[0]
|
||
75 | elif (id == 0x02): |
||
76 | target_neck_tilt = msg.target_position[0]
|
||
77 | elif (id == 0x04): |
||
78 | target_eyes_tilt = msg.target_position[0]
|
||
79 | elif (id == 0x05): |
||
80 | target_eye_l_pan = msg.target_position[0]
|
||
81 | elif (id == 0x06): |
||
82 | target_eye_r_pan = msg.target_position[0]
|
||
83 | |||
84 | 6a233066 | Simon Schulz | def incoming_humotion_gaze_target(msg): |
85 | print("> incoming gaze @%f" % (msg.gaze_timestamp.to_sec()))
|
||
86 | |||
87 | if (msg.gaze_type == gaze.GAZETYPE_RELATIVE):
|
||
88 | #find out the overall gaze state using TF:
|
||
89 | (tf_pan, tf_tilt) = fetch_gaze_from_tf(msg.gaze_timestamp) |
||
90 | #relative targets
|
||
91 | rel_pan = msg.pan |
||
92 | rel_tilt = msg.tilt |
||
93 | #combined:
|
||
94 | target_pan = tf_pan + rel_pan |
||
95 | target_tilt = tf_tilt + rel_tilt |
||
96 | #current position:
|
||
97 | now = rospy.get_rostime() |
||
98 | (now_pan, now_tilt) = fetch_gaze_from_tf(rospy.Time(0))
|
||
99 | now_pan |
||
100 | c1e27eb0 | Simon Schulz | print("%f %f %f %f %f %f %f %f %f %f %f %f %f #DUMP" % (
|
101 | 6a233066 | Simon Schulz | now.to_sec(), |
102 | now_pan, |
||
103 | now_tilt, |
||
104 | msg.gaze_timestamp.to_sec(), |
||
105 | rel_pan, |
||
106 | rel_tilt, |
||
107 | target_pan, |
||
108 | c1e27eb0 | Simon Schulz | target_tilt, |
109 | target_neck_pan, |
||
110 | target_neck_tilt, |
||
111 | target_eyes_tilt, |
||
112 | target_eye_l_pan, |
||
113 | target_eye_r_pan, |
||
114 | 6a233066 | Simon Schulz | )) |
115 | #print("pan = %f, tilt=%f" % (pan* 180.0 / math.pi,tilt* 180.0 / math.pi))
|
||
116 | |||
117 | else:
|
||
118 | print("> NOT dumping absolute targets...")
|
||
119 | |||
120 | |||
121 | |||
122 | |||
123 | if __name__ == '__main__': |
||
124 | new_transform = False
|
||
125 | rospy.init_node('dump_relative_gazetarget')
|
||
126 | |||
127 | #humotion gaze subscriber
|
||
128 | print("> setting up subscriber on %s" % (humotion_gaze_topic))
|
||
129 | rospy.Subscriber(humotion_gaze_topic, gaze, incoming_humotion_gaze_target) |
||
130 | |||
131 | c1e27eb0 | Simon Schulz | #target angles
|
132 | print("> setting up subscriber on %s" % (topic_joint_target))
|
||
133 | if USE_MEKA_TOPICS:
|
||
134 | rospy.Subscriber(humotion_gaze_topic, JointTrajectory, incoming_target_position_meka) |
||
135 | else:
|
||
136 | rospy.Subscriber(topic_joint_target, xsc3_server.msg.joint_state, incoming_target_position_flobi) |
||
137 | 6a233066 | Simon Schulz | |
138 | #tf listener
|
||
139 | listener = tf.TransformListener() |
||
140 | |||
141 | print("> waiting for transform...")
|
||
142 | try:
|
||
143 | listener.waitForTransform(tf_topic_base, tf_topic_gaze_r, rospy.Time(), rospy.Duration(4.0))
|
||
144 | except tf.Exception:
|
||
145 | print("> ERROR: timed out while waiting for transform %s->%s to become available\n" % (tf_topic_base, tf_topic_gaze_r))
|
||
146 | exit(errno.ETIMEDOUT)
|
||
147 | print("> first transform arrived, will start now...")
|
||
148 | |||
149 | rate = rospy.Rate(50.0)
|
||
150 | |||
151 | while not rospy.is_shutdown(): |
||
152 | |||
153 | rate.sleep() |