diff --git a/source/armarx/navigation/gui-plugins/CMakeLists.txt b/source/armarx/navigation/gui-plugins/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0b821f039461902aa7dddab6d07b19ca82b6123c
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/CMakeLists.txt
@@ -0,0 +1,2 @@
+
+add_subdirectory(LocationGraphEditor)
\ No newline at end of file
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1c9c13a4964b7ab9230750b3b83c53018be13c4b
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -0,0 +1,57 @@
+set(LIB_NAME "LocationGraphEditorGuiPlugin")
+armarx_set_target("${LIB_NAME}")
+
+# most qt components will be linked against in the call armarx_gui_library
+#armarx_find_qt(QtCore QtGui QtDesigner)
+
+# ArmarXGui gets included through depends_on_armarx_package(ArmarXGui "OPTIONAL")
+# in the toplevel CMakeLists.txt
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+
+# do not rename this variable, it is used in armarx_gui_library()...
+set(SOURCES
+ LocationGraphEditorWidgetController.cpp
+)
+set(HEADERS
+ LocationGraphEditorWidgetController.h
+)
+set(GUI_UIS
+ LocationGraphEditorWidget.ui
+)
+
+
+# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
+set(COMPONENT_LIBS
+ # ArmarXGui
+ SimpleConfigDialog
+
+ # RobotAPI
+ armem
+
+ # MemoryX
+ MemoryXCore
+ MemoryXMemoryTypes
+)
+
+
+if(ArmarXGui_FOUND)
+ armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+
+
+ # ToDo: Remove
+ find_package(MemoryX QUIET)
+ armarx_build_if(MemoryX_FOUND "MemoryX not available")
+ if(MemoryX_FOUND)
+ target_include_directories(${LIB_NAME} PUBLIC ${MemoryX_INCLUDE_DIRS})
+ endif()
+
+
+ #find_package(MyLib QUIET)
+ #armarx_build_if(MyLib_FOUND "MyLib not available")
+ # all target_include_directories must be guarded by if(Xyz_FOUND)
+ # for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+ #if(MyLib_FOUND)
+ # target_include_directories(LocationGraphEditorGuiPlugi PUBLIC ${MyLib_INCLUDE_DIRS})
+ #endif()
+endif()
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..4e2a178272eddfe8b546c748238d39793885fbee
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -0,0 +1,572 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>LocationGraphEditorWidget</class>
+ <widget class="QWidget" name="LocationGraphEditorWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>783</width>
+ <height>664</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>LocationGraphEditorWidget</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <widget class="QGroupBox" name="sceneGroupBox">
+ <property name="title">
+ <string>Navigation Graphs from Navigation Graph Segment</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <item>
+ <widget class="QPushButton" name="refreshScenesButton">
+ <property name="toolTip">
+ <string>Reloads the list of scenes from the memory</string>
+ </property>
+ <property name="text">
+ <string>Refresh List</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="scenesComboBox">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="drawSceneButton">
+ <property name="toolTip">
+ <string>Draws the selected scene</string>
+ </property>
+ <property name="text">
+ <string>Load</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSplitter" name="splitter_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <widget class="QFrame" name="graphFrame">
+ <property name="frameShape">
+ <enum>QFrame::NoFrame</enum>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_4">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QWidget" name="graphHead" native="true">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QLabel" name="label_graph">
+ <property name="text">
+ <string>Graph</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="buttonRedraw">
+ <property name="text">
+ <string>Repaint</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="buttonClear">
+ <property name="text">
+ <string>Clear graph</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QToolButton" name="buttonRotateClock">
+ <property name="text">
+ <string>↻</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QToolButton" name="buttonRotateCounterClock">
+ <property name="text">
+ <string>↺</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="Line" name="line_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_zoom">
+ <property name="text">
+ <string>Zoom</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QDoubleSpinBox" name="viewZoomFactor">
+ <property name="decimals">
+ <number>5</number>
+ </property>
+ <property name="singleStep">
+ <double>0.001000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QToolButton" name="buttonAutoAdjust">
+ <property name="text">
+ <string>Auto</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGraphicsView" name="graphicsViewGraph"/>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tables" native="true">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QSplitter" name="splitter_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <widget class="QSplitter" name="splitter">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <widget class="QWidget" name="layoutWidget">
+ <layout class="QVBoxLayout" name="verticalLayout_nodes">
+ <item>
+ <widget class="QLabel" name="label_nodes">
+ <property name="text">
+ <string>Nodes</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QTableWidget" name="tableWidgetNodes">
+ <property name="editTriggers">
+ <set>QAbstractItemView::NoEditTriggers</set>
+ </property>
+ <column>
+ <property name="text">
+ <string>Name</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>X</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Y</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Yaw Angle</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="layoutWidget2">
+ <layout class="QVBoxLayout" name="verticalLayout_edges">
+ <item>
+ <widget class="QLabel" name="label_edges">
+ <property name="text">
+ <string>Edges</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QTableWidget" name="tableWidgetEdges">
+ <property name="editTriggers">
+ <set>QAbstractItemView::NoEditTriggers</set>
+ </property>
+ <column>
+ <property name="text">
+ <string>Node 1</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Node 2</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ <widget class="QTabWidget" name="tabWidget">
+ <property name="currentIndex">
+ <number>0</number>
+ </property>
+ <widget class="QWidget" name="tabWidgetPage1">
+ <attribute name="title">
+ <string>Add Node</string>
+ </attribute>
+ <layout class="QFormLayout" name="formLayout">
+ <property name="fieldGrowthPolicy">
+ <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
+ </property>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_12">
+ <property name="text">
+ <string>NodeId</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="editNodeId">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Name</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="editNodeName"/>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>Frame</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="editFrameName"/>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Agent</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLineEdit" name="editAgentName"/>
+ </item>
+ <item row="7" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>X</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxX">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-1000000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1000000.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Y</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxY">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-1000000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1000000.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Z</string>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxZ">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-1000000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1000000.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>R in deg</string>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxRoll">
+ <property name="minimum">
+ <double>-360.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>360.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>P in deg</string>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxPitch">
+ <property name="minimum">
+ <double>-360.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>360.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="12" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>Y in deg</string>
+ </property>
+ </widget>
+ </item>
+ <item row="12" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxYaw">
+ <property name="minimum">
+ <double>-360.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>360.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="13" column="0">
+ <widget class="QPushButton" name="btnAdd">
+ <property name="text">
+ <string>Add</string>
+ </property>
+ </widget>
+ </item>
+ <item row="13" column="1">
+ <widget class="QPushButton" name="btnEdit">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Edit</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="editSceneName"/>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_13">
+ <property name="text">
+ <string>Scene</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tab">
+ <attribute name="title">
+ <string>Add Edge</string>
+ </attribute>
+ <layout class="QFormLayout" name="gridLayout">
+ <property name="fieldGrowthPolicy">
+ <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
+ </property>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_10">
+ <property name="text">
+ <string>Start Node Id</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLineEdit" name="editStartNodeId">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="labelStartNode">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Start Node</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="editStartNodeName">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>End Node Id</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="editEndNodeId">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="labelEndNode">
+ <property name="text">
+ <string>End Node</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="editEndNodeName">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0" colspan="2">
+ <widget class="QPushButton" name="btnAddEdge">
+ <property name="text">
+ <string>Add Double-sided Edge</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="5" column="0" colspan="2">
+ <widget class="QPushButton" name="btnAddEdgeStartEnd">
+ <property name="text">
+ <string>Add Edge Start -> End</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0" colspan="2">
+ <widget class="QPushButton" name="btnAddEdgeEndStart">
+ <property name="text">
+ <string>Add Edge End -> Start</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0" colspan="2">
+ <widget class="QLabel" name="labelAddEdgeStatus">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cb576778e44adbe84f9193f6e0e4a81a4a458704
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -0,0 +1,1162 @@
+/*
+ * 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 Navigation::gui-plugins::LocationGraphEditorWidgetController
+ * \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 "LocationGraphEditorWidgetController.h"
+
+#include <MemoryX/libraries/memorytypes/variants/GraphNode/GraphNode.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <VirtualRobot/MathTools.h>
+
+
+// Qt headers
+#include <Qt>
+#include <QtGlobal>
+#include <QPushButton>
+#include <QLabel>
+#include <QLineEdit>
+#include <QHBoxLayout>
+#include <QMenu>
+
+// std
+#include <memory>
+#include <sstream>
+#include <unordered_set>
+#include <tuple>
+#include <filesystem>
+#include <fstream>
+
+
+// static values and helper functions
+#define DEFAULT_PRIOR_KNOWLEDGE_NAME "PriorKnowledge"
+#define DEFAULT_DEBUG_DRAWER_NAME "DebugDrawerUpdates"
+#define DEFAULT_DEBUG_DRAWER_LAYER_NAME "DebugDrawerUpdates_Graph"
+
+/**
+ * @brief The default width of lines drawn onto the debug layer and scene.
+ */
+static const ::Ice::Float LINE_WIDTH_DEFAULT = 5;
+/**
+ * @brief The width of selected lines drawn onto the debug layer and scene.
+ */
+static const ::Ice::Float LINE_WIDTH_SELECTED = 10;
+
+/**
+ * @brief The default color of lines drawn onto the debug layer and scene.
+ */
+static const ::armarx::DrawColor COLOR_DEFAULT = {0.5f, 0.5f, 1.f, 0.2f};
+/**
+ * @brief The color of highlighted lines drawn onto the debug layer and scene.
+ */
+static const ::armarx::DrawColor COLOR_HIGHLIGHT = {1.f, 0.0f, 0.f, 1.f};
+
+/**
+ * @brief The scale factor for elements drawn onto the scene.
+ */
+static const float SCENE_SCALE_FACTOR = 2;
+/**
+ * @brief The scale factor for nodes drawn onto the scene.
+ */
+static const float SCENE_NODES_SCALE_FACTOR = 3 * SCENE_SCALE_FACTOR;
+/**
+ * @brief The scale factor for edges drawn onto the scene.
+ */
+static const float SCENE_LINE_SCALE_FACTOR = SCENE_SCALE_FACTOR;
+
+/**
+ * @brief The increment used when a rotation button is pressed.
+ *
+ * A positive rotation is counter clockwise.
+ * This value should be positive.
+ *
+ * rotation buttons: LocationGraphEditorWidgetController::widget.buttonRotateClock and
+ * LocationGraphEditorWidgetController::widget.buttonRotateCounterClock
+ */
+static const float VIEW_ROTATE_STEP_SIZE_CC = 45;
+
+static const QString SETTING_LAST_SCENE = "lastScene";
+
+
+namespace armarx
+{
+ /**
+ * @brief Returns the name used on the debug layer.
+ * @param edge The edge.
+ * @return The name used on the debug layer.
+ */
+ inline std::string iceName(const LocationGraphEditorWidgetController::EdgeId& edge)
+ {
+ std::stringstream s;
+ s << "edge_" << edge.first << "_" << edge.second;
+ return s.str();
+ }
+
+ /**
+ * @brief iceName Returns the name used on the debug layer.
+ * @param nodeName The node.
+ * @return The name used on the debug layer.
+ */
+ inline std::string iceName(const LocationGraphEditorWidgetController::NodeId& nodeName)
+ {
+ return nodeName;
+ }
+
+
+ QString LocationGraphEditorWidgetController::GetWidgetName()
+ {
+ return "Navigation.LocationGraphEditor";
+ }
+ QIcon LocationGraphEditorWidgetController::GetWidgetIcon()
+ {
+ return QIcon {":// icons/graph_visu.svg"};
+ }
+
+
+ LocationGraphEditorWidgetController::LocationGraphEditorWidgetController() :
+ debugDrawerTopicName {DEFAULT_DEBUG_DRAWER_NAME},
+ viewAngle {0},
+ debugDrawerLayerName {DEFAULT_DEBUG_DRAWER_LAYER_NAME},
+ priorKnowledgeProxyName {DEFAULT_PRIOR_KNOWLEDGE_NAME},
+ settings {"KIT", "LocationGraphEditorWidgetController"}
+ {
+ widget.setupUi(getWidget());
+
+ loadAutomaticSettings();
+ editStartNodeNext = true;
+
+ // Add scene
+ std::unique_ptr<QGraphicsScene> scenePtr{new QGraphicsScene};
+ scene = scenePtr.get();
+ widget.graphicsViewGraph->setScene(scenePtr.release());
+ MouseEventProcessor* mep = new MouseEventProcessor(this);
+ widget.graphicsViewGraph->installEventFilter(mep);
+
+ // Transform view
+ transformView();
+ }
+
+
+ LocationGraphEditorWidgetController::~LocationGraphEditorWidgetController()
+ {
+ saveAutomaticSettings();
+ }
+
+
+ QPointer<QDialog> LocationGraphEditorWidgetController::getConfigDialog(QWidget* parent)
+ {
+ if (!dialog)
+ {
+ dialog = new SimpleConfigDialog(parent);
+ }
+
+ dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>("MemoryNameSystem", "Memory Name System", memoryNameSystemName);
+ dialog->addProxyFinder<memoryx::PriorKnowledgeInterfacePrx>("PriorKnowledge", "Prior Knowledge", priorKnowledgeProxyName);
+
+ return qobject_cast<SimpleConfigDialog*>(dialog);
+ }
+
+
+ void LocationGraphEditorWidgetController::configured()
+ {
+ memoryNameSystemName = dialog->getProxyName("MemoryNameSystem");
+ priorKnowledgeProxyName = dialog->getProxyName("PriorKnowledge");
+ }
+
+
+ void LocationGraphEditorWidgetController::loadSettings(QSettings* settings)
+ {
+ memoryNameSystemName = settings->value("memoryNameSystemName", QString::fromStdString(memoryNameSystemName)).toString().toStdString();
+ priorKnowledgeProxyName = settings->value("priorKnowledgeProxyName", QString::fromStdString(debugDrawerLayerName)).toString().toStdString();
+ }
+
+
+ void LocationGraphEditorWidgetController::saveSettings(QSettings* settings)
+ {
+ settings->setValue("memoryNameSystemName", QString::fromStdString(memoryNameSystemName));
+ settings->setValue("priorKnowledgeProxyName", QString::fromStdString(priorKnowledgeProxyName));
+ }
+
+
+ void LocationGraphEditorWidgetController::onInitComponent()
+ {
+ usingProxy(priorKnowledgeProxyName);
+ usingProxy("GraphNodePoseResolver");
+ offeringTopic(debugDrawerTopicName);
+ }
+
+
+ void LocationGraphEditorWidgetController::onConnectComponent()
+ {
+ debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
+
+ debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
+ priorKnowledgePrx = getProxy<memoryx::PriorKnowledgeInterfacePrx>(priorKnowledgeProxyName);
+ getProxy(gnpr, "GraphNodePoseResolver");
+
+ if (priorKnowledgePrx->hasGraphSegment())
+ {
+ ARMARX_VERBOSE << "get Proxy to graph segment";
+ graphSeg = priorKnowledgePrx->getGraphSegment();
+ widget.sceneGroupBox->setEnabled(true);
+ widget.sceneGroupBox->setTitle("Scenes from graph memory segment");
+ }
+ else
+ {
+ widget.sceneGroupBox->setEnabled(false);
+ widget.sceneGroupBox->setTitle("Scenes from graph memory segment (No graph memory segment available)");
+ }
+
+ widget.tableWidgetNodes->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
+ widget.tableWidgetEdges->horizontalHeader()->setResizeMode(QHeaderView::Stretch);
+
+ widget.tableWidgetNodes->setContextMenuPolicy(Qt::CustomContextMenu);
+ widget.tableWidgetEdges->setContextMenuPolicy(Qt::CustomContextMenu);
+
+
+ // tables
+ QObject::connect(widget.tableWidgetNodes, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(nodeTableDoubleClicked(int, int)), Qt::UniqueConnection);
+ QObject::connect(widget.tableWidgetNodes, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(tableWidgetNodesCustomContextMenu(QPoint)), Qt::UniqueConnection);
+ QObject::connect(widget.tableWidgetEdges, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(edgeTableDoubleClicked(int, int)), Qt::UniqueConnection);
+ QObject::connect(widget.tableWidgetEdges, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(tableWidgetEdgesCustomContextMenu(QPoint)), Qt::UniqueConnection);
+
+ QObject::connect(widget.btnAdd, SIGNAL(clicked()), this, SLOT(addNewGraphNode()), Qt::UniqueConnection);
+ QObject::connect(widget.btnAddEdge, SIGNAL(clicked()), this, SLOT(addNewEdgeBoth()), Qt::UniqueConnection);
+ QObject::connect(widget.btnAddEdgeStartEnd, SIGNAL(clicked()), this, SLOT(addNewEdgeStartEnd()), Qt::UniqueConnection);
+ QObject::connect(widget.btnAddEdgeEndStart, SIGNAL(clicked()), this, SLOT(addNewEdgeEndStart()), Qt::UniqueConnection);
+ QObject::connect(widget.btnEdit, SIGNAL(clicked()), this, SLOT(editGraphNode()), Qt::UniqueConnection);
+
+ // zoom
+ QObject::connect(widget.viewZoomFactor, SIGNAL(valueChanged(double)), this, SLOT(transformView()), Qt::UniqueConnection);
+ // rota
+ QObject::connect(widget.buttonRotateClock, SIGNAL(clicked()), this, SLOT(viewRotatedClock()), Qt::UniqueConnection);
+ QObject::connect(widget.buttonRotateCounterClock, SIGNAL(clicked()), this, SLOT(viewRotatedCounterClock()), Qt::UniqueConnection);
+ // redraw+clear
+ QObject::connect(widget.buttonRedraw, SIGNAL(clicked()), this, SLOT(redraw()), Qt::UniqueConnection);
+ QObject::connect(widget.buttonClear, SIGNAL(clicked()), this, SLOT(clearGraph()), Qt::UniqueConnection);
+ // auto adjust
+ QObject::connect(widget.buttonAutoAdjust, SIGNAL(clicked()), this, SLOT(adjustView()), Qt::UniqueConnection);
+ // memory
+ QObject::connect(widget.refreshScenesButton, SIGNAL(clicked()), this, SLOT(updateSceneList()), Qt::UniqueConnection);
+ QObject::connect(widget.drawSceneButton, SIGNAL(clicked()), this, SLOT(drawScene()), Qt::UniqueConnection); // BUTTON LOAD
+ QObject::connect(widget.scenesComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectedSceneChanged(int)), Qt::UniqueConnection);
+
+ ARMARX_VERBOSE << "connected";
+
+ updateSceneList();
+ }
+
+
+ void LocationGraphEditorWidgetController::loadAutomaticSettings()
+ {
+ lastSelectedSceneName = settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
+ }
+
+
+ void LocationGraphEditorWidgetController::saveAutomaticSettings()
+ {
+ settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
+ }
+
+
+ void LocationGraphEditorWidgetController::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 = widget.tableWidgetEdges->rowCount();
+ widget.tableWidgetEdges->setRowCount(row + 1);
+ widget.tableWidgetEdges->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node1dat.node->getName())});
+ widget.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);
+ }
+
+
+ void LocationGraphEditorWidgetController::addNode(const memoryx::GraphNodeBasePtr& node)
+ {
+ ARMARX_CHECK_EXPRESSION(node);
+ auto nodeId = node->getId();
+
+ armarx::FramedPosePtr globalNodePose;
+ try
+ {
+ globalNodePose = armarx::FramedPosePtr::dynamicCast(gnpr->resolveToGlobalPose(node));
+ }
+ catch (...)
+ {
+ return;
+ }
+
+ if (hasNode(nodeId))
+ {
+ NodeData& oldNode = nodes.at(nodeId);
+ ARMARX_VERBOSE << "Node: " << nodeId << " was overwritten! Old: "
+ << oldNode.pose->position->x << ", " << oldNode.pose->position->y << ", " << getYawAngle(oldNode.pose) << "| New: "
+ << globalNodePose->position->x << ", " << globalNodePose->position->y << ", " << getYawAngle(globalNodePose);
+ // update node data
+ // table
+ widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 1, new QTableWidgetItem {QString::number(globalNodePose->position->x)});
+ widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 2, new QTableWidgetItem {QString::number(globalNodePose->position->y)});
+ widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 3, new QTableWidgetItem {QString::number(getYawAngle(globalNodePose))});
+
+ // data
+ oldNode.pose = globalNodePose;
+
+ // update connected edges
+ for (const auto& edge : edges)
+ {
+ if ((edge.first.first == nodeId) || (edge.first.second == nodeId))
+ {
+ updateEdge(edge.first);
+ }
+ }
+ }
+ else
+ {
+ // add&draw node
+ // table
+ int row = widget.tableWidgetNodes->rowCount();
+ widget.tableWidgetNodes->setRowCount(row + 1);
+ widget.tableWidgetNodes->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node->getName())});
+ widget.tableWidgetNodes->setItem(row, 1, new QTableWidgetItem {QString::number(globalNodePose->position->x)});
+ widget.tableWidgetNodes->setItem(row, 2, new QTableWidgetItem {QString::number(globalNodePose->position->y)});
+ widget.tableWidgetNodes->setItem(row, 3, new QTableWidgetItem {QString::number(getYawAngle(globalNodePose))});
+ // scene
+ QGraphicsEllipseItem* graphicsItem = dynamic_cast<QGraphicsEllipseItem*>(new GraphVisualizerGraphicsEllipseItem
+ {
+ *this, node->getName(),
+ globalNodePose->position->x, -globalNodePose->position->y, 0, 0
+ });
+ // auto graphicsItem= scene->addEllipse(node->x,-node->y,0,0);
+ scene->addItem(graphicsItem);
+ // setToolTip on graphicsItem does not work
+ graphicsItem->setZValue(std::numeric_limits<qreal>::max());
+ dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString {"Node:"} +QString::fromStdString(node->getName()));
+
+ // data
+ NodeData data {node, globalNodePose, graphicsItem, row, false};
+ nodes.insert({nodeId, data});
+ }
+
+ updateNode(nodeId);
+ }
+
+
+ void LocationGraphEditorWidgetController::clearEdges()
+ {
+ for (auto& edge : edges)
+ {
+ // remove from graphics scene
+ scene->removeItem(edge.second.graphicsItem);
+ delete edge.second.graphicsItem;
+ // remove from debug layer
+ debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(edge.first));
+ }
+
+ // clear table widget
+ widget.tableWidgetEdges->clearContents();
+ widget.tableWidgetEdges->setRowCount(0);
+ // clear data structures
+ edges.clear();
+ }
+
+
+ void LocationGraphEditorWidgetController::clearGraph()
+ {
+ // remove from debug layer
+ for (auto& edge : edges)
+ {
+ debugDrawer->removeLineVisu(debugDrawerLayerName, iceName(edge.first));
+ }
+
+ for (auto& node : nodes)
+ {
+ debugDrawer->removeArrowVisu(debugDrawerLayerName, iceName(node.first));
+ debugDrawer->removeTextVisu(debugDrawerLayerName, iceName(node.first) + "text");
+ debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(node.first));
+ }
+
+ // clear scene
+ scene->clear();
+ // clear table widgets
+ widget.tableWidgetEdges->clearContents();
+ widget.tableWidgetEdges->setRowCount(0);
+ widget.tableWidgetNodes->clearContents();
+ widget.tableWidgetNodes->setRowCount(0);
+ // clear data structures
+
+ edges.clear();
+ nodes.clear();
+ }
+
+
+ void LocationGraphEditorWidgetController::resetHighlight()
+ {
+ for (auto& edge : edges)
+ {
+ if (edge.second.highlighted)
+ {
+ edge.second.highlighted = false;
+ updateEdge(edge.first);
+ }
+ }
+
+ for (auto& node : nodes)
+ {
+ if (node.second.highlighted)
+ {
+ node.second.highlighted = false;
+ updateNode(node.first);
+ }
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::updateEdge(const EdgeId& id)
+ {
+ const EdgeData& data = edges.at(id);
+ auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
+ QColor qColor;
+ qColor.setRedF(color.r);
+ qColor.setGreenF(color.g);
+ qColor.setBlueF(color.b);
+
+ auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
+ armarx::Vector3Ptr posStart = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(nodes.at(id.first).pose->position)->toEigen());
+ posStart->z += posStart->z < 1 ? 10 : 0;
+ armarx::Vector3Ptr posEnd = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(nodes.at(id.second).pose->position)->toEigen());
+ posEnd->z += posEnd->z < 1 ? 10 : 0;
+ // debug layer
+ debugDrawer->setLineVisu(debugDrawerLayerName,
+ iceName(id),
+ posStart,
+ posEnd,
+ lineWidth,
+ color);
+
+ data.graphicsItem->setLine(nodes[id.first].pose->position->x, -nodes[id.first].pose->position->y,
+ nodes[id.second].pose->position->x, -nodes[id.second].pose->position->y);
+
+ // scene
+ QPen pen {qColor};
+ pen.setWidthF(lineWidth * SCENE_LINE_SCALE_FACTOR);
+ data.graphicsItem->setPen(pen);
+ // table
+ QFont font {};
+ font.setBold(data.highlighted);
+ widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 0)->setData(Qt::BackgroundRole, qColor);
+ widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 0)->setFont(font);
+ widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 1)->setData(Qt::BackgroundRole, qColor);
+ widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 1)->setFont(font);
+ }
+
+
+ void LocationGraphEditorWidgetController::updateNode(const NodeId& id)
+ {
+ NodeData& data = nodes.at(id);
+
+ if (editStartNodeNext)
+ {
+ widget.editStartNodeId->setText(QString::fromStdString(data.node->getId()));
+ widget.editStartNodeName->setText(QString::fromStdString(data.node->getName()));
+ }
+ else
+ {
+ widget.editEndNodeId->setText(QString::fromStdString(data.node->getId()));
+ widget.editEndNodeName->setText(QString::fromStdString(data.node->getName()));
+ }
+
+ setEditFields(data);
+
+ editStartNodeNext = !editStartNodeNext;
+
+ auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
+ QColor qColor;
+ qColor.setRedF(color.r);
+ qColor.setGreenF(color.g);
+ qColor.setBlueF(color.b);
+
+ auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
+
+ // debug layer
+ float yaw = getYawAngle(data.pose) / 180 * M_PI;
+ Eigen::AngleAxisf aa(yaw, Eigen::Vector3f(0, 0, 1));
+ Eigen::Vector3f dir {0, 1, 0};
+ dir = aa.toRotationMatrix() * dir;
+ debugDrawer->setArrowVisu(debugDrawerLayerName, iceName(id), data.pose->position,
+ new armarx::Vector3(dir),
+ armarx::DrawColor {0, 0, 1, 1},
+ 100,
+ lineWidth);
+ debugDrawer->setTextVisu(debugDrawerLayerName, iceName(id) + "text", data.node->getName(), data.pose->position, armarx::DrawColor {0, 0, 1, 1}, 10);
+
+ // scene
+ data.graphicsItem->setPen(QPen {qColor});
+ data.graphicsItem->setBrush(QBrush {qColor});
+ data.graphicsItem->setRect(data.pose->position->x - lineWidth * SCENE_NODES_SCALE_FACTOR / 2,
+ -data.pose->position->y - lineWidth * SCENE_NODES_SCALE_FACTOR / 2,
+ lineWidth * SCENE_NODES_SCALE_FACTOR,
+ lineWidth * SCENE_NODES_SCALE_FACTOR);
+ // table
+ QFont font {};
+ font.setBold(data.highlighted);
+ for (int i = 0; i <= 3; ++i)
+ {
+ widget.tableWidgetNodes->item(data.tableWidgetNodesIndex, i)->setData(Qt::BackgroundRole, qColor);
+ widget.tableWidgetNodes->item(data.tableWidgetNodesIndex, i)->setFont(font);
+ }
+
+ // highlight all edges between highlighted nodes
+ std::vector<memoryx::GraphNodeBasePtr> highlightedNodes;
+ for (const auto& nodeData : nodes)
+ {
+ if (nodeData.second.highlighted)
+ {
+ highlightedNodes.push_back(nodeData.second.node);
+ }
+ }
+
+ for (auto& edge : edges)
+ {
+ if (edge.second.highlighted)
+ {
+ edge.second.highlighted = false;
+ updateEdge(edge.first);
+ }
+ }
+
+ for (const auto& nodeFrom : highlightedNodes)
+ {
+ for (const auto& nodeTo : highlightedNodes)
+ {
+ const auto nodeFromId = nodeFrom->getId();
+ const auto nodeToId = nodeTo->getId();
+ if (hasEdge(nodeFromId, nodeToId))
+ {
+ auto edgeId = toEdge(nodeFromId, nodeToId);
+ auto& edge = edges[edgeId];
+ edge.highlighted = true;
+ updateEdge(edgeId);
+ }
+ }
+ }
+ }
+
+
+ float LocationGraphEditorWidgetController::getYawAngle(const armarx::PoseBasePtr& pose) const
+ {
+ Eigen::Vector3f rpy;
+ armarx::PosePtr p = armarx::PosePtr::dynamicCast(pose);
+ VirtualRobot::MathTools::eigen4f2rpy(p->toEigen(), rpy);
+ return VirtualRobot::MathTools::rad2deg(rpy[2]);
+ }
+
+
+ void LocationGraphEditorWidgetController::highlightEdge(const std::string& node1Id, const std::string& node2Id, bool highlighted)
+ {
+ if (!hasEdge(node1Id, node2Id))
+ {
+ ARMARX_WARNING << "No edge for: " << node1Id << " and " << node2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
+ return;
+ }
+
+ EdgeId edge = toEdge(node1Id, node2Id);
+
+ if (edges.at(edge).highlighted != highlighted)
+ {
+ edges.at(edge).highlighted = highlighted;
+ updateEdge(edge);
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::highlightNode(const std::string& nodeId, bool highlighted)
+ {
+ if (!hasNode(nodeId))
+ {
+ ARMARX_WARNING << "No node: " << nodeId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
+ return;
+ }
+
+ if (nodes.at(nodeId).highlighted != highlighted)
+ {
+ nodes.at(nodeId).highlighted = highlighted;
+ updateNode(nodeId);
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::nodeTableDoubleClicked(int row, int)
+ {
+ auto nodeIt = std::find_if(nodes.cbegin(), nodes.cend(), [&](const std::pair<std::string, NodeData>& d)
+ {
+ // return d.second.node->getName() == widget.tableWidgetNodes->item(row, 0)->text().toStdString();
+ return d.second.tableWidgetNodesIndex == row;
+ });
+ auto nodeId = nodeIt->second.node->getId();
+ nodeDoubleClicked(nodeId);
+ }
+
+
+ void LocationGraphEditorWidgetController::edgeTableDoubleClicked(int row, int)
+ {
+ auto edgeIt = std::find_if(edges.cbegin(), edges.cend(), [&](const std::pair<EdgeId, EdgeData>& d)
+ {
+ return d.second.tableWidgetEdgesIndex == row;
+ });
+
+ edgeDoubleClicked(edgeIt->first);
+ }
+
+
+
+ void LocationGraphEditorWidgetController::nodeDoubleClicked(NodeId id)
+ {
+ nodes.at(id).highlighted ^= true;
+ updateNode(id);
+ }
+
+
+ void LocationGraphEditorWidgetController::edgeDoubleClicked(EdgeId id)
+ {
+ // edges.at(id).highlighted ^= true;
+ // updateEdge(id);
+ bool highlight = !nodes.at(id.first).highlighted && !nodes.at(id.second).highlighted;
+ nodes.at(id.first).highlighted = highlight;
+ nodes.at(id.second).highlighted = highlight;
+ updateNode(id.first);
+ updateNode(id.second);
+ }
+
+
+ void LocationGraphEditorWidgetController::redraw()
+ {
+ drawScene();
+ }
+
+
+ void LocationGraphEditorWidgetController::setEditFields(const NodeData& nodeData)
+ {
+ widget.editNodeId->setText(QString::fromStdString(nodeData.node->getId()));
+ widget.editSceneName->setText(QString::fromStdString(nodeData.node->getScene()));
+ widget.editNodeName->setText(QString::fromStdString(nodeData.node->getName()));
+ widget.editFrameName->setText(QString::fromStdString(nodeData.pose->frame));
+ widget.editAgentName->setText(QString::fromStdString(nodeData.pose->agent));
+
+ Eigen::Vector3f rpy;
+ VirtualRobot::MathTools::eigen4f2rpy(nodeData.pose->toEigen(), rpy);
+ widget.spinBoxX->setValue(nodeData.pose->position->x);
+ widget.spinBoxY->setValue(nodeData.pose->position->y);
+ widget.spinBoxZ->setValue(nodeData.pose->position->z);
+
+ widget.spinBoxRoll->setValue(VirtualRobot::MathTools::rad2deg(rpy[0]));
+ widget.spinBoxPitch->setValue(VirtualRobot::MathTools::rad2deg(rpy[1]));
+ widget.spinBoxYaw->setValue(VirtualRobot::MathTools::rad2deg(rpy[2]));
+ }
+
+
+ void LocationGraphEditorWidgetController::refreshGraph()
+ {
+ clearGraph();
+ drawScene();
+ }
+
+
+ void LocationGraphEditorWidgetController::transformView()
+ {
+ double d = widget.viewZoomFactor->value();
+ widget.graphicsViewGraph->setTransform(QTransform::fromScale(d, d).rotate(viewAngle));
+ }
+
+
+ void LocationGraphEditorWidgetController::viewRotatedClock()
+ {
+ viewAngle = std::fmod(viewAngle + VIEW_ROTATE_STEP_SIZE_CC, 360);
+ transformView();
+ }
+
+
+ void LocationGraphEditorWidgetController::viewRotatedCounterClock()
+ {
+ viewAngle = std::fmod(viewAngle + 360 - VIEW_ROTATE_STEP_SIZE_CC, 360);
+ transformView();
+ }
+
+
+ void LocationGraphEditorWidgetController::adjustView()
+ {
+ float maxX = std::numeric_limits<float>::min();
+ float minX = std::numeric_limits<float>::max();
+ float maxY = std::numeric_limits<float>::min();
+ float minY = std::numeric_limits<float>::max();
+
+ // search bounding box
+ for (const auto& node : nodes)
+ {
+ maxX = (maxX < node.second.pose->position->x) ? node.second.pose->position->x : maxX;
+ minX = (minX > node.second.pose->position->x) ? node.second.pose->position->x : minX;
+ maxY = (maxY < node.second.pose->position->y) ? node.second.pose->position->y : maxY;
+ minY = (minY > node.second.pose->position->y) ? node.second.pose->position->y : minY;
+ }
+
+ auto deltaX = maxX - minX; // >=0
+ auto deltaY = maxY - minY; // >=0
+
+ // compare ratio of graph and view. if both horizontal (vertical) ->rotate to 0 or 180 (90,270)
+ if (std::signbit(deltaX / deltaY - 1) == std::signbit(widget.graphicsViewGraph->width() / widget.graphicsViewGraph->height() - 1))
+ {
+ // same => rotate to 0 or 180
+ viewAngle = (viewAngle < std::abs(180 - viewAngle)) ? 0 : 180;
+ // set zoom => update
+ widget.viewZoomFactor->setValue(std::min(widget.graphicsViewGraph->width() / deltaX,
+ widget.graphicsViewGraph->height() / deltaY) * 0.9);
+ }
+ else
+ {
+ // different rotate to 90 or 270
+ viewAngle = (std::abs(90 - viewAngle) < std::abs(270 - viewAngle)) ? 90 : 270;
+ // set zoom => update
+ widget.viewZoomFactor->setValue(std::min(widget.graphicsViewGraph->width() / deltaY,
+ widget.graphicsViewGraph->height() / deltaX) * 0.9);
+ }
+
+ }
+
+
+ bool LocationGraphEditorWidgetController::addNewEdge(const std::string& fromId, const std::string& toId)
+ {
+ std::string errorMsg;
+
+ if (!graphSeg->hasEntityById(fromId))
+ {
+ errorMsg = "start node with Id '" + fromId + "' not found in segment";
+ }
+ else if (!graphSeg->hasEntityById(toId))
+ {
+ errorMsg = "end node with Id '" + toId + "' not found in segment";
+
+ }
+ else if (fromId == toId)
+ {
+ errorMsg = "starting and ending node are the same";
+ }
+ else
+ {
+ auto fromNode = graphSeg->getNodeById(fromId);
+ for (const auto& adjacent : fromNode->getAdjacentNodes())
+ {
+ if (toId == adjacent->getId())
+ {
+ errorMsg = "edge '" + fromNode->getName() + "' -> '" + adjacent->getName() + "' already exists";
+ break;
+ }
+ }
+ }
+
+ if (errorMsg.empty())
+ {
+ widget.labelAddEdgeStatus->setText(QString::fromStdString("Ok"));
+ graphSeg->addEdge(fromId, toId);
+ gnpr->forceRefetch(fromId);
+ gnpr->forceRefetch(toId);
+ addEdge(fromId, toId);
+ updateNode(fromId);
+ updateNode(toId);
+ widget.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : lime; }");
+ }
+ else
+ {
+ ARMARX_WARNING << errorMsg;
+ widget.labelAddEdgeStatus->setText(QString::fromStdString(errorMsg));
+ widget.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : orange; }");
+ }
+
+ return errorMsg.empty();
+ }
+
+
+ void LocationGraphEditorWidgetController::addNewEdgeBoth()
+ {
+ std::string startId = widget.editStartNodeId->text().toStdString();
+ std::string endId = widget.editEndNodeId->text().toStdString();
+ addNewEdge(startId, endId);
+ addNewEdge(endId, startId);
+ }
+
+ void LocationGraphEditorWidgetController::addNewEdgeStartEnd()
+ {
+ std::string startId = widget.editStartNodeId->text().toStdString();
+ std::string endId = widget.editEndNodeId->text().toStdString();
+ addNewEdge(startId, endId);
+ }
+
+
+ void LocationGraphEditorWidgetController::addNewEdgeEndStart()
+ {
+ std::string startId = widget.editEndNodeId->text().toStdString();
+ std::string endId = widget.editStartNodeId->text().toStdString();
+ addNewEdge(startId, endId);
+ }
+
+
+ void LocationGraphEditorWidgetController::selectedSceneChanged(int i)
+ {
+ const auto current = widget.scenesComboBox->currentText();
+ if (!current.isEmpty())
+ {
+ lastSelectedSceneName = current;
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::updateSceneList()
+ {
+ auto scenes = graphSeg->getScenes();
+ widget.scenesComboBox->clear();
+ int idx = -1;
+
+ for (std::size_t i = 0; i < scenes.size(); i++)
+ {
+ const auto currentScene = QString::fromStdString(scenes[i]);
+ widget.scenesComboBox->addItem(currentScene);
+
+ if (currentScene == lastSelectedSceneName)
+ {
+ idx = i;
+ }
+ }
+
+ widget.scenesComboBox->setCurrentIndex(idx);
+ }
+
+
+ void LocationGraphEditorWidgetController::drawScene()
+ {
+ std::vector<std::string> highlightedNodes;
+ for (const auto& nodeData : nodes)
+ {
+ bool sameScene = widget.scenesComboBox->currentText().toStdString() == nodeData.second.node->getScene();
+ if (nodeData.second.highlighted && sameScene)
+ {
+ highlightedNodes.push_back(nodeData.first);
+ }
+ }
+
+ clearGraph();
+ auto graphNodes = graphSeg->getNodesByScene(widget.scenesComboBox->currentText().toStdString());
+
+ // add nodes
+ for (auto& node : graphNodes)
+ {
+ auto pos = armarx::FramedPosePtr::dynamicCast(node->getPose());
+
+ if (!pos || node->isMetaEntity())
+ {
+ continue;
+ }
+
+ addNode(node);
+ }
+
+ // add edges
+ for (auto& node : graphNodes)
+ {
+ auto nodeId = node->getId();
+
+ for (int i = 0; i < node->getOutdegree(); i++)
+ {
+ auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
+ ARMARX_CHECK_EXPRESSION(adjacent);
+ auto adjacentId = adjacent->getId();
+
+ addEdge(nodeId, adjacentId);
+ }
+ }
+
+ for (const auto& nodeId : highlightedNodes)
+ {
+ auto nodeIt = std::find_if(nodes.begin(), nodes.end(), [&](const std::pair<std::string, NodeData>& d)
+ {
+ return d.first == nodeId;
+ });
+
+ if (nodeIt != nodes.end())
+ {
+ nodeIt->second.highlighted = true;
+ updateNode(nodeIt->first);
+ }
+ }
+
+ adjustView();
+ }
+
+
+ void LocationGraphEditorWidgetController::selectScene()
+ {
+ // QString fi = QFileDialog::getOpenFileName(this, tr("Open Scene File"), QString(), tr("XML Files (*.xml)"));
+ // std::string xmlSceneFile = std::string(fi.toLatin1());
+ // loadScene(xmlSceneFile);
+ }
+
+
+ void LocationGraphEditorWidgetController::loadScene(const std::string& xmlFile)
+ {
+ // VirtualRobot::ScenePtr SceneIO::loadScene(const std::string& xmlFile)
+ // {
+ // load file
+ std::ifstream in(xmlFile.c_str());
+
+ if (!in.is_open())
+ {
+ ARMARX_WARNING << "Could not open XML file:" << xmlFile;
+ return;
+ }
+
+ std::stringstream buffer;
+ buffer << in.rdbuf();
+ std::string sceneXML(buffer.str());
+ std::filesystem::path filenameBaseComplete(xmlFile);
+ std::filesystem::path filenameBasePath = filenameBaseComplete.parent_path();
+ std::string basePath = filenameBasePath.string();
+
+ in.close();
+
+ // VirtualRobot::ScenePtr res = createSceneFromString(robotXML, basePath);
+ // THROW_VR_EXCEPTION_IF(!res, "Error while parsing file " << xmlFile);
+
+ // return res;
+ // }
+ }
+
+
+ void LocationGraphEditorWidgetController::addKitchenGraph()
+ {
+ std::string scene {"GraphKitchen"};
+ graphSeg->clearScene(scene);
+
+ // if you insist on hardcoding scenes, use these convenience functions for improved readability:
+ auto addNode = [&](const std::string & name, float x, float y, float angle)
+ {
+ graphSeg->addNode(new ::memoryx::GraphNode {x, y, angle, name, scene});
+ ARMARX_INFO_S << "added node '" << name << "' at (" << x << ", " << y << ", " << angle << "rad)";
+ };
+
+ auto addEdges = [&](const std::string & nodeFromName, const std::string & nodeToName, bool bidirectional)
+ {
+ auto nodeFrom = graphSeg->getNodeFromSceneByName(scene, nodeFromName);
+ auto nodeTo = graphSeg->getNodeFromSceneByName(scene, nodeToName);
+ ARMARX_CHECK_EXPRESSION(nodeFrom);
+ ARMARX_CHECK_EXPRESSION(nodeTo);
+ ARMARX_INFO_S << "'" << nodeFrom->getName() << "' -> '" << nodeTo->getName() << "', status: " << graphSeg->addEdge(nodeFrom->getId(), nodeTo->getId());
+ if (bidirectional)
+ {
+ ARMARX_INFO_S << "'" << nodeTo->getName() << "' -> '" << nodeFrom->getName() << "', status: " << graphSeg->addEdge(nodeTo->getId(), nodeFrom->getId());
+ }
+ };
+
+ // ex
+ addNode("initialnode", 2900, 7000, 0);
+ addNode("sideboard", 3400, 7000, 0);
+ addEdges("initialnode", "sideboard", true);
+ }
+
+
+ void LocationGraphEditorWidgetController::addNewGraphNode()
+ {
+ Eigen::Matrix4f mat;
+ Eigen::Vector3f rpy;
+ Eigen::Vector3f pos;
+ rpy << VirtualRobot::MathTools::deg2rad(widget.spinBoxRoll->value()),
+ VirtualRobot::MathTools::deg2rad(widget.spinBoxPitch->value()),
+ VirtualRobot::MathTools::deg2rad(widget.spinBoxYaw->value());
+ pos << widget.spinBoxX->value(),
+ widget.spinBoxY->value(),
+ widget.spinBoxZ->value();
+ VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
+ armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
+ widget.editFrameName->text().toStdString(),
+ widget.editAgentName->text().toStdString());
+ memoryx::GraphNodePtr node = new memoryx::GraphNode(pose,
+ widget.editNodeName->text().toStdString(),
+ widget.editSceneName->text().toStdString());
+ auto entityId = graphSeg->addNode(node);
+ gnpr->forceRefetch(entityId);
+ node->setId(entityId);
+ if (widget.scenesComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
+ {
+ widget.editNodeId->setText(QString::fromStdString(entityId));
+ addNode(node);
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::editGraphNode()
+ {
+ Eigen::Matrix4f mat;
+ Eigen::Vector3f rpy;
+ Eigen::Vector3f pos;
+ rpy << VirtualRobot::MathTools::deg2rad(widget.spinBoxRoll->value()),
+ VirtualRobot::MathTools::deg2rad(widget.spinBoxPitch->value()),
+ VirtualRobot::MathTools::deg2rad(widget.spinBoxYaw->value());
+ pos << widget.spinBoxX->value(),
+ widget.spinBoxY->value(),
+ widget.spinBoxZ->value();
+ VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
+ armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
+ widget.editFrameName->text().toStdString(),
+ widget.editAgentName->text().toStdString());
+ memoryx::GraphNodePtr node = new memoryx::GraphNode(pose,
+ widget.editNodeName->text().toStdString(),
+ widget.editSceneName->text().toStdString());
+ auto id = widget.editNodeId->text().toStdString();
+
+ node->setId(id);
+ graphSeg->updateEntity(id, node);
+ gnpr->forceRefetch(id);
+ if (widget.scenesComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
+ {
+ addNode(node);
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::tableWidgetNodesCustomContextMenu(QPoint pos)
+ {
+ int row = widget.tableWidgetNodes->rowAt(pos.y());
+ auto nodeIt = std::find_if(nodes.begin(), nodes.end(), [&](const std::pair<std::string, NodeData>& d)
+ {
+ return d.second.tableWidgetNodesIndex == row;
+ });
+ QMenu menu;
+ QAction* deleteAction = menu.addAction("Delete Node");
+
+ if (menu.exec(QCursor::pos()) == deleteAction)
+ {
+ ARMARX_CHECK_EXPRESSION(nodeIt != nodes.end());
+ graphSeg->removeNode(nodeIt->second.node->getId());
+ drawScene();
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::tableWidgetEdgesCustomContextMenu(QPoint pos)
+ {
+ int row = widget.tableWidgetEdges->rowAt(pos.y());
+ auto edgeIt = std::find_if(edges.begin(), edges.end(), [&](const std::pair<EdgeId, EdgeData>& d)
+ {
+ return d.second.tableWidgetEdgesIndex == row;
+ });
+ QMenu menu;
+ QAction* deleteAction = menu.addAction("Delete Edge");
+
+ if (menu.exec(QCursor::pos()) == deleteAction)
+ {
+ ARMARX_CHECK_EXPRESSION(edgeIt != edges.end());
+ graphSeg->removeEdge(edgeIt->first.first, edgeIt->first.second);
+ drawScene();
+ }
+ }
+
+
+ bool MouseEventProcessor::eventFilter(QObject* obj, QEvent* event)
+ {
+ if (obj == gvw->widget.graphicsViewGraph && event->type() == QEvent::MouseButtonPress)
+ {
+ QMouseEvent* me = static_cast<QMouseEvent*>(event);
+ if (me->button() == Qt::LeftButton)
+ {
+ QPointF scenePoint = gvw->widget.graphicsViewGraph->mapToScene(me->pos());
+ scenePoint.setY(-scenePoint.y()); // not sure why
+
+ float minDist = std::numeric_limits<float>::max();
+ auto bestIt = gvw->nodes.cend();
+
+ for (auto it = gvw->nodes.cbegin(); it != gvw->nodes.cend(); ++it)
+ {
+ float deltaX = it->second.pose->position->x - scenePoint.x();
+ float deltaY = it->second.pose->position->y - scenePoint.y();
+ float dist = std::sqrt(deltaX * deltaX + deltaY * deltaY);
+
+ if (dist < minDist)
+ {
+ minDist = dist;
+ bestIt = it;
+ }
+ }
+
+ if (bestIt != gvw->nodes.cend())
+ {
+ gvw->nodeDoubleClicked(bestIt->first);
+ }
+ }
+ }
+ else if (event->type() == QEvent::Resize)
+ {
+ gvw->adjustView();
+ }
+ return QObject::eventFilter(obj, event);
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
new file mode 100644
index 0000000000000000000000000000000000000000..b87a5c2ab6a82461562a3c2f08903cbac9c2508f
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -0,0 +1,487 @@
+/*
+ * 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 Navigation::gui-plugins::LocationGraphEditorWidgetController
+ * @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 <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
+
+
+#include <MemoryX/components/PriorKnowledge/PriorKnowledge.h>
+#include <MemoryX/interface/gui/GraphVisualizerInterface.h>
+#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
+
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <QDialog>
+#include <QGraphicsScene>
+#include <QGraphicsLineItem>
+#include <QGraphicsEllipseItem>
+#include <QMainWindow>
+#include <QMouseEvent>
+
+#include <string>
+#include <map>
+#include <vector>
+#include <tuple>
+
+
+namespace armarx
+{
+ class GraphVisualizerGraphicsEllipseItem;
+ class GraphVisualizerGraphicsLineItem;
+
+ class LocationGraphEditorWidgetController;
+
+ class MouseEventProcessor;
+
+
+ /**
+ \page Navigation-GuiPlugins-LocationGraphEditor LocationGraphEditor
+ \brief The LocationGraphEditor allows visualizing ...
+
+ \image html LocationGraphEditor.png
+ The user can
+
+ API Documentation \ref LocationGraphEditorWidgetController
+
+ \see LocationGraphEditorGuiPlugin
+ */
+
+
+ /**
+ * \class LocationGraphEditorGuiPlugin
+ * \ingroup ArmarXGuiPlugins
+ * \brief LocationGraphEditorGuiPlugin brief description
+ *
+ * Detailed description
+ */
+
+ /**
+ * \class LocationGraphEditorWidgetController
+ * \brief LocationGraphEditorWidgetController brief one line description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT
+ LocationGraphEditorWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate < LocationGraphEditorWidgetController >
+ {
+ Q_OBJECT
+ friend class MouseEventProcessor;
+
+
+ public:
+
+ /// The type of node ids. (This type implies the node exists)
+ using NodeId = const std::string;
+
+ /// The type of edge ids. (This type implies the edge exists)
+ using EdgeId = const std::pair<const std::string, const std::string>;
+
+
+ static QString GetWidgetName();
+ static QIcon GetWidgetIcon();
+
+
+ explicit LocationGraphEditorWidgetController();
+ virtual ~LocationGraphEditorWidgetController() override;
+
+
+ //// @see ArmarXWidgetController::loadSettings()
+ void loadSettings(QSettings* settings) override;
+ /// @see ArmarXWidgetController::saveSettings()
+ void saveSettings(QSettings* settings) override;
+
+
+ QPointer<QDialog> getConfigDialog(QWidget* parent = nullptr) override;
+ void configured() override;
+
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+
+
+ //because the above load/save functions are *obviously* for "Save/Load Gui Config." :/
+ virtual void loadAutomaticSettings();
+ virtual void saveAutomaticSettings();
+
+
+ // slice interface implementation
+ bool hasEdge(const std::string& node1, const std::string& node2)
+ {
+ return (edges.find(toEdge(node1, node2)) != edges.end());
+ }
+ bool hasNode(const std::string& id)
+ {
+ return (nodes.find(id) != nodes.end());
+ }
+
+
+ public slots:
+
+ // slice interface implementation
+ void addEdge(const std::string& node1Id, const std::string& node2Id);
+ void addNode(const memoryx::GraphNodeBasePtr& node);
+
+ void highlightEdge(const std::string& node1Id, const std::string& node2Id, bool highlighted = true);
+ void highlightNode(const std::string& nodeId, bool highlighted = true);
+
+ void clearEdges();
+ void clearGraph();
+
+ void resetHighlight();
+
+ void redraw();
+ void refreshGraph();
+
+ void tableWidgetNodesCustomContextMenu(QPoint pos);
+ void tableWidgetEdgesCustomContextMenu(QPoint pos);
+
+
+ private slots:
+ /**
+ * @brief add kitchen graph (H2T Armar3a robot kitchen)
+ */
+ void addKitchenGraph();
+
+ void selectedSceneChanged(int i);
+ void updateSceneList();
+
+ void drawScene();
+
+ /**
+ * @brief Toggles the double clicked node's selection state.
+ * @param row Identifies the node.
+ */
+ void nodeTableDoubleClicked(int row, int);
+ /**
+ * @brief Toggles the double clicked edge's selection state.
+ * @param row Identifies the edge.
+ */
+ void edgeTableDoubleClicked(int row, int);
+
+ /**
+ * @brief Toggles the double clicked node's selection state.
+ * @param id Identifies the node.
+ */
+ void nodeDoubleClicked(NodeId id);
+
+ /**
+ * @brief Toggles the double clicked edge's selection state.
+ * @param id Identifies the edge.
+ */
+ void edgeDoubleClicked(EdgeId id);
+
+ /**
+ * @brief Rotates the view clockwise.
+ *
+ * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
+ */
+ void viewRotatedClock();
+
+ /**
+ * @brief Rotates the view counter clockwise.
+ *
+ * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
+ */
+ void viewRotatedCounterClock();
+
+ /**
+ * @brief Applies the current transforamtion to the view.
+ */
+ void transformView();
+
+ /**
+ * @brief Adjusts the view's zoom and rotation to display most of the graph.
+ */
+ void adjustView();
+
+ bool addNewEdge(const std::string& from, const std::string& to);
+ void addNewEdgeBoth();
+ void addNewEdgeStartEnd();
+ void addNewEdgeEndStart();
+
+ void addNewGraphNode();
+ void editGraphNode();
+
+
+ private:
+
+ /// Widget Form
+ Ui::LocationGraphEditorWidget widget;
+
+
+ /**
+ * @brief The NodeData struct holds data required for the node.
+ * The name is stored in the key used in the map nodes.
+ */
+ struct NodeData
+ {
+ /**
+ * @brief The Entity of the graph segment this struct represents
+ */
+ memoryx::GraphNodeBasePtr node;
+
+ /**
+ * @brief The pose drawn to debugDrawer.
+ */
+ armarx::FramedPosePtr pose;
+
+ /**
+ * @brief The ellipse in the scene.
+ */
+ QGraphicsEllipseItem* graphicsItem;
+
+ /**
+ * @brief The row in the table tableWidgetNodes.
+ */
+ int tableWidgetNodesIndex;
+
+ /**
+ * @brief Whether the node is highlighted.
+ */
+ bool highlighted;
+ };
+
+ /**
+ * @brief The EdgeData struct holds data required for the edge.
+ * The name is stored in the key used in the map edges.
+ */
+ struct EdgeData
+ {
+ /**
+ * @brief The line in the scene.
+ */
+ QGraphicsLineItem* graphicsItem;
+ /**
+ * @brief The row in the table tableWidgetEdges.
+ */
+ int tableWidgetEdgesIndex;
+
+ /**
+ * @brief Whether the edge is highlighted.
+ */
+ bool highlighted;
+ };
+
+
+ /**
+ * @brief Returns the EdgeId corresponding to two nodes.
+ * @param node1 First node id.
+ * @param node2 Second node id.
+ * @return The EdgeId corresponding to two nodes.
+ */
+ static EdgeId toEdge(const std::string& node1, const std::string& node2)
+ {
+ return EdgeId {node1, node2};
+ }
+
+ /**
+ * @brief Updates an edge.
+ * @param The edge to update.
+ */
+ void updateEdge(const EdgeId& id);
+
+ /**
+ * @brief Updates a node.
+ * @param The node to update.
+ */
+ void updateNode(const NodeId& id);
+
+ void setEditFields(const NodeData& node);
+
+ /**
+ * @brief The topic name used by debugDrawer.
+ */
+ std::string debugDrawerTopicName;
+
+ /**
+ * @brief Used to draw onto debug layers.
+ */
+ ::armarx::DebugDrawerInterfacePrx debugDrawer;
+
+ /**
+ * @brief The config dialog.
+ */
+ QPointer<SimpleConfigDialog> dialog;
+
+ float getYawAngle(const armarx::PoseBasePtr& pose) const;
+
+ /**
+ * @brief The scene displayed in the widget.
+ *
+ * For y coordinates -pos->y is used to mirror the scene on the y axis.
+ * If pos->y would be used the graph displayed in the scene would not
+ * match the graph drawn to the debug layer.
+ */
+ QPointer<QGraphicsScene> scene;
+
+ /**
+ * @brief The nodes.
+ */
+ std::map<std::string, NodeData> nodes;
+
+ /**
+ * @brief The edges.
+ */
+ std::map<EdgeId, EdgeData> edges;
+
+ /**
+ * @brief The view's rotation angle.
+ */
+ qreal viewAngle;
+
+ /**
+ * @brief The layer to draw on.
+ */
+ std::string debugDrawerLayerName;
+
+ bool editStartNodeNext;
+
+ std::string priorKnowledgeProxyName;
+ memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx;
+ memoryx::GraphNodePoseResolverInterfacePrx gnpr;
+ memoryx::GraphMemorySegmentBasePrx graphSeg;
+ QSettings settings;
+ QString lastSelectedSceneName;
+
+ /**
+ * selectScene(): private function called when button load is pushed, and calls the function loadScene()
+ */
+ void selectScene();
+
+ /**
+ * @brief loadScene Private function that parses XML file to load a scene
+ * @param xmlFile
+ */
+ void loadScene(const std::string& xmlFile);
+
+
+ friend class GraphVisualizerGraphicsEllipseItem;
+ friend class GraphVisualizerGraphicsLineItem;
+
+
+ private:
+
+ std::string memoryNameSystemName = "MemoryNameSystem";
+ armem::client::MemoryNameSystem memoryNameSystem;
+
+ };
+
+
+ /**
+ * @brief Required to override the double click event. This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsEllipseItem : public QGraphicsEllipseItem
+ {
+ public:
+ using NodeId = LocationGraphEditorWidgetController::NodeId;
+
+ GraphVisualizerGraphicsEllipseItem(LocationGraphEditorWidgetController& visuWidget, NodeId name, qreal x, qreal y, qreal width, qreal height, QGraphicsItem* parent = nullptr):
+ QGraphicsEllipseItem {x, y, width, height, parent},
+ id {name},
+ parentVisuWidget(visuWidget)
+ {
+ }
+
+ ~GraphVisualizerGraphicsEllipseItem() override
+ {
+ }
+ protected:
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override
+ {
+ parentVisuWidget.nodeDoubleClicked(id);
+ }
+ private:
+ /**
+ * @brief Required to identify the element.
+ */
+ const NodeId id;
+
+ /**
+ * @brief Required to call nodeDoubleClicked on it. (This class is no QObject so it does not support signals)
+ */
+ LocationGraphEditorWidgetController& parentVisuWidget;
+ };
+
+
+
+ /**
+ * @brief Required to override the double click event. This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsLineItem : public QGraphicsLineItem
+ {
+ public:
+ using EdgeId = LocationGraphEditorWidgetController::EdgeId;
+
+ GraphVisualizerGraphicsLineItem(LocationGraphEditorWidgetController& visuWidget, EdgeId name, qreal x1, qreal y1, qreal x2, qreal y2, QGraphicsItem* parent = 0):
+ QGraphicsLineItem {x1, y1, x2, y2, parent},
+ id {name},
+ parentVisuWidget(visuWidget)
+ {
+ }
+
+ ~GraphVisualizerGraphicsLineItem() override
+ {
+ }
+ signals:
+ protected:
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override
+ {
+ parentVisuWidget.edgeDoubleClicked(id);
+ }
+ private:
+ /**
+ * @brief Required to identify the element.
+ */
+ const EdgeId id;
+
+ /// Required to call edgeDoubleClicked on it. (This class is no QObject so it does not support signals)
+ LocationGraphEditorWidgetController& parentVisuWidget;
+ };
+
+
+ class MouseEventProcessor : public QObject
+ {
+ Q_OBJECT
+ public:
+
+ MouseEventProcessor(LocationGraphEditorWidgetController* gvw) : gvw(gvw) {}
+
+
+ protected:
+
+ LocationGraphEditorWidgetController* gvw;
+ bool eventFilter(QObject* obj, QEvent* event) override;
+
+ };
+
+}