Writer.cpp

Go to the documentation of this file.

#include "Writer.h"
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
 
#include "utils.h"
 
namespace armarx::armem::articulated_object
{
    void
    Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
    {
        ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
 
        const std::string prefix = propertyPrefix;
 
        def->optional(properties.memoryName, prefix + "MemoryName");
 
        def->optional(properties.coreInstanceSegmentName,
                      prefix + "CoreSegment",
                      "Name of the memory core segment to use for object instances.");
        def->optional(properties.coreClassSegmentName,
                      prefix + "CoreSegment",
                      "Name of the memory core segment to use for object classes.");
 
        ARMARX_IMPORTANT << "Writer: add property '" << prefix << "ProviderName'";
        def->required(
            properties.providerName, prefix + "write.ProviderName", "Name of this provider");
    }
 
    void
    Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
    {
        // Wait for the memory to become available and add it as dependency.
        ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
        try
        {
            memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
            memoryReader = memoryNameSystem.useReader(properties.memoryName);
            ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName << "'";
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
            return;
        }
 
        const auto resultCoreClassSegment =
            memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
 
        const auto resultCoreInstanceSegmentName =
            memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
    }
 
 
    std::optional<armem::MemoryID>
    Writer::storeOrGetClass(const ArticulatedObject& obj) const
    {
        ARMARX_TRACE;
 
        // key: name of object: RobotDescription::name
        const std::unordered_map<std::string, MemoryID> knownObjects = queryDescriptions(Time::Now());
        ARMARX_VERBOSE << "Known articulated objects " << simox::alg::get_keys(knownObjects);
 
        const auto objectId = knownObjects.find(obj.description.name);
 
        // check if exists
        if (objectId != knownObjects.end())
        {
            return objectId->second;
        }
 
        ARMARX_INFO << simox::alg::get_keys(knownObjects);
 
        throw LocalException("articulated object class " + obj.description.name + " not found");
 
        // otherwise create
        if (properties.allowClassCreation)
        {
            return storeClass(obj);
        }
 
        return std::nullopt;
    }
 
    std::optional<armem::MemoryID>
    Writer::storeClass(const ArticulatedObject& obj) const
    {
        std::lock_guard g{memoryWriterMutex};
 
        ARMARX_DEBUG << "Trying to create core segment + provider segment";
 
        // TODO(fabian.reister): variable provider segment
        const auto result =
            memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
 
        if (not result.success)
        {
            ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
            return std::nullopt;
        }
 
        const auto& timestamp = obj.timestamp;
 
        const auto providerId = armem::MemoryID(result.segmentID);
        const auto entityID =
            providerId.withEntityName(obj.description.name).withTimestamp(timestamp);
 
        armem::EntityUpdate update;
        update.entityID = entityID;
 
        arondto::RobotDescription aronArticulatedObjectDescription;
        toAron(aronArticulatedObjectDescription, obj.description);
 
        update.instancesData = {aronArticulatedObjectDescription.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 std::nullopt;
        }
 
        return updateResult.snapshotID;
    }
 
    std::string
    Writer::getProviderName() const
    {
        return properties.providerName;
    }
 
    void
    Writer::setProviderName(const std::string& providerName)
    {
        this->properties.providerName = providerName;
    }
 
    bool
    Writer::storeInstance(const ArticulatedObject& obj, const bool isStatic) const
    {
        std::lock_guard g{memoryWriterMutex};
 
        const auto& timestamp = obj.timestamp;
 
        ARMARX_CHECK(not obj.instance.empty()) << "An object instance name must be provided!";
        const std::string entityName = obj.description.name + "/" + obj.instance;
 
        ARMARX_DEBUG << "Storing articulated object instance '" << entityName << "' (provider '"
                     << properties.providerName << "')";
 
        const auto providerId = armem::MemoryID()
                                    .withMemoryName(properties.memoryName)
                                    .withCoreSegmentName(properties.coreInstanceSegmentName)
                                    .withProviderSegmentName(properties.providerName);
 
        armem::EntityUpdate update;
        update.entityID = providerId.withEntityName(entityName);
        // .withTimestamp(timestamp); // You only need to specify the entity ID, not the snapshot ID
 
        // arondto::Robot aronArticulatedObject;
        // robot::toAron(aronArticulatedObject, obj);
 
        arondto::ObjectInstance objectInstance;
        toAron(objectInstance, obj.config);
 
        const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
 
        if (not classId)
        {
            ARMARX_WARNING << "Could not get class for object " << obj.description.name;
            return false;
        }
 
        // install memory link
        toAron(objectInstance.classID, *classId);
 
        // set object instance id
        const MemoryID memoryInstanceId = classId->withEntityName(entityName);
 
        armem::MemoryID id;
        id.setEntityID(memoryInstanceId.getEntityID());
 
        armarx::ObjectID objectId(id.entityName);
        ARMARX_DEBUG << "Object ID: " << objectId;
 
        ARMARX_CHECK_NOT_EMPTY(objectId.instanceName())
            << "An object instance name must be provided!";
 
        armarx::arondto::ObjectID cs;
        cs.className = objectId.className();
        cs.instanceName = objectId.instanceName();
        cs.dataset = objectId.dataset();
 
        objectInstance.pose.objectID = cs;
        objectInstance.pose.providerName = properties.providerName;
        objectInstance.pose.attachmentValid = false;
 
        objectInstance.pose.isStatic = isStatic;
        objectInstance.pose.confidence = 1.0;
 
        update.instancesData = {objectInstance.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_WARNING << updateResult.errorMessage;
        }
 
        return updateResult.success;
    }
 
    bool
    Writer::store(const ArticulatedObject& obj, const bool isStatic) const
    {
        const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
 
        if (not classId)
        {
            ARMARX_WARNING << "Could not get class id for object " << obj.description.name << "! "
                           << "Known classes are " << simox::alg::get_keys(queryDescriptions(Time::Now()));
            return false;
        }
 
        return storeInstance(obj, isStatic);
    }
 
    // TODO this is a duplicate
    std::optional<robot_state::description::RobotDescription>
    Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
    {
        // clang-format off
        const armem::wm::ProviderSegment& providerSegment = memory
                .getCoreSegment(properties.coreClassSegmentName)
                .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
        // clang-format on
 
        if (const armem::wm::EntityInstance* instance = findFirstInstance(providerSegment))
        {
            return convertRobotDescription(*instance);
        }
        else
        {
            ARMARX_WARNING << "No entity snapshots found";
            return std::nullopt;
        }
    }
 
    std::unordered_map<std::string, armem::MemoryID>
    Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
    {
        const armem::wm::CoreSegment& coreSegment =
            memory.getCoreSegment(properties.coreClassSegmentName);
 
        std::unordered_map<std::string, armem::MemoryID> descriptions;
        coreSegment.forEachEntity(
            [&descriptions](const wm::Entity& entity)
            {
                if (entity.empty())
                {
                    ARMARX_WARNING << "No entity found";
                    return true;
                }
 
                const armem::wm::EntitySnapshot& sn = entity.getFirstSnapshot();
                if (const auto robotDescription = convertRobotDescription(sn.getInstance(0)))
                {
                    const armem::MemoryID snapshotID(sn.id());
                    descriptions.insert({robotDescription->name, snapshotID});
                }
                return true;
            });
 
        return descriptions;
    }
 
    std::unordered_map<std::string, armem::MemoryID>
    Writer::queryDescriptions(const armem::Time& timestamp) const
    {
        // Query all entities from provider.
        armem::client::query::Builder qb;
 
        // clang-format off
        qb
        .coreSegments().withName(properties.coreClassSegmentName)
        .providerSegments().all()
        .entities().all()
        .snapshots().beforeTime(timestamp);
        // clang-format on
 
        const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
 
        ARMARX_DEBUG << "Lookup result in reader: " << qResult;
 
        if (not qResult.success) /* c++20 [[unlikely]] */
        {
            return {};
        }
 
        return getRobotDescriptions(qResult.memory);
    }
 
} // namespace armarx::armem::articulated_object