Implement Locations MemoryX -> ArMem

source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp

@@ -22,7 +22,10 @@
 
 #include "GraphImportExport.h"
 
-#include <armarx/navigation/locations/aron/Location.aron.generated.h>
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
+#include <armarx/navigation/location/constants.h>
+#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
+#include <armarx/navigation/graph/constants.h>
 
 #include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
@@ -37,6 +40,13 @@
 namespace armarx::nav
 {
 
+    GraphImportExport::GraphImportExport()
+    {
+        properties.memoryName = "Navigation";
+        properties.locationCoreSegmentName = loc::coreSegmentName;
+        properties.graphCoreSegmentName = graph::coreSegmentName;
+    }
+
     armarx::PropertyDefinitionsPtr GraphImportExport::createPropertyDefinitions()
     {
         armarx::PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
@@ -159,9 +169,14 @@ namespace armarx::nav
         {
             exportLocations(tab.sceneComboBox.getValue());
         }
+
         if (tab.locationsMemoryXToArMemButton.wasClicked())
         {
-            proxies.navigationWriter.addSegment();
+            clearArMemProviderSegment(getLocationProviderSegmentID());
+        }
+        if (tab.graphMemoryXToArMemButton.wasClicked())
+        {
+            clearArMemProviderSegment(getGraphProviderSegmentID());
         }
     }
 
@@ -173,13 +188,18 @@ namespace armarx::nav
     }
 
 
-    void GraphImportExport::clearProviderSegment()
+    void GraphImportExport::clearArMemProviderSegment(const armem::MemoryID& providerSegmentID)
     {
-        armem::data::AddSegmentInput input;
-        input.coreSegmentName = "Locations";
-        input.providerSegmentName = tab.providerSegmentLine.getValue();
-        input.clearWhenExists = true;
-        proxies.navigationWriter.addSegment(input);
+        const bool clearWhenExists = true;
+        auto result = proxies.navigationWriter.addSegment(providerSegmentID, clearWhenExists);
+        if (result.success)
+        {
+            ARMARX_IMPORTANT << "Cleared ArMem provider segment " << providerSegmentID << ".";
+        }
+        else
+        {
+            ARMARX_WARNING << result.errorMessage;
+        }
     }
 
 
@@ -192,33 +212,36 @@ namespace armarx::nav
         for (memoryx::GraphNodeBasePtr& node : graphNodes)
         {
             ARMARX_CHECK_NOT_NULL(node);
-
-            armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose());
-            if (pose and not node->isMetaEntity())
+            if (not node->isMetaEntity())
             {
                 // ID is just some random MongoDB hash
                 const std::string nodeId = node->getId();
+                // This is the readable name entered in the GUI.
                 const std::string name = node->getName();
 
+                armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose());
+                ARMARX_CHECK_NOT_NULL(pose);
+
                 FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node));
                 ARMARX_CHECK_NOT_NULL(globalNodePose);
 
+                // `pose` and `globalNodePose` seem to be identical. Is the last step necessary?
+                // Maybe `pose` could be non-global.
+
                 ARMARX_VERBOSE
                         << std::setprecision(2) << std::fixed
                         << "Processing node " << (commit.updates.size() + 1)
                         << "\n- ID: \t" << nodeId
                         << "\n- Name: \t" << name
-                        << "\n- pose: \n" << pose->toEigen()
-                        << "\n- resolved global pose: \n" << globalNodePose->toEigen()
+                        << "\n- Pose: \n" << pose->toEigen()
+                        << "\n- Resolved global pose: \n" << globalNodePose->toEigen()
                            ;
 
-                nav::locs::arondto::Location data;
+                nav::loc::arondto::Location data;
                 data.globalRobotPose = globalNodePose->toEigen();
 
                 armem::EntityUpdate& update = commit.add();
-                update.entityID = armem::MemoryID(
-                        "Navigation", "Locations", tab.providerSegmentLine.getValue(), name
-                );
+                update.entityID = getLocationProviderSegmentID().withEntityName(name);
                 update.timeCreated = time;
                 update.instancesData = { data.toAron() };
             }
@@ -285,14 +308,14 @@ namespace armarx::nav
 
     }
 
-    armem::MemoryID GraphImportExport::getLocationsProviderSegmentID() const
+    armem::MemoryID GraphImportExport::getLocationProviderSegmentID()
     {
         return armem::MemoryID(properties.memoryName,
-                               properties.locationsCoreSegmentName,
+                               properties.locationCoreSegmentName,
                                tab.providerSegmentLine.getValue());
     }
 
-    armem::MemoryID GraphImportExport::getGraphProviderSegmentID() const
+    armem::MemoryID GraphImportExport::getGraphProviderSegmentID()
     {
         return armem::MemoryID(properties.memoryName,
                                properties.graphCoreSegmentName,

source/armarx/navigation/components/GraphImportExport/GraphImportExport.h

@@ -57,6 +57,9 @@ namespace armarx::nav
     {
     public:
 
+        GraphImportExport();
+
+
         /// @see armarx::ManagedIceObject::getDefaultName()
         std::string getDefaultName() const override;
 
@@ -93,21 +96,21 @@ namespace armarx::nav
 
         void refreshScenes();
 
-        void clearProviderSegment();
-
         void exportLocations(const std::string& sceneName);
         void importLocations();
         void exportGraph();
         void importGraph();
 
+        void clearArMemProviderSegment(const armem::MemoryID& providerSegmentID);
+
 
         void drawScene(const std::string& sceneName);
         void addEdge(const std::string& node1Id, const std::string& node2Id);
         void addNode(const memoryx::GraphNodeBasePtr& node);
 
 
-        armem::MemoryID getLocationsProviderSegmentID() const;
-        armem::MemoryID getGraphProviderSegmentID() const;
+        armem::MemoryID getLocationProviderSegmentID();
+        armem::MemoryID getGraphProviderSegmentID();
 
 
     private:
@@ -126,9 +129,9 @@ namespace armarx::nav
         /// Properties shown in the Scenario GUI.
         struct Properties
         {
-            std::string memoryName = "Navigation";
-            std::string locationsCoreSegmentName = "Location";
-            std::string graphCoreSegmentName = "Graph";
+            std::string memoryName;
+            std::string locationCoreSegmentName;
+            std::string graphCoreSegmentName;
         };
         Properties properties;