DispatcherThread.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_DISPATCHERTHREAD_H_
#define _MIRA_DISPATCHERTHREAD_H_

#include <set>
#include <deque>

#ifndef Q_MOC_RUN
#include <boost/function.hpp>
#include <boost/thread/condition_variable.hpp>
#endif

#include <error/LoggingAux.h>
#include <utils/Time.h>
#include <utils/StringAlgorithms.h>
#include <thread/Thread.h>
#include <fw/Status.h>
#include <fw/Runnable.h>

namespace mira {


template<typename T, typename Sequence = std::vector<T>,
         typename Compare  = std::less<typename Sequence::value_type> >
class IteratablePriorityQueue : public std::priority_queue<T,Sequence,Compare>
{
    typedef std::priority_queue<T,Sequence,Compare> Base;
public:

    // types
    typedef typename Sequence::iterator iterator;
    typedef typename Sequence::const_iterator const_iterator;

public:
    explicit IteratablePriorityQueue(const Compare& x, const Sequence& s) :
        Base(x,s) {}

    explicit IteratablePriorityQueue(const Compare& x = Compare(), Sequence&& s = Sequence()) :
        Base(x,std::move(s)) {}


    template<typename InputIterator>
    IteratablePriorityQueue(InputIterator first, InputIterator last, const Compare& x, const Sequence& s) :
        Base(first,last,x,s) {}

    template<typename InputIterator>
    IteratablePriorityQueue(InputIterator first, InputIterator last, const Compare& x, Sequence&& s) :
        Base(first,last,x,std::move(s)) {}

public:

    iterator begin() { return this->c.begin(); }
    iterator end() { return this->c.end(); }

    const_iterator begin() const { return this->c.begin(); }
    const_iterator end() const { return this->c.end(); }


};






class DispatcherThread
{
public:

    class Timer;
    typedef boost::function<void(const Timer&)> TimerCallback;

    class Timer : public IRunnable, boost::noncopyable
    {
    protected:
        friend class DispatcherThread;

        Timer(TimerCallback callback, Duration period,
              Duration tolerance = Duration::invalid());

        Timer(TimerCallback callback, Time invokationTime);

    protected:

        void updateInvocationTime() {
            currentExpected += mPeriod;
        }

        void run(DispatcherThread* dispatcher) override;

    public:

        bool isActive() const { return mActive; }

        void setPeriod(Duration period) { mPeriod = period; }

        Duration getPeriod() const { return mPeriod; }

        void setTolerance(Duration tolerance) { mTolerance = tolerance; }

        Duration getTolerance() const { return mTolerance; }

        Duration getExceedance() const
        {
            if (mPeriod > Duration::seconds(0))
            {
                Duration dt = current - currentExpected;
                if (dt > mTolerance)
                    return dt;
            }
            return Duration::seconds(0);
        }


    public:

        void start();

        void stop();
        Time getNextInvocationTime() const {
            return currentExpected;
        }

    public:

        Time last;

        Time currentExpected;

        Time current;

        Duration lastDuration;

    private:

        TimerCallback mCallback;
        Duration mPeriod;
        Duration mTolerance;
        bool mOneShot;
        bool mActive;
    };

    typedef boost::shared_ptr<Timer> TimerPtr;

public:

    DispatcherThread(const std::string& name = "");

    virtual ~DispatcherThread();

    void setName(const std::string& name);

    bool insertRunnable(IRunnablePtr runnable, bool singleton = false, Time time=Time::now());
    void removeRunnable(IRunnablePtr runnable);


    void addImmediateHandler(IRunnablePtr runnable);

    template <typename F>
    void addImmediateHandlerFunction(F&& fn, DiagnosticsModulePtr errorModule=nullptr) {
        IRunnablePtr r(new FunctionRunnable<F>(static_cast<F&&>(fn),errorModule));
        addImmediateHandler(r);
    }

    void addFinalizeHandler(IRunnablePtr runnable);

    template <typename F>
    void addFinalizeHandlerFunction(F&& fn, DiagnosticsModulePtr errorModule=nullptr) {
        IRunnablePtr r(new FunctionRunnable<F>(static_cast<F&&>(fn),errorModule));
        addFinalizeHandler(r);
    }

    TimerPtr createTimer(Duration period, Duration periodTolerance,
                         TimerCallback callback, bool oneshot=false);

    TimerPtr createTimer(Duration period, TimerCallback callback,
                         bool oneshot=false);

    TimerPtr createTimer(Time time, TimerCallback callback);

    void removeTimer(TimerPtr timer);

    bool hasTimer(TimerPtr timer);

    void start(bool startThread = true);

    void stop();

    bool hasUnrecoverableFailure() const;

    class XUnrecoverableFailure {};

    bool isRunning() const;

    bool spin(Duration maxWait = Duration::infinity());

    boost::thread::id getThreadID() const {
        return mThreadId;
    }

    [[nodiscard]] bool hasWork(Time horizon = Time::now()) const;

protected:
    void run();

private:

    void runThread();

private:

    bool processImmediateHandlers();
    bool processFinalizeHandlers();
    bool processSpin(const Duration& maxWait);

private:

    bool isInterruptionRequested();

protected:

    struct QueueItem
    {
        QueueItem() {}
        QueueItem(IRunnablePtr r, Time t) : runnable(r), time(t), toBeRemoved(false) {}


        IRunnablePtr runnable;
        Time time;
        bool toBeRemoved;

        bool operator<(const QueueItem& other) const { return time>other.time; }
    };

    typedef IteratablePriorityQueue<QueueItem, std::deque<QueueItem>> Queue;


    Queue mQueue;
    std::set<IRunnablePtr> mPendingRunnables;

    mutable boost::mutex mConditionMutex;
    boost::condition_variable mCondition;


    void postProcessTimer(TimerPtr timer);

    std::string mName;

    std::list<IRunnablePtr> mImmediateHandlers;
    mutable boost::mutex mImmediateHandlerMutex;

    std::list<IRunnablePtr> mFinalizeHandlers;
    boost::mutex mFinalizeHandlerMutex;

    std::set<TimerPtr> mTimers;
    boost::mutex mTimerMutex; // protects above handler sets

    bool mUnrecoverableFailure;
    bool mIsRunning;

    // unfortunatelly, we need our own interruption requested flag, since
    // boost::this_thread::interruption_requested() is cleared after a
    // boost::thread_interrupted exception is thrown, however, we need this
    // flag to be true, until we really have terminated
    bool mInterruptionRequested;

    boost::thread mThread;
    boost::thread::id mThreadId;
    bool mPendingSignal;

};

typedef DispatcherThread::Timer Timer;
typedef DispatcherThread::TimerPtr TimerPtr;
typedef DispatcherThread::TimerCallback TimerCallback;


} // namespace

#endif /* _MIRA_DISPATCHERTHREAD_H_ */