RobotUnitModuleLogging.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::ArmarXObjects::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "RobotUnitModuleLogging.h"
 
#include <regex>
 
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "RobotAPI/components/units/RobotUnit/util/NonRtTiming.h"
 
#include "../util/ControlThreadOutputBuffer.h"
#include "RobotUnitModuleControlThreadDataBuffer.h"
#include "RobotUnitModuleDevices.h"
 
namespace armarx::RobotUnitModule::details
{
    struct DoLoggingDurations
    {
        struct DurationsEntry
        {
            DurationsEntry()
            {
                t = IceUtil::Time::seconds(0);
            }
 
            IceUtil::Time t;
 
            void
            start()
            {
                t -= armarx::rtNow();
                ++n;
            }
 
            void
            stop()
            {
                t += armarx::rtNow();
            }
 
            double
            ms() const
            {
                return t.toMilliSecondsDouble();
            }
 
            std::size_t n = 0;
        };
 
        DurationsEntry header;
        DurationsEntry header_csv;
        DurationsEntry header_stream;
        DurationsEntry sens;
        DurationsEntry sens_csv;
        DurationsEntry sens_stream;
        DurationsEntry sens_stream_elem;
        DurationsEntry ctrl;
        DurationsEntry ctrl_csv;
        DurationsEntry ctrl_stream;
        DurationsEntry ctrl_stream_elem;
        DurationsEntry backlog;
        DurationsEntry msg;
    };
} // namespace armarx::RobotUnitModule::details
 
namespace armarx::RobotUnitModule
{
    void
    Logging::addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token,
                              const std::string& marker,
                              const Ice::Current&)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{rtLoggingMutex};
        if (!rtLoggingEntries.count(token->getId()))
        {
            throw InvalidArgumentException{"addMarkerToRtLog called for a nonexistent log"};
        }
        rtLoggingEntries.at(token->getId())->marker = marker;
    }
 
    SimpleRemoteReferenceCounterBasePtr
    Logging::startRtLogging(const std::string& formatString,
                            const Ice::StringSeq& loggingNames,
                            const Ice::Current&)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        StringStringDictionary alias;
        for (const auto& name : loggingNames)
        {
            alias.emplace(name, "");
        }
        return startRtLoggingWithAliasNames(formatString, alias, Ice::emptyCurrent);
    }
 
    void
    Logging::stopRtLogging(const SimpleRemoteReferenceCounterBasePtr& token, const Ice::Current&)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{rtLoggingMutex};
        ARMARX_CHECK_NOT_NULL(token);
        try
        {
            if (!rtLoggingEntries.count(token->getId()))
            {
                throw InvalidArgumentException{"stopRtLogging called for a nonexistent log"};
            }
            ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file "
                           << rtLoggingEntries.at(token->getId())->filename;
            rtLoggingEntries.at(token->getId())->stopLogging = true;
        }
        catch (...)
        {
            ARMARX_WARNING << "Error during attempting to stop rt logging";
        }
    }
 
    Ice::StringSeq
    Logging::getLoggingNames(const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        Ice::StringSeq result;
        for (const auto& data : sensorDeviceValueMetaData)
        {
            for (auto& fieldData : data.fields)
            {
                result.push_back(fieldData.name);
            }
        }
        for (const auto& datas : controlDeviceValueMetaData)
        {
            for (const auto& data : datas)
            {
                for (auto& fieldData : data.fields)
                {
                    result.push_back(fieldData.name);
                }
            }
        }
        return result;
    }
 
    SimpleRemoteReferenceCounterBasePtr
    Logging::startRtLoggingWithAliasNames(const std::string& formatString,
                                          const StringStringDictionary& aliasNames,
                                          const Ice::Current&)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        FileSystemPathBuilder pb{formatString};
        std::lock_guard<std::mutex> guard{rtLoggingMutex};
        if (rtLoggingEntries.count(pb.getPath()))
        {
            throw InvalidArgumentException{"There already is a logger for the path '" +
                                           pb.getPath() + "'"};
        }
        rtLoggingEntries[pb.getPath()].reset(new CSVLoggingEntry());
        auto ptr = rtLoggingEntries[pb.getPath()];
        CSVLoggingEntry& e = *ptr;
        e.filename = pb.getPath();
        pb.createParentDirectories();
        e.stream.open(e.filename);
        if (!e.stream)
        {
            rtLoggingEntries.erase(pb.getPath());
            throw LogicError{"RtLogging could not open filestream for '" + pb.getPath() + "'"};
        }
        ARMARX_VERBOSE << "Start logging to " << e.filename
                       << ". Names (pattern, replacement name): " << aliasNames;
 
        std::stringstream header;
        header << "marker;iteration;timestamp;TimeSinceLastIteration";
        auto logDev = [&](const std::string& dev)
        {
            ARMARX_TRACE_LITE;
            for (const auto& [key, value] : aliasNames)
            {
                if (MatchName(key, dev))
                {
                    header << ";" << (value.empty() ? dev : value);
                    return true;
                }
            }
            return false;
        };
 
        //get logged sensor device values
        {
            ARMARX_TRACE;
            e.loggedSensorDeviceValues.reserve(sensorDeviceValueMetaData.size());
            for (const auto& valData : sensorDeviceValueMetaData)
            {
                e.loggedSensorDeviceValues.emplace_back();
                auto& svfieldsFlags = e.loggedSensorDeviceValues.back(); //vv
                svfieldsFlags.reserve(valData.fields.size());
                for (const auto& field : valData.fields)
                {
                    svfieldsFlags.emplace_back(logDev(field.name));
                }
            }
        }
        //get logged control device values
        {
            ARMARX_TRACE;
            e.loggedControlDeviceValues.reserve(controlDeviceValueMetaData.size());
            for (const auto& datas : controlDeviceValueMetaData)
            {
                e.loggedControlDeviceValues.emplace_back();
                auto& deviceCtrlFlags = e.loggedControlDeviceValues.back(); //vv
                deviceCtrlFlags.reserve(datas.size());
                for (const auto& valData : datas)
                {
                    deviceCtrlFlags.emplace_back();
                    auto& ctrlFieldFlags = deviceCtrlFlags.back(); //v
                    ctrlFieldFlags.reserve(valData.fields.size());
 
                    for (const auto& field : valData.fields)
                    {
                        ctrlFieldFlags.emplace_back(logDev(field.name));
                    }
                }
            }
        }
        ARMARX_TRACE;
 
        //write header
        e.stream << header.str()
                 << std::flush; // newline is written at the beginning of each log line
        //create and return handle
        auto block = getArmarXManager()->createSimpleRemoteReferenceCount(
            [ptr]()
            {
                ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << ptr->filename;
                ptr->stopLogging = true;
            },
            e.filename,
            IceUtil::Time::milliSeconds(100));
        auto counter = block->getReferenceCounter();
        block->activateCounting();
        ARMARX_DEBUG_S << "RobotUnit: start RtLogging for file " << ptr->filename;
        return counter;
    }
 
    void
    Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{rtLoggingMutex};
        FileSystemPathBuilder pb{formatString};
        pb.createParentDirectories();
        std::ofstream outCSV{pb.getPath() + ".csv"};
        if (!outCSV)
        {
            throw LogicError{"writeRecentIterationsToFile could not open filestream for '" +
                             pb.getPath() + ".csv'"};
        }
        std::ofstream outMsg{pb.getPath() + ".messages"};
        if (!outMsg)
        {
            throw LogicError{"writeRecentIterationsToFile could not open filestream for '" +
                             pb.getPath() + ".messages'"};
        }
        ARMARX_INFO << "writing the last " << backlog.size() << " iterations to " << pb.getPath()
                    << ".{csv, messages}";
        //write csv header
        {
            outCSV << "iteration;timestamp";
            for (const auto& vs : sensorDeviceValueMetaData)
            {
                for (const auto& f : vs.fields)
                {
                    outCSV << ";" << f.name;
                }
            }
            for (const auto& vvs : controlDeviceValueMetaData)
            {
                for (const auto& vs : vvs)
                {
                    for (const auto& f : vs.fields)
                    {
                        outCSV << ";" << f.name;
                    }
                }
            }
            outCSV << std::endl;
        }
 
        for (const ::armarx::detail::ControlThreadOutputBufferEntry& iteration : backlog)
        {
            //write csv data
            {
                outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
                //sens
                {
                    for (const SensorValueBase* val : iteration.sensors)
                    {
                        for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
                             ++idxField)
                        {
                            std::string s;
                            val->getDataFieldAs(idxField, s);
                            outCSV << ";" << s;
                        }
                    }
                }
                //ctrl
                {
                    for (const auto& vals : iteration.control)
                    {
                        for (const ControlTargetBase* val : vals)
                        {
                            for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
                                 ++idxField)
                            {
                                std::string s;
                                val->getDataFieldAs(idxField, s);
                                outCSV << ";" << s;
                            }
                        }
                    }
                }
                outCSV << std::endl;
            }
            //write message data
            {
                bool atLeastOneMessage = false;
                for (const ::armarx::detail::RtMessageLogEntryBase* msg :
                     iteration.messages.getEntries())
                {
                    if (!msg)
                    {
                        break;
                    }
                    outMsg << "[" << msg->getTime().toDateTime() << "] iteration "
                           << iteration.iteration << ":\n"
                           << msg->format() << std::endl;
                    atLeastOneMessage = true;
                }
                if (atLeastOneMessage)
                {
                    outMsg << "\nmessages lost: " << iteration.messages.messagesLost
                           << " (required additional "
                           << iteration.messages.requiredAdditionalBufferSpace << " bytes, "
                           << iteration.messages.requiredAdditionalEntries << " message entries)\n"
                           << std::endl;
                }
            }
        }
    }
 
    RobotUnitDataStreaming::DataStreamingDescription
    Logging::startDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
                                const RobotUnitDataStreaming::Config& config,
                                const Ice::Current&)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        if (!receiver)
        {
            throw InvalidArgumentException{"Receiver proxy is NULL!"};
        }
        std::lock_guard<std::mutex> guard{rtLoggingMutex};
        if (rtDataStreamingEntry.count(receiver))
        {
            throw InvalidArgumentException{"There already is a logger for the given receiver"};
        }
 
        RobotUnitDataStreaming::DataStreamingDescription result;
        DataStreamingEntry& streamingEntry = rtDataStreamingEntry[receiver];
        getProperty(streamingEntry.rtStreamMaxClientErrors,
                    "RTLogging_StreamingDataMaxClientConnectionFailures");
 
 
        ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
                    << ". Values: " << config.loggingNames;
        auto devMatchesAnyKey = [&](const std::string& dev)
        {
            for (const auto& key : config.loggingNames)
            {
                if (MatchName(key, dev))
                {
                    return true;
                }
            }
            return false;
        };
 
        const auto handleVal = [&](const ValueMetaData& valData,
                                   DataStreamingEntry& streamingEntry,
                                   RobotUnitDataStreaming::DataStreamingDescription& descr)
            -> std::vector<DataStreamingEntry::OutVal>
        {
            ARMARX_TRACE_LITE;
            std::vector<DataStreamingEntry::OutVal> result;
            result.resize(valData.fields.size());
            for (std::size_t i = 0; i < valData.fields.size(); ++i)
            {
                if (!devMatchesAnyKey(valData.fields.at(i).name))
                {
                    continue; //do not add to result and skipp during processing
                }
                auto& descrEntr = descr.entries[valData.fields.at(i).name];
                //*INDENT-OFF*
                // clang-format off
#define make_case(Type, TName)                                                                     \
    (typeid(Type) == *valData.fields.at(i).type)                                                   \
    {                                                                                              \
        descrEntr.index = streamingEntry.num##TName##s;                                            \
        descrEntr.type = RobotUnitDataStreaming::NodeType##TName;                                  \
        result.at(i).idx = streamingEntry.num##TName##s;                                           \
        result.at(i).value = DataStreamingEntry::ValueT::TName;                                    \
        ++streamingEntry.num##TName##s;                                                            \
    }
                if make_case (bool, Bool)
                else if make_case (Ice::Byte, Byte)
                else if make_case (Ice::Short, Short)
                else if make_case (Ice::Int, Int)
                else if make_case (Ice::Long, Long)
                else if make_case (Ice::Float, Float)
                else if make_case (Ice::Double, Double)
                else if make_case (std::uint16_t, Long)
                else if make_case (std::uint32_t, Long)
                else {
                    ARMARX_CHECK_EXPRESSION(false)
                                << "This code sould be unreachable! "
                                   "The type of "
                                << valData.fields.at(i).name << " is not handled correctly!";
                }
#undef make_case
            }
            return result;
        };
        //*INDENT-ON*
        // clang-format on
        //get logged sensor device values
        {
            ARMARX_TRACE;
            streamingEntry.sensDevs.reserve(sensorDeviceValueMetaData.size());
            for (const auto& valData : sensorDeviceValueMetaData)
            {
                streamingEntry.sensDevs.emplace_back(handleVal(valData, streamingEntry, result));
            }
        }
        //get logged control device values
        {
            ARMARX_TRACE;
            streamingEntry.ctrlDevs.reserve(controlDeviceValueMetaData.size());
            for (const auto& devData : controlDeviceValueMetaData)
            {
                streamingEntry.ctrlDevs.emplace_back();
                auto& ctrlDevEntrs = streamingEntry.ctrlDevs.back();
                ctrlDevEntrs.reserve(devData.size());
                for (const auto& valData : devData)
                {
                    ctrlDevEntrs.emplace_back(handleVal(valData, streamingEntry, result));
                }
            }
        }
 
        return result;
    }
 
    void
    Logging::stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
                               const Ice::Current&)
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{rtLoggingMutex};
 
        if (rtDataStreamingEntry.count(receiver) == 0u)
        {
            ARMARX_INFO << "stopDataStreaming called for a nonexistent log";
            return;
        }
 
        ARMARX_INFO_S << "RobotUnit: request to stop DataStreaming for " << receiver->ice_id();
        rtDataStreamingEntry.at(receiver).stopStreaming = true;
    }
 
    void
    Logging::_preFinishRunning()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        defaultLogHandle = nullptr;
        if (rtLoggingTask.joinable())
        {
            ARMARX_DEBUG << "shutting down rt logging task";
            stopRtLoggingTask = true;
            rtLoggingTask.join();
            ARMARX_DEBUG << "shutting down rt logging task done";
        }
    }
 
    void
    Logging::_preFinishControlThreadInitialization()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        controlThreadId = LogSender::getThreadId();
        ControlThreadOutputBuffer::RtLoggingInstance =
            &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
 
        ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestepMs;
        stopRtLoggingTask = false;
        Metronome metronome(Duration::MilliSeconds(rtLoggingTimestepMs));
        rtLoggingTask = std::thread{
            [this]
            {
                using clock_t = std::chrono::steady_clock;
                const auto now = [] { return clock_t::now(); };
                Metronome metronome(Duration::MilliSeconds(rtLoggingTimestepMs));
                while (!stopRtLoggingTask)
                {
                    const auto start = now();
                    doLogging();
                    const std::uint64_t ms =
                        std::chrono::duration_cast<std::chrono::milliseconds>(now() - start)
                            .count();
                    if (ms > rtLoggingTimestepMs)
                    {
                        ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms
                                       << " ms > " << rtLoggingTimestepMs
                                       << " ms (message printed every 10 seconds)";
                    }
                    metronome.waitForNextTick();
                }
            }};
    }
 
    void
    Logging::doLogging()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        std::lock_guard<std::mutex> guard{rtLoggingMutex};
        const auto now = armarx::rtNow();
        // entries are removed last
 
        //remove backlog entries
        const auto start_time_remove_backlog_entries = armarx::rtNow();
        {
            if (rtLoggingBacklogEnabled)
            {
                while (!backlog.empty() &&
                       (backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now or
                        backlog.size() > rtLoggingBacklogMaxSize))
                {
                    backlog.pop_front();
                }
            }
        }
        //log all
        const auto start_time_log_all = armarx::rtNow();
        details::DoLoggingDurations dlogdurs;
        {
            if (!rtLoggingEntries.empty() || !rtDataStreamingEntry.empty())
            {
                ARMARX_DEBUG << deactivateSpam() << "Number of logs    " << rtLoggingEntries.size()
                             << '\n'
                             << "Number of streams " << rtDataStreamingEntry.size();
            }
 
            if (numberOfEntriesToLog < 1)
            {
                _module<ControlThreadDataBuffer>()
                    .getControlThreadOutputBuffer()
                    .forEachNewLoggingEntry(
                        [this, &dlogdurs, &now](const auto& data, auto i, auto num)
                        {
                            ARMARX_TRACE;
                            doLogging(dlogdurs, now, data, i, num);
                        });
            }
            else
            { // only log newest entry
                _module<ControlThreadDataBuffer>()
                    .getControlThreadOutputBuffer()
                    .forLatestLoggingEntry(
                        [this, &dlogdurs, &now](const auto& data, auto i, auto num)
                        {
                            ARMARX_TRACE;
                            doLogging(dlogdurs, now, data, i, num);
                        },
                        numberOfEntriesToLog);
            }
        }
        ARMARX_DEBUG << ::deactivateSpam() << "the last " << backlog.size()
                     << " iterations are stored";
        //flush all files
        const auto start_time_flush_all_files = armarx::rtNow();
        {
            for (auto& pair : rtLoggingEntries)
            {
                pair.second->stream << std::flush;
            }
        }
 
        //remove entries
        const auto start_time_remove_entries = armarx::rtNow();
        {
            ARMARX_TRACE;
            std::vector<std::string> toRemove;
            toRemove.reserve(rtLoggingEntries.size());
            for (auto& [key, value] : rtLoggingEntries)
            {
                if (value->stopLogging)
                {
                    //can't remove the current elemet
                    //(this would invalidate the current iterator)
                    toRemove.emplace_back(key);
                }
            }
            for (const auto& rem : toRemove)
            {
                rtLoggingEntries.erase(rem);
            }
        }
        //deal with data streaming
        const auto start_time_data_streaming = armarx::rtNow();
        {
            ARMARX_TRACE;
            std::vector<RobotUnitDataStreaming::ReceiverPrx> toRemove;
            toRemove.reserve(rtDataStreamingEntry.size());
            for (auto& [prx, data] : rtDataStreamingEntry)
            {
                ARMARX_DEBUG_S << data.entryBuffer.size() << " entries to send to "
                               << prx->ice_toString();
                if (data.stopStreaming)
                {
                    toRemove.emplace_back(prx);
                }
                else
                {
                    data.send(prx, rtDataStreamingMsgID);
                }
            }
            ++rtDataStreamingMsgID;
            for (const auto& prx : toRemove)
            {
                rtDataStreamingEntry.erase(prx);
            }
        }
        // clang-format off
        const auto end_time = armarx::rtNow();
        const auto time_total = (end_time - now).toMilliSecondsDouble();
        ARMARX_DEBUG_S << deactivateSpam(1)
                     << "rtlogging time required:        " << time_total << "ms\n"
                     << "    time_remove_backlog_entries "
                     << (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
                     << "    time_log_all                "
                     << (start_time_flush_all_files - start_time_log_all).toMilliSecondsDouble() << "ms\n"
                     << "        header                  " << dlogdurs.header.ms() << "ms\t(" << dlogdurs.header.n
                     << " calls)\n"
                     << "            csv                 " << dlogdurs.header_csv.ms() << "ms\t("
                     << dlogdurs.header_csv.n << " calls)\n"
                     << "            stream              " << dlogdurs.header_stream.ms() << "ms\t("
                     << dlogdurs.header_stream.n << " calls)\n"
                     << "        sens                    " << dlogdurs.sens.ms() << "ms\t(" << dlogdurs.sens.n
                     << " calls)\n"
                     << "            csv                 " << dlogdurs.sens_csv.ms() << "ms\t(" << dlogdurs.sens_csv.n
                     << " calls)\n"
                     << "            stream              " << dlogdurs.sens_stream.ms() << "ms\t("
                     << dlogdurs.sens_stream.n << " calls)\n"
                     << "                per elem        " << dlogdurs.sens_stream_elem.ms() << "ms\t("
                     << dlogdurs.sens_stream_elem.n << " calls)\n"
                     << "        ctrl                    " << dlogdurs.ctrl.ms() << "ms\t(" << dlogdurs.ctrl.n
                     << " calls)\n"
                     << "            csv                 " << dlogdurs.ctrl_csv.ms() << "ms\t(" << dlogdurs.ctrl_csv.n
                     << " calls)\n"
                     << "            stream              " << dlogdurs.ctrl_stream.ms() << "ms\t("
                     << dlogdurs.ctrl_stream.n << " calls)\n"
                     << "                per elem        " << dlogdurs.ctrl_stream_elem.ms() << "ms\t("
                     << dlogdurs.ctrl_stream_elem.n << " calls)\n"
                     << "        backlog                 " << dlogdurs.backlog.ms() << "ms\t(" << dlogdurs.backlog.n
                     << " calls)\n"
                     << "        msg                     " << dlogdurs.msg.ms() << "ms\t(" << dlogdurs.msg.n
                     << " calls)\n"
                     << "    time_flush_all_files        "
                     << (start_time_remove_entries - start_time_flush_all_files).toMilliSecondsDouble() << "ms\n"
                     << "    time_remove_entries         "
                     << (start_time_data_streaming - start_time_remove_entries).toMilliSecondsDouble() << "ms\n"
                     << "    time_data_streaming         "
                     << (end_time - start_time_data_streaming).toMilliSecondsDouble() << "ms\n";
        // clang-format on
    }
 
    void
    Logging::doLogging(details::DoLoggingDurations& durations,
                       const IceUtil::Time& now,
                       const ControlThreadOutputBuffer::Entry& data,
                       std::size_t i,
                       std::size_t num)
    {
 
        ARMARX_TRACE;
 
        // Header.
        {
            durations.header.start();
            ARMARX_TRACE;
            //base (marker;iteration;timestamp)
            if (!rtLoggingEntries.empty())
            {
                durations.header_csv.start();
                for (auto& [_, e] : rtLoggingEntries)
                {
                    e->stream << "\n"
                              << e->marker << ";" << data.iteration << ";"
                              << data.sensorValuesTimestamp.toMicroSeconds() << ";"
                              << data.timeSinceLastIteration.toMicroSeconds();
                    e->marker.clear();
                }
                durations.header_csv.stop();
            }
            //streaming
            if (!rtDataStreamingEntry.empty())
            {
                durations.header_stream.start();
                for (auto& [_, e] : rtDataStreamingEntry)
                {
                    e.processHeader(data);
                }
                durations.header_stream.stop();
            }
            durations.header.stop();
        }
 
        // Process devices.
        { // Sensors.
            {
                ARMARX_TRACE;
                durations.sens.start();
                //sensors
                for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
                {
                    const SensorValueBase* val = data.sensors.at(idxDev);
                    //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
                    for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
                         ++idxField)
                    {
                        if (!rtLoggingEntries.empty())
                        {
                            durations.sens_csv.start();
                            const auto str = val->getDataFieldAs<std::string>(idxField);
                            for (auto& [_, entry] : rtLoggingEntries)
                            {
                                if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
                                {
                                    entry->stream << ";" << str;
                                }
                            }
                            durations.sens_csv.stop();
                        }
                        if (!rtDataStreamingEntry.empty())
                        {
                            durations.sens_stream.start();
                            for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
                            {
                                durations.sens_stream_elem.start();
                                rtStreamingEntry.processSens(*val, idxDev, idxField);
                                durations.sens_stream_elem.stop();
                            }
                            durations.sens_stream.stop();
                        }
                    }
                }
                durations.sens.stop();
            }
 
            // Controller.
            {
                durations.ctrl.start();
                ARMARX_TRACE;
                //joint controllers
                for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
                {
                    const auto& vals = data.control.at(idxDev);
                    //control value (e.g. v_platform)
                    for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
                    {
                        const ControlTargetBase* val = vals.at(idxVal);
                        //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
                        for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
                             ++idxField)
                        {
                            if (!rtLoggingEntries.empty())
                            {
                                durations.ctrl_csv.start();
                                std::string str;
                                val->getDataFieldAs(idxField, str); // expensive function
                                for (auto& [_, entry] : rtLoggingEntries)
                                {
                                    if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(
                                            idxField))
                                    {
                                        entry->stream << ";" << str;
                                    }
                                }
                                durations.ctrl_csv.stop();
                            }
                            if (!rtDataStreamingEntry.empty())
                            {
                                durations.ctrl_stream.start();
                                for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
                                {
                                    durations.ctrl_stream_elem.start();
                                    rtStreamingEntry.processCtrl(*val, idxDev, idxVal, idxField);
                                    durations.ctrl_stream_elem.stop();
                                }
                                durations.ctrl_stream.stop();
                            }
                        }
                    }
                }
 
                durations.ctrl.stop();
            }
        } // namespace armarx::RobotUnitModule
 
        //finish processing
        {
            //store data to backlog
            {
                if (rtLoggingBacklogEnabled)
                {
                    durations.backlog.start();
                    ARMARX_TRACE;
                    if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
                    {
                        backlog.emplace_back(data, true); //true for minimal copy
                    }
                    durations.backlog.stop();
                }
            }
            //print + reset messages
            {
                durations.msg.start();
                ARMARX_TRACE;
                for (const ::armarx::detail::RtMessageLogEntryBase* ptr :
                     data.messages.getEntries())
                {
                    if (!ptr)
                    {
                        break;
                    }
                    ptr->print(controlThreadId);
                }
                durations.msg.stop();
            }
        }
    }
 
    bool
    Logging::MatchName(const std::string& pattern, const std::string& name)
    {
        ARMARX_TRACE;
        if (pattern.empty())
        {
            return false;
        }
        static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
        if (!std::regex_match(pattern, pattern_regex))
        {
            throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
        }
        static const std::regex reg_dot{"[.]"};
        static const std::regex reg_star{"[*]"};
        const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
        const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
        const std::regex key_regex{rpl2};
        return std::regex_search(name, key_regex);
    }
 
    void
    Logging::_postOnInitRobotUnit()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
        ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
 
        messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
        messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
        ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
 
        messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
        messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
        ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
 
        rtLoggingBacklogRetentionTime =
            IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
        rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
        rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
        getProperty(numberOfEntriesToLog, "RTLogging_LogLastNMessagesOnly");
        ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
        ARMARX_IMPORTANT << "Initializing RTLogging with the following parameters: "
                         << VAROUT(rtLoggingTimestepMs) << VAROUT(messageBufferSize)
                         << VAROUT(messageBufferMaxSize) << VAROUT(messageBufferNumberEntries)
                         << VAROUT(messageBufferMaxNumberEntries)
                         << VAROUT(rtLoggingBacklogRetentionTime) << VAROUT(rtLoggingBacklogMaxSize)
                         << VAROUT(rtLoggingBacklogEnabled) << VAROUT(numberOfEntriesToLog);
    }
 
    void
    Logging::_postFinishDeviceInitialization()
    {
        ARMARX_TRACE;
        throwIfInControlThread(BOOST_CURRENT_FUNCTION);
        //init buffer
        {
            ARMARX_TRACE;
            std::size_t ctrlThreadPeriodUs =
                static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
            std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
            std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
 
            const auto bufferSize =
                _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
                    nBuffers,
                    _module<Devices>().getControlDevices(),
                    _module<Devices>().getSensorDevices(),
                    messageBufferSize,
                    messageBufferNumberEntries,
                    messageBufferMaxSize,
                    messageBufferMaxNumberEntries);
            ARMARX_INFO << "RTLogging activated. Using " << nBuffers << " buffers "
                        << "(buffersize = " << bufferSize << " bytes)";
        }
        //init logging names + field types
        {
            ARMARX_TRACE;
            const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
            {
                ValueMetaData data;
                const auto names = val->getDataFieldNames();
                data.fields.resize(names.size());
 
                for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
                {
                    std::string const& fieldName = names[fieldIdx];
                    data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
                    data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
                }
                return data;
            };
 
            //sensorDevices
            controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
            for (const auto& cd : _module<Devices>().getControlDevices().values())
            {
                ARMARX_TRACE;
                controlDeviceValueMetaData.emplace_back();
                auto& dataForDev = controlDeviceValueMetaData.back();
                dataForDev.reserve(cd->getJointControllers().size());
                for (auto jointC : cd->getJointControllers())
                {
                    dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
                                                              "ctrl." + cd->getDeviceName() + "." +
                                                                  jointC->getControlMode()));
                }
            }
            //sensorDevices
            sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
            for (const auto& sd : _module<Devices>().getSensorDevices().values())
            {
                ARMARX_TRACE;
                sensorDeviceValueMetaData.emplace_back(
                    makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
            }
        }
        //start logging thread is done in rtinit
        //maybe add the default log
        {
            ARMARX_TRACE;
            const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
            if (!loggingpath.empty())
            {
                defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
            }
        }
    }
 
    void
    Logging::DataStreamingEntry::clearResult()
    {
        ARMARX_TRACE;
        for (auto& e : result)
        {
            entryBuffer.emplace_back(std::move(e));
        }
        result.clear();
    }
 
    RobotUnitDataStreaming::TimeStep
    Logging::DataStreamingEntry::allocateResultElement() const
    {
        ARMARX_TRACE;
        RobotUnitDataStreaming::TimeStep data;
        data.bools.resize(numBools);
        data.bytes.resize(numBytes);
        data.shorts.resize(numShorts);
        data.ints.resize(numInts);
        data.longs.resize(numLongs);
        data.floats.resize(numFloats);
        data.doubles.resize(numDoubles);
        return data;
    }
 
    RobotUnitDataStreaming::TimeStep
    Logging::DataStreamingEntry::getResultElement()
    {
        ARMARX_TRACE;
        if (entryBuffer.empty())
        {
            return allocateResultElement();
        }
        auto e = std::move(entryBuffer.back());
        entryBuffer.pop_back();
        return e;
    }
 
    void
    Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
    {
        ARMARX_TRACE;
        if (stopStreaming)
        {
            return;
        }
 
        auto& data = result.emplace_back(getResultElement());
 
        data.iterationId = e.iteration;
        data.timestampUSec =
            armarx::mapRtTimestampToNonRtTimestamp(e.sensorValuesTimestamp).toMicroSeconds();
        data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
    }
 
    void
    WriteTo(const auto& dentr,
            const Logging::DataStreamingEntry::OutVal& out,
            const auto& val,
            std::size_t fidx,
            auto& data)
    {
        ARMARX_TRACE;
        using enum_t = Logging::DataStreamingEntry::ValueT;
        try
        {
            ARMARX_TRACE;
            switch (out.value)
            {
                case enum_t::Bool:
                    bool b;
                    val.getDataFieldAs(fidx, b);
                    data.bools.at(out.idx) = b;
                    return;
                case enum_t::Byte:
                    val.getDataFieldAs(fidx, data.bytes.at(out.idx));
                    return;
                case enum_t::Short:
                    val.getDataFieldAs(fidx, data.shorts.at(out.idx));
                    return;
                case enum_t::Int:
                    val.getDataFieldAs(fidx, data.ints.at(out.idx));
                    return;
                case enum_t::Long:
                    val.getDataFieldAs(fidx, data.longs.at(out.idx));
                    return;
                case enum_t::Float:
                    val.getDataFieldAs(fidx, data.floats.at(out.idx));
                    return;
                case enum_t::Double:
                    val.getDataFieldAs(fidx, data.doubles.at(out.idx));
                    return;
                case enum_t::Skipped:
                    return;
            }
        }
        catch (...)
        {
            ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
                         << "\n"
                         << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
                         << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
                         << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
                         << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
                         << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
                         << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
                         << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
            throw;
        }
    }
 
    void
    Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
                                             std::size_t didx,
                                             std::size_t vidx,
                                             std::size_t fidx)
    {
        ARMARX_TRACE;
        if (stopStreaming)
        {
            return;
        }
        auto& data = result.back();
        const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
        WriteTo(*this, o, val, fidx, data);
    }
 
    void
    Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
                                             std::size_t didx,
                                             std::size_t fidx)
    {
        ARMARX_TRACE;
        if (stopStreaming)
        {
            return;
        }
        auto& data = result.back();
        const OutVal& o = sensDevs.at(didx).at(fidx);
        WriteTo(*this, o, val, fidx, data);
    }
 
    void
    Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
                                      std::uint64_t msgId)
    {
        ARMARX_TRACE;
        const auto start_send = armarx::rtNow();
        const auto num_timesteps = result.size();
        updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
        const auto start_clear = armarx::rtNow();
        clearResult();
        const auto end = armarx::rtNow();
        ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
                       << "\n    update " << (start_clear - start_send).toMilliSecondsDouble()
                       << "ms (" << num_timesteps << " timesteps)"
                       << "\n    clear  " << (end - start_clear).toMilliSecondsDouble() << "ms";
 
        //now execute all ready callbacks
        std::size_t i = 0;
        for (; i < updateCalls.size(); ++i)
        {
            try
            {
                if (!updateCalls.at(i)->isCompleted())
                {
                    break;
                }
                r->end_update(updateCalls.at(i));
                connectionFailures = 0;
            }
            catch (...)
            {
                ARMARX_TRACE;
                ++connectionFailures;
                if (connectionFailures > rtStreamMaxClientErrors)
                {
                    stopStreaming = true;
                    ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
                                     << connectionFailures << " iterations! Removing receiver";
                    updateCalls.clear();
                    break;
                }
            }
        }
        if (!updateCalls.empty())
        {
            updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
        }
    }
} // namespace armarx::RobotUnitModule