hlrc

#!/usr/bin/env python

import roslib

import rospy

import math

import tf

import errno

from humotion.msg import *

"""

small code snippet to dump humotion relative gaze targets

combined with the current gaze direction from tf

"""

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"

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_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 #DUMP" % (

            now.to_sec(),

            now_pan,

            now_tilt,

            msg.gaze_timestamp.to_sec(),

            rel_pan,

            rel_tilt,

            target_pan,

            target_tilt

            ))

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

    #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()