StampedDataQueue.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_STAMPEDDATAQUEUE_H_
#define _MIRA_STAMPEDDATAQUEUE_H_

#include <error/Exceptions.h>
#include <utils/Stamped.h>

#include <math/Eigen.h>

namespace mira {


template <typename T >
class StampedDataQueue
{
public:

    typedef std::list<Stamped<T>, Eigen::aligned_allocator<T>> Container;

    typedef Stamped<T> value_type;

    typedef typename Container::const_iterator const_iterator;

    typedef typename Container::size_type size_type;

public:

    StampedDataQueue(size_type maxCapacity=100,
                     Duration maxStorageTime = Duration::seconds(10) ) :
        mMaxCapacity(maxCapacity), mMaxStorageTime(maxStorageTime) {}

public:

    void setMaxStorageTime(Duration maxStorageTime)
    {
        mMaxStorageTime = maxStorageTime;
        prune();
    }

    // STL conform operations:

    size_type capacity() const { return mMaxCapacity; }

    void reserve(size_type n) {
        if(n>mMaxCapacity)
            mMaxCapacity=n;
    }


    size_type size() const { return mStorage.size(); }

    bool empty() const { return mStorage.empty(); }


    bool insert(const Stamped<T>& data)
    {
        typename Container::iterator it = mStorage.begin();

        // check if new data is too old, if so, then skip it
        if(it!=mStorage.end() && it->timestamp > data.timestamp + mMaxStorageTime)
            return false;

        // find position to insert the data
        for(;it != mStorage.end(); ++it)
        {
            // if the timestamp of the item at "it" is older, then we need to insert
            // before "it"
            if (data.timestamp >= it->timestamp)
                break;
        }

        // guarantee STRICTLY monotonic decreasing and overwrite
        // an existing entry with the same timestamp, if any!
        if(it!=mStorage.end() && it->timestamp==data.timestamp)
            *it = data;
        else {
            // if data is older than all elements in the queue and queue is 
            // already full skip insertion
            if (it == mStorage.end() && mStorage.size() == mMaxCapacity)
                return false;
            // otherwise insert the data before "it"
            mStorage.insert(it, data);

            // prune the data (throw away oldest elements if necessary)
            prune();
        }

        return true;
    }

    void clear() { mStorage.clear(); }

public:

    const Stamped<T>& getData(const Time& timestamp) const
    {
        if (mStorage.empty())
            MIRA_THROW(XRuntime, "Trying to get data from empty StampedDataQueue");

        const_iterator older = findFirstOlderItem(timestamp);
        // older already is the first element -> there is no newer one
        if(older==mStorage.begin()) 
            return *older;

        const_iterator newer = older;
        --newer;

        // there is no older element return the newer one (oldest in queue)
        if(older==mStorage.end())
            return *newer;

        // return closest value
        if( (newer->timestamp - timestamp) <
            (timestamp - older->timestamp))
            return *newer;
        else
            return *older;
    }

    std::pair<const_iterator,const_iterator> getInterval(const Time& timestamp,
                                                         std::size_t size,
                                                         std::size_t before,
                                                         std::size_t after)
    {
        assert(size==0 || size >= before+after);

        const_iterator older = findFirstOlderItem(timestamp);

        // |----|-----|------------|------|-----|
        // ^              ^        ^            ^
        // begin        timestamp older         end

        // move backward in time until we reach the end of the buffer, 
        // or got the requested number of slots
        const_iterator last = older;
        for(std::size_t i=0; i<before && last!=mStorage.end(); ++i, ++last) ;

        const_iterator first = older;
        std::size_t i = 0;
        for(; i<after && first!=mStorage.begin(); ++i, --first) ;

        // now ensure the size constraint (if there's one)
        if(size!=0) {
            std::size_t left = size-std::distance(first,last);
            while(left>0 && first!=mStorage.begin()) {
                --first;
                --left;
            }
            while(left>0 && last!=mStorage.end()) {
                ++last;
                --left;
            }

            if(left>0)
                MIRA_THROW(XRuntime,
                           "Not enough items in StampedDataQueue to obtain "
                           "the requested interval: " << left <<
                           " items missing.")
        }

        return std::make_pair(first, last);
    }

private:

    void prune()
    {
        // the oldest "allowed" timestamp
        Time oldestTimestamp = mStorage.begin()->timestamp - mMaxStorageTime;

        // remove the elements at the end that are too old or if there are too many
        while(!mStorage.empty() && 
              (mStorage.rbegin()->timestamp < oldestTimestamp ||
               mStorage.size() > mMaxCapacity) )
            mStorage.pop_back();
    }


    // find the first iterator that is older than our searched timestamp
    typename Container::const_iterator findFirstOlderItem(const Time& timestamp) const
    {
        // move from newest to oldest slot
        for(typename Container::const_iterator it=mStorage.begin();
                                               it!=mStorage.end(); ++it)
            if(it->timestamp < timestamp)
                return it;

        // if queue is empty or the oldest queue element is already newer than
        // the requested timestamp, we end up here
        return mStorage.end();
    }

private:

    size_type  mMaxCapacity;
    Duration mMaxStorageTime;

    Container mStorage;
};


}

#endif