From d7905a239d2b04055156d1eb8aad4984b98a4322 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 18 Aug 2021 15:51:12 +0200
Subject: [PATCH] Move code to library
---
.../GraphImportExport/GraphImportExport.cpp | 108 +++++++++---------
.../GraphImportExport/GraphImportExport.h | 4 +
source/armarx/navigation/graph/CMakeLists.txt | 1 +
source/armarx/navigation/graph/Graph.cpp | 34 ++++++
source/armarx/navigation/graph/Graph.h | 3 +
5 files changed, 95 insertions(+), 55 deletions(-)
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index bf9394bc..3804695b 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -280,64 +280,11 @@ namespace armarx::nav
void GraphImportExport::graphMemoryxToArmem(const std::string& sceneName)
{
memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
-
- nav::graph::Graph graph;
- std::map<std::string, nav::graph::Graph::Vertex> vertexMap;
-
- // Add nodes
- semrel::ShapeID nextVertexID { 0 };
- for (memoryx::GraphNodeBasePtr& node : graphNodes)
- {
- ARMARX_CHECK_NOT_NULL(node);
- if (not node->isMetaEntity())
- {
- // This is the readable name entered in the GUI.
- const std::string name = node->getName();
-
- FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node));
- ARMARX_CHECK_NOT_NULL(globalNodePose);
-
- ARMARX_VERBOSE << "\n- Adding node: \t" << name;
-
- nav::graph::Graph::Vertex& vertex = vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
- vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
- toAron(vertex.attrib().aron.locationID, getLocationProviderSegmentID().withEntityName(name));
- vertex.attrib().aron.globalRobotPose = globalNodePose->toEigen();
-
- nextVertexID++;
- }
- }
-
- // Add edges
- for (memoryx::GraphNodeBasePtr& node : graphNodes)
- {
- const auto& sourceVertex = vertexMap.at(node->getName());
- for (int i = 0; i < node->getOutdegree(); i++)
- {
- auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
- ARMARX_CHECK_NOT_NULL(adjacent);
-
- const auto& targetVertex = vertexMap.at(adjacent->getName());
-
- ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " -> \t" << adjacent->getName();
-
- nav::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
- edge.attrib().aron.sourceVertexID = static_cast<long>(sourceVertex.attrib().aron.vertexID);
- edge.attrib().aron.targetVertexID = static_cast<long>(targetVertex.attrib().aron.vertexID);
- }
- }
+ nav::graph::Graph graph = toArmemGraph(graphNodes);
// Build ARON Graph
nav::graph::arondto::Graph aron;
- for (auto vertex : graph.vertices())
- {
- aron.vertices.push_back(vertex.attrib().aron);
- }
- for (auto edge : graph.edges())
- {
- aron.edges.push_back(edge.attrib().aron);
- }
-
+ toAron(aron, graph);
// Build commit
const armem::Time time = armem::Time::now();
@@ -386,4 +333,55 @@ namespace armarx::nav
tab.providerSegmentLine.getValue());
}
+ nav::graph::Graph GraphImportExport::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
+ {
+ nav::graph::Graph graph;
+ std::map<std::string, nav::graph::Graph::Vertex> vertexMap;
+
+ // Add nodes
+ semrel::ShapeID nextVertexID { 0 };
+ for (memoryx::GraphNodeBasePtr node : graphNodes)
+ {
+ ARMARX_CHECK_NOT_NULL(node);
+ if (not node->isMetaEntity())
+ {
+ // This is the readable name entered in the GUI.
+ const std::string name = node->getName();
+
+ FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node));
+ ARMARX_CHECK_NOT_NULL(globalNodePose);
+
+ ARMARX_VERBOSE << "\n- Adding node: \t" << name;
+
+ nav::graph::Graph::Vertex& vertex = vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
+ vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
+ toAron(vertex.attrib().aron.locationID, getLocationProviderSegmentID().withEntityName(name));
+ vertex.attrib().aron.globalRobotPose = globalNodePose->toEigen();
+
+ nextVertexID++;
+ }
+ }
+
+ // Add edges
+ for (memoryx::GraphNodeBasePtr node : graphNodes)
+ {
+ const auto& sourceVertex = vertexMap.at(node->getName());
+ for (int i = 0; i < node->getOutdegree(); i++)
+ {
+ auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
+ ARMARX_CHECK_NOT_NULL(adjacent);
+
+ const auto& targetVertex = vertexMap.at(adjacent->getName());
+
+ ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " -> \t" << adjacent->getName();
+
+ nav::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
+ edge.attrib().aron.sourceVertexID = static_cast<long>(sourceVertex.attrib().aron.vertexID);
+ edge.attrib().aron.targetVertexID = static_cast<long>(targetVertex.attrib().aron.vertexID);
+ }
+ }
+
+ return graph;
+ }
+
}
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
index f653a43c..90878834 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
@@ -22,6 +22,8 @@
#pragma once
+#include <armarx/navigation/graph/forward_declarations.h>
+
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
@@ -106,6 +108,8 @@ namespace armarx::nav
armem::MemoryID getLocationProviderSegmentID();
armem::MemoryID getGraphProviderSegmentID();
+ nav::graph::Graph toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes);
+
private:
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index aec5e4a1..064597f7 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -19,6 +19,7 @@ armarx_add_library(
HEADERS
constants.h
+ forward_declarations.h
Graph.h
)
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index 2f4c4e49..d8350099 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -26,5 +26,39 @@
namespace armarx::nav::graph
{
+}
+
+
+namespace armarx::nav
+{
+
+ void graph::toAron(arondto::Graph& dto, const Graph& bo)
+ {
+ dto = {};
+ for (auto vertex : bo.vertices())
+ {
+ dto.vertices.push_back(vertex.attrib().aron);
+ }
+ for (auto edge : bo.edges())
+ {
+ dto.edges.push_back(edge.attrib().aron);
+ }
+ }
+
+
+ void graph::fromAron(const arondto::Graph& dto, Graph& bo)
+ {
+ bo = {};
+ for (const arondto::Vertex& vertex : dto.vertices)
+ {
+ auto v = bo.addVertex(semrel::ShapeID(vertex.vertexID));
+ v.attrib().aron = vertex;
+ }
+ for (const arondto::Edge& edge : dto.edges)
+ {
+ auto e = bo.addEdge(semrel::ShapeID(edge.sourceVertexID), semrel::ShapeID(edge.targetVertexID));
+ e.attrib().aron = edge;
+ }
+ }
}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index e3d61432..10ec0e29 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -45,4 +45,7 @@ namespace armarx::nav::graph
using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
+ void toAron(arondto::Graph& dto, const Graph& bo);
+ void fromAron(const arondto::Graph& dto, Graph& bo);
+
}
--
GitLab