Revision e832c8ef client/python/tools/dump_relative_gazetargets.py
client/python/tools/dump_relative_gazetargets.py | ||
---|---|---|
11 | 11 |
small code snippet to dump humotion relative gaze targets |
12 | 12 |
combined with the current gaze direction from tf |
13 | 13 |
""" |
14 |
USE_MEKA_TOPICS=False |
|
15 |
|
|
16 |
humotion_gaze_topic = "/flobi/humotion/gaze/target" |
|
17 |
tf_topic_base = "BASE_LINK" |
|
18 |
tf_topic_gaze_l = "EYES_LEFT_UD_VIRTUAL_LINK" |
|
19 |
tf_topic_gaze_r = "EYES_RIGHT_UD_VIRTUAL_LINK" |
|
14 |
USE_MEKA_TOPICS=True |
|
20 | 15 |
|
21 | 16 |
#joint targets |
22 | 17 |
if USE_MEKA_TOPICS: |
23 |
from trajectory.msg import * |
|
18 |
humotion_gaze_topic = "/meka/humotion/gaze/target" |
|
19 |
tf_topic_base = "upper" |
|
20 |
tf_topic_gaze_l = "shell" |
|
21 |
tf_topic_gaze_r = "shell" |
|
22 |
from trajectory_msgs.msg import * |
|
24 | 23 |
topic_joint_target = "/meka_roscontrol/head_position_trajectory_controller/command" |
25 | 24 |
else: |
25 |
humotion_gaze_topic = "/flobi/humotion/gaze/target" |
|
26 |
tf_topic_base = "BASE_LINK" |
|
27 |
tf_topic_gaze_l = "EYES_LEFT_UD_VIRTUAL_LINK" |
|
28 |
tf_topic_gaze_r = "EYES_RIGHT_UD_VIRTUAL_LINK" |
|
26 | 29 |
from xsc3_server.msg import * |
27 | 30 |
topic_joint_target = "/xsc3/custom_joint_state" |
28 | 31 |
|
... | ... | |
56 | 59 |
|
57 | 60 |
|
58 | 61 |
def incoming_target_position_meka(msg): |
59 |
print("> incoming target") |
|
60 |
print msg |
|
62 |
global target_neck_pan, target_neck_tilt, target_eyes_tilt, target_eye_l_pan, target_eye_r_pan |
|
63 |
#print("> incoming target") |
|
64 |
for x in range(len(msg.joint_names)): |
|
65 |
if (msg.joint_names[x] == "head_j0"): |
|
66 |
target_neck_tilt = msg.points[0].positions[x] |
|
67 |
elif (msg.joint_names[x] == "head_j1"): |
|
68 |
target_neck_pan = msg.points[0].positions[x] |
|
69 |
target_neck_tilt = 0.0 |
|
70 |
target_eyes_tilt = 0.0 |
|
71 |
target_eye_l_pan = 0.0 |
|
72 |
target_eye_r_pan = 0.0 |
|
73 |
|
|
61 | 74 |
|
62 | 75 |
|
63 | 76 |
def incoming_target_position_flobi(msg): |
... | ... | |
127 | 140 |
#target angles |
128 | 141 |
print("> setting up subscriber on %s" % (topic_joint_target)) |
129 | 142 |
if USE_MEKA_TOPICS: |
130 |
rospy.Subscriber(humotion_gaze_topic, JointTrajectory, incoming_target_position_meka)
|
|
143 |
rospy.Subscriber(topic_joint_target,JointTrajectory, incoming_target_position_meka)
|
|
131 | 144 |
else: |
132 | 145 |
rospy.Subscriber(topic_joint_target, xsc3_server.msg.joint_state, incoming_target_position_flobi) |
133 | 146 |
|
Also available in: Unified diff