ReadStream.cpp

Go to the documentation of this file.

#include "ReadStream.h"
 
#include <ArmarXCore/core/time/Clock.h>
 
#include <RobotAPI/libraries/armem/client/query/Builder.h>
 
namespace armarx::armem::client
{
    ReadStream::ReadStream() : metronome{armarx::core::time::Frequency::Hertz(10)}
    {
    }
 
    ReadStream::ReadStream(const Reader& reader,
                           const MemoryID& queriedId,
                           const core::time::Frequency& maxPollFrequency) :
        reader{reader}, queriedId{queriedId}, metronome{maxPollFrequency}
    {
        timeStart = armarx::core::time::Clock::Now();
    }
 
    std::optional<wm::EntitySnapshot>
    ReadStream::pollBlocking(const SnapshotCallbackT& callback)
    {
        if (isPolling.exchange(true))
        {
            throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
        }
        pollingStoppedExternally = false;
 
        auto result = _pollBlocking(callback);
 
        isPolling = false;
        return result;
    }
 
    void
    ReadStream::pollAsync(const SnapshotCallbackT& callback)
    {
        if (isPolling.exchange(true))
        {
            throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
        }
        pollingStoppedExternally = false;
 
        this->pollingThread = std::thread([&]() { this->_pollBlocking(callback); });
    }
 
    std::optional<wm::EntitySnapshot>
    ReadStream::_pollBlocking(const SnapshotCallbackT& callback)
    {
        while (not pollingStoppedExternally)
        {
            auto snapshot = _pollOnce(callback);
            if (snapshot.has_value())
            {
                return snapshot;
            }
        }
        return std::nullopt;
    }
 
    void
    ReadStream::stop()
    {
        pollingStoppedExternally = true;
        if (pollingThread.joinable())
        {
            pollingThread.join();
            isPolling = false;
        }
    }
 
    std::optional<wm::EntitySnapshot>
    ReadStream::pollOnce(const SnapshotCallbackT& callback)
    {
        if (isPolling.exchange(true))
        {
            throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
        }
 
        auto snapshot = _pollOnce(callback);
 
        isPolling = false;
        return snapshot;
    }
 
    std::optional<wm::EntitySnapshot>
    ReadStream::_pollOnce(const SnapshotCallbackT& callback)
    {
        // Make sure to not busy wait. Also wait until probably data is available in first iteration.
        metronome.waitForNextTick();
 
        auto timeEnd = armarx::core::time::Clock::Now();
        auto makeQuery = [this, &timeEnd](const MemoryID& id)
        {
            query::Builder qb;
            query::CoreSegmentSelector& core =
                id.hasCoreSegmentName() ? qb.coreSegments().withID(id) : qb.coreSegments().all();
            query::ProviderSegmentSelector& prov = id.hasProviderSegmentName()
                                                       ? core.providerSegments().withID(id)
                                                       : core.providerSegments().all();
            query::EntitySelector& entity =
                id.hasEntityName() ? prov.entities().withID(id) : prov.entities().all();
            entity.snapshots().timeRange(timeStart, timeEnd);
 
            return qb.buildQueryInput();
        };
 
        auto query = makeQuery(queriedId);
 
        auto result = reader.query(query);
 
        if (result.success)
        {
            using EntitySnapshotReference =
                std::reference_wrapper<armarx::armem::wm::EntitySnapshot>;
            // Copy references of snapshots into vector to sort them.
            std::vector<EntitySnapshotReference> snapshots;
 
            result.memory.forEachSnapshot([&snapshots](armarx::armem::wm::EntitySnapshot& snapshot)
                                          { snapshots.push_back(snapshot); });
 
            // Sort correctly.
            std::sort(snapshots.begin(),
                      snapshots.end(),
                      [](const EntitySnapshotReference& a, const EntitySnapshotReference& b)
                      { return a.get().id().timestamp < b.get().id().timestamp; });
 
            // Determine the next start time.
            DateTime nextStart;
            if (snapshots.size() > 0)
            {
                // Because they are sorted, back() has the highest time stamp.
                nextStart = snapshots.back().get().id().timestamp +
                            armarx::core::time::Duration::MicroSeconds(1);
            }
            else
            {
                nextStart = timeStart;
            }
 
            // Call the callback on all snapshots.
            for (const auto& snapshot : snapshots)
            {
                // Assert times in correct interval.
                ARMARX_CHECK_LESS_EQUAL(timeStart, snapshot.get().id().timestamp);
                ARMARX_CHECK_GREATER_EQUAL(timeEnd, snapshot.get().id().timestamp);
 
                const bool continue_ = callback(snapshot.get());
                if (not continue_)
                {
                    return snapshot;
                }
            }
 
            timeStart = nextStart;
        }
        else
        {
            ARMARX_WARNING
                << deactivateSpam()
                << "Received an error in ReadStream when querying data from a "
                   "memory. The error was '"
                << result.errorMessage
                << "'. Continue with stream, perhaps the memory was not yet initialized.";
        }
 
        return std::nullopt;
    }
 
 
} // namespace armarx::armem::client