hlrc

#!/usr/bin/env python

import roslib

import rospy

import math

import tf

import errno

from humotion.msg import *

import array

"""

small code snippet to dump humotion relative gaze targets

combined with the current gaze direction from tf

"""

USE_MEKA_TOPICS=True

#joint targets

if USE_MEKA_TOPICS:

    humotion_gaze_topic = "/meka/humotion/gaze/target"

    tf_topic_base   = "upper"

    tf_topic_gaze_l = "shell"

    tf_topic_gaze_r = "shell"

    from trajectory_msgs.msg import *

    topic_joint_target = "/meka_roscontrol/head_position_trajectory_controller/command"

else:

    humotion_gaze_topic = "/flobi/humotion/gaze/target"

    tf_topic_base   = "BASE_LINK"

    tf_topic_gaze_l = "EYES_LEFT_UD_VIRTUAL_LINK"

    tf_topic_gaze_r = "EYES_RIGHT_UD_VIRTUAL_LINK"

    from xsc3_server.msg import *

    topic_joint_target = "/xsc3/custom_joint_state"

target_neck_pan = 0.0

target_neck_tilt = 0.0

target_eyes_tilt = 0.0

target_eye_l_pan = 0.0

target_eye_r_pan = 0.0

def fetch_gaze_from_tf(timestamp):

    try:

        #fetch right eye

        (trans,rot) = listener.lookupTransform(tf_topic_base, tf_topic_gaze_r, timestamp)

        euler  = tf.transformations.euler_from_quaternion(rot, axes='sxyz')

        pan_r  = -euler[2]

        tilt_r = -euler[1]

        #fetch left eye

        (trans,rot) = listener.lookupTransform(tf_topic_base, tf_topic_gaze_l, timestamp)

        euler  = tf.transformations.euler_from_quaternion(rot, axes='sxyz')

        pan_l  = -euler[2]

        tilt_l = -euler[1]

        #combine to overall gaze direction:

        pan  = (pan_l + pan_r) / 2.0 * 180.0 / math.pi

        tilt = (tilt_l + tilt_r) / 2.0 * 180.0 / math.pi

        return (pan, tilt)

    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):

        print("> WARNING: no transformation %s-->%s at %s found" % (tf_topic_base, tf_topic_gaze_l, timestamp))

        return (0.0, 0.0)

def incoming_target_position_meka(msg):

    global target_neck_pan, target_neck_tilt, target_eyes_tilt, target_eye_l_pan, target_eye_r_pan

    #print("> incoming target")

    for x in range(len(msg.joint_names)):

        if (msg.joint_names[x] == "head_j0"):

            target_neck_tilt = msg.points[0].positions[x]

        elif (msg.joint_names[x] == "head_j1"):

            target_neck_pan = msg.points[0].positions[x]

    target_neck_tilt = 0.0

    target_eyes_tilt = 0.0 

    target_eye_l_pan = 0.0

    target_eye_r_pan = 0.0

def incoming_target_position_flobi(msg):

    global target_neck_pan, target_neck_tilt, target_eyes_tilt, target_eye_l_pan, target_eye_r_pan

    #python 2.x interprets int8_t as string, reconvert this to an array:

    ids = list(array.array("B", msg.id))

    id = ids[0]

    #print("> incoming target for joint %d" % (id))

    if (id == 0x01):

        target_neck_pan = - 180.0/math.pi * msg.target_position[0]

    elif (id == 0x02):

        target_neck_tilt =  180.0/math.pi *  msg.target_position[0]

    elif (id == 0x04):

        target_eyes_tilt =  180.0/math.pi * msg.target_position[0]

    elif (id == 0x05):

        target_eye_l_pan =  180.0/math.pi * msg.target_position[0]

    elif (id == 0x06):

        target_eye_r_pan =  180.0/math.pi * msg.target_position[0]

def incoming_humotion_gaze_target(msg):

    print("> incoming gaze @%f" % (msg.gaze_timestamp.to_sec()))

    if (msg.gaze_type == gaze.GAZETYPE_RELATIVE):

        #find out the overall gaze state using TF:

        (tf_pan, tf_tilt) = fetch_gaze_from_tf(msg.gaze_timestamp)

        #relative targets

        rel_pan  = msg.pan

        rel_tilt = msg.tilt

        #combined:

        target_pan  = tf_pan + rel_pan

        target_tilt = tf_tilt + rel_tilt

        #current position:

        now = rospy.get_rostime()

        (now_pan, now_tilt) = fetch_gaze_from_tf(rospy.Time(0))

        now_pan

        print("%f %f %f %f %f %f %f %f %f %f %f %f %f #DUMP" % (

            now.to_sec(),

            now_pan,

            now_tilt,

            msg.gaze_timestamp.to_sec(),

            rel_pan,

            rel_tilt,

            target_pan,

            target_tilt,

            target_neck_pan,

            target_neck_tilt,

            target_eyes_tilt,

            target_eye_l_pan,

            target_eye_r_pan,

            ))

        #print("pan = %f, tilt=%f" % (pan* 180.0 / math.pi,tilt* 180.0 / math.pi))

    else:

        print("> NOT dumping absolute targets...")

if __name__ == '__main__':

    new_transform = False

    rospy.init_node('dump_relative_gazetarget')

    #humotion gaze subscriber

    print("> setting up subscriber on %s" % (humotion_gaze_topic))

    rospy.Subscriber(humotion_gaze_topic, gaze, incoming_humotion_gaze_target)

    #target angles

    print("> setting up subscriber on %s" % (topic_joint_target))

    if USE_MEKA_TOPICS:

        rospy.Subscriber(topic_joint_target,JointTrajectory, incoming_target_position_meka)

    else:

        rospy.Subscriber(topic_joint_target, xsc3_server.msg.joint_state, incoming_target_position_flobi)

    #tf listener

    listener = tf.TransformListener()

    print("> waiting for transform...")

    try:

        listener.waitForTransform(tf_topic_base, tf_topic_gaze_r, rospy.Time(), rospy.Duration(4.0))

    except tf.Exception:

        print("> ERROR: timed out while waiting for transform %s->%s to become available\n" % (tf_topic_base, tf_topic_gaze_r))

        exit(errno.ETIMEDOUT)

    print("> first transform arrived, will start now...")

    rate = rospy.Rate(50.0)

    while not rospy.is_shutdown():

        rate.sleep()