humotion

* Excellence Initiative.

*/

#include "humotion/server/controller.h"

#include "humotion/server/eye_motion_generator.h"

#include "humotion/server/eyebrow_motion_generator.h"

#include "humotion/server/neck_motion_generator.h"

#include "humotion/timestamp.h"

// using namespace std;

// using namespace humotion;

// using namespace humotion::server;

        debug_data.insert(dataset.begin(), dataset.end());

    }

    return debug_data;

}

//! publish all target angles to the devices:

//! NOTE: this is done in an extra loop to have a low delay between consequent sets:

void Controller::publish_targets() {

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

    double pan, tilt, roll;

    humotion::GazeState absolute_gaze = relative;

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

    } else {

        // all fine, we can reconstruct the absolute target

        // fetch head / camera pose during that timestamp

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

                JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);

        last_known_absolute_target_pan_       = neck_pan + (eye_l_pan + eye_r_pan)/2.0;

        //

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

    tilt -= relative.tilt_offset;

    roll -= relative.roll_offset;

    // build up absolute target

    absolute_gaze.gaze_type  = GazeState::GAZETYPE_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);