commit 4b77b008b100cabdc8035290920e639244a93cdc
Author: Florian Lier <flier@techfak.uni-bielefeld.de>
Date:   Wed Jun 8 09:34:01 2016 +0200

    bughunt

diff --git a/src/server/controller.cpp b/src/server/controller.cpp
index 0bd1a3b..72e6f00 100644
--- a/src/server/controller.cpp
+++ b/src/server/controller.cpp
@@ -25,6 +25,8 @@
 * Excellence Initiative.
 */
 
+#include <string>
+
 #include "humotion/server/controller.h"
 #include "humotion/server/eye_motion_generator.h"
 #include "humotion/server/eyebrow_motion_generator.h"
@@ -33,6 +35,7 @@
 #include "humotion/server/neck_motion_generator.h"
 #include "humotion/timestamp.h"
 
+
 // using namespace std;
 // using namespace humotion;
 // using namespace humotion::server;
@@ -100,9 +103,18 @@ debug_data_t Controller::get_debug_data() {
         debug_data.insert(dataset.begin(), dataset.end());
     }
 
+    // fetch data from controller as well
+    debug_data_t controller_dataset = debug_data_;
+    debug_data.insert(controller_dataset.begin(), controller_dataset.end());
+
     return debug_data;
 }
 
+//! store debug data
+void Controller::store_debug_data(std::string name, float value) {
+    debug_data_[name] = value;
+}
+
 //! 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() {
@@ -114,6 +126,9 @@ void Controller::publish_targets() {
 
 humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeState relative) {
     double pan, tilt, roll;
+    double neck_pan = 0.0;
+    double neck_tilt = 0.0;
+
     humotion::GazeState absolute_gaze = relative;
 
     // incoming gaze state wants to set a relative gaze angle
@@ -146,7 +161,7 @@ humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeSta
     } else {
         // all fine, we can reconstruct the absolute target
         // fetch head / camera pose during that timestamp
-        double neck_pan  = joint_interface_->get_ts_position(
+        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);
@@ -154,7 +169,7 @@ humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeSta
                 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;
         //
-        double neck_tilt = joint_interface_->get_ts_position(
+        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);
@@ -176,15 +191,25 @@ humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeSta
     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;
-    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);
-
+    //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);
+
+    // store debug data:
+    store_debug_data("controller/last_known_absolute_target_pan", last_known_absolute_target_pan_);
+    store_debug_data("controller/pan", pan);
+    store_debug_data("controller/neck_pan", neck_pan);
+    store_debug_data("controller/absolute_gaze_pan", absolute_gaze.pan);
+    // store_debug_data("controller/relative_timestamp", relative_target_timestamp.to_seconds());
+    // printf("BEFORE DBG %f\n", relative.timestamp.to_seconds());
+    std::srand(std::time(0)); // use current time as seed for random generator
+    int random_variable = std::rand();
+    store_debug_data("controller/relative_timestamp2", random_variable);
+    printf("DBG %i\n", random_variable);
     // 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);
