Writer.cpp

Go to the documentation of this file.

#include "Writer.h"
 
#include <iterator>
 
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/human/aron_conversions.h>
 
 
namespace armarx::navigation::memory::client::human
{
    Writer::~Writer() = default;
 
    bool
    Writer::store(const armarx::navigation::human::Humans& humans,
                  //   const std::string& name,
                  const std::string& providerName,
                  const armem::Time& timestamp)
    {
        std::lock_guard g{writeMutex};
 
        const auto result =
            memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
 
        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("humans").withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        std::transform(humans.begin(),
                       humans.end(),
                       std::back_inserter(update.instancesData),
                       [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr {
                            navigation::human::arondto::Human dto;
                            toAron(dto, human);
 
                            return dto.toAron();
                       });
 
 
        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;
    }
 
    bool
    Writer::store(const armarx::navigation::human::HumanGroups& groups,
                  //   const std::string& name,
                  const std::string& providerName,
                  const armem::Time& timestamp)
    {
        std::lock_guard g{writeMutex};
 
        const auto result =
            memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
 
        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("groups").withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        std::transform(groups.begin(),
                       groups.end(),
                       std::back_inserter(update.instancesData),
                       [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr {
                            navigation::human::arondto::HumanGroup dto;
                            toAron(dto, group);
 
                            return dto.toAron();
                       });
 
 
        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;
    }
 
    std::string
    Writer::propertyPrefix() const
    {
        return "mem.nav.human.";
    }
 
    armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
    Writer::defaultProperties() const
    {
        return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
                                            .coreSegmentName =
                                                memory::constants::HumanCoreSegmentName};
    }
 
} // namespace armarx::navigation::memory::client::human