Channel.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_CHANNEL_H_
#define _MIRA_CHANNEL_H_

#ifndef _MIRA_FRAMEWORK_H_
#  error "Channel.h must be included via the Framework.h. You must not include it directly. Use #include <fw/Framework.h> instead"
#endif

#ifndef Q_MOC_RUN
#include <boost/shared_ptr.hpp>
#include <boost/type_traits/is_polymorphic.hpp>
#endif

#include <error/Exceptions.h>

#include <factory/TypeId.h>

#include <utils/EnumToFlags.h>
#include <utils/IteratorRangeContainer.h>

#include <fw/Framework.h>

#include <fw/AbstractChannel.h>
#include <fw/ChannelSubscriber.h>
#include <fw/ChannelReadWrite.h>
#include <fw/ChannelReadInterval.h>
#include <fw/ChannelPromoteByTypename.h>

namespace mira {


MIRA_DEFINE_SERIALIZABLE_EXCEPTION(XInvalidRead, XRuntime)


template <typename T>
class Channel;



template<typename T>
class ConcreteChannel : public AbstractChannel
{
public:

    typedef ChannelBuffer<T> Buffer;
    typedef typename Buffer::ValueType ValueType;
    typedef typename Buffer::Slot Slot;

    typedef ChannelSubscriber<T, ChannelRead<T> > Subscriber;
    typedef boost::shared_ptr<Subscriber> SubscriberPtr;

public:

    ConcreteChannel(const std::string& id) :
        AbstractChannel(id, new ChannelBuffer<T>)
    {
        // make sure, that our ConcreteChannel class has the same size
        // as AbstractChannel and that we are not polymorphic. Both is
        // important for our channel_cast to work as expected.
        static_assert(sizeof(ConcreteChannel)==sizeof(AbstractChannel),
                      "ConcreteChannel must have the same size as AbstractChannel");

        static_assert(boost::is_polymorphic<ConcreteChannel>::value==false,
                      "ConcreateChannel and AbstractChannel must not be polymorphic");
    }

public:

    void addSubscriber()
    {
        boost::mutex::scoped_lock lock(mSubscribersMutex);
        mNrOfSubscribersWithoutChannelSubscriber++;
    }

    void addSubscriber(SubscriberPtr subscriber)
    {
        boost::mutex::scoped_lock lock(mSubscribersMutex);
        mSubscribers.push_back(subscriber);
        subscriber->attachChannel(this);
    }

public:

    ChannelRead<T> read();

    ChannelRead<T> read(const Time& timestamp, SlotQueryMode mode = NEAREST_SLOT,
                        const Duration& tolerance = Duration::infinity());

    ChannelReadInterval<T> readInterval(const Time& timestamp, std::size_t nrSlots,
                                        std::size_t olderSlots, std::size_t newerSlots,
                                        IntervalFillMode fillMode = PREFER_NEWER);

    ChannelReadInterval<T> readInterval(const Time& from,
                                        const Time& to = Time::eternity());

    ChannelWrite<T> write();

public:
    friend class Channel<T>;
};



template<typename T>
struct ChannelCaster {
    static ConcreteChannel<T>* cast(AbstractChannel* p)
    {
        // promote the channel if necessary (this will throw an exception, if
        // the channel cannot be promoted to the desired type)
        promote_channel<T>(p);

        assert(p->isTyped()); // at the point p will always be typed

        // check the type of our buffer's slot manually
        if(typeId<T>()!=p->getTypeId())
            MIRA_THROW(XBadCast, "Cannot cast channel '" << p->getID()
                      << "' (type '" << p->getTypename() << "') "
                      << " to typed channel of type '" << typeName<T>() << "'");

        // cast us "hard" using a static_cast.
        // This is safe, since we have checked the types above
        return static_cast<ConcreteChannel<T>*>(p);
    }
};


// specialization to cast to untyped ConcreteChannel (void)
template<>
struct ChannelCaster<void> {
    static ConcreteChannel<void>* cast(AbstractChannel* p)
    {
        // cast us "hard" using a static_cast.
        // This is safe, since all channels can be casted to untyped channel
        return static_cast<ConcreteChannel<void>* >(p);
    }
};


// specialization to cast polymorphic channels
template<typename T>
struct ChannelCaster<T*> {
    static ConcreteChannel<T*>* cast(AbstractChannel* p)
    {
        static_assert(std::is_base_of<mira::Object,T>::value,
                      "Pointers that are used in channels must be derived from "
                      "mira::Object. Pointers to other classes cannot be used.");

        // promote the channel if necessary (this will throw an exception, if the
        // the channel cannot be promoted to the desired type)
        promote_channel<T*>(p);

        // channel must be typed after promotion
        assert(p->getBuffer()->isPolymorphic());

        // cast us "hard" using a static_cast.
        // This is safe, since we have checked the types above
        return static_cast<ConcreteChannel<T*>* >(p);
    }
};


template<typename T>
inline ConcreteChannel<T>* channel_cast(AbstractChannel* p)
{
    return ChannelCaster<T>::cast(p);
}



template <typename T>
class Channel;

template<typename T>
inline Channel<T> channel_cast(Channel<void> channel);


enum ChannelAccessFlags {
    CHANNEL_ACCESS_NONE  = 0x00,    
    CHANNEL_ACCESS_READ  = 0x01,    
    CHANNEL_ACCESS_WRITE = 0x02     
};
MIRA_ENUM_TO_FLAGS(ChannelAccessFlags)


namespace {

template <typename T>
struct ParamHelper {
    typedef T type;
};

template<>
struct ParamHelper<void> {
    typedef int type; // arbitrary, it will not be used for T=void actually,
                      // but needs to name a valid argument type
};

}


template <typename T>
class Channel
{
public:

    Channel() : mChannel(), mAccessFlags(CHANNEL_ACCESS_NONE) {}

    Channel(ConcreteChannel<T>* channel, ChannelAccessFlags accessFlags) :
        mChannel(channel), mAccessFlags(accessFlags) {
        assert(mChannel!=NULL);
    }

    void reset() {
        mChannel=NULL;
        mAccessFlags = CHANNEL_ACCESS_NONE;
    }

    bool isValid() const {
        return mChannel!=NULL;
    }

    void validate() const {
        if(mChannel==NULL)
            MIRA_THROW(XAccessViolation,
                       "No channel assigned to the channel proxy (of type " <<
                        typeName<T>() << "). "
                       "You can obtain a channel using Authority::getChannel()!");
    }


    const std::string& getID() const {
        validate();
        return mChannel->getID();
    }

    int getTypeId() const {
        validate();
        return mChannel->getTypeId();
    }

    bool isTyped() const { return getTypeId()>=0; }

    Typename getTypename() const {
        validate();
        return mChannel->getTypename();
    }

    void setTypename(const Typename& type) {
        validate();
        mChannel->setTypename(type);
    }

    TypeMetaPtr getTypeMeta() const {
        validate();
        return mChannel->getTypeMeta();
    }

    void setTypeMeta(TypeMetaPtr meta) {
        validate();
        mChannel->setTypeMeta(meta);
    }


public:


    bool isEmpty() const {
        validate();
        return mChannel->getNrOfSlots() == 0;
    }

    bool hasSubscriber() const {
        validate();
        return mChannel->hasSubscriber();
    }

    bool hasPublisher() const {
        validate();
        return mChannel->hasPublisher();
    }

    bool hasSoloSlot() const {
        validate();
        return mChannel->getBuffer()->getMaxSlots() == 1;
    }

    std::size_t getMaxSlots() const {
        validate();
        return mChannel->getBuffer()->getMaxSlots();
    }

    std::size_t getMinSlots() const {
        validate();
        return mChannel->getBuffer()->getMinSlots();
    }

    Duration getStorageDuration() const {
        validate();
        return mChannel->getBuffer()->getStorageDuration();
    }

    bool isAutoIncreasingStorageDuration() const {
        validate();
        return mChannel->getBuffer()->isAutoIncreasingStorageDuration();
    }

    std::size_t getNrOfSlots() const {
        validate();
        return mChannel->getBuffer()->getSize();
    }


public:


    ChannelRead<T> waitForData(const Duration& timeout = Duration::infinity()) const
    {
        validateReadAccess();

        Time end;
        if(!timeout.isValid() || timeout.isInfinity())
            end = Time::eternity();
        else
            end = Time::now() + timeout;

        while(!boost::this_thread::interruption_requested())
        {
            try {
                return mChannel->read();
            } catch(XInvalidRead& ex) {}
            if(Time::now()>end) // handle timeout
                break;
            MIRA_SLEEP(50)
        }
        return ChannelRead<T>();
    }


    bool waitForPublisher(const Duration& timeout = Duration::infinity()) const
    {
        validateReadAccess();

        Time end;
        if(!timeout.isValid() || timeout.isInfinity())
            end = Time::eternity();
        else
            end = Time::now() + timeout;

        while(!boost::this_thread::interruption_requested())
        {
            if(hasPublisher())
                return true;
            if(Time::now()>end) // handle timeout
                break;
            MIRA_SLEEP(50)
        }
        return false; // timeout
    }


    ChannelRead<T> read() {
        validateReadAccess();
        return mChannel->read();
    }

    ChannelRead<T> read(const Time& timestamp, SlotQueryMode mode=NEAREST_SLOT,
                        const Duration& tolerance = Duration::infinity()) {
        validateReadAccess();
        return mChannel->read(timestamp, mode, tolerance);
    }

    ChannelRead<T> read(uint32 sequenceID,
                        const Duration& searchInterval = Duration::seconds(1)) {
        validateReadAccess();
        Time newestSlotTime = mChannel->read()->timestamp;
        ChannelReadInterval<T> interval = readInterval(newestSlotTime - searchInterval, newestSlotTime);
        typename ChannelReadInterval<T>::const_iterator iter = interval.begin();
        for(; iter != interval.end(); ++iter){
            if(iter->sequenceID == sequenceID)
                return (ChannelRead<T>)iter;
        }
        MIRA_THROW(XInvalidRead, "No slot with sequenceID "<<sequenceID<<
                   " found within searchInterval of channel");
    }

    ChannelRead<T> read(const Time& timestamp, const Duration& tolerance) {
        validateReadAccess();
        return mChannel->read(timestamp, NEAREST_SLOT, tolerance);
    }

    ChannelReadInterval<T> readInterval(const Time& timestamp,
                                        std::size_t nrSlots,
                                        std::size_t olderSlots,
                                        std::size_t newerSlots,
                                        IntervalFillMode fillMode = PREFER_NEWER) {
        validateReadAccess();
        return mChannel->readInterval(timestamp, nrSlots, olderSlots,
                                      newerSlots, fillMode);
    }

    ChannelReadInterval<T> readInterval(const Time& from,
                                        const Time& to=Time::eternity()) {
        validateReadAccess();
        return mChannel->readInterval(from, to);
    }

    ChannelWrite<T> write() {
        validateWriteAccess();
        return mChannel->write();
    }

    template <typename Fn = Stamped<T>(*)(const Stamped<T>&)>
    ChannelWrite<T> write(bool copyLatestData, Fn fn = [](const auto& s){ return s; }) {
        validateWriteAccess();

        // check if we should and need to copy data into the write slot
        if(copyLatestData
            && (mChannel->getBuffer()->getMaxSlots()>1) // only necessary if we may have more than one slot
            && (mChannel->getBuffer()->getSize()>0))    // and only if we have at least one slot yet
        {
            ChannelRead<T> latest = mChannel->read();
            Stamped<T> tmp(fn(*latest));
            latest.finish();
            ChannelWrite<T> writeSlot = mChannel->write();
            *writeSlot = tmp;
            return writeSlot;

        } else
            return mChannel->write();
    }

    Stamped<T> get() {
        validateReadAccess();
        ChannelRead<T> value = mChannel->read();
        return *value;
    }

    Stamped<T> get(const Time& timestamp, SlotQueryMode mode=NEAREST_SLOT,
                   const Duration& tolerance = Duration::infinity()) {
        validateReadAccess();
        ChannelRead<T> value = mChannel->read(timestamp, mode, tolerance);
        return *value;
    }

    Stamped<T> get(const Time& timestamp, Duration tolerance) {
        validateReadAccess();
        ChannelRead<T> value = mChannel->read(timestamp, NEAREST_SLOT, tolerance);
        return *value;
    }

private:


    struct GetTime
    {
        GetTime(const Time& iT0) : t0(iT0) {}

        //typedef const Time& result_type;
        typedef float result_type;
        result_type operator()(const Stamped<T>& p) const {
            //return p.timestamp;
            return (p.timestamp-t0).totalMilliseconds();
        }

        Time t0;
    };

    struct GetValue
    {
        typedef const T& result_type;
        result_type operator()(const Stamped<T>& p) const {
            return p;
        }
    };


public:

    template <typename Filter>
    Stamped<T> get(const Time& timestamp, Filter&& filter) {
        validateReadAccess();

        ChannelReadInterval<T> data;
        try
        {
            data = readInterval(timestamp, filter.samples(),
                                filter.samplesBefore(),
                                filter.samplesAfter());
            // check filter requirements
            if (!filter.canExtrapolate())
            {
                int samplesBefore = filter.samplesBefore();
                int samplesAfter = filter.samplesAfter();
                // a rbegin in ChannelReadInterval could speed this up
                for (auto it = data.begin(); it != data.end(); ++it)
                {
                    if (it->timestamp <= timestamp)
                        --samplesBefore;
                    else if (it->timestamp >= timestamp)
                        --samplesAfter;
                }

                if (samplesBefore > 0)
                    MIRA_THROW(XRuntime, "Filter::samplesBefore: Requirements not met. Missing " << samplesBefore << " items.");
                if (samplesAfter > 0)
                    MIRA_THROW(XRuntime, "Filter::samplesAfter: Requirements not met. Missing " << samplesAfter << " items.");
            }
        }
        catch(Exception&)
        {
            // read interval failed so test if the channel only contains a single data element
            // if so return it as a special case for interpolation of channels that will only
            // receive a single update during their lifetime
            if (mChannel->getNrOfSlots() == 1)
                return get();
            // more than one data element in the channel but still not sufficient for interpolation
            // rethrow is our only option
            throw;
        }

        const Time t0 = data.begin()->timestamp;

        typedef typename ChannelReadInterval<T>::const_iterator const_iterator;
        typedef boost::transform_iterator<GetTime, const_iterator> GetTimeIterator;
        GetTimeIterator begin1 = GetTimeIterator(data.begin(), GetTime(t0));
        GetTimeIterator end1   = GetTimeIterator(data.end()  , GetTime(t0));

        typedef boost::transform_iterator<GetValue, const_iterator> GetValueIterator;

        GetValueIterator begin2=GetValueIterator(data.begin(), GetValue());
        GetValueIterator end2  =GetValueIterator(data.end()  , GetValue());

        typedef mira::IteratorRangeContainer<GetTimeIterator> TimeContainer;
        typedef mira::IteratorRangeContainer<GetValueIterator> ValueContainer;
        TimeContainer  timeContainer(begin1, end1);
        ValueContainer valueContainer(begin2, end2);

        return makeStamped(filter.template apply<float, T>(timeContainer,
                                                           valueContainer,
                                                           (timestamp-t0).totalMilliseconds()),
                                                           timestamp);
    }

    template <typename U = void>
    void post(const Stamped<T>& value) {
        validateWriteAccess();
        ChannelWrite<T> writeSlot = mChannel->write();
        *writeSlot = value;
        writeSlot.finish(); // explicit finish to avoid throwing in destructor
    }

    template <typename U = void>
    void post(Stamped<T>&& value) {
        validateWriteAccess();
        ChannelWrite<T> writeSlot = mChannel->write();
        *writeSlot = std::move(value);
        writeSlot.finish(); // explicit finish to avoid throwing in destructor
    }

    template <typename U>
    typename std::enable_if<!std::is_base_of_v<StampedHeader, std::decay_t<U>>>::type
    post(U&& value, const Time& timestamp = Time::now()) {
        validateWriteAccess();
        ChannelWrite<T> writeSlot = mChannel->write();
        *writeSlot = makeStamped(std::forward<U>(value), timestamp);
        writeSlot.finish(); // explicit finish to avoid throwing in destructor
    }

    template <typename U = T>
    typename std::enable_if<!std::is_void<U>::value>::type
    post(const typename ParamHelper<T>::type& value, const Time& timestamp = Time::now()) {
        validateWriteAccess();
        ChannelWrite<T> writeSlot = mChannel->write();
        *writeSlot = makeStamped(value, timestamp);
        writeSlot.finish(); // explicit finish to avoid throwing in destructor
    }

    template <typename U = T>
    typename std::enable_if<!std::is_void<U>::value>::type
    post(typename ParamHelper<T>::type&& value, const Time& timestamp = Time::now()) {
        validateWriteAccess();
        ChannelWrite<T> writeSlot = mChannel->write();
        *writeSlot = makeStamped(std::move(value), timestamp);
        writeSlot.finish(); // explicit finish to avoid throwing in destructor
    }


public:

    template<typename U>
    friend Channel<U> channel_cast(Channel<void> channel);

public:

    void dbgDump(bool brief=true) { mChannel->dbgDump(brief); }

private:

    void validateReadAccess() const
    {
        validate();
        if((mAccessFlags & CHANNEL_ACCESS_READ)==0)
            MIRA_THROW(XAccessViolation, "You are not allowed to read from channel '"
                       << mChannel->getID() << "'. Forgot to subscribe?");
    }

    void validateWriteAccess() const
    {
        validate();
        if((mAccessFlags & CHANNEL_ACCESS_WRITE)==0)
            MIRA_THROW(XAccessViolation, "You are not allowed to write to channel '"
                       << mChannel->getID() << "'. Forgot to publish?");
    }

private:

    ConcreteChannel<T>* mChannel;
    ChannelAccessFlags mAccessFlags;
};


template<typename U>
inline Channel<U> channel_cast(Channel<void> channel)
{
    channel.validate();
    return Channel<U>(channel_cast<U>(channel.mChannel), channel.mAccessFlags);
}


} // namespace

#include "impl/ChannelReadWrite.hpp"
#include "impl/ChannelReadInterval.hpp"

#endif