armem/dev => master

source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp

+#include "Writer.h"
+
+#include <IceUtil/Time.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <mutex>
+#include <optional>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
+
+
+namespace armarx::armem::articulated_object
+{
+    Writer::Writer(armem::ClientComponentPluginUser& component): component(component) {}
+
+    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.");
+        def->required(properties.providerName, prefix + "ProviderName", "Name of this provider");
+    }
+
+    void Writer::connect()
+    {
+        // Wait for the memory to become available and add it as dependency.
+        ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
+        auto result = component.useMemory(properties.memoryName);
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+            return;
+        }
+
+        ARMARX_IMPORTANT << "Writer: Connected to memory '" << properties.memoryName;
+
+        memoryWriter.setWritingMemory(result.proxy);
+        memoryReader.setReadingMemory(result.proxy);
+
+        const auto resultCoreClassSegment = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+
+        const auto resultCoreInstanceSegmentName =
+            memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
+
+        armem::MemoryID refId = armem::MemoryID(resultCoreClassSegment.segmentID);
+
+        armem::MemoryID id;
+        id.setCoreSegmentID(refId); // listen to all provider segments!
+
+        updateKnownObjects();
+        memoryReader.subscribe(id, this, &Writer::updateKnownObjects);
+    }
+
+    void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
+    {
+
+        arondto::RobotDescription aronArticulatedObjectDescription;
+        // aronArticulatedObjectDescription.fromAron(snapshotId.ent);
+
+        // TODO(fabian.reister): implement
+    }
+
+    void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+    {
+        ARMARX_INFO << "New objects available!";
+        updateKnownObjects();
+    }
+
+    void Writer::updateKnownObjects()
+    {
+        knownObjects = queryDescriptions(IceUtil::Time::now());
+    }
+
+    std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
+    {
+        const auto objectId = knownObjects.find(obj.description.name);
+
+        // check if exists
+        if (objectId != knownObjects.end())
+        {
+            return objectId->second;
+        }
+
+        // otherwise create
+        if (properties.allowClassCreation)
+        {
+            return storeClass(obj);
+        }
+
+        return std::nullopt;
+    }
+
+    std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
+    {
+        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.timeCreated   = 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;
+        }
+
+        // update cache (TODO: likely remove this)
+        knownObjects[obj.description.name] = updateResult.snapshotID;
+
+        return updateResult.snapshotID;
+    }
+
+    bool Writer::storeInstance(const ArticulatedObject& obj)
+    {
+        std::lock_guard g{memoryWriterMutex};
+
+        ARMARX_DEBUG << "Trying to create core segment + provider segment";
+
+        const auto result =
+            memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
+
+        if (not result.success)
+        {
+            ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
+            return false;
+        }
+
+        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::Robot aronArticulatedObject;
+        robot::toAron(aronArticulatedObject, obj);
+
+        const auto descriptionId = storeOrGetClass(obj);
+
+        if (not descriptionId)
+        {
+            ARMARX_WARNING << "Could not get class for object " << obj.description.name;
+            return false;
+        }
+
+        // install memory link
+        toAron(aronArticulatedObject.description, *descriptionId);
+
+        update.instancesData = {aronArticulatedObject.toAron()};
+        update.timeCreated   = 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 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(knownObjects);
+            return false;
+        }
+
+        // TODO(fabian.reister): integrate memory link
+        return storeInstance(obj);
+    }
+
+    // const std::string& Writer::getPropertyPrefix() const
+    // {
+    //     return propertyPrefix;
+    // }
+
+
+    // TODO this is a duplicate
+    std::optional<robot::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
+        const auto entities = simox::alg::get_values(providerSegment.entities());
+
+        if (entities.empty())
+        {
+            ARMARX_WARNING << "No entity found";
+            return std::nullopt;
+        }
+
+        const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+        if (entitySnapshots.empty())
+        {
+            ARMARX_WARNING << "No entity snapshots found";
+            return std::nullopt;
+        }
+
+        // TODO(fabian.reister): check if 0 available
+        const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+        return robot::convertRobotDescription(instance);
+    }
+
+    std::unordered_map<std::string, armem::MemoryID>Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+    {
+        std::unordered_map<std::string, armem::MemoryID> descriptions;
+
+        const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+
+        for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+        {
+            for (const auto& [name, entity] : providerSegment.entities())
+            {
+                if (entity.empty())
+                {
+                    ARMARX_WARNING << "No entity found";
+                    continue;
+                }
+
+                const auto entitySnapshots = simox::alg::get_values(entity.history());
+                const armem::wm::EntitySnapshot& sn = entitySnapshots.front();
+                const armem::wm::EntityInstance& instance = sn.getInstance(0);
+
+                const auto robotDescription = robot::convertRobotDescription(instance);
+
+                if (robotDescription)
+                {
+                    const armem::MemoryID snapshotID(sn.id());
+                    descriptions.insert({robotDescription->name, snapshotID});
+                }
+            }
+
+        }
+
+        return descriptions;
+    }
+
+
+    std::unordered_map<std::string, armem::MemoryID> Writer::queryDescriptions(const armem::Time& timestamp)
+    {
+        // Query all entities from provider.
+        armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties.coreClassSegmentName)
+        .providerSegments().all()
+        .entities().all()
+        .snapshots().latest(); // TODO 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
\ No newline at end of file