/src/timestamped_list.cpp - Diff - humotion - Research for Cognitive Interaction

Revision 0c8d22a5 src/timestamped_list.cpp

     * Excellence Initiative.
     */
     #include "timestamped_list.h"
     #include "assert.h"
     #include <assert.h>
     using namespace std;
     using namespace boost;
     using namespace humotion;
     #include "humotion/timestamped_list.h"
     TimestampedList::TimestampedList(unsigned int s){
         //initialize the list to its desired size:
     using boost::mutex;
     using humotion::TimestampedList;
     TimestampedList::TimestampedList(unsigned int s) {
         // initialize the list to its desired size:
         TimestampedFloat now;
         tsf_list.resize(s, now);
+    }
     TimestampedList::TimestampedList(TimestampedList const &l){
         //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:
     TimestampedList::TimestampedList(TimestampedList const &l) {
         // 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(l.access_mutex);
         //now do a deep copy with locking!
         // now do a deep copy with locking!
         tsf_list = l.tsf_list;
+    }
     void TimestampedList::copy_tsf_list_to(timestamped_float_list_t *target){
         //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:
     void TimestampedList::copy_tsf_list_to(timestamped_float_list_t *target) {
         // 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);
         *target = tsf_list;
+    }
     Timestamp TimestampedList::get_first_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:
     humotion::Timestamp TimestampedList::get_first_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 Timestamp(0,0);
         if (tsf_list.empty()) {
             return Timestamp(0, 0);
+        }
         timestamped_float_list_t::iterator it = tsf_list.begin();
         return it->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:
     humotion::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 Timestamp(0,0);
         if (tsf_list.empty()) {
             return Timestamp(0, 0);
+        }
         timestamped_float_list_t::iterator it = tsf_list.end();
-...
         return it->timestamp;
+    }
     void TimestampedList::insert(Timestamp timestamp, float val){
         //erase first element:
     void TimestampedList::insert(Timestamp timestamp, float val) {
         // erase first element:
         tsf_list.pop_front();
         tsf_list.push_back(TimestampedFloat(timestamp, val));
         //printf("insert [%5.3f] = %5.1f\n",timestamp,val);
         // printf("insert [%5.3f] = %5.1f\n",timestamp,val);
+    }
     float TimestampedList::get_newest_value(){
         //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:
     float TimestampedList::get_newest_value() {
         // 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()){
         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(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:
     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:
         mutex::scoped_lock scoped_lock(access_mutex);
         ///target_ts -= 0.001 * 4;
         TimestampedFloat previous;
         //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.to_seconds(),tsf_list.begin()->timestamp.to_seconds());
         // 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.to_seconds(), tsf_list.begin()->timestamp.to_seconds());
                     return it->value;
                 }else{
                     //do interpolation
                 } else {
                     // do interpolation
                     return interpolate(*it, previous, target_ts);
+                }
+            }
             previous = *it;
+        }
         //we reached the end, return the last value
         printf("> warning: found no timestamp >= than %f in timestamped list...\n", target_ts.to_seconds());
         // we reached the end, return the last value
         printf("> warning: found no timestamp >= 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.to_seconds());
         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, Timestamp timestamp){
         //a->timestamp < timestamp <= b->timestamp
     float TimestampedList::interpolate(TimestampedFloat a, TimestampedFloat b, Timestamp timestamp) {
         // a->timestamp < timestamp <= b->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;
-...
+    }
     ///tests
     void TimestampedList::run_tests(){
     // tests
     void TimestampedList::run_tests() {
         int size = 10;
         TimestampedList list(size);
         for(int i=0; i<size; i++){
             list.insert(Timestamp(i * 100.0,0), i*10.0);
         for (int i = 0; i < size; i++) {
             list.insert(Timestamp(i * 100.0, 0), i*10.0);
+        }
         //test algorithm:
         // test algorithm:
         //test exact match:
         for(int i=0; i<size; i++){
             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);
         // test exact match:
         for (int i =0 ; i < size; i++) {
             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(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);
         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(Timestamp(1000.0 ,0), 200.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(1250, 0)) == -50.0);
         printf("passed test 3\n");
         exit(0);
         exit(EXIT_SUCCESS);
+    }

Also available in: Unified diff