Revision c1e27eb0 client/python/tools/dump_relative_gazetargets.py

View differences:

client/python/tools/dump_relative_gazetargets.py
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()


  		












Also available in:  Unified diff