From 031fe9cb19d8e88042d98b24c680ecf23181d910 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 18 Aug 2021 15:24:46 +0200
Subject: [PATCH] Implement graph export
---
.../GraphImportExport/CMakeLists.txt | 9 +
.../GraphImportExport/GraphImportExport.cpp | 203 +++++++++---------
.../GraphImportExport/GraphImportExport.h | 22 +-
source/armarx/navigation/graph/CMakeLists.txt | 21 +-
source/armarx/navigation/graph/Graph.cpp | 30 +++
source/armarx/navigation/graph/Graph.h | 48 +++++
source/armarx/navigation/graph/aron/Graph.xml | 48 ++++-
7 files changed, 263 insertions(+), 118 deletions(-)
create mode 100644 source/armarx/navigation/graph/Graph.cpp
create mode 100644 source/armarx/navigation/graph/Graph.h
diff --git a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
index 5ed95be3..16d673fe 100644
--- a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
+++ b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
@@ -54,6 +54,15 @@ if(MemoryX_FOUND)
endif()
+
+#find_package(SemanticObjectRelations QUIET)
+#armarx_build_if(SemanticObjectRelations_FOUND "SemanticObjectRelations not available")
+#if(SemanticObjectRelations_FOUND)
+# target_link_libraries(${LIB_NAME} PRIVATE SemanticObjectRelations)
+# target_include_directories(${LIB_NAME} PRIVATE SemanticObjectRelations)
+#endif()
+
+
# Add dependencies
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index f90f4834..bf9394bc 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -20,17 +20,22 @@
* GNU General Public License
*/
+#include <VirtualRobot/VirtualRobot.h>
+
#include "GraphImportExport.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 <armarx/navigation/graph/Graph.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -47,28 +52,14 @@ namespace armarx::nav
properties.graphCoreSegmentName = graph::coreSegmentName;
}
+
armarx::PropertyDefinitionsPtr GraphImportExport::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
- // Publish to a topic (passing the TopicListenerPrx).
- // def->topic(myTopicListener);
-
- // Subscribe to a topic (passing the topic name).
- // def->topic<PlatformUnitListener>("MyTopic");
-
- // Use (and depend on) another component (passing the ComponentInterfacePrx).
def->component(proxies.priorKnowledge);
def->component(proxies.graphNodePoseResolver, "GraphNodePoseResolver");
-
- // Add a required property. (The component won't start without a value being set.)
- // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
-
- // Add an optionalproperty.
- // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
-
-
return def;
}
@@ -124,12 +115,12 @@ namespace armarx::nav
tab.dryRun.setValue(false);
- tab.locationsMemoryXToArMemButton.setLabel("Locations MemoryX -> ArMem");
- tab.locationsArMemToMemoryXButton.setLabel("Locations ArMem -> MemoryX (WIP)");
+ tab.locationsMemoryxToArmemButton.setLabel("Locations MemoryX -> ArMem");
+ tab.locationsArmemToMemoryxButton.setLabel("Locations ArMem -> MemoryX (WIP)");
tab.locationsClearArMemButton.setLabel("Clear ArMem Locations");
- tab.graphMemoryXToArMemButton.setLabel("Graph MemoryX -> ArMem (WIP)");
- tab.graphArMemToMemoryXButton.setLabel("Graph ArMem -> MemoryX (WIP)");
+ tab.graphMemoryxToArmemButton.setLabel("Graph MemoryX -> ArMem");
+ tab.graphArmemToMemoryxButton.setLabel("Graph ArMem -> MemoryX (WIP)");
tab.graphClearArMemButton.setLabel("Clear ArMem Graphs");
@@ -145,11 +136,11 @@ namespace armarx::nav
grid.add(Label("Dry Run:"), {row, 0}).add(tab.dryRun, {row, 1});
++row;
- grid.add(tab.locationsMemoryXToArMemButton, {row, 0}).add(tab.locationsArMemToMemoryXButton, {row, 1})
+ grid.add(tab.locationsMemoryxToArmemButton, {row, 0}).add(tab.locationsArmemToMemoryxButton, {row, 1})
.add(tab.locationsClearArMemButton, {row, 2});
++row;
- grid.add(tab.graphMemoryXToArMemButton, {row, 0}).add(tab.graphArMemToMemoryXButton, {row, 1})
+ grid.add(tab.graphMemoryxToArmemButton, {row, 0}).add(tab.graphArmemToMemoryxButton, {row, 1})
.add(tab.graphClearArMemButton, {row, 2});
++row;
}
@@ -165,16 +156,29 @@ namespace armarx::nav
{
refreshScenes();
}
- if (tab.locationsMemoryXToArMemButton.wasClicked())
+
+ if (tab.locationsMemoryxToArmemButton.wasClicked())
+ {
+ locationsMemoryxToArmem(tab.sceneComboBox.getValue());
+ }
+ if (tab.locationsArmemToMemoryxButton.wasClicked())
{
- exportLocations(tab.sceneComboBox.getValue());
+ locationsArmemToMemoryx(tab.sceneComboBox.getValue());
+ }
+ if (tab.graphMemoryxToArmemButton.wasClicked())
+ {
+ graphMemoryxToArmem(tab.sceneComboBox.getValue());
+ }
+ if (tab.graphArmemToMemoryxButton.wasClicked())
+ {
+ graphArmemToMemoryx(tab.sceneComboBox.getValue());
}
- if (tab.locationsMemoryXToArMemButton.wasClicked())
+ if (tab.locationsClearArMemButton.wasClicked())
{
clearArMemProviderSegment(getLocationProviderSegmentID());
}
- if (tab.graphMemoryXToArMemButton.wasClicked())
+ if (tab.graphClearArMemButton.wasClicked())
{
clearArMemProviderSegment(getGraphProviderSegmentID());
}
@@ -203,7 +207,7 @@ namespace armarx::nav
}
- void GraphImportExport::exportLocations(const std::string& sceneName)
+ void GraphImportExport::locationsMemoryxToArmem(const std::string& sceneName)
{
const armem::Time time = armem::Time::now();
armem::Commit commit;
@@ -252,7 +256,7 @@ namespace armarx::nav
armem::CommitResult result = proxies.navigationWriter.commit(commit);
if (result.allSuccess())
{
- ARMARX_IMPORTANT << "Successfully exported " << result.results.size() << " locations.";
+ ARMARX_IMPORTANT << "Successfully exported " << result.results.size() << " locations from MemoryX to ArMem.";
}
else
{
@@ -266,48 +270,108 @@ namespace armarx::nav
}
- void GraphImportExport::drawScene(const std::string& sceneName)
+ void GraphImportExport::locationsArmemToMemoryx(const std::string& sceneName)
+ {
+ ARMARX_IMPORTANT << "locationsArmemToMemoryx() is WIP!";
+ (void) sceneName;
+ }
+
+
+ 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);
-
- armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose());
- if (pose and not node->isMetaEntity())
+ if (not node->isMetaEntity())
{
- const std::string nodeId = node->getId();
-
+ // 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++;
}
}
-#if 0
// Add edges
for (memoryx::GraphNodeBasePtr& node : graphNodes)
{
- std::string nodeId = node->getId();
+ 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);
- auto adjacentId = adjacent->getId();
- addEdge(nodeId, adjacentId);
+ 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);
}
}
-#endif
+
+ // 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);
+ }
+
+
+ // Build commit
+ const armem::Time time = armem::Time::now();
+ armem::EntityUpdate update;
+ update.entityID = getGraphProviderSegmentID().withEntityName(sceneName);
+ update.timeCreated = time;
+ update.instancesData = { aron.toAron() };
+
+ if (not tab.dryRun.getValue())
+ {
+ armem::EntityUpdateResult result = proxies.navigationWriter.commit(update);
+ if (result.success)
+ {
+ ARMARX_IMPORTANT << "Successfully exported graph '" << sceneName << "' from MemoryX to ArMem.";
+ }
+ else
+ {
+ ARMARX_WARNING << result.errorMessage;
+ }
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Dry Run - skipping commit.";
+ }
}
- void GraphImportExport::addNode(const memoryx::GraphNodeBasePtr& node)
+ void GraphImportExport::graphArmemToMemoryx(const std::string& sceneName)
{
- (void) node;
-
+ ARMARX_IMPORTANT << "graphArmemToMemoryx() is WIP!";
+ (void) sceneName;
}
+
armem::MemoryID GraphImportExport::getLocationProviderSegmentID()
{
return armem::MemoryID(properties.memoryName,
@@ -322,61 +386,4 @@ namespace armarx::nav
tab.providerSegmentLine.getValue());
}
-
-#if 0
- void GraphImportExport::addEdge(const std::string& node1Id, const std::string& node2Id)
- {
- if (!hasNode(node1Id))
- {
- ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node1Id << " does not exist.";
- return;
- }
-
- if (!hasNode(node2Id))
- {
- ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node2Id << " does not exist.";
- return;
- }
-
- auto node1dat = nodes.at(node1Id);
- auto node2dat = nodes.at(node2Id);
-
- if (hasEdge(node1Id, node2Id))
- {
- //nothing needs to be updated
- ARMARX_WARNING << "Edge: '" << node1dat.node->getName() << "' -> '" << node2dat.node->getName() << "' already exists.";
- return;
- }
-
- auto edgeId = toEdge(node1Id, node2Id);
-
- //add
- //table
- int row = ui.tableWidgetEdges->rowCount();
- ui.tableWidgetEdges->setRowCount(row + 1);
- ui.tableWidgetEdges->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node1dat.node->getName())});
- ui.tableWidgetEdges->setItem(row, 1, new QTableWidgetItem {QString::fromStdString(node2dat.node->getName())});
- //debug layer will be done later
- //scene
- QGraphicsLineItem* graphicsItem = dynamic_cast<QGraphicsLineItem*>(new GraphVisualizerGraphicsLineItem
- {
- *this, edgeId,
- node1dat.pose->position->x, -node1dat.pose->position->y,
- node2dat.pose->position->x, -node2dat.pose->position->y
- });
- //auto graphicsItem= scene->addLine(node1dat.pos->x,-node1dat.pos->y,
- // node2dat.pos->x,-node2dat.pos->y);
- scene->addItem(graphicsItem);
- //setToolTip on graphicsItem does not work
- dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString {"Edge:"} +QString::fromStdString(node1dat.node->getName()) +
- QString {" <-> "} +QString::fromStdString(node2dat.node->getName()));
- //data
- EdgeData data {graphicsItem, row, false};
- edges[edgeId] = data;
-
- updateEdge(edgeId);
- }
-#endif
-
-
}
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
index 06215c7d..f653a43c 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
@@ -96,19 +96,13 @@ namespace armarx::nav
void refreshScenes();
- void exportLocations(const std::string& sceneName);
- void importLocations();
- void exportGraph();
- void importGraph();
+ void locationsMemoryxToArmem(const std::string& sceneName);
+ void locationsArmemToMemoryx(const std::string& sceneName);
+ void graphMemoryxToArmem(const std::string& sceneName);
+ void graphArmemToMemoryx(const std::string& sceneName);
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 getLocationProviderSegmentID();
armem::MemoryID getGraphProviderSegmentID();
@@ -144,12 +138,12 @@ namespace armarx::nav
armarx::RemoteGui::Client::LineEdit providerSegmentLine;
- armarx::RemoteGui::Client::Button locationsMemoryXToArMemButton;
- armarx::RemoteGui::Client::Button locationsArMemToMemoryXButton;
+ armarx::RemoteGui::Client::Button locationsMemoryxToArmemButton;
+ armarx::RemoteGui::Client::Button locationsArmemToMemoryxButton;
armarx::RemoteGui::Client::Button locationsClearArMemButton;
- armarx::RemoteGui::Client::Button graphMemoryXToArMemButton;
- armarx::RemoteGui::Client::Button graphArMemToMemoryXButton;
+ armarx::RemoteGui::Client::Button graphMemoryxToArmemButton;
+ armarx::RemoteGui::Client::Button graphArmemToMemoryxButton;
armarx::RemoteGui::Client::Button graphClearArMemButton;
armarx::RemoteGui::Client::CheckBox dryRun;
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index ae1cf290..aec5e4a1 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -6,13 +6,20 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
LIBS
+ # ArmarXCore
ArmarXCoreInterfaces
ArmarXCore
- # ${PROJECT_NAME}Core
+
+ # RobotAPI
+ aron
+
SOURCES
constants.cpp
+ Graph.cpp
+
HEADERS
constants.h
+ Graph.h
)
@@ -22,9 +29,21 @@ add_library(
"${LIB_NAME}"
)
+
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
aron/Graph.xml
)
+
+
+find_package(VTK QUIET)
+armarx_build_if(VTK_FOUND "VTK not available")
+
+find_package(SemanticObjectRelations QUIET)
+armarx_build_if(SemanticObjectRelations_FOUND "SemanticObjectRelations not available")
+if(SemanticObjectRelations_FOUND)
+ target_link_libraries(${LIB_NAME} PUBLIC SemanticObjectRelations)
+ # target_include_directories(${LIB_NAME} PUBLIC SemanticObjectRelations)
+endif()
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
new file mode 100644
index 00000000..2f4c4e49
--- /dev/null
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -0,0 +1,30 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package MemoryX::ArmarXObjects::GraphImportExport
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Graph.h"
+
+
+namespace armarx::nav::graph
+{
+
+
+}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
new file mode 100644
index 00000000..e3d61432
--- /dev/null
+++ b/source/armarx/navigation/graph/Graph.h
@@ -0,0 +1,48 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package MemoryX::ArmarXObjects::GraphImportExport
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
+
+#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
+
+
+namespace armarx::nav::graph
+{
+
+ struct VertexAttribs : public semrel::ShapeVertex
+ {
+ armarx::nav::graph::arondto::Vertex aron;
+ };
+ struct EdgeAttribs
+ {
+ armarx::nav::graph::arondto::Edge aron;
+ };
+ struct GraphAttribs
+ {
+ };
+
+ using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
+
+
+}
diff --git a/source/armarx/navigation/graph/aron/Graph.xml b/source/armarx/navigation/graph/aron/Graph.xml
index 69d0f78a..18e1c847 100644
--- a/source/armarx/navigation/graph/aron/Graph.xml
+++ b/source/armarx/navigation/graph/aron/Graph.xml
@@ -1,19 +1,57 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <!--Include include="armarx/navigation/location/aron/Location.aron.generated.h" /-->
</CodeIncludes>
<AronIncludes>
- <!-- <Include include="<armarx/navigation/location/aron/Location.xml>" /> -->
+ <!-- <Include include="<armarx/navigation/location/aron/Location.xml>" autoinclude="true" /> -->
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
</AronIncludes>
<GenerateTypes>
+ <Object name='armarx::nav::graph::arondto::Vertex'>
+
+ <ObjectChild key='vertexID'>
+ <Long />
+ </ObjectChild>
+
+ <ObjectChild key='locationID'>
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key='globalRobotPose'>
+ <Pose />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::nav::graph::arondto::Edge'>
+
+ <ObjectChild key='sourceVertexID'>
+ <Long />
+ </ObjectChild>
+
+ <ObjectChild key='targetVertexID'>
+ <Long />
+ </ObjectChild>
+
+ </Object>
+
+
<Object name='armarx::nav::graph::arondto::Graph'>
- <!--ObjectChild key='location'>
- <armarx::nav::loc::arondto::Location />
- </ObjectChild-->
+ <ObjectChild key='vertices'>
+ <List>
+ <armarx::nav::graph::arondto::Vertex />
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='edges'>
+ <List>
+ <armarx::nav::graph::arondto::Edge />
+ </List>
+ </ObjectChild>
</Object>
--
GitLab