Draft: Make RobotStateMemory ready

Goal:

• Make stable (especially race conditions)
• Improve runtime
• Add profiling/monitoring
• Refactor code
• Add missing interfaces

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp

@@ -63,22 +63,21 @@ namespace armarx::armem::client::robot_state::localization
     {
         ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
 
-        def->optional(properties.localizationSegment,
-                      propertyPrefix + "localizationSegment",
+        def->optional(properties.coreSegmentID.memoryName, propertyPrefix + "MemoryName");
+        def->optional(properties.coreSegmentID.coreSegmentName,
+                      propertyPrefix + "LocalizationSegmentName",
                       "Name of the localization memory core segment to use.");
-
-        def->optional(properties.memoryName, propertyPrefix + "Memory");
     }
 
     void TransformWriter::connect()
     {
         // Wait for the memory to become available and add it as dependency.
-        ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName
+        ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.coreSegmentID.memoryName
                          << "' ...";
         try
         {
-            memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
-            ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName << "'";
+            memoryWriter = memoryNameSystem.useWriter(properties.coreSegmentID);
+            ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '" << properties.coreSegmentID << "'";
         }
         catch (const armem::error::CouldNotResolveMemoryServer& e)
         {
@@ -91,50 +90,19 @@ namespace armarx::armem::client::robot_state::localization
     {
         std::lock_guard g{memoryWriterMutex};
 
-        ARMARX_DEBUG << "Trying to create core segment + provider segment";
-
-        const auto providerId = [&]() -> std::optional<armem::MemoryID>
-        {
-            try
-            {
-                const auto result =
-                memoryWriter.addSegment(properties.localizationSegment, transform.header.agent);
-
-                if (not result.success)
-                {
-                    ARMARX_WARNING << "Could not obtain provider id! Reason: " <<  result.errorMessage;
-                    return std::nullopt;
-                }
-
-                return armem::MemoryID(result.segmentID);
-            }
-            catch (...)
-            {
-                ARMARX_WARNING << "Could not obtain provider id!";
-                return std::nullopt;
-            }
-        }();
-
-        if (not providerId)
-        {
-            return false;
-        }
-
+        const MemoryID providerId = properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
         // const auto& timestamp = transform.header.timestamp;
-        const auto timestamp = IceUtil::Time::now(); // FIXME remove
-
-
-        const auto entityID   = providerId->withEntityName(transform.header.parentFrame + "," +
-                                transform.header.frame).withTimestamp(timestamp);
+        const MemoryID entityID = providerId.withEntityName(
+                                      transform.header.parentFrame + "," + transform.header.frame);
+        const Time timestamp = Time::now(); // FIXME remove
 
         armem::EntityUpdate update;
-        update.entityID    = entityID;
+        update.entityID = entityID;
+        update.timeCreated = timestamp;
 
         arondto::Transform aronTransform;
         toAron(aronTransform, transform);
-
         update.instancesData = {aronTransform.toAron()};
-        update.timeCreated = timestamp;
 
         ARMARX_DEBUG << "Committing " << update << " at time " << transform.header.timestamp;
         armem::EntityUpdateResult updateResult = memoryWriter.commit(update);