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;

}

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

    }

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

    it--;

    return it->timestamp;

}

void TimestampedList::insert(double 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()){

        return 0.0;

    }

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

        it--;

        return it->value;

}

float TimestampedList::get_interpolated_value(double 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", (TimestampedFloat::get_timestamp() - target_ts)*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);

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

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

        return previous.value;

}

float TimestampedList::interpolate(TimestampedFloat a, TimestampedFloat b, double timestamp){

        //a->timestamp < timestamp <= b->timestamp

    double dist_a = timestamp - a.timestamp;

        double dist_b = b.timestamp - timestamp;

        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(i * 100.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);

        }

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

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

        printf("passed test 3\n");

        exit(0);

}