armem: laser scanner features writer

source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp

@@ -24,24 +24,24 @@ namespace armarx::armem::vision::laser_scanner_features::client
 
         const std::string prefix = propertyPrefix;
 
-        def->optional(properties.coreSegmentName,
-                      prefix + "CoreSegment",
-                      "Name of the mapping memory core segment to use.");
+        // def->optional(properties.coreSegmentName,
+        //               prefix + "CoreSegment",
+        //               "Name of the mapping memory core segment to use.");
 
-        def->optional(properties.memoryName, prefix + "MemoryName");
+        // def->optional(properties.memoryName, prefix + "MemoryName");
     }
 
     void
     Writer::connect()
     {
         // Wait for the memory to become available and add it as dependency.
-        ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << properties.memoryName
+        ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
                          << "' ...";
         try
         {
             memoryWriter =
-                memoryNameSystem.useWriter(MemoryID().withMemoryName(properties.memoryName));
-            ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << properties.memoryName
+                memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
+            ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
                              << "'";
         }
         catch (const armem::error::CouldNotResolveMemoryServer& e)
@@ -50,7 +50,7 @@ namespace armarx::armem::vision::laser_scanner_features::client
             return;
         }
 
-        ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << properties.memoryName;
+        ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
     }
 
     bool
@@ -60,30 +60,42 @@ namespace armarx::armem::vision::laser_scanner_features::client
     {
         std::lock_guard g{memoryWriterMutex};
 
-        const auto result = memoryWriter.addSegment(properties.memoryName,
-                                                    constants::laserScannerFeaturesCoreSegment);
+        // const auto result = memoryWriter.addSegment(constants::memoryName,
+        //                                             constants::laserScannerFeaturesCoreSegment);
 
-        if (not result.success)
-        {
-            ARMARX_ERROR << result.errorMessage;
+        // if (not result.success)
+        // {
+        //     ARMARX_ERROR << result.errorMessage;
 
-            // TODO(fabian.reister): message
-            return false;
-        }
+        //     // TODO(fabian.reister): message
+        //     return false;
+        // }
+
+        const auto entityID = armem::MemoryID()
+                                    .withMemoryName(constants::memoryName)
+                                    .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
+                                    .withProviderSegmentName(providerName)
+                                    .withEntityName(features.frame)
+                                    .withTimestamp(timestamp);
 
-        const auto providerId = armem::MemoryID(result.segmentID).withProviderSegmentName(providerName);
-        const auto entityID = providerId.withEntityName(features.frame).withTimestamp(timestamp);
+        ARMARX_INFO << "Memory id is " << entityID.str();
 
         armem::EntityUpdate update;
         update.entityID = entityID;
 
+        ARMARX_TRACE;
+
         arondto::LaserScannerFeatures dto;
         toAron(dto, features);
 
+        ARMARX_TRACE;
+
         update.instancesData = {dto.toAron()};
         update.timeCreated = timestamp;
 
         ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+
+        ARMARX_TRACE;
         armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
         ARMARX_DEBUG << updateResult;

source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h

@@ -73,8 +73,8 @@ namespace armarx::armem::vision::laser_scanner_features::client
         // Properties
         struct Properties
         {
-            std::string memoryName = "Vision";
-            std::string coreSegmentName = "LaserScannerFeatures";
+            // std::string memoryName = "Vision";
+            // std::string coreSegmentName = "LaserScannerFeatures";
         } properties;
 
         std::mutex memoryWriterMutex;

source/RobotAPI/libraries/armem_vision/constants.h

@@ -25,6 +25,9 @@
 
 namespace armarx::armem::vision::constants
 {
+    const inline std::string memoryName = "Vision";
+
+    // core segments
     const inline std::string laserScannerFeaturesCoreSegment = "LaserScannerFeatures";
 
 } // namespace armarx::armem::vision::constants

source/RobotAPI/libraries/armem_vision/types.h

@@ -61,15 +61,15 @@ namespace armarx::armem::vision
 
     struct Ellipsoid
     {
-        Eigen::Isometry3f pose;
+        Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
 
-        Eigen::Vector2f radii;
+        Eigen::Vector2f radii = Eigen::Vector2f::Zero();
     };
 
     struct Circle
     {
-        Eigen::Vector2f center;
-        float radius;
+        Eigen::Vector2f center = Eigen::Vector2f::Zero();
+        float radius = 0.F;
     };
 
     struct LaserScannerFeature