commit ad7b6f1890f2a05609ec0ec430707de8e9e07f6a
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Thu Feb 11 12:43:30 2016 +0100

    before removal of position control mode

diff --git a/examples/yarp_icub/include/icub_data_receiver.h b/examples/yarp_icub/include/icub_data_receiver.h
index 0b73711..7104a22 100644
--- a/examples/yarp_icub/include/icub_data_receiver.h
+++ b/examples/yarp_icub/include/icub_data_receiver.h
@@ -30,6 +30,8 @@ private:
     yarp::sig::Vector calculate_velocities(yarp::sig::Vector positions,
                               yarp::sig::Vector timestamps);
 
+    void dump_incoming_data();
+
     float target_eye_pan_;
     float target_eye_vergence_;
 
diff --git a/examples/yarp_icub/src/icub_data_receiver.cpp b/examples/yarp_icub/src/icub_data_receiver.cpp
index 1cdbe9b..c7b0065 100644
--- a/examples/yarp_icub/src/icub_data_receiver.cpp
+++ b/examples/yarp_icub/src/icub_data_receiver.cpp
@@ -1,7 +1,8 @@
 #include "icub_data_receiver.h"
 #include <humotion/server/joint_interface.h>
 #include <yarp/os/Property.h>
-//#include <boost/format.hpp>
+#include <boost/format.hpp>
+
 using std::cout;
 using std::cerr;
 using std::string;
@@ -10,9 +11,13 @@ using yarp::dev::IEncodersTimed;
 using yarp::sig::Vector;
 
 #define ICUB_DATA_RECEIVER_USE_ENCODERSPEED 0
+#define ICUB_DATA_RECEIVER_DUMP_DATA 0
 
-iCubDataReceiver::iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface)
-    : yarp::os::RateThread(period) {
+//! constructor
+//! \param period_ms for the yarp rate thread
+//! \param icub_jointinterface
+iCubDataReceiver::iCubDataReceiver(int period_ms, iCubJointInterface *icub_jointinterface)
+    : yarp::os::RateThread(period_ms) {
 
     // store pointer to icub jointinterface
     icub_jointinterface_ = icub_jointinterface;
@@ -33,13 +38,19 @@ iCubDataReceiver::iCubDataReceiver(int period, iCubJointInterface *icub_jointint
     timestamps_.resize(joints);
 }
 
+//! yarp rate thread initializer
 bool iCubDataReceiver::threadInit() {
     return true;
 }
 
+//! yarp thread release function
 void iCubDataReceiver::threadRelease() {
 }
 
+//! manully calculate joint velocities (instead of using joint encoder speeds)
+//! \param positions vector with current encoder position
+//! \param timestamps vector with the associated timestamps
+//! \return velocities as vector
 Vector iCubDataReceiver::calculate_velocities(Vector positions, Vector timestamps) {
     Vector velocities;
     velocities.resize(positions.size());
@@ -64,6 +75,7 @@ Vector iCubDataReceiver::calculate_velocities(Vector positions, Vector timestamp
     return velocities;
 }
 
+//! main loop routine, called by yarp rate thread
 void iCubDataReceiver::run() {
     float velocity;
 
@@ -89,10 +101,18 @@ void iCubDataReceiver::run() {
     //small hack to tell humotion to update the lid angle
     //fixme: use real id
     store_incoming_position(100, 0.0, timestamps_[0]);
+
+    #if ICUB_DATA_RECEIVER_DUMP_DATA
+    dump_incoming_data();
+    #endif
 }
 
-void iCubDataReceiver::store_incoming_position(int icub_id, double value, double timestamp) {
-    cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << value << ")\n";
+//! store incoming position for a given icub joint
+//! \param icub _id icub joint id
+//! \param position
+//! \param timestamp
+void iCubDataReceiver::store_incoming_position(int icub_id, double position, double timestamp) {
+    cout << "iCubDataReceiver::store_incoming_position(icub=" << icub_id << ", " << position << ")\n";
 
     // store joint position in humotion backend
     if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||
@@ -101,9 +121,9 @@ void iCubDataReceiver::store_incoming_position(int icub_id, double value, double
         // instead of using seperate left/right pan the icub uses
         // a combined pan angle and vergence. therfore we have to convert this here:
         if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {
-            target_eye_pan_ = value;
+            target_eye_pan_ = position;
         } else {
-            target_eye_vergence_ = -value;
+            target_eye_vergence_ = -position;
         }
 
         float left  = target_eye_pan_ + target_eye_vergence_/2.0;
@@ -120,15 +140,19 @@ void iCubDataReceiver::store_incoming_position(int icub_id, double value, double
     } else {
         if (icub_id == iCubJointInterface::ID_NECK_PAN) {
             // icub uses an inverted neck pan specification
-            value = -value;
+            position = -position;
         }
 
         // known configured mapping between joint ids
         int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);
-        icub_jointinterface_->store_incoming_position(humotion_id, value, timestamp);
+        icub_jointinterface_->store_incoming_position(humotion_id, position, timestamp);
     }
 }
 
+//! store incoming velocity for a given icub joint
+//! \param icub_id icub joint id
+//! \param velocity
+//! \param timestamp
 void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, double timestamp) {
     cout << "iCubDataReceiver::store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";
 
@@ -167,4 +191,21 @@ void iCubDataReceiver::store_incoming_velocity(int icub_id, double velocity, dou
     }
 }
 
+//! helper for debugging purposes, feed this data into gnuplot for visual inspection
+void iCubDataReceiver::dump_incoming_data() {
+    // use gnuplot for viz:
+    // ./icub_humotion_server --robot icub | grep "INCOMING" |tee  log
+    // @gnuplot: plot "log" using 0:1 w l t "p neck tilt", "log" using 0:2 w l t "v neck tilt", \
+    //                "log" using 0:5 w l t "p neck pan", "log" using 0:6 w l t "v neck pan", \
+    //                "log" using 0:7 w l t "p eyes ud", "log" using 0:8 w l t "v eyes ud", \
+    //                "log" using 0:9 w l t "p eyes vergence", "log" using 0:10 w l t "v eyes vergence"
+    cout << "\n";
+            // publish data to humotion
+    for(int i=0; i<positions_.size(); i++){
+        cout << positions_[i] << " ";
+        cout << velocities_[i] << " ";
+    }
+    cout << " #INCOMING_DATA_DUMP\n";
+}
+
 
