commit 6353defa624d2b1f97724258ee138619866f3cfa
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Wed Dec 2 16:44:04 2015 +0100

    timestamp issue

diff --git a/examples/meka/include/mekajointinterface.h b/examples/meka/include/mekajointinterface.h
index 47cd00c..f2f3da6 100644
--- a/examples/meka/include/mekajointinterface.h
+++ b/examples/meka/include/mekajointinterface.h
@@ -60,7 +60,7 @@ private:
     ros::Publisher target_publisher;
 
 
-    double get_timestamp_ms();
+    double get_timestamp_s();
     double target_angle[ICUB_JOINT_ID_ENUM_SIZE];
     double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];
 
diff --git a/examples/meka/src/mekajointinterface.cpp b/examples/meka/src/mekajointinterface.cpp
index a408d09..3f4c3bd 100644
--- a/examples/meka/src/mekajointinterface.cpp
+++ b/examples/meka/src/mekajointinterface.cpp
@@ -6,10 +6,11 @@ using namespace std;
 
 void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){
     //fetch current timestamp
-    double timestamp = get_timestamp_ms();
+    double timestamp = msg.header.stamp.toSec();
 
     //iterate through incoming joints and filter out joints we need:
     for(int i=0; i<msg.name.size(); i++){
+
         string name = msg.name[i];
         //printf("incoming data for joint '%s'\n", name.c_str());
 
@@ -38,6 +39,9 @@ void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & ms
         }
     }
 
+    //dummy data uses current time
+    timestamp = get_timestamp_s();
+
     //store dummy positions for joints we do not know about:
     store_dummy_data(ID_LIP_LEFT_UPPER, timestamp);
     store_dummy_data(ID_LIP_LEFT_LOWER, timestamp);
@@ -97,38 +101,17 @@ void MekaJointInterface::run(){
 }
 
 
-double MekaJointInterface::get_timestamp_ms(){
+double MekaJointInterface::get_timestamp_s(){
     struct timespec spec;
     clock_gettime(CLOCK_REALTIME, &spec);
-    return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;
+    return spec.tv_sec + spec.tv_nsec / 1.0e9;
 }
 
 //! set the target position of a joint
 //! \param enum id of joint
 //! \param float value
 void MekaJointInterface::publish_target_position(int e){
-    #if 0
-    //first: convert humotion enum to our enum:
-    int id = convert_enum_to_motorid(e);
-    if (id == -1){
-        return; //we are not interested in that data, so we just return here
-    }
-
-    if (id == ICUB_ID_NECK_PAN){
-        //PAN seems to be swapped
-        store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
-    }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
-        //icub handles eyes differently, we have to set pan angle + vergence
-        float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
-        float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);
-        //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
-
-        store_joint(ICUB_ID_EYES_PAN, pan);
-        store_joint(ICUB_ID_EYES_VERGENCE, vergence);
-    }else{
-        store_joint(id, joint_target[e]);
-    }
-    #endif
+   //we do this in execute motion for all joints at once...
 }
 
 
diff --git a/include/humotion/server/middleware_ros.h b/include/humotion/server/middleware_ros.h
index fed80c8..175e645 100644
--- a/include/humotion/server/middleware_ros.h
+++ b/include/humotion/server/middleware_ros.h
@@ -59,7 +59,6 @@ public:
     void tick();
 
 private:
-    double convert_ros_to_timestamp_ms(ros::Time t);
     bool tick_necessary;
     void incoming_mouth_target(const humotion::mouth::ConstPtr& msg);
     void incoming_gaze_target(const humotion::gaze::ConstPtr& msg);
diff --git a/src/server/middleware_ros.cpp b/src/server/middleware_ros.cpp
index 80f2930..7277bf8 100644
--- a/src/server/middleware_ros.cpp
+++ b/src/server/middleware_ros.cpp
@@ -98,10 +98,6 @@ void MiddlewareROS::incoming_mouth_target(const humotion::mouth::ConstPtr& msg){
     controller->set_mouth_target(mouth_state);
 }
 
-//! this provides a consistent transformation of ros ts to double
-double MiddlewareROS::convert_ros_to_timestamp_ms(ros::Time t){
-        return t.sec + t.nsec/1000000000.0;
-}
 
 //! callback to handle incoming gaze  target
 void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg){
@@ -131,7 +127,7 @@ void MiddlewareROS::incoming_gaze_target(const humotion::gaze::ConstPtr& msg){
     }else{
         gaze_state.gaze_type = GazeState::GAZETYPE_RELATIVE;
     }
-    gaze_state.timestamp = convert_ros_to_timestamp_ms(msg->header.stamp);
+    gaze_state.timestamp = msg->header.stamp.toSec();
 
     controller->set_gaze_target(gaze_state);
 
