| 5 |
5 |
import tf
|
| 6 |
6 |
import errno
|
| 7 |
7 |
from humotion.msg import *
|
|
8 |
import array
|
| 8 |
9 |
|
| 9 |
10 |
"""
|
| 10 |
11 |
small code snippet to dump humotion relative gaze targets
|
| 11 |
12 |
combined with the current gaze direction from tf
|
| 12 |
13 |
"""
|
|
14 |
USE_MEKA_TOPICS=False
|
|
15 |
|
| 13 |
16 |
humotion_gaze_topic = "/flobi/humotion/gaze/target"
|
| 14 |
17 |
tf_topic_base = "BASE_LINK"
|
| 15 |
18 |
tf_topic_gaze_l = "EYES_LEFT_UD_VIRTUAL_LINK"
|
| 16 |
19 |
tf_topic_gaze_r = "EYES_RIGHT_UD_VIRTUAL_LINK"
|
| 17 |
20 |
|
|
21 |
#joint targets
|
|
22 |
if USE_MEKA_TOPICS:
|
|
23 |
from trajectory.msg import *
|
|
24 |
topic_joint_target = "/meka_roscontrol/head_position_trajectory_controller/command"
|
|
25 |
else:
|
|
26 |
from xsc3_server.msg import *
|
|
27 |
topic_joint_target = "/xsc3/custom_joint_state"
|
|
28 |
|
|
29 |
|
|
30 |
target_neck_pan = 0.0
|
|
31 |
target_neck_tilt = 0.0
|
|
32 |
target_eyes_tilt = 0.0
|
|
33 |
target_eye_l_pan = 0.0
|
|
34 |
target_eye_r_pan = 0.0
|
| 18 |
35 |
|
| 19 |
36 |
def fetch_gaze_from_tf(timestamp):
|
| 20 |
37 |
try:
|
| ... | ... | |
| 37 |
54 |
print("> WARNING: no transformation %s-->%s at %s found" % (tf_topic_base, tf_topic_gaze_l, timestamp))
|
| 38 |
55 |
return (0.0, 0.0)
|
| 39 |
56 |
|
|
57 |
|
|
58 |
def incoming_target_position_meka(msg):
|
|
59 |
print("> incoming target")
|
|
60 |
print msg
|
|
61 |
|
|
62 |
|
|
63 |
def incoming_target_position_flobi(msg):
|
|
64 |
global target_neck_pan, target_neck_tilt, target_eyes_tilt, target_eye_l_pan, target_eye_r_pan
|
|
65 |
#python 2.x interprets int8_t as string, reconvert this to an array:
|
|
66 |
ids = list(array.array("B", msg.id))
|
|
67 |
id = ids[0]
|
|
68 |
#print("> incoming target for joint %d" % (id))
|
|
69 |
if (id == 0x01):
|
|
70 |
target_neck_pan = msg.target_position[0]
|
|
71 |
elif (id == 0x02):
|
|
72 |
target_neck_tilt = msg.target_position[0]
|
|
73 |
elif (id == 0x04):
|
|
74 |
target_eyes_tilt = msg.target_position[0]
|
|
75 |
elif (id == 0x05):
|
|
76 |
target_eye_l_pan = msg.target_position[0]
|
|
77 |
elif (id == 0x06):
|
|
78 |
target_eye_r_pan = msg.target_position[0]
|
|
79 |
|
| 40 |
80 |
def incoming_humotion_gaze_target(msg):
|
| 41 |
81 |
print("> incoming gaze @%f" % (msg.gaze_timestamp.to_sec()))
|
| 42 |
82 |
|
| ... | ... | |
| 53 |
93 |
now = rospy.get_rostime()
|
| 54 |
94 |
(now_pan, now_tilt) = fetch_gaze_from_tf(rospy.Time(0))
|
| 55 |
95 |
now_pan
|
| 56 |
|
print("%f %f %f %f %f %f %f %f #DUMP" % (
|
|
96 |
print("%f %f %f %f %f %f %f %f %f %f %f %f %f #DUMP" % (
|
| 57 |
97 |
now.to_sec(),
|
| 58 |
98 |
now_pan,
|
| 59 |
99 |
now_tilt,
|
| ... | ... | |
| 61 |
101 |
rel_pan,
|
| 62 |
102 |
rel_tilt,
|
| 63 |
103 |
target_pan,
|
| 64 |
|
target_tilt
|
|
104 |
target_tilt,
|
|
105 |
target_neck_pan,
|
|
106 |
target_neck_tilt,
|
|
107 |
target_eyes_tilt,
|
|
108 |
target_eye_l_pan,
|
|
109 |
target_eye_r_pan,
|
| 65 |
110 |
))
|
| 66 |
111 |
#print("pan = %f, tilt=%f" % (pan* 180.0 / math.pi,tilt* 180.0 / math.pi))
|
| 67 |
112 |
|
| ... | ... | |
| 79 |
124 |
print("> setting up subscriber on %s" % (humotion_gaze_topic))
|
| 80 |
125 |
rospy.Subscriber(humotion_gaze_topic, gaze, incoming_humotion_gaze_target)
|
| 81 |
126 |
|
|
127 |
#target angles
|
|
128 |
print("> setting up subscriber on %s" % (topic_joint_target))
|
|
129 |
if USE_MEKA_TOPICS:
|
|
130 |
rospy.Subscriber(humotion_gaze_topic, JointTrajectory, incoming_target_position_meka)
|
|
131 |
else:
|
|
132 |
rospy.Subscriber(topic_joint_target, xsc3_server.msg.joint_state, incoming_target_position_flobi)
|
| 82 |
133 |
|
| 83 |
134 |
#tf listener
|
| 84 |
135 |
listener = tf.TransformListener()
|