Implement graph export

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")

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
-
-
 }

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;

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()

source/armarx/navigation/graph/Graph.cpp

+/*
+ * 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
+{
+
+
+}

source/armarx/navigation/graph/Graph.h

+/*
+ * 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>;
+
+
+}

source/armarx/navigation/graph/aron/Graph.xml

 <?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>