ControlThreadOutputBuffer.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <vector>
 
// For unknown reasons, this needs to be included before expression exception.
// TODO: investigate
#include <ArmarXCore/core/logging/LoggingUtil.h>
 
// Remaining, normal includes.
#include <ArmarXCore/core/util/PropagateConst.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 
#include "../ControlTargets/ControlTargetBase.h"
#include "../Devices/ControlDevice.h"
#include "../Devices/SensorDevice.h"
#include "../SensorValues/SensorValueBase.h"
#include "HeterogenousContinuousContainer.h"
#include "RtTiming.h"
 
namespace armarx
{
    class RobotUnit;
    struct ControlThreadOutputBuffer;
} // namespace armarx
 
namespace armarx::RobotUnitModule
{
    class Logging;
}
 
namespace armarx::detail
{
    struct RtMessageLogEntryBase
    {
        RtMessageLogEntryBase() : time{armarx::rtNow()}
        {
        }
 
        virtual ~RtMessageLogEntryBase() = default;
 
        RtMessageLogEntryBase& setLoggingLevel(MessageTypeT lvl);
        RtMessageLogEntryBase& deactivateSpam(float sec);
        RtMessageLogEntryBase& deactivateSpamTag(std::uint64_t tag);
 
        void print(Ice::Int controlThreadId) const;
        virtual std::size_t line() const = 0;
        virtual std::string file() const = 0;
        virtual std::string func() const = 0;
        virtual std::string format() const = 0;
        IceUtil::Time getTime() const;
 
        ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(RtMessageLogEntryBase)
    protected:
        friend struct RtMessageLogBuffer;
 
    private:
        MessageTypeT loggingLevel{MessageTypeT::UNDEFINED};
        float deactivateSpamSec{0};
        bool printMsg{false};
        std::uint64_t deactivateSpamTag_;
        IceUtil::Time time;
    };
 
    struct RtMessageLogEntryDummy : RtMessageLogEntryBase
    {
        static RtMessageLogEntryDummy Instance;
 
    protected:
        std::string format() const final override;
        std::size_t line() const final override;
        std::string file() const final override;
        std::string func() const final override;
 
        ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER
    };
 
    struct RtMessageLogEntryNull : RtMessageLogEntryBase
    {
        static RtMessageLogEntryNull Instance;
 
    protected:
        std::string
        format() const final override
        {
            return "";
        }
 
        std::size_t
        line() const final override
        {
            return 0;
        }
 
        std::string
        file() const final override
        {
            return "";
        }
 
        std::string
        func() const final override
        {
            return "";
        }
 
        ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER
    };
 
    struct RtMessageLogBuffer
    {
        RtMessageLogBuffer(const RtMessageLogBuffer& other, bool minimize = false);
        RtMessageLogBuffer(std::size_t bufferSize,
                           std::size_t numEntries,
                           std::size_t bufferMaxSize,
                           std::size_t bufferMaxNumberEntries);
        ~RtMessageLogBuffer();
 
        RtMessageLogBuffer() = delete;
        RtMessageLogBuffer(RtMessageLogBuffer&&) = delete;
        RtMessageLogBuffer& operator=(RtMessageLogBuffer&&) = delete;
        RtMessageLogBuffer& operator=(const RtMessageLogBuffer&) = delete;
 
        void reset(std::size_t& bufferSize, std::size_t& numEntries, std::size_t iterationCount);
 
        const std::vector<const RtMessageLogEntryBase*>& getEntries() const;
 
        std::size_t getMaximalBufferSize() const;
        std::size_t getMaximalNumberOfBufferEntries() const;
 
    private:
        void deleteAll();
 
        friend struct ControlThreadOutputBufferEntry;
        friend struct ::armarx::ControlThreadOutputBuffer;
        friend class ::armarx::RobotUnit;
        friend class ::armarx::RobotUnitModule::Logging;
 
        const std::size_t initialBufferSize;
        const std::size_t initialBufferEntryNumbers;
        const std::size_t bufferMaxSize;
        const std::size_t bufferMaxNumberEntries;
 
        std::vector<std::uint8_t> buffer;
        std::size_t bufferSpace;
        void* bufferPlace;
 
        std::vector<const RtMessageLogEntryBase*> entries;
        std::size_t entriesWritten{0};
 
        std::size_t requiredAdditionalBufferSpace{0};
        std::size_t requiredAdditionalEntries{0};
        std::size_t messagesLost{0};
 
        std::size_t maxAlign{1};
    };
 
    struct ControlThreadOutputBufferEntry
    {
        //default ctors / ops
 
        ControlThreadOutputBufferEntry(
            const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
            const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
            std::size_t messageBufferSize,
            std::size_t messageBufferNumberEntries,
            std::size_t messageBufferMaxSize,
            std::size_t messageBufferMaxNumberEntries);
        ControlThreadOutputBufferEntry(const ControlThreadOutputBufferEntry& other,
                                       bool minimize = false);
 
        ControlThreadOutputBufferEntry() = delete;
        ControlThreadOutputBufferEntry(ControlThreadOutputBufferEntry&&) = delete;
        ControlThreadOutputBufferEntry& operator=(ControlThreadOutputBufferEntry&&) = delete;
        ControlThreadOutputBufferEntry& operator=(const ControlThreadOutputBufferEntry&) = delete;
 
        std::size_t getDataBufferSize() const;
 
        //data
        /// @brief Timestamp in wall time (never use the virtual time for this)
        IceUtil::Time writeTimestamp;
        IceUtil::Time sensorValuesTimestamp;
        IceUtil::Time timeSinceLastIteration;
        std::vector<PropagateConst<SensorValueBase*>> sensors;
        std::vector<std::vector<PropagateConst<ControlTargetBase*>>> control;
        std::size_t iteration{0};
 
        RtMessageLogBuffer messages;
 
    private:
        std::vector<std::uint8_t> buffer;
    };
} // namespace armarx::detail
 
namespace armarx
{
    struct ControlThreadOutputBuffer
    {
        using Entry = detail::ControlThreadOutputBufferEntry;
        using RtMessageLogEntryBase = detail::RtMessageLogEntryBase;
        using ConsumerFunctor = std::function<void(const Entry&, std::size_t, std::size_t)>;
        std::size_t initialize(std::size_t numEntries,
                               const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
                               const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
                               std::size_t messageBufferSize,
                               std::size_t messageBufferNumberEntries,
                               std::size_t messageBufferMaxSize,
                               std::size_t messageBufferMaxNumberEntries);
 
        ~ControlThreadOutputBuffer();
 
        static ControlThreadOutputBuffer*
        GetRtLoggingInstance()
        {
            return RtLoggingInstance;
        }
 
        //write
        Entry& getWriteBuffer();
        void commitWrite();
 
        //read
        const Entry& getReadBuffer() const;
        bool updateReadBuffer() const;
 
        //logging read
        void resetLoggingPosition() const;
        void forEachNewLoggingEntry(ConsumerFunctor consumer);
        void forLatestLoggingEntry(ConsumerFunctor consumer, size_t numberOfEntriesToLog);
 
        std::size_t getNumberOfBytes() const;
 
        template <class LoggingEntryT, class... Ts>
        RtMessageLogEntryBase* addMessageToLog(Ts&&... args);
 
    private:
        friend class RobotUnitModule::Logging; ///TODO change code to make this unnecessary
        /// @brief this pointer is used for rt message logging and is not null in the control thread
        static thread_local ControlThreadOutputBuffer* RtLoggingInstance;
 
        std::size_t toBounds(std::size_t idx) const;
 
        //settings and initialization:
        bool isInitialized{false};
        std::size_t numEntries{0};
 
        std::atomic_size_t writePosition{0};
        mutable std::atomic_size_t onePastLoggingReadPosition{0};
        mutable std::atomic_size_t onePastReadPosition{0};
 
        std::vector<Entry> entries;
        std::vector<std::uint8_t> data;
 
        std::size_t messageBufferSize{0};
        std::size_t messageBufferEntries{0};
    };
 
    using SensorAndControl = ControlThreadOutputBuffer::Entry;
} // namespace armarx
 
#define ARMARX_RT_LOGF(...)                                                                        \
    (*(_detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION, __LINE__, __VA_ARGS__, true)))
 
#define _detail_ARMARX_RT_LOGF(file_, func_, line_, FormatString, ...)                             \
    (                                                                                              \
        [&]                                                                                        \
        {                                                                                          \
            using namespace ::armarx;                                                              \
            using RtMessageLogEntryBase = ControlThreadOutputBuffer::RtMessageLogEntryBase;        \
            struct RtMessageLogEntry : RtMessageLogEntryBase                                       \
            {                                                                                      \
                using TupleT = decltype(std::make_tuple(__VA_ARGS__));                             \
                const TupleT tuple;                                                                \
                ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER                                       \
                std::size_t                                                                        \
                line() const final override                                                        \
                {                                                                                  \
                    return line_;                                                                  \
                }                                                                                  \
                std::string                                                                        \
                file() const final override                                                        \
                {                                                                                  \
                    return file_;                                                                  \
                }                                                                                  \
                std::string                                                                        \
                func() const final override                                                        \
                {                                                                                  \
                    return func_;                                                                  \
                }                                                                                  \
                std::string                                                                        \
                format() const final override                                                      \
                {                                                                                  \
                    return TupleToStringF<0, std::tuple_size<TupleT>::value - 1>(FormatString,     \
                                                                                 tuple);           \
                }                                                                                  \
                RtMessageLogEntry(TupleT tuple) : tuple{std::move(tuple)}                          \
                {                                                                                  \
                }                                                                                  \
                RtMessageLogEntry(const RtMessageLogEntry&) = default;                             \
            };                                                                                     \
            if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance())                       \
            {                                                                                      \
                return ::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance()                 \
                    ->addMessageToLog<RtMessageLogEntry>(__VA_ARGS__);                             \
            }                                                                                      \
            else                                                                                   \
            {                                                                                      \
                *(loghelper(file_, line_, func_))                                                  \
                    << "Redirected RT Logging:\n"                                                  \
                    << RtMessageLogEntry(std::make_tuple(__VA_ARGS__)).format();                   \
                return dynamic_cast<RtMessageLogEntryBase*>(                                       \
                    &::armarx::detail::RtMessageLogEntryNull::Instance);                           \
            }                                                                                      \
        }())
 
#define ARMARX_RT_LOGF_DEBUG(...)                                                                  \
    ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::DEBUG)
#define ARMARX_RT_LOGF_VERBOSE(...)                                                                \
    ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::VERBOSE)
#define ARMARX_RT_LOGF_INFO(...)                                                                   \
    ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::INFO)
#define ARMARX_RT_LOGF_IMPORTANT(...)                                                              \
    ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::IMPORTANT)
#define ARMARX_RT_LOGF_WARNING(...)                                                                \
    ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
#define ARMARX_RT_LOGF_WARN(...)                                                                   \
    ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
#define ARMARX_RT_LOGF_ERROR(...)                                                                  \
    ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::ERROR)
#define ARMARX_RT_LOGF_FATAL(...)                                                                  \
    ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::FATAL)
 
//impl
namespace armarx::detail
{
    inline IceUtil::Time
    RtMessageLogEntryBase::getTime() const
    {
        return time;
    }
 
    inline std::string
    RtMessageLogEntryDummy::format() const
    {
        throw std::logic_error{"called 'format' of RtMessageLogEntryDummy"};
    }
 
    inline std::size_t
    RtMessageLogEntryDummy::line() const
    {
        throw std::logic_error{"called 'line' of RtMessageLogEntryDummy"};
    }
 
    inline std::string
    RtMessageLogEntryDummy::file() const
    {
        throw std::logic_error{"called 'file' of RtMessageLogEntryDummy"};
    }
 
    inline std::string
    RtMessageLogEntryDummy::func() const
    {
        throw std::logic_error{"called 'func' of RtMessageLogEntryDummy"};
    }
 
    inline RtMessageLogBuffer::RtMessageLogBuffer(std::size_t bufferSize,
                                                  std::size_t numEntries,
                                                  std::size_t bufferMaxSize,
                                                  std::size_t bufferMaxNumberEntries) :
        initialBufferSize{bufferSize},
        initialBufferEntryNumbers{numEntries},
        bufferMaxSize{bufferMaxSize},
        bufferMaxNumberEntries{bufferMaxNumberEntries},
        buffer(bufferSize, 0),
        bufferSpace{buffer.size()},
        bufferPlace{buffer.data()},
        entries(numEntries, nullptr)
    {
        ARMARX_DEBUG << "RtMessageLogBuffer created with bufferSize=" << bufferSize
                     << " and numEntries=" << numEntries << " and bufferMaxSize=" << bufferMaxSize
                     << " and bufferMaxNumberEntries " << bufferMaxNumberEntries;
    }
 
    inline RtMessageLogBuffer::~RtMessageLogBuffer()
    {
        deleteAll();
    }
 
    inline const std::vector<const RtMessageLogEntryBase*>&
    RtMessageLogBuffer::getEntries() const
    {
        return entries;
    }
 
    inline std::size_t
    RtMessageLogBuffer::getMaximalBufferSize() const
    {
        return bufferMaxSize;
    }
 
    inline std::size_t
    RtMessageLogBuffer::getMaximalNumberOfBufferEntries() const
    {
        return bufferMaxNumberEntries;
    }
 
    inline std::size_t
    ControlThreadOutputBufferEntry::getDataBufferSize() const
    {
        return buffer.size();
    }
} // namespace armarx::detail
 
namespace armarx
{
    inline std::size_t
    ControlThreadOutputBuffer::toBounds(std::size_t idx) const
    {
        return idx % numEntries;
    }
 
    inline std::size_t
    ControlThreadOutputBuffer::getNumberOfBytes() const
    {
        return data.size();
    }
 
    template <class LoggingEntryT, class... Ts>
    inline ControlThreadOutputBuffer::RtMessageLogEntryBase*
    ControlThreadOutputBuffer::addMessageToLog(Ts&&... args)
    {
        detail::RtMessageLogBuffer& messages = getWriteBuffer().messages;
        if (messages.entries.size() <= messages.entriesWritten)
        {
            ++messages.requiredAdditionalEntries;
            ++messages.messagesLost;
            return &detail::RtMessageLogEntryDummy::Instance;
        }
        messages.maxAlign = std::max(messages.maxAlign, alignof(LoggingEntryT));
        void* place = std::align(alignof(LoggingEntryT),
                                 sizeof(LoggingEntryT),
                                 messages.bufferPlace,
                                 messages.bufferSpace);
        if (!place)
        {
            messages.requiredAdditionalBufferSpace +=
                sizeof(LoggingEntryT) + alignof(LoggingEntryT) - 1;
            ++messages.messagesLost;
            return &detail::RtMessageLogEntryDummy::Instance;
        }
        RtMessageLogEntryBase* entry =
            new (place) LoggingEntryT(std::make_tuple(std::forward<Ts>(args)...));
        ARMARX_CHECK_NOT_NULL(entry);
        messages.bufferPlace = static_cast<std::uint8_t*>(place) + sizeof(LoggingEntryT);
 
        messages.bufferSpace -= sizeof(LoggingEntryT);
 
        ARMARX_CHECK_IS_NULL(messages.entries.at(messages.entriesWritten));
        messages.entries.at(messages.entriesWritten++) = entry;
        return entry;
    }
} // namespace armarx