ControlThreadOutputBuffer.cpp

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
 */
#include "ControlThreadOutputBuffer.h"
 
#include <memory>
#include <utility>
 
namespace armarx
{
    detail::RtMessageLogEntryDummy detail::RtMessageLogEntryDummy::Instance;
    detail::RtMessageLogEntryNull detail::RtMessageLogEntryNull::Instance;
    thread_local ControlThreadOutputBuffer* ControlThreadOutputBuffer::RtLoggingInstance{nullptr};
 
    ControlThreadOutputBuffer::Entry&
    ControlThreadOutputBuffer::getWriteBuffer()
    {
        ARMARX_CHECK_EXPRESSION(isInitialized);
        return entries.at(toBounds(writePosition));
    }
 
    void
    ControlThreadOutputBuffer::commitWrite()
    {
        ARMARX_CHECK_EXPRESSION(isInitialized);
        getWriteBuffer().iteration = writePosition;
        ++writePosition;
    }
 
    const ControlThreadOutputBuffer::Entry&
    ControlThreadOutputBuffer::getReadBuffer() const
    {
        ARMARX_CHECK_EXPRESSION(isInitialized);
        if (!onePastReadPosition)
        {
            //data is not initialized
            return entries.back();
        }
        return entries.at(toBounds(onePastReadPosition - 1));
    }
 
    bool
    ControlThreadOutputBuffer::updateReadBuffer() const
    {
        ARMARX_CHECK_EXPRESSION(isInitialized);
        std::size_t localWritePosition = this->writePosition;
        if (onePastReadPosition == localWritePosition)
        {
            //already up to date
            return false;
        }
        onePastReadPosition = localWritePosition;
        return true;
    }
 
    void
    ControlThreadOutputBuffer::resetLoggingPosition() const
    {
        ARMARX_CHECK_EXPRESSION(isInitialized);
        onePastLoggingReadPosition = writePosition.load();
    }
 
    void
    ControlThreadOutputBuffer::forEachNewLoggingEntry(ConsumerFunctor consumer)
    {
        ARMARX_TRACE;
        ARMARX_CHECK(isInitialized);
        ARMARX_DEBUG << VAROUT(entries.size());
        const size_t writePosition_local = writePosition.load(); // copy to prevent external changes
        if (writePosition_local - onePastLoggingReadPosition >= numEntries)
        {
            ARMARX_VERBOSE
                << "There are " << writePosition_local - onePastLoggingReadPosition
                << " unlogged entries, but only the last " << numEntries
                << " are saved! "
                   "There seems to be something wrong (e.g. the rt logging threw an exception, "
                   "the system load is too high or the logging takes to long). "
                   "The log position will be reset to the newest entry!";
            resetLoggingPosition();
        }
        //the number of new entries
        auto numNewEntries = writePosition_local - onePastLoggingReadPosition;
 
        if (numNewEntries >= numEntries)
        {
            ARMARX_VERBOSE << " more new entries (" << numNewEntries << ") than space ("
                           << numEntries << ") -> Skipping everything else";
            return;
        }
        //consume all
        const std::size_t num = writePosition_local - onePastLoggingReadPosition;
        ARMARX_DEBUG << num << " new entries to be treated";
        for (std::size_t offset = 0; onePastLoggingReadPosition < writePosition_local;
             ++onePastLoggingReadPosition, ++offset)
        {
            ARMARX_TRACE;
 
            detail::ControlThreadOutputBufferEntry& entry =
                entries.at(toBounds(onePastLoggingReadPosition));
            consumer(entry, offset, num);
            entry.messages.reset(messageBufferSize, messageBufferEntries, entry.iteration);
 
            //update how many
            auto newNumNewEntries = writePosition_local - onePastLoggingReadPosition;
            if (newNumNewEntries * 2 > numEntries)
            {
                ARMARX_VERBOSE << deactivateSpam(5) << "RT-Logging is slow! "
                               << "The RT-Thread writes faster new data than the RT-Logging thread "
                                  "consumes it! "
                               << " old/new/max number of entries: " << numNewEntries << " /"
                               << newNumNewEntries << " / " << numEntries;
            }
            numNewEntries = newNumNewEntries;
            if (numNewEntries >= numEntries)
            {
                ARMARX_VERBOSE << " more new entries (" << numNewEntries << ") than space ("
                               << numEntries << ") -> Skipping everything else";
                return;
            }
        }
    }
 
    void
    ControlThreadOutputBuffer::forLatestLoggingEntry(ConsumerFunctor consumer,
                                                     size_t numberOfEntriesToLog)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_EXPRESSION(isInitialized);
        size_t unloggedEntries = writePosition - onePastLoggingReadPosition;
        if (unloggedEntries >= numEntries)
        {
            ARMARX_VERBOSE
                << "There are " << unloggedEntries << " unlogged entries, but only the last "
                << numEntries
                << " are saved! "
                   "There seems to be something wrong (e.g. the rt logging threw an exception, "
                   "the system load is too high or the logging takes to long). "
                   "The log position will be reset to the newest entry!";
            resetLoggingPosition();
            unloggedEntries = writePosition - onePastLoggingReadPosition;
        }
        if (unloggedEntries > numberOfEntriesToLog)
        {
            ARMARX_DEBUG << "not logging all " << unloggedEntries << ". Only the last "
                         << numberOfEntriesToLog << " will be used.";
 
            const std::size_t onePastLoggingReadPositionLastRead =
                onePastLoggingReadPosition.load();
 
            // skip all messages except the latest numberOfEntriesToLog
            onePastLoggingReadPosition.store(writePosition.load() - numberOfEntriesToLog);
 
            // reset skipped entries
            for (std::size_t i = onePastLoggingReadPositionLastRead;
                 i < onePastLoggingReadPosition - 1;
                 i++)
            {
                detail::ControlThreadOutputBufferEntry& entry = entries.at(toBounds(i));
                entry.messages.reset(messageBufferSize, messageBufferEntries, entry.iteration);
            }
        }
 
        // use already existing impl.
        forEachNewLoggingEntry(std::move(consumer));
    }
 
    std::size_t
    ControlThreadOutputBuffer::initialize(
        std::size_t initialNumEntries,
        const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
        const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
        std::size_t initialMessageBufferSize,
        std::size_t messageBufferNumberEntries,
        std::size_t messageBufferMaxSize,
        std::size_t messageBufferMaxNumberEntries)
    {
        ARMARX_CHECK_EXPRESSION(!isInitialized);
        numEntries = initialNumEntries;
        messageBufferSize = initialMessageBufferSize;
        //decide whether to use a triple buffer (in case no rtlogging is used)
        ARMARX_CHECK_NOT_EQUAL(numEntries, 0);
        entries.reserve(numEntries);
        entries.emplace_back(controlDevices,
                             sensorDevices,
                             messageBufferSize,
                             messageBufferNumberEntries,
                             messageBufferMaxSize,
                             messageBufferMaxNumberEntries);
        for (std::size_t i = 1; i < numEntries; ++i)
        {
            entries.emplace_back(entries.at(0));
        }
 
        isInitialized = true;
        return entries.at(0).getDataBufferSize();
    }
 
    ControlThreadOutputBuffer::~ControlThreadOutputBuffer()
    {
        if (!isInitialized)
        {
            return;
        }
        for (Entry& e : entries)
        {
            for (auto& s : e.sensors)
            {
                s->~SensorValueBase();
            }
            for (auto& cs : e.control)
            {
                for (auto& c : cs)
                {
                    c->~ControlTargetBase();
                }
            }
        }
    }
 
    ControlThreadOutputBuffer::RtMessageLogEntryBase&
    ControlThreadOutputBuffer::RtMessageLogEntryBase::setLoggingLevel(MessageTypeT lvl)
    {
        printMsg = true;
        loggingLevel = lvl;
        return *this;
    }
 
    ControlThreadOutputBuffer::RtMessageLogEntryBase&
    ControlThreadOutputBuffer::RtMessageLogEntryBase::deactivateSpam(float sec)
    {
        printMsg = true;
        deactivateSpamSec = sec;
        return *this;
    }
 
    ControlThreadOutputBuffer::RtMessageLogEntryBase&
    ControlThreadOutputBuffer::RtMessageLogEntryBase::deactivateSpamTag(std::uint64_t tag)
    {
        printMsg = true;
        deactivateSpamTag_ = tag;
        return *this;
    }
 
    void
    detail::RtMessageLogEntryBase::print(Ice::Int controlThreadId) const
    {
        if (printMsg)
        {
            (checkLogLevel(loggingLevel))
                ? _GlobalDummyLogSender
                : (*loghelper(file().c_str(), line(), func().c_str())
                        ->setBacktrace(false)
                        ->setTag({"RtLogMessages"})
                        ->setThreadId(controlThreadId))
                      << loggingLevel
                      << ::deactivateSpam(deactivateSpamSec, to_string(deactivateSpamTag_))
                      << format();
        }
    }
 
    const auto PotentiallyMinimizeMember = [](auto& member, bool minimize)
    { return minimize ? 0 : member; };
 
    detail::RtMessageLogBuffer::RtMessageLogBuffer(const detail::RtMessageLogBuffer& other,
                                                   bool minimize) :
        initialBufferSize{PotentiallyMinimizeMember(other.initialBufferSize, minimize)},
        initialBufferEntryNumbers{
            PotentiallyMinimizeMember(other.initialBufferEntryNumbers, minimize)},
        bufferMaxSize{PotentiallyMinimizeMember(other.bufferMaxSize, minimize)},
        bufferMaxNumberEntries{PotentiallyMinimizeMember(other.bufferMaxNumberEntries, minimize)},
        buffer(minimize ? other.buffer.size() + other.maxAlign - 1 - other.bufferSpace
                        : other.buffer.size(),
               0),
        bufferSpace{buffer.size()},
        bufferPlace{buffer.data()},
        entries(minimize ? other.entriesWritten : other.entries.size(), nullptr),
        entriesWritten{0},
        requiredAdditionalBufferSpace{other.requiredAdditionalBufferSpace},
        requiredAdditionalEntries{other.requiredAdditionalEntries},
        messagesLost{other.messagesLost},
        maxAlign{1}
    {
        ARMARX_DEBUG << "copying RtMessageLogBuffer with minimize = " << minimize << " "
                     << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
                     << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
                     << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
                     << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
                     << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace)
                     << " " << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost)
                     << " " << VAROUT(maxAlign);
        for (std::size_t idx = 0; idx < other.entries.size() && other.entries.at(idx); ++idx)
        {
            const RtMessageLogEntryBase* entry = other.entries.at(idx);
            ARMARX_CHECK_NOT_NULL(entry);
            maxAlign = std::max(maxAlign, entry->_alignof());
            void* place = std::align(entry->_alignof(), entry->_sizeof(), bufferPlace, bufferSpace);
            const auto hint = ARMARX_STREAM_PRINTER
            {
                out << "entry " << idx << " of " << other.entriesWritten
                    << "\nbuffer first       = " << static_cast<void*>(&buffer.front())
                    << "\nbuffer last        = " << static_cast<void*>(&buffer.back())
                    << "\nbuffer size        = " << buffer.size()
                    << "\nbuffer place       = " << bufferPlace
                    << "\nbuffer space       = " << bufferSpace
                    << "\nentry size         = " << entry->_sizeof()
                    << "\nentry align        = " << entry->_alignof() << "\n"
                    << "\nother buffer size  = " << other.buffer.size()
                    << "\nother buffer space = " << other.bufferSpace
                    << "\nother max align    = " << other.maxAlign << "\n"
                    << "\nthis               = " << this;
            };
            ARMARX_CHECK_NOT_NULL(place) << hint;
            ARMARX_CHECK_LESS_EQUAL(static_cast<void*>(&buffer.front()), static_cast<void*>(place))
                << hint;
            ARMARX_CHECK_LESS_EQUAL(static_cast<void*>(place), static_cast<void*>(&buffer.back()))
                << hint;
            ARMARX_CHECK_LESS_EQUAL(
                static_cast<void*>(static_cast<std::uint8_t*>(place) + entry->_sizeof() - 1),
                static_cast<void*>(&buffer.back()))
                << hint;
            ARMARX_CHECK_LESS_EQUAL(place, &buffer.back());
            ARMARX_CHECK_LESS(entriesWritten, entries.size());
            ARMARX_CHECK_EXPRESSION(!entries.at(entriesWritten));
            entries.at(entriesWritten++) = entry->_placementCopyConstruct(place);
            bufferSpace -= entry->_sizeof();
            bufferPlace = static_cast<std::uint8_t*>(place) + entry->_sizeof();
        }
        ARMARX_CHECK_EQUAL(entriesWritten, other.entriesWritten);
        ARMARX_CHECK_EQUAL(maxAlign, other.maxAlign);
        if (minimize)
        {
            ARMARX_CHECK_EQUAL(entriesWritten, entries.size());
        }
    }
 
    void
    detail::RtMessageLogBuffer::reset(std::size_t& bufferSize,
                                      std::size_t& numEntries,
                                      std::size_t iterationCount)
    {
        deleteAll();
        if (requiredAdditionalEntries || entries.size() < numEntries)
        {
            const auto numExcessEntries =
                std::max(requiredAdditionalEntries, numEntries - entries.size());
            const auto requiredSize = entries.size() + numExcessEntries;
            ARMARX_VERBOSE << "Iteration " << iterationCount << " required "
                           << requiredAdditionalEntries << " | " << numExcessEntries
                           << " additional message entries. \n"
                           << "The requested total number of entries is " << requiredSize << ". \n"
                           << "The current number of entries is " << entries.size() << ". \n"
                           << "The maximal number of entries is "
                           << getMaximalNumberOfBufferEntries();
            if (requiredSize > getMaximalNumberOfBufferEntries())
            {
                ARMARX_VERBOSE << deactivateSpam(1, to_string(requiredSize)) << "Iteration "
                               << iterationCount << " would require " << requiredSize
                               << " message entries, but the maximal number of entries is "
                               << getMaximalNumberOfBufferEntries();
            }
            numEntries = std::max(requiredSize, getMaximalNumberOfBufferEntries());
            entries.resize(numEntries, nullptr);
            requiredAdditionalEntries = 0;
        }
        if (requiredAdditionalBufferSpace)
        {
            const auto requiredSpace = buffer.size() + requiredAdditionalBufferSpace;
            ARMARX_WARNING << "Iteration " << iterationCount << " required "
                           << requiredAdditionalBufferSpace
                           << " additional bytes for messages in the buffer. \n"
                           << "Therefore the new required size is " << requiredSpace << ". \n"
                           << "The current size of the buffer is " << buffer.size() << ". \n"
                           << "The maximal size of the buffer is " << getMaximalBufferSize();
            if (requiredSpace > getMaximalBufferSize())
            {
                ARMARX_WARNING << deactivateSpam(1, to_string(requiredSpace)) << "Iteration "
                               << iterationCount << " would require " << requiredSpace
                               << " bytes for messages buffer, but the maximal buffer size is "
                               << getMaximalBufferSize();
            }
            bufferSize = std::min(requiredSpace, getMaximalBufferSize());
            buffer.resize(bufferSize, 0);
            requiredAdditionalBufferSpace = 0;
            messagesLost = 0;
            bufferSpace = buffer.size();
            bufferPlace = buffer.data();
        }
        maxAlign = 1;
    }
 
    void
    detail::RtMessageLogBuffer::deleteAll()
    {
        for (std::size_t idx = 0; idx < entries.size() && entries.at(idx); ++idx)
        {
            entries.at(idx)->~RtMessageLogEntryBase();
            entries.at(idx) = nullptr;
        }
        bufferSpace = buffer.size();
        bufferPlace = buffer.data();
        entriesWritten = 0;
    }
 
    detail::ControlThreadOutputBufferEntry::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) :
        sensorValuesTimestamp{IceUtil::Time::microSeconds(0)},
        timeSinceLastIteration{IceUtil::Time::microSeconds(0)},
        messages{messageBufferSize,
                 messageBufferNumberEntries,
                 messageBufferMaxSize,
                 messageBufferMaxNumberEntries}
    {
        //calculate size in bytes for one buffer
        const std::size_t bytes = [&]
        {
            std::size_t maxAlign = 1;
            std::size_t bytes = 0;
            for (const SensorDevicePtr& sd : sensorDevices.values())
            {
                const auto align = sd->getSensorValue()->_alignof();
                const auto alignShift = (align - bytes % align) % align;
                bytes += alignShift;
                maxAlign = std::max(maxAlign, align);
 
                bytes += sd->getSensorValue()->_sizeof();
            }
            for (const ControlDevicePtr& cd : controlDevices.values())
            {
                for (const JointController* ctrl : cd->getJointControllers())
                {
                    const auto align = ctrl->getControlTarget()->_alignof();
                    const auto alignShift = (align - bytes % align) % align;
                    bytes += alignShift;
                    maxAlign = std::max(maxAlign, align);
 
                    bytes += ctrl->getControlTarget()->_sizeof();
                }
            }
            return bytes + maxAlign - 1;
        }();
 
        buffer.resize(bytes, 0);
        void* place = buffer.data();
        std::size_t space = buffer.size();
 
        auto getAlignedPlace = [&space, &place, this](std::size_t bytes, std::size_t alignment)
        {
            ARMARX_CHECK_EXPRESSION(std::align(alignment, bytes, place, space));
            ARMARX_CHECK_LESS(bytes, space);
            const auto resultPlace = place;
            place = static_cast<std::uint8_t*>(place) + bytes;
            space -= bytes;
            ARMARX_CHECK_LESS_EQUAL(place, &buffer.back());
            return resultPlace;
        };
 
        sensors.reserve(sensorDevices.size());
        for (const SensorDevicePtr& sd : sensorDevices.values())
        {
            const SensorValueBase* sv = sd->getSensorValue();
            sensors.emplace_back(
                sv->_placementConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
        }
 
        control.reserve(controlDevices.size());
        for (const ControlDevicePtr& cd : controlDevices.values())
        {
            control.emplace_back();
            auto& ctargs = control.back();
            const auto ctrls = cd->getJointControllers();
            ctargs.reserve(ctrls.size());
            for (const JointController* ctrl : ctrls)
            {
                const ControlTargetBase* ct = ctrl->getControlTarget();
                ctargs.emplace_back(
                    ct->_placementConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
                ctargs.back()->reset();
            }
        }
    }
 
    detail::ControlThreadOutputBufferEntry::ControlThreadOutputBufferEntry(
        const detail::ControlThreadOutputBufferEntry& other,
        bool minimize) :
        writeTimestamp{other.writeTimestamp},
        sensorValuesTimestamp{other.sensorValuesTimestamp},
        timeSinceLastIteration{other.timeSinceLastIteration},
        iteration{other.iteration},
        messages{other.messages, minimize},
        buffer(other.buffer.size(), 0)
    {
        ARMARX_TRACE;
        ARMARX_DEBUG << "Copy ControlThreadOutputBufferEntry with parameters: " << VAROUT(minimize)
                     << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
                     << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
                     << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
                     << VAROUT(messages.entries.size());
        void* place = buffer.data();
        std::size_t space = buffer.size();
 
        auto getAlignedPlace = [&space, &place, this](std::size_t bytes, std::size_t alignment)
        {
            ARMARX_TRACE;
            ARMARX_CHECK_EXPRESSION(std::align(alignment, bytes, place, space));
            ARMARX_CHECK_LESS(bytes, space);
            const auto resultPlace = place;
            place = static_cast<std::uint8_t*>(place) + bytes;
            space -= bytes;
            ARMARX_CHECK_LESS_EQUAL(place, &buffer.back());
            return resultPlace;
        };
 
        //copy sensor values
        sensors.reserve(other.sensors.size());
        for (const SensorValueBase* sv : other.sensors)
        {
            ARMARX_TRACE;
            sensors.emplace_back(
                sv->_placementCopyConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
        }
 
        //copy control targets
        control.reserve(other.control.size());
        for (const auto& cdctargs : other.control)
        {
            ARMARX_TRACE;
            control.emplace_back();
            auto& ctargs = control.back();
            ctargs.reserve(cdctargs.size());
            for (const ControlTargetBase* ct : cdctargs)
            {
                ctargs.emplace_back(
                    ct->_placementCopyConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
            }
        }
    }
} // namespace armarx