Add snapshot loading

source/Navigation/components/NavigationMemory/NavigationMemory.cpp

@@ -35,7 +35,11 @@
 #include <RobotAPI/libraries/armem/server/query_proc/ltm.h>
 #include <RobotAPI/libraries/armem/client/query.h>
 #include <RobotAPI/libraries/armem/core/operations.h>
+
 #include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <filesystem>
 
 
 namespace armarx::nav
@@ -58,6 +62,10 @@ namespace armarx::nav
 
 
         workingMemory().name() = "Navigation";
+        def->optional(properties.snapshotToLoad, "p.snapshotToLoad",
+                      "Memory snapshot to load at start up \n"
+                      "(e.g. 'PriorKnowledgeData/navigation-graphs/snapshot').");
+
         def->optional(properties.locationGraph.visuLocations, "p.locationGraph.visuLocation",
                       "Enable visualization of locations.");
         def->optional(properties.locationGraph.visuGraphEdges, "p.locationGraph.visuGraphEdges",
@@ -69,6 +77,25 @@ namespace armarx::nav
     }
 
 
+    std::optional<std::filesystem::path>
+    findSnapshotDirectory(const std::filesystem::path& specifiedPath, const std::string& memoryName)
+    {
+        std::vector<std::filesystem::path> candidates
+        {
+            specifiedPath,
+            specifiedPath / memoryName,
+        };
+        for (const auto& candidate : candidates)
+        {
+            if (candidate.filename() == memoryName and std::filesystem::is_directory(candidate))
+            {
+                return candidate;
+            }
+        }
+        return std::nullopt;
+    }
+
+
     void
     NavigationMemory::onInitComponent()
     {
@@ -98,6 +125,31 @@ namespace armarx::nav
         workingMemory().addCoreSegment(nav::graph::coreSegmentID.coreSegmentName,
                                      nav::graph::arondto::Graph::toAronType());
 
+
+        if (not properties.snapshotToLoad.empty())
+        {
+            std::string resolved = ArmarXDataPath::resolvePath(properties.snapshotToLoad);
+            if (auto path = findSnapshotDirectory(resolved, workingMemory().name()))
+            {
+                const std::string key = path->filename();
+                armem::server::ltm::disk::MemoryManager manager;
+                manager.setName(key);
+                manager.setBasePath(path.value());
+                manager.reload();
+
+                armem::client::QueryBuilder builder;
+                builder.all();
+                armem::client::QueryInput queryInput = builder.buildQueryInput();
+                queryInput.addQueryTargetToAll(armem::query::data::QueryTarget::LTM);
+
+                armem::server::query_proc::ltm::MemoryQueryProcessor processor;
+                armem::wm::Memory memory = processor.process(queryInput.toIce(), manager.getCacheAndLutNotConverted());
+                manager.convert(memory);
+
+                ARMARX_INFO << "Loaded " << armem::print(memory);
+                workingMemory().update(armem::toCommit(memory));
+            }
+        }
     }
 
 

source/Navigation/components/NavigationMemory/NavigationMemory.h

@@ -95,6 +95,7 @@ namespace armarx::nav
         /// Properties shown in the Scenario GUI.
         struct Properties
         {
+            std::string snapshotToLoad = "";
 
             struct LocationGraph
             {