ArMemToImage.cpp

Go to the documentation of this file.

#include "ArMemToImage.h"
 
#include <sstream>
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
 
#include <Image/ByteImage.h>
 
namespace visionx::armem_images
{
 
    ArMemToImage::ArMemToImage() : detail::ImageAdapter("ArMemToImage")
    {
    }
 
    template <class AronImageT>
    void
    ArMemToImage::addImages(const armarx::armem::MemoryID& entityID,
                            const std::vector<size_t>& imageIndices)
    {
        for (size_t i = 0; i < imageIndices.size(); ++i)
        {
            armarx::armem::MemoryID instanceID = entityID;
            instanceID.instanceIndex = static_cast<int>(i);
 
            size_t index = imageIndices[i];
            if (index >= images.size())
            {
                images.resize(index + 1);
            }
            images[index].reset(new Image<AronImageT>(instanceID, index));
        }
    }
 
    void
    ArMemToImage::addImagesRGB(const armarx::armem::MemoryID& entityID,
                               const std::vector<size_t>& imageIndices)
    {
        addImages<arondto::ImageRGB>(entityID, imageIndices);
    }
 
    void
    ArMemToImage::addImagesDepth(const armarx::armem::MemoryID& entityID,
                                 const std::vector<size_t>& imageIndices)
    {
        addImages<arondto::ImageDepth>(entityID, imageIndices);
    }
 
    armarx::armem::MemoryID
    ArMemToImage::getMemoryID() const
    {
        armarx::armem::MemoryID id = images.front()->instanceID.getMemoryID();
        for (size_t i = 0; i < images.size(); ++i)
        {
            ARMARX_CHECK_EQUAL(images[i]->instanceID.memoryName, id.memoryName)
                << "Currently, all images must be from the same memory.";
        }
        return id;
    }
 
    Eigen::Vector2i
    ArMemToImage::getImageDimensions() const
    {
        Eigen::Vector2i dims = dims.Zero();
        for (const auto& image : images)
        {
            const cv::Mat& img = image->getImage();
            dims(0) = std::max(dims(0), img.cols);
            dims(1) = std::max(dims(1), img.rows);
        }
        return dims;
    }
 
    void
    ArMemToImage::fetchUpdates(const armarx::armem::MemoryID& entityID,
                               const std::vector<armarx::armem::MemoryID>& updatedSnapshotIDs)
    {
        (void)entityID;
        auto it = std::find_if(updatedSnapshotIDs.begin(),
                               updatedSnapshotIDs.end(),
                               [this](const armarx::armem::MemoryID& id)
                               { return id.timestamp > timestamp; });
        if (it != updatedSnapshotIDs.end())
        {
            TIMING_START(Fetch);
            fetchLatest();
            TIMING_END_STREAM(Fetch, ARMARX_VERBOSE);
 
            if (debugObserver)
            {
                debugObserver->setDebugChannel(
                    debugObserverChannelName,
                    {
                        {"Fetch [us]", new armarx::Variant(Fetch.toMicroSecondsDouble())},
                        {"Timestamp [ms]",
                         new armarx::Variant(
                             timestamp.toDurationSinceEpoch().toMilliSecondsDouble())},
                    });
            }
        }
    }
 
    std::string
    ArMemToImage::summarizeStructure() const
    {
        std::stringstream ss;
        for (size_t i = 0; i < images.size(); ++i)
        {
            ss << "- [" << images[i]->imageIndex << "] id=" << images[i]->instanceID << "\n";
        }
        return ss.str();
    }
 
    armarx::armem::client::QueryResult
    ArMemToImage::fetchLatest()
    {
        if (!memoryReader)
        {
            armarx::armem::ClientQueryResult r;
            r.success = false;
            r.errorMessage = "No memory reader.";
            return r;
        }
 
        armarx::armem::ClientQueryBuilder qb;
        for (const auto& image : images)
        {
            image->addQuery(qb);
        }
 
        armarx::armem::ClientQueryResult result = memoryReader.query(qb.buildQueryInput());
        if (result.success)
        {
            updateAronImages(result.memory);
        }
        return result;
    }
 
    void
    ArMemToImage::updateAronImages(const armarx::armem::wm::Memory& memory)
    {
        for (const auto& image : images)
        {
            updated |= image->updateAronImage(memory);
            timestamp = std::max(timestamp, image->instanceID.timestamp);
        }
    }
 
    std::string
    ArMemToImage::printFormats() const
    {
        std::stringstream ss;
        for (const auto& image : images)
        {
            ss << "- " << image->printFormat() << "\n";
        }
        return ss.str();
    }
 
} // namespace visionx::armem_images