From d3d655187744cd560b76e885d6663d44d03ef5db Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 2 Sep 2021 10:04:16 +0200
Subject: [PATCH] Add snapshot loading
---
.../NavigationMemory/NavigationMemory.cpp | 52 +++++++++++++++++++
.../NavigationMemory/NavigationMemory.h | 1 +
2 files changed, 53 insertions(+)
diff --git a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
index c16efecf..d2267148 100644
--- a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/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));
+ }
+ }
}
diff --git a/source/Navigation/components/NavigationMemory/NavigationMemory.h b/source/Navigation/components/NavigationMemory/NavigationMemory.h
index 58b274bd..f769f90a 100644
--- a/source/Navigation/components/NavigationMemory/NavigationMemory.h
+++ b/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
{
--
GitLab