/ - Diff - humotion - Research for Cognitive Interaction

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

     #pragma once
     #include <math.h>
     #include "timestamp.h"
     namespace humotion{
     class GazeState{
-...
             int gaze_type;
             //when was this target requested?
             double timestamp;
             Timestamp timestamp;
             //is this relative or
             enum GAZE_STATE_TYPE{

     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;

     /*
     * 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;
     };
+    }

     #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;
     };

     #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;
     };

     Header header
     #type of pan/tilt/roll targets
     time gaze_timestamp
     int32 gaze_type
     int32 GAZETYPE_ABSOLUTE=0
     int32 GAZETYPE_RELATIVE=1
-...
     float32 roll
     float32 vergence
     float32 pan_offset
     float32 tilt_offset
     float32 roll_offset

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

     #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;
-...
         //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);

     //! 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
-...
         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){

     //! \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++;
-...
     //! \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);
+    }

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

     #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);
+    }

         *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();
-...
         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));
-...
         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;
+        }
-...
     	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:
-...
         ///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
-...
+    		}
     		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);
-...
     	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);

humotion

Revision 32327f15