humotion

/*

* 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.

*/

#include "timestamped_list.h"

#include "assert.h"

using namespace std;

using namespace boost;

using namespace humotion;

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:

    mutex::scoped_lock scoped_lock(l.access_mutex);

    //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:

    mutex::scoped_lock scoped_lock(access_mutex);

    *target = tsf_list;

}

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

    }

    timestamped_float_list_t::iterator it = tsf_list.end();

    it--;

    return it->timestamp;

}

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

}

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()){

        printf("> WARNING: requested newest value from empty list, returning 0.0\n");

        return 0.0;

    }

    timestamped_float_list_t::iterator it = tsf_list.end();

    it--;

    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:

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

                return it->value;

            }else{

                //do interpolation

                return interpolate(*it, previous, 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.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());

    return previous.value;

}

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;

    float interpolation = a.value + (dist_a / dist) * (b.value - a.value);

    return interpolation;

}

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

    }

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

    }

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

    printf("passed test 2\n");

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

}