| 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 |
|