Added Writer and empty Reader class for GraspCandidate

source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt

@@ -6,17 +6,22 @@ armarx_set_target("Library: ${LIB_NAME}")
 armarx_add_library(
     LIBS     RobotAPICore
              RobotStatechartHelpers # Contains RobotNameHelper
+             RobotAPI::armem
     SOURCES  box_to_grasp_candidates.cpp
              grasp_candidate_drawer.cpp
              GraspCandidateHelper.cpp
              BimanualGraspCandidateHelper.cpp
              aron_conversions.cpp
+             GraspCandidateWriter.cpp
+             GraspCandidateReader.cpp
     HEADERS  box_to_grasp_candidates.h
              box_to_grasp_candidates.ipp
              grasp_candidate_drawer.h
              GraspCandidateHelper.h
              BimanualGraspCandidateHelper.h
              aron_conversions.h
+             GraspCandidateWriter.h
+             GraspCandidateReader.h
 )
 armarx_enable_aron_file_generation_for_target(
     TARGET_NAME

source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp

+#include "GraspCandidateReader.h"
+
+
+
+namespace armarx::armem
+{
+    GraspCandidateReader::GraspCandidateReader()
+    {
+
+    }
+}

source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h

+#pragma once
+
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
+namespace armarx::armem
+{
+    class GraspCandidateReader
+    {
+    public:
+        GraspCandidateReader();
+
+
+    private:
+        std::string memoryName = "GraspCandidate";
+
+    };
+}

source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp

+#include "GraspCandidateWriter.h"
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
+
+namespace armarx::armem
+{
+
+
+    GraspCandidateWriter::GraspCandidateWriter(armem::ClientWriterComponentPluginUser& component)
+        : component(component)
+    {
+    }
+
+    void GraspCandidateWriter::connect()
+    {
+        // Wait for the memory to become available and add it as dependency.
+        ARMARX_IMPORTANT << "GraspCandidateWriter: Waiting for memory '" << properties.memoryName
+                         << "' ...";
+        auto result = component.useMemory(properties.memoryName);
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+            return;
+        }
+
+        ARMARX_IMPORTANT << "GraspCandidateWriter: Connected to memory '" << properties.memoryName;
+
+        memoryWriter.setWritingMemory(result.proxy);
+
+    }
+
+    void GraspCandidateWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_DEBUG << "GraspCandidateWriter: registerPropertyDefinitions";
+
+        const std::string prefix = propertyPrefix;
+
+        def->optional(properties.graspMemoryName, prefix + "GraspMemoryName",
+                      "Name of the mapping memory core segment to use.");
+
+        def->optional(properties.memoryName, prefix + "MemoryName");
+
+    }
+
+    bool GraspCandidateWriter::storeGraspCandidate(const armarx::grasping::GraspCandidate& candidate, const int64_t& timestamp)
+    {
+        armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
+
+        toAron(aronGraspCandidate, candidate);
+
+        auto dict = aronGraspCandidate.toAron();
+
+        return commitToMemory(dict, candidate.providerName, timestamp);
+    }
+
+    bool GraspCandidateWriter::storeBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate, const int64_t& timestamp)
+    {
+        armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
+
+        toAron(aronGraspCandidate, candidate);
+
+        auto dict = aronGraspCandidate.toAron();
+
+        return commitToMemory(dict, candidate.providerName, timestamp);
+    }
+
+    bool GraspCandidateWriter::storeGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates, const int64_t& timestamp)
+    {
+        bool success = true;
+        for (armarx::grasping::GraspCandidatePtr candidate : candidates)
+        {
+            if (! storeGraspCandidate(*candidate, timestamp))
+            {
+                success = false;
+            }
+        }
+        return success;
+    }
+
+    bool GraspCandidateWriter::storeBimanualGraspCandidateSeq(const armarx::grasping::BimanualGraspCandidateSeq& candidates, const int64_t& timestamp)
+    {
+        bool success = true;
+        for (armarx::grasping::BimanualGraspCandidatePtr candidate : candidates)
+        {
+            if (! storeBimanualGraspCandidate(*candidate, timestamp))
+            {
+                success = false;
+            }
+        }
+        return success;
+    }
+
+    bool GraspCandidateWriter::commitToMemory(const armarx::aron::datanavigator::DictNavigatorPtr& dict, const std::string& providerName, const int64_t& timestamp)
+    {
+        std::lock_guard g{memoryWriterMutex};
+
+        const auto result =
+            memoryWriter.addSegment(properties.graspMemoryName, providerName);
+
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+
+            // TODO(fabian.reister): message
+            return false;
+        }
+
+        const auto iceTimestamp = Time::microSeconds(timestamp);
+
+        const auto providerId = armem::MemoryID(result.segmentID);
+        const auto entityID =
+            providerId.withEntityName(providerName).withTimestamp(iceTimestamp);
+
+        armem::EntityUpdate update;
+        update.entityID = entityID;
+
+
+
+        update.instancesData = {dict};
+        update.timeCreated = iceTimestamp;
+
+        ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+        ARMARX_DEBUG << updateResult;
+
+        if (not updateResult.success)
+        {
+            ARMARX_ERROR << updateResult.errorMessage;
+        }
+
+        return updateResult.success;
+    }
+}

source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h

+#pragma once
+
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client.h>
+#include <mutex>
+
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
+
+namespace armarx::armem
+{
+    class GraspCandidateWriter
+    {
+    public:
+        GraspCandidateWriter(armem::ClientWriterComponentPluginUser& component);
+
+        void connect();
+
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+
+        bool storeGraspCandidate(const armarx::grasping::GraspCandidate& candidate, const std::int64_t& timestamp);
+        bool storeBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate, const std::int64_t& timestamp);
+
+        bool storeGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates, const std::int64_t& timestamp);
+        bool storeBimanualGraspCandidateSeq(const armarx::grasping::BimanualGraspCandidateSeq& candidates, const std::int64_t& timestamp);
+
+
+
+    private:
+
+        bool commitToMemory(const armarx::aron::datanavigator::DictNavigatorPtr& dict, const std::string& providerName, const std::int64_t& timestamp);
+
+        armem::client::Writer memoryWriter;
+
+        struct Properties
+        {
+            std::string memoryName         = "Grasp";
+            std::string graspMemoryName  = "GraspCandidate";
+        } properties;
+
+
+        std::mutex memoryWriterMutex;
+
+        const std::string propertyPrefix = "mem.grasping.";
+
+        armem::ClientWriterComponentPluginUser& component;
+
+    };
+
+
+}

source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp

@@ -136,7 +136,10 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, co
     else
     {
         dto.sourceInfoValid = false;
-        toAron(dto.sourceInfo, GraspCandidateSourceInfo());
+        /*GraspCandidateSourceInfo info = GraspCandidateSourceInfo();
+        info.bbox = BoundingBoxPtr();
+        toAron(dto.sourceInfo, info);*/
+        dto.sourceInfo.initialize();
     }
     dto.targetFrame = bo.targetFrame;
 }