Revision caf7373f

View differences:

include/humotion/server/controller.h
28 28 
#pragma once
29 29 
#include "../gaze_state.h"
30 30 
#include "../mouth_state.h"
31 
#include "../timestamp.h"
31 32 
#include "motion_generator.h"
32 33 
#include "joint_interface.h"
33 34 

  		




  ......




  57
  58
  
    
    motion_generator_vector_t motion_generator_vector;


  		




  58
  59
  
    
    JointInterface *joint_interface;


  		




  59
  60
  
    

  		




  
  61
  
    
    Timestamp last_known_absolute_timestamp;


  		




  
  62
  
    
    double last_known_absolute_target_pan;


  		




  
  63
  
    
    double last_known_absolute_target_tilt;


  		




  
  64
  
    
    double last_known_absolute_target_roll;


  		




  60
  65
  
    

  		




  61
  66
  
    
};


  		




  62
  67
  
    

  		












  

    
      include/humotion/timestamp.h
    
  





  44
  44
  
    

  		




  45
  45
  
    
    static Timestamp now(void);


  		




  46
  46
  
    

  		




  
  47
  
    

  		




  
  48
  
    
    bool is_null(void);


  		




  47
  49
  
    
    double to_seconds(void);


  		




  48
  50
  
    

  		




  49
  51
  
    
    bool operator>  (Timestamp &cmp);


  		












  

    
      include/humotion/timestamped_list.h
    
  





  37
  37
  
    
    typedef std::list<TimestampedFloat> timestamped_float_list_t;


  		




  38
  38
  
    

  		




  39
  39
  
    
    TimestampedList(TimestampedList const &l);


  		




  40
  
  
    
    TimestampedList(unsigned int size=100);


  		




  
  40
  
    
    TimestampedList(unsigned int size=10*100); //10 seconds, this is way too much but should do


  		




  41
  41
  
    
    void insert(Timestamp ts, float val);


  		




  42
  42
  
    
    float get_interpolated_value(Timestamp target_ts);


  		




  43
  43
  
    
    float get_newest_value();


  		




  44
  44
  
    
	static void run_tests();


  		




  45
  45
  
    
    void copy_tsf_list_to(timestamped_float_list_t *target);


  		




  46
  46
  
    
    Timestamp get_last_timestamp();


  		




  
  47
  
    
    Timestamp get_first_timestamp();


  		




  47
  48
  
    

  		




  48
  49
  
    
private:


  		




  49
  50
  
    
    mutable boost::mutex access_mutex;


  		












  

    
      src/server/controller.cpp
    
  





  88
  88
  
    
}


  		




  89
  89
  
    

  		




  90
  90
  
    
GazeState Controller::relative_gaze_to_absolute_gaze(GazeState relative){


  		




  
  91
  
    
    double pan, tilt, roll;


  		




  91
  92
  
    
    GazeState absolute_gaze = relative;


  		




  92
  93
  
    

  		




  93
  94
  
    
    //incoming gaze state wants to set a relative gaze angle


  		




  ......




  95
  96
  
    
    //in time and find out where the head was pointing at that specific time:


  		




  96
  97
  
    
    Timestamp relative_target_timestamp = relative.timestamp;


  		




  97
  98
  
    

  		




  98
  
  
    
    //fetch head / camera pose during that timestamp:


  		




  99
  
  
    
    double neck_pan  = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);


  		




  100
  
  
    
    double eye_l_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);


  		




  101
  
  
    
    double eye_r_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);


  		




  102
  
  
    
    double pan       = neck_pan + (eye_l_pan + eye_r_pan)/2.0;


  		




  103
  
  
    
    //


  		




  104
  
  
    
    double neck_tilt = joint_interface->get_ts_position(JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);


  		




  105
  
  
    
    double eye_tilt  = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);


  		




  106
  
  
    
    double tilt      = neck_tilt + eye_tilt;


  		




  107
  
  
    
    //


  		




  108
  
  
    
    double roll      = joint_interface->get_ts_position(JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);


  		




  
  99
  
    
    //check if this timestamp allows a valid conversion:


  		




  
  100
  
    
    Timestamp history_begin = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();


  		




  
  101
  
    
    Timestamp history_end   = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();


  		




  
  102
  
    

  		




  
  103
  
    
    //printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(), history_begin.to_seconds(), history_end.to_seconds());


  		




  
  104
  
    

  		




  
  105
  
    
    //our history keeps the last n=100 elements in a timestamped list


  		




  
  106
  
    
    if ((relative_target_timestamp < history_begin) || (history_begin.is_null())){


  		




  
  107
  
    
        //when the incoming data is older than that it makes no sense


  		




  
  108
  
    
        //to do any guesswork and try to calculate a valid absolute target


  		




  
  109
  
    
        //therefore we will use the last known targets (see below)


  		




  
  110
  
    
        //in case we did not see this timestamp before, show a warning:


  		




  
  111
  
    
        if (last_known_absolute_timestamp != relative_target_timestamp){


  		




  
  112
  
    
            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f [this should not happen]\n", relative_target_timestamp.to_seconds());


  		




  
  113
  
    
        }


  		




  
  114
  
    
    }else{


  		




  
  115
  
    
        //all fine, we can reconstruct the absolute target:


  		




  
  116
  
    
        //fetch head / camera pose during that timestamp:


  		




  
  117
  
    
        double neck_pan  = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp);


  		




  
  118
  
    
        double eye_l_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp);


  		




  
  119
  
    
        double eye_r_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp);


  		




  
  120
  
    
        last_known_absolute_target_pan       = neck_pan + (eye_l_pan + eye_r_pan)/2.0;


  		




  
  121
  
    
        //


  		




  
  122
  
    
        double neck_tilt = joint_interface->get_ts_position(JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp);


  		




  
  123
  
    
        double eye_tilt  = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp);


  		




  
  124
  
    
        last_known_absolute_target_tilt      = neck_tilt + eye_tilt;


  		




  
  125
  
    
        //


  		




  
  126
  
    
        last_known_absolute_target_roll      = joint_interface->get_ts_position(JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp);


  		




  
  127
  
    
        //safe this timestamp as known:


  		




  
  128
  
    
        last_known_absolute_timestamp = relative_target_timestamp;


  		




  
  129
  
    
    }


  		




  
  130
  
    

  		




  
  131
  
    
    pan  = last_known_absolute_target_pan;


  		




  
  132
  
    
    tilt = last_known_absolute_target_tilt;


  		




  
  133
  
    
    roll = last_known_absolute_target_roll;


  		




  109
  134
  
    

  		




  110
  135
  
    
    //substract offsets:


  		




  111
  
  
    
    pan -= relative.pan_offset;


  		




  
  136
  
    
    pan  -= relative.pan_offset;


  		




  112
  137
  
    
    tilt -= relative.tilt_offset;


  		




  113
  138
  
    
    roll -= relative.roll_offset;


  		




  114
  139
  
    

  		




  ......




  126
  151
  
    
    //ros::Time past = now - ros::Duration(5.0);


  		




  127
  152
  
    
    //listener.waitForTransform("/turtle2", now,J "/turtle1", past, "/world", ros::Duration(1.0));


  		




  128
  153
  
    
    //listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);


  		




  129
  
  
    
    absolute_gaze.dump();


  		




  
  154
  
    
    //absolute_gaze.dump();


  		




  
  155
  
    

  		




  130
  156
  
    
    return absolute_gaze;


  		




  131
  157
  
    
}


  		




  132
  158
  
    

  		












  

    
      src/timestamp.cpp
    
  





  50
  50
  
    
    }


  		




  51
  51
  
    
}


  		




  52
  52
  
    

  		




  
  53
  
    
bool Timestamp::is_null(void){


  		




  
  54
  
    
    return (sec == 0) && (nsec == 0);


  		




  
  55
  
    
}


  		




  
  56
  
    

  		




  53
  57
  
    
bool Timestamp::operator< (Timestamp &cmp){


  		




  54
  58
  
    
    if (sec < cmp.sec){


  		




  55
  59
  
    
        return true;


  		












  

    
      src/timestamped_list.cpp
    
  





  57
  57
  
    
    *target = tsf_list;


  		




  58
  58
  
    
}


  		




  59
  59
  
    

  		




  
  60
  
    

  		




  
  61
  
    
Timestamp TimestampedList::get_first_timestamp(){


  		




  
  62
  
    
    //lock the tsf_list for this access. by doing this we assure


  		




  
  63
  
    
    //that no other thread accessing this element can diturb the


  		




  
  64
  
    
    //following atomic instructions:


  		




  
  65
  
    
    mutex::scoped_lock scoped_lock(access_mutex);


  		




  
  66
  
    

  		




  
  67
  
    
    if (tsf_list.empty()){


  		




  
  68
  
    
        return Timestamp(0,0);


  		




  
  69
  
    
    }


  		




  
  70
  
    

  		




  
  71
  
    
    timestamped_float_list_t::iterator it = tsf_list.begin();


  		




  
  72
  
    
    return it->timestamp;


  		




  
  73
  
    
}


  		




  
  74
  
    

  		




  60
  75
  
    
Timestamp TimestampedList::get_last_timestamp(){


  		




  61
  76
  
    
    //lock the tsf_list for this access. by doing this we assure


  		




  62
  77
  
    
    //that no other thread accessing this element can diturb the


  		












Also available in:  Unified diff