Writer.cpp

Go to the documentation of this file.

#include "Writer.h"
 
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
 
namespace armarx::armem::vision::occupancy_grid::client
{
    Writer::~Writer() = default;
 
    bool
    Writer::store(const OccupancyGrid& grid,
                  const std::string& frame,
                  const std::string& providerName,
                  const std::int64_t& timestamp)
    {
        std::lock_guard g{writeMutex};
 
        const auto result = memoryWriter().addSegment(properties().coreSegmentName, providerName);
 
        if (not result.success)
        {
            ARMARX_ERROR << result.errorMessage;
 
            // TODO(fabian.reister): message
            return false;
        }
 
        const auto iceTimestamp = Time(Duration::MicroSeconds(timestamp));
 
        const auto providerId = armem::MemoryID(result.segmentID);
        const auto entityID = providerId.withEntityName(frame).withTimestamp(iceTimestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        arondto::OccupancyGrid aronGrid;
        // currently only sets the header
        toAron(aronGrid, grid);
 
        auto dict = aronGrid.toAron();
        dict->addElement("grid", toAron(grid.grid));
 
        update.instancesData = {dict};
        update.referencedTime = iceTimestamp;
 
        ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
        armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.success)
        {
            ARMARX_ERROR << updateResult.errorMessage;
        }
 
        return updateResult.success;
    }
 
    std::string
    Writer::propertyPrefix() const
    {
        return "mem.vision.occupancy_grid.";
    }
 
    armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
    Writer::defaultProperties() const
    {
 
        return SimpleWriterBase::Properties{.memoryName = "Vision",
                                            .coreSegmentName = "OccupancyGrid"};
    }
} // namespace armarx::armem::vision::occupancy_grid::client