Writer.cpp

Go to the documentation of this file.

#include "Writer.h"
 
#include <mutex>
#include <string>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
 
#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
#include <armarx/navigation/memory/aron_conversions.h>
#include <armarx/navigation/memory/constants.h>
 
namespace armarx::navigation::memory::client::laser_scanner_features
{
 
    Writer::~Writer() = default;
 
    std::string
    Writer::propertyPrefix() const
    {
        return "mem.nav.laser_scanner_features.";
    }
 
    armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
    Writer::defaultProperties() const
    {
        return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
                                            .coreSegmentName =
                                                memory::constants::LaserScannerFeaturesCoreSegment};
    }
 
    bool
    Writer::store(const LaserScannerFeatures& features,
                  const std::string& providerName,
                  const armem::Time& timestamp)
    {
        std::lock_guard g{writeMutex};
 
        // const auto result = memoryWriter.addSegment(constants::memoryName,
        //                                             constants::LaserScannerFeaturesCoreSegment);
 
        // if (not result.success)
        // {
        //     ARMARX_ERROR << result.errorMessage;
 
        //     // TODO(fabian.reister): message
        //     return false;
        // }
 
        const auto entityID = armem::MemoryID()
                                  .withMemoryName(properties().memoryName)
                                  .withCoreSegmentName(properties().coreSegmentName)
                                  .withProviderSegmentName(providerName)
                                  .withEntityName(features.frame)
                                  .withTimestamp(timestamp);
 
        ARMARX_VERBOSE << "Memory id is " << entityID.str();
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        ARMARX_TRACE;
 
        arondto::LaserScannerFeatures dto;
        toAron(dto, features);
 
        ARMARX_TRACE;
 
        update.instancesData = {dto.toAron()};
        update.referencedTime = timestamp;
 
        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
 
        ARMARX_TRACE;
        armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.success)
        {
            ARMARX_ERROR << updateResult.errorMessage;
        }
 
        return updateResult.success;
    }
 
} // namespace armarx::navigation::memory::client::laser_scanner_features