RosMsgSynchronizer.h

Go to the documentation of this file.

#ifndef ROS_MSG_SYNCHRONIZER_H
#define ROS_MSG_SYNCHRONIZER_H

#include <math.h>
#include <std_msgs/Header.h>

template<class T1, class T2 = T1>
class RosMsgSynchronizer
{
public:
    RosMsgSynchronizer()
        : windowDelta(0.0)
    {}
    ~RosMsgSynchronizer() {}

    inline void pushSource1(const T1& source1);
    inline void pushSource2(const T2& source2);

    void purgeBuffer();
    void purgeBuffer(const ros::Time& oldDataTime);
    unsigned int getBufferSize() const {return source1Buffer.size() + source2Buffer.size();}

    inline bool empty() const {return matches.empty();}
    inline unsigned int size() const {return matches.size();}
    inline const std::pair<T1, T2>& front() const
    {
        if (matches.empty()) throw std::out_of_range("front() called on empty RosMsgSynchronizer");
        return matches.begin()->second;
    }
    inline const std::pair<T1, T2>& back() const
    {
        if (matches.empty()) throw std::out_of_range("back() called on empty RosMsgSynchronizer");
        return (--matches.end())->second;
    }
    inline void pop()
    {
        if (!matches.empty()) matches.erase(matches.begin());
    }
    inline void clear() {matches.clear();}

    void setWindowDelta(double windowDelta_in)
    {
        if (windowDelta_in < 0.) throw std::out_of_range("windowDelta must greater than or equal to zero");
        windowDelta = windowDelta_in;
    }

protected:
    std::map<ros::Time, T1> source1Buffer;
    std::map<ros::Time, T2> source2Buffer;
    std::map<ros::Time, std::pair<T1, T2> > matches;

    template <class T3, class T4>
    bool findMatch(std::map<ros::Time, T3>& sourceBuffer, const T4& find, T3& match);

    double windowDelta;
};

template <class T1, class T2>
void RosMsgSynchronizer<T1, T2>::pushSource1(const T1& source1)
{
    T2 match;
    if (findMatch(source2Buffer, source1, match))
    {
        matches[source1.header.stamp] = make_pair(source1, match);
        source2Buffer.erase(match.header.stamp);
    }
    else
    {
        source1Buffer[source1.header.stamp] = source1;
    }
}

template <class T1, class T2>
void RosMsgSynchronizer<T1, T2>::pushSource2(const T2& source2)
{
    T1 match;
    if (findMatch(source1Buffer, source2, match))
    {
        matches[match.header.stamp] = make_pair(match, source2);
        source1Buffer.erase(match.header.stamp);
    }
    else
    {
        source2Buffer[source2.header.stamp] = source2;
    }
}

template <class T1, class T2>
void RosMsgSynchronizer<T1, T2>::purgeBuffer()
{
    source1Buffer.clear();
    source2Buffer.clear();
}

template <class T1, class T2>
void RosMsgSynchronizer<T1, T2>::purgeBuffer(const ros::Time& oldDataTime)
{
    typename std::map<ros::Time, T1>::iterator source1It = source1Buffer.lower_bound(oldDataTime);
    source1Buffer.erase(source1Buffer.begin(), source1It);

    typename std::map<ros::Time, T2>::iterator source2It = source2Buffer.lower_bound(oldDataTime);
    source2Buffer.erase(source2Buffer.begin(), source2It);
}

template <class T1, class T2>
template <class T3, class T4>
bool RosMsgSynchronizer<T1, T2>::findMatch(std::map<ros::Time, T3>& sourceBuffer, const T4& find, T3& match)
{
    // look for closest match in sourceBuffer
    if (sourceBuffer.empty())
    {
        return false;
    }

    double dT1 = windowDelta + 1.;
    double dT2 = dT1;

    // get closest one that is greater
    typename std::map<ros::Time, T3>::iterator sourceIt = sourceBuffer.upper_bound(find.header.stamp);
    // if it exists and is less than the delta, we have a match
    if (sourceIt != sourceBuffer.end() && (dT1 = (sourceIt->first - find.header.stamp).toSec()) <= windowDelta)
    {
        match = sourceIt->second;
    }

    // check the closest one less than to find the closest one
    if (sourceIt != sourceBuffer.begin())
    {
        --sourceIt;
        if ((dT2 = (find.header.stamp - sourceIt->first).toSec()) <= windowDelta && dT2 < dT1)
        {
            dT1 = dT2;
            match = sourceIt->second;
        }
    }

    if (dT1 <= windowDelta)
    {
        return true;
    }

    return false;
}

#endif