ChannelSynchronizer.h

Go to the documentation of this file.

/*
 * Copyright (C) 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.
 */

#pragma once

#include <tuple>

#include <fw/Framework.h>

// Needed for bind_with_variadic_arguments, see inside ChannelSynchronizer class
// See: https://stackoverflow.com/questions/21192659/variadic-templates-and-stdbind#21193316
template<int> // begin with 0 here!
struct placeholder_template
{
};

// Needs to be in namespace std!!
namespace std {


template<int N>
struct is_placeholder<placeholder_template<N>>: integral_constant<int, N+1> // the 1 is important
{
};


} // namespace std


namespace mira {


class ChannelSynchronizerBase
{
public:
    enum SynchronizationMode
    {
        BY_TIMESTAMP,
        BY_SEQUENCE_ID,
    };
};

template <typename ...Ts>
class ChannelSynchronizer : public ChannelSynchronizerBase
{

private:

    // Just some helper functions. Will be evaluated at compile time (compile time recursion).
    template<int C, typename A, typename... As>
    void subscribeToChannelHelper(Authority& authority, const Duration& t)
    {
        std::get<C>(this->mChannels) = authority.subscribe(this->mChannelIDs[C], &ChannelSynchronizer::channelCallback<C, A>, this, t);

        // recursion
        subscribeToChannelHelper<C+1, As...>(authority, t);
    }

    template<int C>
    void subscribeToChannelHelper(Authority& authority, const Duration& t)
    {
    }

    template<int C, typename A, typename... As>
    bool isValidHelper() const
    {
        if(!std::get<C>(this->mChannelReads).isValid())
        {
            return false;
        }

        return isValidHelper<C + 1, As...>();
    }

    template<int C>
    bool isValidHelper() const
    {
        return true;
    }

    template<int C, typename A, typename... As>
    bool waitForPublisherHelper(const Duration& timeout ) const
    {
        if(!std::get<C>(this->mChannels).waitForPublisher(timeout))
        {
            return false;
        }

        return waitForPublisherHelper<C + 1, As...>(timeout);
    }

    template<int C>
    bool waitForPublisherHelper(const Duration& timeout) const
    {
        return true;
    }


    template<int C, typename A, typename... As>
    void unsubscribeHelper(Authority& authority) const
    {
        authority.unsubscribe<A>(std::get<C>(this->mChannels).getID());
        unsubscribeHelper<C + 1, As...>(authority);
    }

    template<int C>
    void unsubscribeHelper(Authority& authority) const
    {
    }

    template<int C, typename A, typename... As>
    bool getNearestChannelReadObjectsOfAllChannels(
        const int& idOfChannelWhoseCallbackGotCalledFirst,
        std::tuple<ChannelRead<Ts>...>& tmpChannelReads,
        const Time& timestamp,
        const uint32_t sequenceID = 0)
    {
        if(C == idOfChannelWhoseCallbackGotCalledFirst)
        {
            // We do not get a ChannelRead object for this ID,
            // as the callback of the channel / type with this ID
            // was called prior to this function.
            return getNearestChannelReadObjectsOfAllChannels<C + 1, As...>(idOfChannelWhoseCallbackGotCalledFirst, tmpChannelReads, timestamp, sequenceID);
        }

        try
        {
            if(this->mSynchronizationMode == SynchronizationMode::BY_TIMESTAMP)
            {
                std::get<C>(tmpChannelReads) = std::get<C>(this->mChannels).read(timestamp, this->mTolerance);
            }
            else if(this->mSynchronizationMode == SynchronizationMode::BY_SEQUENCE_ID)
            {
                std::get<C>(tmpChannelReads) = std::get<C>(this->mChannels).read(sequenceID, this->mTolerance);
            }
        }
        catch (mira::Exception& ex)
        {
            // std::cout << ex.getInfo().message << "\n";
            return false;
        }
        return getNearestChannelReadObjectsOfAllChannels<C + 1, As...>(idOfChannelWhoseCallbackGotCalledFirst, tmpChannelReads, timestamp, sequenceID);
    }

    template<int C>
    bool getNearestChannelReadObjectsOfAllChannels(
        const int& idOfChannelWhoseCallbackGotCalledFirst,
        std::tuple<ChannelRead<Ts>...>& tmpChannelReads,
        const Time& timestamp,
        const uint32_t sequenceID = 0)
    {
        return true;
    }

    template<int C, typename A, typename... As>
    bool checkForNewTimestampsAndUpdate(std::tuple<ChannelRead<Ts>...>& tmpChannelReads)
    {
        ChannelRead<A>& channelRead = std::get<C>(tmpChannelReads);
        if(!channelRead.isValid())
        {
            return false;
        }

        const Time& newTimestamp = channelRead.getTimestamp();
        if(this->mLastTimes[C] == newTimestamp)
        {
            return false;
        }
        this->mLastTimes[C] = newTimestamp;

        return checkForNewTimestampsAndUpdate<C + 1, As...>(tmpChannelReads);
    }

    template<int C>
    bool checkForNewTimestampsAndUpdate(std::tuple<ChannelRead<Ts>...>& tmpChannelReads)
    {
        return true;
    }

    template<int C, typename A, typename... As>
    bool getOldestTimestampOfChannelReadsHelper(Time& oldestTimestamp) 
    {
        const Time& timestamp = std::get<C>(this->mChannels).read().getTimestamp();
        if(timestamp < oldestTimestamp)
        {
            // Get copy and save in oldestTimestamp.
            oldestTimestamp = timestamp;
        }

        return getOldestTimestampOfChannelReadsHelper<C + 1, As...>(oldestTimestamp);
    }

    template<int C>
    bool getOldestTimestampOfChannelReadsHelper(Time& oldestTimestamp) 
    {
        return true;
    }

    template<int C, typename A, typename... As>
    bool getOldestSequenceIDOfChannelReadsHelper(uint32_t& oldestSequenceID) 
    {
        const uint32_t sequenceID = std::get<C>(this->mChannels).read()->sequenceID;
        if(sequenceID < oldestSequenceID)
        {
            oldestSequenceID = sequenceID;
        }


        return getOldestSequenceIDOfChannelReadsHelper<C + 1, As...>(oldestSequenceID);
    }

    template<int C>
    bool getOldestSequenceIDOfChannelReadsHelper(uint32_t& oldestSequenceID) 
    {
        return true;
    }

    template<typename A>
    struct StringArgumentHelper
    {
        StringArgumentHelper() {}
        StringArgumentHelper(const std::string& str) : string(str) {}
        StringArgumentHelper(const char* cStr) : string(cStr) {}

        std::string string;
    };

    template<std::size_t I = 0, std::size_t end, typename... Tp>
    inline typename std::enable_if<I >= end, void>::type
    stringArgumentHelperUnpack(std::tuple<Tp...>& t, std::vector<std::string>& resultVector) { }

    template<std::size_t I = 0, std::size_t end, typename... Tp>
    inline typename std::enable_if<I < end, void>::type
    stringArgumentHelperUnpack(std::tuple<Tp...>& t, std::vector<std::string>& resultVector)
    {
        auto d = std::get<I>(t);
        resultVector.push_back(d.string);
        stringArgumentHelperUnpack<I + 1, end, Tp...>(t, resultVector);
    }

    // C++11 style of unpacking a tuple to individual variables.
    // See: https://stackoverflow.com/questions/7858817/unpacking-a-tuple-to-call-a-matching-function-pointer
    // #######################################
    template<int ...>
    struct sequence { };

    template<int N, int ...S>
    struct generateIntegerSequence : generateIntegerSequence<N-1, N-1, S...> { };

    template<int ...S>
    struct generateIntegerSequence<0, S...> {
        typedef sequence<S...> type;
    };

    template<int ...S>
    void upCallFunctionHelper(sequence<S...>)
    {
        // S will be <0, 1, 2, ..> depending on the number of template parameters.
        // If we have 4 template parameters, for example, the following would look like this:
        // this->mFunction(std::get<0, 1, 2, 3>(this->mChannelReads));
        // Pretty as hell, isn't it?
        this->mFunction(std::get<S>(this->mChannelReads)...);
    }

    template<int SeqLength = sizeof...(Ts)>
    void upCallFunctionHelper()
    {
        // Do the sequence generation internally
        upCallFunctionHelper(typename generateIntegerSequence<SeqLength>::type());
    }

    // bind_with_variadic_arguments (also please check at the beginning of this
    // file for definition of a specialized std is_placeholder type_trait).
    template<class Class, class... Args, int... Is>
    std::function<void (Args...)> bind_with_variadic_placeholders(void (Class::*p)(Args...), Class* obj, sequence<Is...>)
    {
        // std::function<void (Args...)>  y = std::bind(p, obj, boost::placeholders::_1, boost::placeholders::_2);
        std::function<void (Args...)> x = std::bind(p, obj, placeholder_template<Is>{}...);
        return x;
    }

    template<class Class, class... Args>
    std::function<void (Args...)> bind_with_variadic_placeholders(void (Class::*p)(Args...), Class* obj)
    {
        return bind_with_variadic_placeholders(p, obj, typename generateIntegerSequence<sizeof...(Args)>::type());
    }


    template<int C, typename A>
    void channelCallback(mira::ChannelRead<A> channelRead)
    {
        std::tuple<ChannelRead<Ts>...> tmpChannelReads;
        std::get<C>(tmpChannelReads) = channelRead;

        // From all other channels, obtain a ChannelRead object from the slot
        // at the specified timestamp/sequence id. See mira::ChannelRead::read() documentation for more information.
        // If no slot with the exact timestamp exists (most of the time there will be no such slot) a slot will be chosen according to the query mode (see SlotQueryMode).
        // In this case, SlotQueryMode will be NEAREST_SLOT.
        if(!getNearestChannelReadObjectsOfAllChannels<0, Ts...>(C, tmpChannelReads, channelRead.getTimestamp(), channelRead->sequenceID))
            return;

        this->call(tmpChannelReads);
    }

    void call(std::tuple<ChannelRead<Ts>...>& tmpChannelReads)
    {
        // Check if the new ChannelReads have not been received before:
        // We compare the timestamps of all ChannelReads to the last known
        // time stamps for each channel.
        // This is necessary, as tmpChannelReads was filled using the read function
        // (manual read, as only one of the ChannelReads in tmpChannelReads was
        // received by a callback), so it is possible that we get the same
        // ChannelRead object twice (for example if one channel publishes fast
        // and another one quite slow).
        if(!checkForNewTimestampsAndUpdate<0, Ts...>(tmpChannelReads))
        {
            // Not all channels have new data.
            return;
        }

        // mLastTimes has been updated already in checkForNewTimestampsAndUpdate(...), so no
        // need to update it here like in the old ChannelSynchronizer code.
        this->mChannelReads = tmpChannelReads;

        // Call callback, if specified.
        if (this->mFunction)
        {
            // upCallFunctionHelper will unpack the tuple of channel reads and pass the values
            // to mFunction.
            upCallFunctionHelper();
        }
    }

public:

    ChannelSynchronizer()
        // Initialize our mLastTimes vector with the number of elements needed.
        : mLastTimes(std::vector<Time>(sizeof...(Ts), Time::unixEpoch())),
          mSynchronizationMode(BY_TIMESTAMP)
    {
        assert(sizeof...(Ts) > 0 && "Number of template arguments of ChannelSynchronizer has to be > 0!");
    }

    void unsubscribe(Authority& authority)\
    {
        unsubscribeHelper<0, Ts...>(authority);
    }

    void subscribe(Authority& authority,
                   StringArgumentHelper<Ts>... channelIDs,
                   const Duration& t = Duration::milliseconds(100))
    {
        std::tuple<StringArgumentHelper<Ts>...> tuple(channelIDs...);
        this->mChannelIDs.clear();

        stringArgumentHelperUnpack<0, sizeof...(Ts), StringArgumentHelper<Ts>...>(tuple, this->mChannelIDs);
        this->mFunction = NULL;

        this->mTolerance = t;

        subscribeToChannelHelper<0, Ts...>(authority, t);
    }

    void subscribe(Authority& authority,
                   StringArgumentHelper<Ts>... channelIDs,
                   std::function<void (ChannelRead<Ts>...)> fn,
                   const Duration& t = Duration::milliseconds(100))
    {
        std::tuple<StringArgumentHelper<Ts>...> tuple(channelIDs...);

        std::vector<std::string> tmpChannelIDs;
        stringArgumentHelperUnpack<0, sizeof...(Ts), StringArgumentHelper<Ts>...>(tuple, tmpChannelIDs);

        subscribe(authority, tmpChannelIDs, fn, t);
    }

    void subscribe(Authority& authority,
                   const std::vector<std::string>& channelIDs,
                   std::function<void (ChannelRead<Ts>...)> fn,
                   const Duration& t = Duration::milliseconds(100))
    {
        this->mChannelIDs = channelIDs;
        this->mFunction = fn;
        this->mTolerance = t;

        subscribeToChannelHelper<0, Ts...>(authority, t);
    }

    template<typename Class>
    void subscribe(Authority& authority,
                   StringArgumentHelper<Ts>... channelIDs,
                   void (Class::*f)(ChannelRead<Ts>...),
                   Class* obj,
                   const Duration& t = Duration::milliseconds(100))
    {
        std::tuple<StringArgumentHelper<Ts>...> tuple(channelIDs...);

        std::vector<std::string> tmpChannelIDs;
        stringArgumentHelperUnpack<0, sizeof...(Ts), StringArgumentHelper<Ts>...>(tuple, tmpChannelIDs);

        subscribe(authority, tmpChannelIDs, f, obj, t);
    }

    template<typename Class>
    void subscribe(Authority& authority,
                   std::vector<std::string> channelIDs,
                   void (Class::*f)(ChannelRead<Ts>...),
                   Class* obj,
                   const Duration& t = Duration::milliseconds(100))
    {
        std::function<void (ChannelRead<Ts>...)> upcallFunction = bind_with_variadic_placeholders<Class, ChannelRead<Ts>...>(f, obj);
        subscribe(authority, channelIDs, upcallFunction, t);
    }

    std::tuple<ChannelRead<Ts>... > read()
    {
        return this->mChannelReads;
    }

    bool isValid() const
    {
        return isValidHelper<0, Ts...>();
    }

    std::tuple< ChannelRead<Ts>... > waitForData(const Duration& timeout = Duration::infinity())
    {
        Time end;
        if(!timeout.isValid() || timeout.isInfinity())
            end = Time::eternity();
        else
            end = Time::now() + timeout;

        while(!boost::this_thread::interruption_requested())
        {
            if(this->mSynchronizationMode == ChannelSynchronizerBase::SynchronizationMode::BY_TIMESTAMP)
            {
                Time oldestTimestamp = Time::now();
                try
                {
                    if(getOldestTimestampOfChannelReadsHelper<0, Ts...>(oldestTimestamp))
                    {
                        std::tuple<ChannelRead<Ts>...> tmpChannelReads;
                        if(getNearestChannelReadObjectsOfAllChannels<0, Ts...>(-1, tmpChannelReads, oldestTimestamp))
                        {
                            this->mChannelReads = tmpChannelReads;
                            return read();
                        }
                    }
                }
                catch(XInvalidRead& ex) {}

                
            }
            else
            {
                uint32_t oldestSequenceID = std::numeric_limits<uint32_t>::max();
                try
                {
                    if(getOldestSequenceIDOfChannelReadsHelper<0, Ts...>(oldestSequenceID))
                    {
                        std::tuple<ChannelRead<Ts>...> tmpChannelReads;

                        if(getNearestChannelReadObjectsOfAllChannels<0, Ts...>(-1, tmpChannelReads, Time(), oldestSequenceID))
                        {
                            this->mChannelReads = tmpChannelReads;
                            return read();
                        }
                    }
                }
                catch(XInvalidRead& ex) {}
            }
            if(Time::now() > end) /* handle timeout */
                    break;
            MIRA_SLEEP(50);
            
        }
        
        return this->mChannelReads;
    }

    bool waitForPublisher(const Duration& timeout = Duration::infinity()) const
    {
        return waitForPublisherHelper<0, Ts...>(timeout);
    }

    void setSynchronizationMode(const SynchronizationMode& synchronizationMode)
    {
        this->mSynchronizationMode = synchronizationMode;
    }

private:

    std::tuple<Channel<Ts>...> mChannels;
    std::tuple<ChannelRead<Ts>...> mChannelReads;
    std::vector<std::string> mChannelIDs;
    std::vector<Time> mLastTimes;

    Duration mTolerance;
    SynchronizationMode mSynchronizationMode;
    std::function<void (ChannelRead<Ts>...)> mFunction;
};


} // namespace mira

// For backwards compatibility.
// Old ChannelSynchronizers were named ChannelSynchronizer3 for 3 types,
// ChannelSynchronizer4 for 4 types etc.
// This is not necessary anymore, but as we want to provide backwards compatibility,
// we generate ChannelSynchronizer3 to ChannelSynchronizer10 as separate classes.
// They don't have to be used and for future projects, you should just ChannelSynchronizer.
#define MIRA_FW_INTERNAL_NUMBER(z, n, data) \
    BOOST_PP_COMMA_IF(n) BOOST_PP_CAT(data,n)

#define MIRA_CHANNEL_SYNCHRONIZER( TNAME, TNUM ) \
    template < \
        BOOST_PP_REPEAT(TNUM,MIRA_FW_INTERNAL_NUMBER,typename type) \
        > class TNAME : public ChannelSynchronizer<BOOST_PP_REPEAT(TNUM,MIRA_FW_INTERNAL_NUMBER,type)>\
    {\
    };

#define MIRA_FW_GEN_CHANNEL_SYNCHRONIZER( z, n, data) \
    MIRA_CHANNEL_SYNCHRONIZER(BOOST_PP_CAT(ChannelSynchronizer,n), n)\

namespace mira {


// Generate predefined channel synchronizer for 3 to 9 channels.
// Only for backwards compatibility! Not necessary!
BOOST_PP_REPEAT_FROM_TO(3,10,MIRA_FW_GEN_CHANNEL_SYNCHRONIZER,)


} // namespace mira

#undef MIRA_FW_INTERNAL_NUMBER
#undef MIRA_CHANNEL_SYNCHRONIZER
#undef MIRA_FW_GEN_CHANNEL_SYNCHRONIZER