Writer.cpp

Go to the documentation of this file.

#include "Writer.h"
 
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem_laser_scans/constants.h"
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
 
namespace armarx::armem::laser_scans::client
{
 
    Writer::Writer()
    {
    }
 
    Writer::~Writer() = default;
 
    void
    Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
 
        const std::string prefix = propertyPrefix;
    }
 
    void
    Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
                         << "' ...";
        try
        {
            memoryWriter =
                memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
            ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
                             << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
 
        ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
    }
 
    bool
    Writer::storeSensorData(const LaserScan& laserScan,
                            const std::string& frame,
                            const std::string& agentName,
                            const armem::Time& timestamp)
    {
        std::lock_guard g{memoryWriterMutex};
 
        const auto result = memoryWriter.addSegment(constants::memoryName, agentName);
 
        ARMARX_VERBOSE << "Storing scan with " << laserScan.size() << " elements";
 
        if (not result.success)
        {
            ARMARX_ERROR << result.errorMessage;
 
            // TODO(fabian.reister): message
            return false;
        }
 
 
        const auto providerId = armem::MemoryID(result.segmentID);
        const auto entityID = providerId.withEntityName(frame).withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        arondto::LaserScanStamped aronSensorData;
        // currently only sets the header
        toAron(laserScan, timestamp, frame, agentName, aronSensorData);
 
        auto dict = aronSensorData.toAron();
        dict->addElementCopy("scan", toAron(laserScan));
 
        update.instancesData = {dict};
        update.referencedTime = timestamp;
 
        ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
        ARMARX_DEBUG << updateResult;
 
        if (not updateResult.success)
        {
            ARMARX_ERROR << updateResult.errorMessage;
        }
 
        return updateResult.success;
    }
 
} // namespace armarx::armem::laser_scans::client