RobotUnitReader.cpp

Go to the documentation of this file.

#include "RobotUnitReader.h"
 
#include <filesystem>
#include <istream>
 
#include "ArmarXCore/core/time/Frequency.h"
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
namespace armarx::armem::server::robot_state::proprioception
{
 
    RobotUnitReader::RobotUnitReader() = default;
 
    void
    RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
                             armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
                             const std::string& robotTypeName)
    {
        {
            converterProprioception = proprioceptionConverterRegistry.get(robotTypeName);
            ARMARX_CHECK_NOT_NULL(converterProprioception)
                << "No converter for robot type '" << robotTypeName << "' available. \n"
                << "Known are: " << proprioceptionConverterRegistry.getKeys();
 
            config.loggingNames.push_back(properties.sensorPrefix);
            receiver = robotUnitPlugin.startDataStreaming(config);
            description = receiver->getDataDescription();
        }
 
        {
            converterExteroception = exteroceptionConverterRegistry.get(robotTypeName);
            ARMARX_CHECK_NOT_NULL(converterProprioception)
                << "No converter for robot type '" << robotTypeName << "' available. \n"
                << "Known are: " << exteroceptionConverterRegistry.getKeys();
        }
 
        {
            // Thread-local copy of debug observer helper.
            debugObserver = DebugObserverHelper(
                Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
        }
    }
 
    void
    RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
    {
        Metronome metronome(Frequency::HertzDouble(pollFrequency));
 
        while (task and not task->isStopped())
        {
            auto start = std::chrono::high_resolution_clock::now();
 
            if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
            {
                debugObserver->setDebugObserverDatafield(
                    "RobotUnitReader | t commitTimestamp [us]",
                    commit->timestamp.toMicroSecondsSinceEpoch());
 
                // will lock a mutex
                dataBuffer.push(std::move(commit.value()));
            }
            auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
                std::chrono::high_resolution_clock::now() - start);
            if (debugObserver)
            {
                debugObserver->setDebugObserverDatafield("RobotUnitReader | t run [ms]",
                                                         duration.count() / 1000.f);
                debugObserver->sendDebugObserverBatch();
            }
 
            metronome.waitForNextTick();
        }
    }
 
    std::optional<RobotUnitData>
    RobotUnitReader::fetchAndConvertLatestRobotUnitData()
    {
        ARMARX_CHECK_NOT_NULL(converterProprioception);
 
        RobotUnitData result;
 
        std::optional<RobotUnitDataStreaming::TimeStep> data;
        {
            auto start = std::chrono::high_resolution_clock::now();
            data = fetchLatestData();
            result.timestampArrived = armarx::DateTime::Now();
 
            auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
                std::chrono::high_resolution_clock::now() - start);
            if (debugObserver)
            {
                debugObserver->setDebugObserverDatafield("RobotUnitReader | t Fetch [ms]",
                                                         duration.count() / 1000.f);
            }
        }
        if (not data.has_value())
        {
            return std::nullopt;
        }
 
        ARMARX_DEBUG << "RobotUnitReader: Converting data current timestep to commit";
        auto start = std::chrono::high_resolution_clock::now();
 
 
        if (converterProprioception != nullptr)
        {
            result.proprioception = converterProprioception->convert(data.value(), description);
        }
 
        if (converterExteroception != nullptr)
        {
            result.exteroception = converterExteroception->convert(data.value(), description);
        }
 
        result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
 
        auto stop = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
        ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: "
                     << duration;
 
        if (debugObserver)
        {
            debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]",
                                                     duration.count() / 1000.f);
        }
 
        return result;
    }
 
    std::optional<RobotUnitDataStreaming::TimeStep>
    RobotUnitReader::fetchLatestData()
    {
        std::deque<RobotUnitDataStreaming::TimeStep>& data = receiver->getDataBuffer();
        if (debugObserver)
        {
            debugObserver->setDebugObserverDatafield("RobotUnitReader | Buffer Size", data.size());
            if (data.size())
            {
                debugObserver->setDebugObserverDatafield("RobotUnitReader | RT Timestamp USec",
                                                         data.back().timestampUSec);
                debugObserver->setDebugObserverDatafield(
                    "RobotUnitReader | RT Timestamp Since Last Iteration",
                    data.back().timesSinceLastIterationUSec);
                debugObserver->setDebugObserverDatafield(
                    "RobotUnitReader | Timestamp Arrived in RSM",
                    armarx::DateTime::Now().toMicroSecondsSinceEpoch());
            }
        }
        if (data.empty())
        {
            return std::nullopt;
        }
        else
        {
            RobotUnitDataStreaming::TimeStep currentTimestep = data.back();
            data.clear();
            return currentTimestep;
        }
    }
 
 
} // namespace armarx::armem::server::robot_state::proprioception