NJointControllerWithTripleBuffer.h

Go to the documentation of this file.

#pragma once
 
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
 
#include "NJointControllerBase.h"
 
namespace armarx
{
    template <typename ControlDataStruct>
    class NJointControllerWithTripleBuffer : public SynchronousNJointController
    {
    public:
        using MutexType = std::recursive_mutex;
        using LockGuardType = std::lock_guard<std::recursive_mutex>;
 
        NJointControllerWithTripleBuffer(
            const ControlDataStruct& initialCommands = ControlDataStruct()) :
            controlDataTripleBuffer{initialCommands}
        {
        }
 
        void
        rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
                           const IceUtil::Time& timeSinceLastIteration) override
        {
            rtUpdateControlStruct();
            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
        }
 
    protected:
        const ControlDataStruct&
        rtGetControlStruct() const
        {
            return controlDataTripleBuffer.getReadBuffer();
        }
 
        bool
        rtUpdateControlStruct()
        {
            return controlDataTripleBuffer.updateReadBuffer();
        }
 
        void
        writeControlStruct()
        {
            //just lock to be save and reduce the impact of an error
            //also this allows code to unlock the mutex before calling this function
            //(can happen if some lockguard in a scope is used)
            LockGuardType lock(controlDataMutex);
            controlDataTripleBuffer.commitWrite();
        }
 
        ControlDataStruct&
        getWriterControlStruct()
        {
            return controlDataTripleBuffer.getWriteBuffer();
        }
 
        void
        setControlStruct(const ControlDataStruct& newStruct)
        {
            LockGuardType lock{controlDataMutex};
            getWriterControlStruct() = newStruct;
            writeControlStruct();
        }
 
        void
        reinitTripleBuffer(const ControlDataStruct& initial)
        {
            controlDataTripleBuffer.reinitAllBuffers(initial);
        }
 
        mutable MutexType controlDataMutex;
 
    private:
        WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
    };
 
    template <typename ControlDataStruct>
    using NJointControllerWithTripleBufferPtr =
        IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
 
} // namespace armarx