humotion

GazeState Controller::relative_gaze_to_absolute_gaze(GazeState relative){

    GazeState absolute_gaze = relative;

    //incoming gaze state wants to set a relative gaze angle

    //in order to calc the new absolute gaze, we need to go back

    //in time and find out where the head was pointing at that specific time:

    double relative_target_timestamp = relative.timestamp;

    //fetch head / camera pose during that timestamp:

    double neck_pan  = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);

    double eye_l_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);

    double eye_r_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);

    double pan       = neck_pan + (eye_l_pan + eye_r_pan)/2.0;

    //

    double neck_tilt = joint_interface->get_ts_position(JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);

    double eye_tilt  = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);

    double tilt      = neck_tilt + eye_tilt;

    //

    double roll      = joint_interface->get_ts_position(JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);

    //build up absolute target:

    absolute_gaze.type  = GazeState::ABSOLUTE;

    absolute_gaze.pan   = pan + relative.pan;

    absolute_gaze.tilt  = tilt + relative.tilt;

    absolute_gaze.roll  = roll + relative.roll;

    //FIXME: use ros TF for that calculation...

    //see http://wiki.ros.org/tf/Tutorials/Time%20travel%20with%20tf%20%28C%2B%2B%29

    //ros::Time past = now - ros::Duration(5.0);

    //listener.waitForTransform("/turtle2", now,J "/turtle1", past, "/world", ros::Duration(1.0));

    //listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);

    return absolute_gaze;

}

    GazeState target_gaze;

    //relative or absolute gaze update?

    if (new_gaze_target.type == GazeState::RELATIVE){

        //relative gaze target -> calculate target angles

        target_gaze = relative_gaze_to_absolute_gaze(new_gaze_target);

    }else{

        //already absolute gaze, set this

        target_gaze = new_gaze_target;

    }

        (*it)->set_gaze_target(target_gaze);