humotion

#include "humotion/server/controller.h"

#include "humotion/server/eye_motion_generator.h"

#include "humotion/server/eyebrow_motion_generator.h"

#include "humotion/server/eyelid_motion_generator.h"

#include "humotion/server/mouth_motion_generator.h"

#include "humotion/server/neck_motion_generator.h"

#include "humotion/timestamp.h"

// using namespace std;

// using namespace humotion;

// using namespace humotion::server;

using humotion::server::Controller;

Controller::~Controller() {

void Controller::init_motion_generators() {

    // NOTE: the order of these generators is important!

    // eye motion generation:

    // eyelid motion generator

    // neck motion generator

    // mouth motion generator

    // eyebrow motion generator

void Controller::add_motion_generator(MotionGenerator *m) {

void Controller::calculate_targets() {

    Controller::motion_generator_vector_t::iterator it;

    for (it = motion_generator_vector.begin(); it < motion_generator_vector.end(); it++) {

void Controller::publish_targets() {

    Controller::motion_generator_vector_t::iterator it;

    for (it = motion_generator_vector.begin(); it < motion_generator_vector.end(); it++) {

humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeState relative) {

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

    // check if this timestamp allows a valid conversion:

    Timestamp history_begin =

            joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();

    // Timestamp history_end   =

    //       joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();

    // printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(),

    // history_begin.to_seconds(), history_end.to_seconds());

    // our history keeps the last n elements in a timestamped list

    if ((relative_target_timestamp < history_begin) || (history_begin.is_null())) {

        // when the incoming data is older than that it makes no sense

        // to do any guesswork and try to calculate a valid absolute target

        // therefore we will use the last known targets (see below)

        // in case we did not see this timestamp before, show a warning:

        if (last_known_absolute_timestamp != relative_target_timestamp) {

            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f "

                   "[this should not happen]\n", relative_target_timestamp.to_seconds());

    } else {

        // all fine, we can reconstruct the absolute target

        // 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 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);

        last_known_absolute_target_roll      = joint_interface->get_ts_position(

                JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);

        // safe this timestamp as known:

    // substract offsets

    // build up absolute target

    printf("pan  now = %4.1f, rel=%4.1f ===> %4.2f\n", pan, relative.pan, absolute_gaze.pan);

    printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n", tilt, relative.tilt, absolute_gaze.tilt);

    // 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);

    // absolute_gaze.dump();

void Controller::set_activated(void) {

void Controller::set_gaze_target(humotion::GazeState new_gaze_target) {

    if (!activated) {

        // not yet initialized, ignore incoming targets

    humotion::GazeState target_gaze;

    // new_gaze_target.dump();

    // relative or absolute gaze update?

    if (new_gaze_target.gaze_type == GazeState::GAZETYPE_RELATIVE) {

        // relative gaze target -> calculate target angles

    } else {

        // already absolute gaze, set this

    Controller::motion_generator_vector_t::iterator it;

    for (it = motion_generator_vector.begin(); it < motion_generator_vector.end(); it++) {

void Controller::set_mouth_target(MouthState s) {

    if (!activated) {

        // not yet initialized, ignore incoming targets

    Controller::motion_generator_vector_t::iterator it;

    for (it = motion_generator_vector.begin(); it < motion_generator_vector.end(); it++) {