commit 32327f1510747fe65341d6fd61899f904079ad18
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Mon Dec 7 10:42:25 2015 +0100

    moved to cleaner timestamp class and fixed timestamp listsize to 100

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 53a1c44..5a52f07 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -170,8 +170,9 @@ add_library(humotion
     src/server/eyelid_motion_generator.cpp
     src/server/eyebrow_motion_generator.cpp
     src/server/neck_motion_generator.cpp
-    ${DUMMY_HEADER_LIST} 
+    src/timestamp.cpp
     src/timestamped_list.cpp
+    ${DUMMY_HEADER_LIST}
 )
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
diff --git a/include/humotion/gaze_state.h b/include/humotion/gaze_state.h
index 5cd8eeb..5fdcc1e 100644
--- a/include/humotion/gaze_state.h
+++ b/include/humotion/gaze_state.h
@@ -27,6 +27,7 @@
 
 #pragma once
 #include <math.h>
+#include "timestamp.h"
 
 namespace humotion{
 class GazeState{
@@ -49,7 +50,7 @@ public:
         int gaze_type;
 
         //when was this target requested?
-        double timestamp;
+        Timestamp timestamp;
 
         //is this relative or
         enum GAZE_STATE_TYPE{
diff --git a/include/humotion/server/joint_interface.h b/include/humotion/server/joint_interface.h
index b97ddcb..461143e 100644
--- a/include/humotion/server/joint_interface.h
+++ b/include/humotion/server/joint_interface.h
@@ -104,8 +104,8 @@ public:
 
 protected:
 
-    void store_incoming_position(int joint_id, float position, double timestamp);
-    void store_incoming_speed(int joint_id, float speed, double timestamp);
+    void store_incoming_position(int joint_id, float position, Timestamp timestamp);
+    void store_incoming_speed(int joint_id, float speed, Timestamp timestamp);
 
     virtual void enable_joint(int id) = 0;
     virtual void disable_joint(int id) = 0;
diff --git a/include/humotion/timestamp.h b/include/humotion/timestamp.h
new file mode 100644
index 0000000..867bfce
--- /dev/null
+++ b/include/humotion/timestamp.h
@@ -0,0 +1,57 @@
+/*
+* This file is part of humotion
+*
+* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
+* http://opensource.cit-ec.de/projects/humotion
+*
+* This file may be licensed under the terms of the
+* GNU Lesser General Public License Version 3 (the ``LGPL''),
+* or (at your option) any later version.
+*
+* Software distributed under the License is distributed
+* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
+* express or implied. See the LGPL for the specific language
+* governing rights and limitations.
+*
+* You should have received a copy of the LGPL along with this
+* program. If not, go to http://www.gnu.org/licenses/lgpl.html
+* or write to the Free Software Foundation, Inc.,
+* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+*
+* The development of this software was supported by the
+* Excellence Cluster EXC 277 Cognitive Interaction Technology.
+* The Excellence Cluster EXC 277 is a grant of the Deutsche
+* Forschungsgemeinschaft (DFG) in the context of the German
+* Excellence Initiative.
+*/
+
+#pragma once
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string>
+
+namespace humotion{
+
+class Timestamp{
+public:
+    Timestamp(void);
+    Timestamp(int32_t _sec, int32_t _nsec);
+    void set(int32_t _sec, int32_t _nsec);
+
+    double to_seconds(void);
+
+    bool operator>  (Timestamp &cmp);
+    bool operator<= (Timestamp &cmp);
+    bool operator<  (Timestamp &cmp);
+    bool operator>= (Timestamp &cmp);
+    bool operator== (Timestamp &cmp);
+    bool operator!= (Timestamp &cmp);
+
+    int32_t sec;
+    int32_t nsec;
+
+};
+
+
+}
diff --git a/include/humotion/timestamped_float.h b/include/humotion/timestamped_float.h
index 8016d81..54817ce 100644
--- a/include/humotion/timestamped_float.h
+++ b/include/humotion/timestamped_float.h
@@ -30,29 +30,29 @@
 #include <stdio.h>
 #include <unistd.h>
 #include <string>
+#include "timestamp.h"
 
 namespace humotion{
 
 class TimestampedFloat{
 public:
-    TimestampedFloat(){
-		timestamp = 0.0;
+    TimestampedFloat() : timestamp(0, 0){
 		value = 0.0;
 	}
 	
-    TimestampedFloat(double ts, float val){
+    TimestampedFloat(Timestamp ts, float val){
 		timestamp = ts;
 		value = val;
 	}
 	
-	static double get_timestamp(){
+    /*static double get_timestamp(){
 		//fetch time
 		struct timespec tp;
 		clock_gettime(CLOCK_REALTIME, &tp);
 		return tp.tv_sec+tp.tv_nsec/1000000000.0;
-	};
+    };*/
 	
-	double timestamp;
+    Timestamp timestamp;
     float value;
 	
 };
diff --git a/include/humotion/timestamped_list.h b/include/humotion/timestamped_list.h
index c67dbd5..f8cc3b3 100644
--- a/include/humotion/timestamped_list.h
+++ b/include/humotion/timestamped_list.h
@@ -29,24 +29,25 @@
 #include "timestamped_float.h"
 #include <boost/thread/mutex.hpp>
 #include <list>
+#include "timestamp.h"
 
 namespace humotion{
 class TimestampedList{
 public:
     typedef std::list<TimestampedFloat> timestamped_float_list_t;
 
-    TimestampedList( TimestampedList const &l);
-	TimestampedList(unsigned int size=10);
-    void insert(double ts, float val);
-    float get_interpolated_value(double target_ts);
+    TimestampedList(TimestampedList const &l);
+    TimestampedList(unsigned int size=100);
+    void insert(Timestamp ts, float val);
+    float get_interpolated_value(Timestamp target_ts);
     float get_newest_value();
 	static void run_tests();
     void copy_tsf_list_to(timestamped_float_list_t *target);
-    double get_last_timestamp();
+    Timestamp get_last_timestamp();
 
 private:
     mutable boost::mutex access_mutex;
-    float interpolate(TimestampedFloat a, TimestampedFloat b, double timestamp);
+    float interpolate(TimestampedFloat a, TimestampedFloat b, Timestamp timestamp);
     timestamped_float_list_t tsf_list;
 };
 
diff --git a/msg/gaze.msg b/msg/gaze.msg
index 5cde44d..9318c06 100644
--- a/msg/gaze.msg
+++ b/msg/gaze.msg
@@ -1,6 +1,7 @@
 Header header
 
 #type of pan/tilt/roll targets
+time gaze_timestamp
 int32 gaze_type
 int32 GAZETYPE_ABSOLUTE=0
 int32 GAZETYPE_RELATIVE=1
@@ -10,7 +11,6 @@ float32 tilt
 float32 roll
 float32 vergence
 
-
 float32 pan_offset
 float32 tilt_offset
 float32 roll_offset
diff --git a/src/gaze_state.cpp b/src/gaze_state.cpp
index ab1624d..72a18c6 100644
--- a/src/gaze_state.cpp
+++ b/src/gaze_state.cpp
@@ -56,11 +56,12 @@ GazeState::~GazeState(){
 
 void GazeState::dump(){
     //dump values to stdout:
-    printf("> GAZE STATE: %4.2f [%2.1f] %4.2f [%2.1f] %4.2f [%2.1f] (PTR) vergence=%4.2f eyelid_opening=%4.2f/%4.2f eyebrows = %4.2f %4.2f type=%s eyeblink_requests=%d %d\n",
+    printf("> GAZE STATE: %4.2f [%2.1f] %4.2f [%2.1f] %4.2f [%2.1f] (PTR) vergence=%4.2f eyelid_opening=%4.2f/%4.2f eyebrows = %4.2f %4.2f type=%s [ts=%.3f] eyeblink_requests=%d %d\n",
            pan, pan_offset, tilt, tilt_offset, roll, roll_offset, vergence,
            eyelid_opening_upper,eyelid_opening_lower,
            eyebrow_left, eyebrow_right,
            (gaze_type==GAZETYPE_ABSOLUTE?"absolute":(gaze_type==GAZETYPE_RELATIVE?"relative":(gaze_type==GAZETYPE_OVERRIDE?"override":"!INVALID!"))),
+           timestamp.to_seconds(),
            eyeblink_request_left, eyeblink_request_right
            );
 }
diff --git a/src/server/controller.cpp b/src/server/controller.cpp
index d3b152a..67c75cb 100644
--- a/src/server/controller.cpp
+++ b/src/server/controller.cpp
@@ -31,6 +31,7 @@
 #include "server/eyebrow_motion_generator.h"
 #include "server/neck_motion_generator.h"
 #include "server/mouth_motion_generator.h"
+#include "timestamp.h"
 
 using namespace std;
 using namespace humotion;
@@ -92,7 +93,7 @@ GazeState Controller::relative_gaze_to_absolute_gaze(GazeState 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;
+    Timestamp 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);
diff --git a/src/server/gaze_motion_generator.cpp b/src/server/gaze_motion_generator.cpp
index 0993547..85abb8e 100644
--- a/src/server/gaze_motion_generator.cpp
+++ b/src/server/gaze_motion_generator.cpp
@@ -42,7 +42,8 @@ const float GazeMotionGenerator::OMR_LIMIT_TRIGGERS_NECK_SACCADE = 0.95; //0.95
 
 
 //! constructor
-GazeMotionGenerator::GazeMotionGenerator(JointInterface *j, int dof, float t) : ReflexxesMotionGenerator(j, dof, t){
+GazeMotionGenerator::GazeMotionGenerator(JointInterface *j, int dof, float t)
+    : ReflexxesMotionGenerator(j, dof, t){
 }
 
 //! destructor
@@ -61,7 +62,7 @@ void GazeMotionGenerator::set_gaze_target(GazeState new_gaze_target){
     float dist = fabs(requested_gaze_state.distance_pt_abs(new_gaze_target));
 
     //check requested speed
-    float speed = fabs(requested_gaze_state.distance_pt_abs(new_gaze_target)) / (new_gaze_target.timestamp-requested_gaze_state.timestamp);
+    float speed = fabs(requested_gaze_state.distance_pt_abs(new_gaze_target)) / (new_gaze_target.timestamp.to_seconds()-requested_gaze_state.timestamp.to_seconds());
 
     //check magnitude and speed of gaze change to detect eye-neck saccades:
     if (dist > NECK_SACCADE_THRESHOLD){
diff --git a/src/server/joint_interface.cpp b/src/server/joint_interface.cpp
index 35813df..dc0ef87 100644
--- a/src/server/joint_interface.cpp
+++ b/src/server/joint_interface.cpp
@@ -66,13 +66,13 @@ float JointInterface::get_target_position(int joint_id){
 //! \param joint name
 //! \param position
 //! \param timestamp when the position was measured
-void JointInterface::store_incoming_position(int joint_id, float position, double timestamp){
+void JointInterface::store_incoming_position(int joint_id, float position, Timestamp timestamp){
     //lock the tsd_list for this access. by doing this we assure
     //that no other thread accessing this element can diturb the
     //following atomic instructions:
     mutex::scoped_lock scoped_lock(joint_ts_position_map_access_mutex);
 
-    //printf("> humotion: incoming joint position for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,position,timestamp);
+    //printf("> humotion: incoming joint position for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
     joint_ts_position_map[joint_id].insert(timestamp, position);
 
     incoming_position_count++;
@@ -91,13 +91,13 @@ unsigned int JointInterface::get_and_clear_incoming_position_count(){
 //! \param joint name
 //! \param speed
 //! \param timestamp when the position was measured
-void JointInterface::store_incoming_speed(int joint_id, float speed, double timestamp){
+void JointInterface::store_incoming_speed(int joint_id, float speed, Timestamp timestamp){
     //lock the tsd_list for this access. by doing this we assure
     //that no other thread accessing this element can diturb the
     //following atomic instructions:
     mutex::scoped_lock scoped_lock(joint_ts_speed_map_access_mutex);
 
-    //printf("> humotion: incoming joint speed for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,speed,timestamp);
+    //printf("> humotion: incoming joint speed for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,speed,timestamp.to_seconds());
     joint_ts_speed_map[joint_id].insert(timestamp, speed);
 }
 
diff --git a/src/server/middleware_ros.cpp b/src/server/middleware_ros.cpp
index 7277bf8..85fd84f 100644
--- a/src/server/middleware_ros.cpp
+++ b/src/server/middleware_ros.cpp
@@ -127,7 +127,8 @@ void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg){
     }else{
         gaze_state.gaze_type = GazeState::GAZETYPE_RELATIVE;
     }
-    gaze_state.timestamp = msg->header.stamp.toSec();
+
+    gaze_state.timestamp.set(msg->gaze_timestamp.sec, msg->gaze_timestamp.nsec);
 
     controller->set_gaze_target(gaze_state);
 
diff --git a/src/timestamp.cpp b/src/timestamp.cpp
new file mode 100644
index 0000000..904fe79
--- /dev/null
+++ b/src/timestamp.cpp
@@ -0,0 +1,78 @@
+#include "timestamp.h"
+
+using namespace std;
+using namespace humotion;
+
+Timestamp::Timestamp(void){
+    //init of an empty timestamp will be assigned to the current system time
+    struct timespec tp;
+    clock_gettime(CLOCK_REALTIME, &tp);
+    sec  = tp.tv_sec;
+    nsec = tp.tv_nsec;
+}
+
+Timestamp::Timestamp(int32_t _sec, int32_t _nsec){
+    set(_sec, _nsec);
+}
+
+void Timestamp::set(int32_t _sec, int32_t _nsec){
+    sec  = _sec;
+    nsec = _nsec;
+}
+
+double Timestamp::to_seconds(void){
+    return sec + ((double)nsec)/1000000000.0;
+}
+
+bool Timestamp::operator<= (Timestamp &cmp){
+    if (sec < cmp.sec){
+        return true;
+    }else if (sec > cmp.sec){
+        return false;
+    }else{ //(a.sec == b.sec)
+        //seconds are equal, check nsec:
+        return (nsec <= cmp.nsec);
+    }
+}
+
+bool Timestamp::operator< (Timestamp &cmp){
+    if (sec < cmp.sec){
+        return true;
+    }else if (sec > cmp.sec){
+        return false;
+    }else{ //(a.sec == b.sec)
+        //seconds are equal, check nsec:
+        return (nsec < cmp.nsec);
+    }
+}
+
+bool Timestamp::operator>= (Timestamp &cmp){
+    if (sec > cmp.sec){
+        return true;
+    }else if (sec < cmp.sec){
+        return false;
+    }else{ //(a.sec == b.sec)
+        //seconds are equal, check nsec:
+        return (nsec >= cmp.nsec);
+    }
+}
+
+bool Timestamp::operator> (Timestamp &cmp){
+    if (sec > cmp.sec){
+        return true;
+    }else if (sec < cmp.sec){
+        return false;
+    }else{ //(a.sec == b.sec)
+        //seconds are equal, check nsec:
+        return (nsec > cmp.nsec);
+    }
+}
+
+bool Timestamp::operator== (Timestamp &cmp){
+    return (sec == cmp.sec) && (nsec == cmp.nsec);
+}
+
+bool Timestamp::operator!= (Timestamp &cmp){
+    return !(*this == cmp);
+}
+
diff --git a/src/timestamped_list.cpp b/src/timestamped_list.cpp
index 3172b64..2f723fe 100644
--- a/src/timestamped_list.cpp
+++ b/src/timestamped_list.cpp
@@ -57,14 +57,14 @@ void TimestampedList::copy_tsf_list_to(timestamped_float_list_t *target){
     *target = tsf_list;
 }
 
-double TimestampedList::get_last_timestamp(){
+Timestamp TimestampedList::get_last_timestamp(){
     //lock the tsf_list for this access. by doing this we assure
     //that no other thread accessing this element can diturb the
     //following atomic instructions:
     mutex::scoped_lock scoped_lock(access_mutex);
 
     if (tsf_list.empty()){
-        return 0.0;
+        return Timestamp(0,0);
     }
 
     timestamped_float_list_t::iterator it = tsf_list.end();
@@ -72,7 +72,7 @@ double TimestampedList::get_last_timestamp(){
     return it->timestamp;
 }
 
-void TimestampedList::insert(double timestamp, float val){
+void TimestampedList::insert(Timestamp timestamp, float val){
 	//erase first element:
     tsf_list.pop_front();
     tsf_list.push_back(TimestampedFloat(timestamp, val));
@@ -86,6 +86,7 @@ float TimestampedList::get_newest_value(){
     mutex::scoped_lock scoped_lock(access_mutex);
 
     if (tsf_list.empty()){
+        printf("> WARNING: requested newest value from empty list, returning 0.0\n");
         return 0.0;
     }
 
@@ -94,7 +95,7 @@ float TimestampedList::get_newest_value(){
 	return it->value;
 }
 
-float TimestampedList::get_interpolated_value(double target_ts){
+float TimestampedList::get_interpolated_value(Timestamp target_ts){
     //lock the tsf_list for this access. by doing this we assure
     //that no other thread accessing this element can diturb the
     //following atomic instructions:
@@ -103,14 +104,14 @@ float TimestampedList::get_interpolated_value(double target_ts){
     ///target_ts -= 0.001 * 4;
 	
     TimestampedFloat previous;
-    //printf("> latency %3.2fms\n", (TimestampedFloat::get_timestamp() - target_ts)*1000.0);
+    //printf("> latency %3.2fms\n", (Timestamped().to_seconds() - target_ts.to_seconds())*1000.0);
 
     for ( timestamped_float_list_t::iterator it = tsf_list.begin(); it != tsf_list.end(); ++it ){
 		if (it->timestamp >= target_ts){
 			//ok found close target
             if (it == tsf_list.begin()){
 				//no preceding element
-                printf("> warning, timestamp %6.3f smaller than first element %6.3f in timestamped list. this should not happen (increase ts buffer?)\n",target_ts,tsf_list.begin()->timestamp);
+                printf("> warning, timestamp %6.3f smaller than first element %6.3f in timestamped list. this should not happen (increase ts buffer?)\n",target_ts.to_seconds(),tsf_list.begin()->timestamp.to_seconds());
 				return it->value;
 			}else{
 				//do interpolation
@@ -119,18 +120,19 @@ float TimestampedList::get_interpolated_value(double target_ts){
 		}
 		previous = *it;
 	}
+
 	//we reached the end, return the last value
-	printf("> warning: found no timestamp bigger than %f in timestamped list...\n", target_ts);
+    printf("> warning: found no timestamp bigger than %f in timestamped list...\n", target_ts.to_seconds());
 	printf("           this should not happen as images will always be behind\n");
-	printf("           the motor data. returning most recent value (ts=%f)\n", previous.timestamp);
+    printf("           the motor data. returning most recent value (ts=%f)\n", previous.timestamp.to_seconds());
 	
 	return previous.value;
 }
 
-float TimestampedList::interpolate(TimestampedFloat a, TimestampedFloat b, double timestamp){
+float TimestampedList::interpolate(TimestampedFloat a, TimestampedFloat b, Timestamp timestamp){
 	//a->timestamp < timestamp <= b->timestamp
-    double dist_a = timestamp - a.timestamp;
-	double dist_b = b.timestamp - timestamp;
+    double dist_a = timestamp.to_seconds() - a.timestamp.to_seconds();
+    double dist_b = b.timestamp.to_seconds() - timestamp.to_seconds();
 	double dist   = dist_a + dist_b;
 	
     float interpolation = a.value + (dist_a / dist) * (b.value - a.value);
@@ -144,31 +146,32 @@ void TimestampedList::run_tests(){
 	TimestampedList list(size);
 	
 	for(int i=0; i<size; i++){
-		list.insert(i * 100.0, i*10.0);
+        list.insert(Timestamp(i * 100.0,0), i*10.0);
 	}
 	
 	//test algorithm:
 	
 	//test exact match:
 	for(int i=0; i<size; i++){
-		printf("> testing get_interpolated_value(%f) == %f (value read back = %f)\n",i*100.0, i*10.0, list.get_interpolated_value(i*100.0));
-		assert( list.get_interpolated_value(i*100.0) == i*10.0);
+        Timestamp ts(i*100.0,0);
+        printf("> testing get_interpolated_value(%f) == %f (value read back = %f)\n",ts.to_seconds(), i*10.0, list.get_interpolated_value(ts));
+        assert( list.get_interpolated_value(ts) == i*10.0);
 	}
 	printf("passed test 1\n");
 	
-	assert(list.get_interpolated_value(0.0) == 0.0);
-	assert(list.get_interpolated_value(110.0) == 11.0);
-	assert(list.get_interpolated_value(150.0) == 15.0);
-	assert(list.get_interpolated_value(999990.0) == 90.0);
+    assert(list.get_interpolated_value(Timestamp(0.0,0)) == 0.0);
+    assert(list.get_interpolated_value(Timestamp(110.0,0)) == 11.0);
+    assert(list.get_interpolated_value(Timestamp(150.0,0)) == 15.0);
+    assert(list.get_interpolated_value(Timestamp(999990.0,0)) == 90.0);
 	
 	printf("passed test 2\n");
 	
-	list.insert(1000.0, 200.0);
-	list.insert(1300.0, -100.0);
-	assert(list.get_interpolated_value(1100) == 100.0);
-	assert(list.get_interpolated_value(1200) == 0.0);
-	assert(list.get_interpolated_value(1300) == -100.0);
-	assert(list.get_interpolated_value(1250) == -50.0);
+    list.insert(Timestamp(1000.0 ,0), 200.0);
+    list.insert(Timestamp(1300.0, 0), -100.0);
+    assert(list.get_interpolated_value(Timestamp(1100, 0)) == 100.0);
+    assert(list.get_interpolated_value(Timestamp(1200, 0)) == 0.0);
+    assert(list.get_interpolated_value(Timestamp(1300, 0)) == -100.0);
+    assert(list.get_interpolated_value(Timestamp(1250, 0)) == -50.0);
 	
 	printf("passed test 3\n");
 	exit(0);
