diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index d83500993b1851016485e1ad140cb4f1c87a9225..bce79ae3d6c1222928d2b87ea764f0c24a6527e1 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -26,6 +26,16 @@
namespace armarx::nav::graph
{
+ std::string VertexAttribs::getName() const
+ {
+ return aron.locationID.entityName;
+ }
+
+ Eigen::Matrix4f VertexAttribs::getPose() const
+ {
+ return aron.globalRobotPose;
+ }
+
}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index 10ec0e29a5718bc22471125791323c080bc93b2a..dd5704bcbdb82f65fa28189e249635662f0f3ea1 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -33,6 +33,9 @@ namespace armarx::nav::graph
struct VertexAttribs : public semrel::ShapeVertex
{
armarx::nav::graph::arondto::Vertex aron;
+
+ std::string getName() const;
+ Eigen::Matrix4f getPose() const;
};
struct EdgeAttribs
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index 1c9c13a4964b7ab9230750b3b83c53018be13c4b..3d865290897959ee93c8edb876c21f3cdcc94061 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -12,9 +12,25 @@ armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
# do not rename this variable, it is used in armarx_gui_library()...
set(SOURCES
LocationGraphEditorWidgetController.cpp
+
+ FunctionalEventFilter.cpp
+ GraphScene.cpp
+ GuiGraph.cpp
+
+ widgets/EdgeTableWidget.cpp
+ widgets/VertexDataWidget.cpp
+ widgets/VertexTableWidget.cpp
)
set(HEADERS
LocationGraphEditorWidgetController.h
+
+ FunctionalEventFilter.h
+ GraphScene.h
+ GuiGraph.h
+
+ widgets/EdgeTableWidget.h
+ widgets/VertexDataWidget.h
+ widgets/VertexTableWidget.h
)
set(GUI_UIS
LocationGraphEditorWidget.ui
@@ -29,24 +45,14 @@ set(COMPONENT_LIBS
# RobotAPI
armem
- # MemoryX
- MemoryXCore
- MemoryXMemoryTypes
+ Navigation::Location
+ Navigation::Graph
)
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)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1ffe763d0c7060d2c8de191daa01c5ca3f1604f7
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.cpp
@@ -0,0 +1,18 @@
+#include "FunctionalEventFilter.h"
+
+
+namespace simox::gui
+{
+
+ FunctionalEventFilter::FunctionalEventFilter(Function&& function) :
+ function(function)
+ {
+ }
+
+
+ bool FunctionalEventFilter::eventFilter(QObject* obj, QEvent* event)
+ {
+ return function(obj, event);
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..3513a795e4509898fb747d6653e0f92ccf87b30d
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <QObject>
+
+#include <functional>
+
+class QEvent;
+
+
+namespace simox::gui
+{
+
+ class FunctionalEventFilter : public QObject
+ {
+ Q_OBJECT
+
+ public:
+
+ using Function = std::function<bool(QObject* obj, QEvent* event)>;
+
+ FunctionalEventFilter(Function&& function);
+
+
+ protected:
+
+ bool eventFilter(QObject* obj, QEvent* event) override;
+
+ Function function;
+
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b927dcf1250f9e929e0bd6d4d95dac5bebff42b
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
@@ -0,0 +1,165 @@
+#include "GraphScene.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SemanticObjectRelations/Shapes/Shape.h>
+
+#include <Eigen/Core>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ QGraphicsEllipseItem*
+ GraphScene::addVertex(const graph::Graph::Vertex& vertex)
+ {
+ const Eigen::Matrix4f& pose = vertex.attrib().getPose();
+
+ // To capture by copy
+ semrel::ShapeID vertexID = vertex.objectID();
+
+ GraphVisualizerGraphicsEllipseItem* item = new GraphVisualizerGraphicsEllipseItem
+ {
+ [this, vertexID]() { emit vertexSelected(vertexID); },
+ static_cast<qreal>(pose(0, 3)),
+ static_cast<qreal>(- pose(1, 3)),
+ 0, 0
+ };
+ addItem(item);
+
+ // setToolTip on graphicsItem does not work
+ item->setZValue(std::numeric_limits<qreal>::max());
+ // dynamic_cast<QGraphicsItem*>(item)->setToolTip(QString::fromStdString("Vertex '" + name + "'"));
+ item->setToolTip(QString::fromStdString("Vertex '" + vertex.attrib().getName() + "'"));
+
+ return item;
+ }
+
+
+ QGraphicsLineItem*
+ GraphScene::addEdge(const graph::Graph::Edge& edge)
+ {
+ semrel::ShapeID sourceID = edge.sourceObjectID();
+ semrel::ShapeID targetID = edge.targetObjectID();
+
+ Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
+ Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
+
+ GraphVisualizerGraphicsLineItem* item = new GraphVisualizerGraphicsLineItem
+ {
+ [this, sourceID, targetID]() { emit edgeSelected(sourceID, targetID); },
+ sourcePose(0, 3), - sourcePose(1, 3),
+ targetPose(0, 3), - targetPose(1, 3)
+ };
+ addItem(item);
+
+ // setToolTip on item does not work
+ std::stringstream toolTip;
+ toolTip << "Edge '" << edge.source().attrib().getName()
+ << "' -> '" << edge.target().attrib().getName();
+ item->setToolTip(QString::fromStdString(toolTip.str()));
+
+ return item;
+ }
+
+
+ void GraphScene::updateVertex(GuiGraph::Vertex& vertex)
+ {
+ QGraphicsEllipseItem* item = vertex.attrib().graphicsItem;
+ ARMARX_CHECK_NOT_NULL(item);
+
+ const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
+
+ QColor color = vertex.attrib().highlighted ? colorSelected : colorDefault;
+ double lineWidth = vertex.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
+
+ item->setPen(QPen {color});
+ item->setBrush(QBrush {color});
+ item->setRect( pose(0, 3) - lineWidth * verticesScaleFactor / 2,
+ - pose(1, 3) - lineWidth * verticesScaleFactor / 2,
+ lineWidth * verticesScaleFactor,
+ lineWidth * verticesScaleFactor);
+ }
+
+
+ void GraphScene::updateEdge(GuiGraph::Edge& edge)
+ {
+ QGraphicsLineItem* item = edge.attrib().graphicsItem;
+ ARMARX_CHECK_NOT_NULL(item);
+
+ Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
+ Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
+
+ QColor color = edge.attrib().highlighted ? colorSelected : colorDefault;
+ double lineWidth = edge.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
+
+ QPen pen {color};
+ pen.setWidthF(lineWidth * lineScaleFactor);
+
+ item->setPen(pen);
+ item->setLine(sourcePose(0, 3), - sourcePose(1, 3),
+ targetPose(0, 3), - targetPose(1, 3));
+ }
+
+
+ void GraphScene::removeVertex(QGraphicsEllipseItem*& item)
+ {
+ removeItem(item);
+ delete item;
+ item = nullptr;
+ }
+
+
+ void GraphScene::removeEdge(QGraphicsLineItem*& item)
+ {
+ removeItem(item);
+ delete item;
+ item = nullptr;
+ }
+
+
+ GraphVisualizerGraphicsEllipseItem::GraphVisualizerGraphicsEllipseItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x, qreal y,
+ qreal width, qreal height,
+ QGraphicsItem* parent) :
+ QGraphicsEllipseItem {x, y, width, height, parent},
+ onDoubleClicked{onDoubleClicked}
+ {
+ }
+
+
+ GraphVisualizerGraphicsEllipseItem::~GraphVisualizerGraphicsEllipseItem()
+ {
+ }
+
+
+ void GraphVisualizerGraphicsEllipseItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ {
+ onDoubleClicked();
+ }
+
+
+
+ GraphVisualizerGraphicsLineItem::GraphVisualizerGraphicsLineItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x1, qreal y1, qreal x2, qreal y2,
+ QGraphicsItem* parent) :
+ QGraphicsLineItem {x1, y1, x2, y2, parent},
+ onDoubleClicked{onDoubleClicked}
+ {
+ }
+
+
+ GraphVisualizerGraphicsLineItem::~GraphVisualizerGraphicsLineItem()
+ {
+ }
+
+
+ void GraphVisualizerGraphicsLineItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ {
+ onDoubleClicked();
+ }
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
new file mode 100644
index 0000000000000000000000000000000000000000..514bd6a403e1d21b43e60bbfdf9d3c14988b8220
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
@@ -0,0 +1,119 @@
+#pragma once
+
+#include "GuiGraph.h"
+
+#include <armarx/navigation/graph/Graph.h>
+
+#include <QGraphicsScene>
+#include <QGraphicsEllipseItem>
+#include <QGraphicsLineItem>
+
+#include <functional>
+
+
+namespace semrel
+{
+ struct ShapeID;
+}
+namespace armarx::nav::locgrapheditor
+{
+ class GraphScene : public QGraphicsScene
+ {
+ Q_OBJECT
+
+ public:
+
+ using QGraphicsScene::QGraphicsScene;
+
+ QGraphicsEllipseItem* addVertex(const graph::Graph::Vertex& vertex);
+ QGraphicsLineItem* addEdge(const graph::Graph::Edge& Edge);
+
+ void updateVertex(GuiGraph::Vertex& vertex);
+ void updateEdge(GuiGraph::Edge& edge);
+
+ void removeVertex(QGraphicsEllipseItem*& item);
+ void removeEdge(QGraphicsLineItem*& item);
+
+
+ public slots:
+
+
+ signals:
+
+ void vertexSelected(const semrel::ShapeID& vertexID);
+ void edgeSelected(const semrel::ShapeID& sourceID, const semrel::ShapeID& targetID);
+
+
+ public:
+
+ double lineWidthDefault = 5;
+ double lineWidthSelected = 10;
+
+ double sceneScaleFactor = 2;
+ double verticesScaleFactor = 3.0 * sceneScaleFactor;
+ double lineScaleFactor = 1.0 * sceneScaleFactor;
+
+ QColor colorDefault = QColor::fromRgb(128, 128, 255);
+ QColor colorSelected = QColor::fromRgb(255, 128, 0);
+
+ };
+
+
+
+ /**
+ * @brief Required to override the double click event.
+ * This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsEllipseItem : public QGraphicsEllipseItem
+ {
+ public:
+
+ GraphVisualizerGraphicsEllipseItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x, qreal y, qreal width, qreal height,
+ QGraphicsItem* parent = nullptr);
+
+ ~GraphVisualizerGraphicsEllipseItem() override;
+
+
+ protected:
+
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
+
+
+ std::function<void(void)> onDoubleClicked;
+
+ };
+
+
+
+ /**
+ * @brief Required to override the double click event.
+ * This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsLineItem : public QGraphicsLineItem
+ {
+ public:
+
+ GraphVisualizerGraphicsLineItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x1, qreal y1,
+ qreal x2, qreal y2,
+ QGraphicsItem* parent = nullptr);
+
+ ~GraphVisualizerGraphicsLineItem() override;
+
+
+ protected:
+
+
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
+
+
+ std::function<void(void)> onDoubleClicked;
+
+ };
+
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..377f726e295e0918776d19eb7154ed72f06b2ff1
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
@@ -0,0 +1,73 @@
+/*
+ * 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 "GuiGraph.h"
+
+#include <SimoxUtility/math/convert/rad_to_deg.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+}
+
+namespace armarx::nav
+{
+
+ float locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
+ {
+ Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(pose);
+ return simox::math::rad_to_deg(rpy[2]);
+ }
+
+ auto locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
+ {
+ GuiGraph gui;
+ for (auto v : nav.vertices())
+ {
+ gui.addVertex(v.objectID(), { v.attrib() });
+ }
+ for (auto e : nav.edges())
+ {
+ gui.addEdge(e.sourceObjectID(), e.targetObjectID(), { e.attrib() });
+ }
+ return gui;
+ }
+
+
+ nav::graph::Graph locgrapheditor::fromGuiGraph(const GuiGraph& gui)
+ {
+ graph::Graph nav;
+ for (auto v : gui.vertices())
+ {
+ nav.addVertex(v.objectID(), { v.attrib() });
+ }
+ for (auto e : gui.edges())
+ {
+ nav.addEdge(e.sourceObjectID(), e.targetObjectID(), { e.attrib() });
+ }
+ return nav;
+ }
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
new file mode 100644
index 0000000000000000000000000000000000000000..f0b6e0bba09fcc68240f42eb24f9798220092950
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
@@ -0,0 +1,86 @@
+/*
+ * 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/>.
+ *
+ * @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/Graph.h>
+
+#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
+
+
+class QGraphicsEllipseItem;
+class QGraphicsLineItem;
+class QTableWidgetItem;
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ /**
+ * @brief The NodeData struct holds data required for the node.
+ * The name is stored in the key used in the map vertices.
+ */
+ struct VertexData : public nav::graph::VertexAttribs
+ {
+ /// The ellipse in the scene.
+ QGraphicsEllipseItem* graphicsItem = nullptr;
+
+ /// The item in the table tableWidgetVertices.
+ QTableWidgetItem* tableWidgetItem = nullptr;
+
+ /// Whether the node is highlighted.
+ bool highlighted = false;
+ };
+
+
+ /**
+ * @brief The EdgeData struct holds data required for the edge.
+ * The name is stored in the key used in the map edges.
+ */
+ struct EdgeData : public nav::graph::EdgeAttribs
+ {
+ /// The line in the scene.
+ QGraphicsLineItem* graphicsItem = nullptr;
+
+ /// The item in the table tableWidgetEdges.
+ QTableWidgetItem* tableWidgetItem = nullptr;
+
+ /// Whether the edge is highlighted.
+ bool highlighted = false;
+ };
+
+
+ struct GraphData : public nav::graph::GraphAttribs
+ {
+ };
+
+
+ using GuiGraph = semrel::RelationGraph<VertexData, EdgeData, GraphData>;
+
+
+ GuiGraph toGuiGraph(const graph::Graph& graph);
+ graph::Graph fromGuiGraph(const GuiGraph& graph);
+
+
+
+ float getYawAngleDegree(const Eigen::Matrix4f& pose);
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index 4e2a178272eddfe8b546c748238d39793885fbee..6a33a0128448d77ed4e8c65e410055cb448c93ad 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>783</width>
- <height>664</height>
+ <height>684</height>
</rect>
</property>
<property name="windowTitle">
@@ -21,7 +21,7 @@
</property>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
- <widget class="QPushButton" name="refreshScenesButton">
+ <widget class="QPushButton" name="refreshGraphsButton">
<property name="toolTip">
<string>Reloads the list of scenes from the memory</string>
</property>
@@ -31,7 +31,7 @@
</widget>
</item>
<item>
- <widget class="QComboBox" name="scenesComboBox">
+ <widget class="QComboBox" name="graphsComboBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -41,7 +41,7 @@
</widget>
</item>
<item>
- <widget class="QPushButton" name="drawSceneButton">
+ <widget class="QPushButton" name="loadGraphButton">
<property name="toolTip">
<string>Draws the selected scene</string>
</property>
@@ -54,7 +54,7 @@
</widget>
</item>
<item>
- <widget class="QSplitter" name="splitter_3">
+ <widget class="QSplitter" name="verticalSplitter">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
@@ -98,7 +98,7 @@
<item>
<widget class="QPushButton" name="buttonClear">
<property name="text">
- <string>Clear graph</string>
+ <string>Clear Graph</string>
</property>
</widget>
</item>
@@ -173,391 +173,89 @@
</widget>
<widget class="QWidget" name="tables" native="true">
<layout class="QVBoxLayout" name="verticalLayout">
+ <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="QSplitter" name="splitter_2">
+ <widget class="QSplitter" name="horizontalSplitter">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
- <widget class="QSplitter" name="splitter">
+ <widget class="QWidget" name="verticalLayout_12Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_12">
+ <item>
+ <widget class="QGroupBox" name="locationsTableGroupBox">
+ <property name="title">
+ <string>Locations (Graph Vertices)</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_7">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="verticalLayout_13Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_13">
+ <item>
+ <widget class="QGroupBox" name="edgesTableGroupBox">
+ <property name="title">
+ <string>Graph Edges</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_8">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="verticalLayout_6Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_6">
+ <item>
+ <widget class="QGroupBox" name="locationDataGroupBox">
+ <property name="title">
+ <string>Location Data</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_3"/>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QSplitter" name="horizontalSplitter_2">
<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>
@@ -565,6 +263,22 @@
</widget>
</widget>
</item>
+ <item>
+ <widget class="QLabel" name="statusLabel">
+ <property name="font">
+ <font>
+ <pointsize>9</pointsize>
+ <italic>true</italic>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
<resources/>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index cb576778e44adbe84f9193f6e0e4a81a4a458704..9510d82c920b9e8500e91322dbfe2aede3416466 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -21,69 +21,36 @@
*/
#include "LocationGraphEditorWidgetController.h"
+#include "widgets/EdgeTableWidget.h"
+#include "widgets/VertexDataWidget.h"
+#include "widgets/VertexTableWidget.h"
-#include <MemoryX/libraries/memorytypes/variants/GraphNode/GraphNode.h>
+#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <armarx/navigation/location/constants.h>
+#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <VirtualRobot/MathTools.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <SimoxUtility/color/Color.h>
// Qt headers
-#include <Qt>
-#include <QtGlobal>
-#include <QPushButton>
+#include <QDialog>
+#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <QHBoxLayout>
#include <QMenu>
+#include <QMouseEvent>
+#include <QObject>
+#include <QPushButton>
// 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.
@@ -99,28 +66,30 @@ static const float VIEW_ROTATE_STEP_SIZE_CC = 45;
static const QString SETTING_LAST_SCENE = "lastScene";
-namespace armarx
+namespace armarx::nav::locgrapheditor
{
+
+
/**
* @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::string iceName(const GuiGraph::Edge& edge)
{
- std::stringstream s;
- s << "edge_" << edge.first << "_" << edge.second;
- return s.str();
+ std::stringstream ss;
+ ss << "edge " << std::to_string(edge.sourceObjectID()) << " -> " << std::to_string(edge.targetObjectID());
+ return ss.str();
}
/**
* @brief iceName Returns the name used on the debug layer.
- * @param nodeName The node.
+ * @param vertexName The vertex.
* @return The name used on the debug layer.
*/
- inline std::string iceName(const LocationGraphEditorWidgetController::NodeId& nodeName)
+ std::string iceName(const GuiGraph::Vertex& vertex)
{
- return nodeName;
+ return std::to_string(vertex.objectID());
}
@@ -135,26 +104,122 @@ namespace armarx
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;
+
+ dataWidgets.edgeTable = new EdgeTableWidget();
+ dataWidgets.vertexTable = new VertexTableWidget();
+ dataWidgets.vertexData = new VertexDataWidget();
+ widget.edgesTableGroupBox->layout()->addWidget(dataWidgets.edgeTable);
+ widget.locationsTableGroupBox->layout()->addWidget(dataWidgets.vertexTable);
+ widget.locationDataGroupBox->layout()->addWidget(dataWidgets.vertexData);
// 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);
+ view.view = widget.graphicsViewGraph;
+ view.setScene(new GraphScene());
+
+ auto eventFilter = [this](QObject* obj, QEvent* event) -> bool
+ {
+#if 0
+ if (obj == this->view.view && event->type() == QEvent::MouseButtonPress)
+ {
+ QMouseEvent* me = static_cast<QMouseEvent*>(event);
+ if (me->button() == Qt::LeftButton)
+ {
+ QPointF scenePoint = this->view.view->mapToScene(me->pos());
+ scenePoint.setY(- scenePoint.y()); // not sure why
+
+ float minDist = std::numeric_limits<float>::max();
+ auto bestIt = this->vertices.cend();
+
+ for (auto it = this->vertices.cbegin(); it != this->vertices.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 != this->vertices.cend())
+ {
+ this->highlightVertex(bestIt->first);
+ }
+ }
+ }
+ else if (event->type() == QEvent::Resize)
+ {
+ this->adjustView();
+ }
+#endif
+ return QObject::eventFilter(obj, event);
+ };
+ view.view->installEventFilter(new simox::gui::FunctionalEventFilter(eventFilter));
+
// Transform view
transformView();
+
+ // Widgets -> This
+
+ // Memory Access
+ connect(widget.refreshGraphsButton, &QPushButton::clicked, this, &This::updateGraphList);
+ connect(widget.loadGraphButton, &QPushButton::clicked, this, &This::loadGraph);
+
+
+#if 0
+ // Tables
+ connect(dataWidgets.vertexTable, &QTableWidget::cellDoubleClicked, this, &This::vertexTableDoubleClicked);
+ connect(dataWidgets.vertexTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetVerticesCustomContextMenu);
+ connect(dataWidgets.edgeTable, &QTableWidget::cellDoubleClicked, this, &This::edgeTableDoubleClicked);
+ connect(dataWidgets.edgeTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetEdgesCustomContextMenu);
+
+ connect(widget.btnAdd, &QPushButton::clicked, this, &This::addNewGraphVertex);
+ connect(widget.btnAddEdge, &QPushButton::clicked, this, &This::addNewEdgeBoth);
+ connect(widget.btnAddEdgeStartEnd, &QPushButton::clicked, this, &This::addNewEdgeStartEnd);
+ connect(widget.btnAddEdgeEndStart, &QPushButton::clicked, this, &This::addNewEdgeEndStart);
+ connect(widget.btnEdit, &QPushButton::clicked, this, &This::editGraphVertex);
+#endif
+
+ // Zoom
+ connect(widget.viewZoomFactor, QOverload<double>::of(&QDoubleSpinBox::valueChanged), this, &This::transformView);
+
+#if 0
+ // Rotation
+ connect(widget.buttonRotateClock, &QPushButton::clicked, this, &This::viewRotatedClock);
+ connect(widget.buttonRotateCounterClock, &QPushButton::clicked, this, &This::viewRotatedCounterClock);
+
+ // Redraw+clear
+ connect(widget.buttonRedraw, &QPushButton::clicked, this, &This::redraw);
+ connect(widget.buttonClear, &QPushButton::clicked, this, &This::clearGraph);
+
+ // Auto adjust view
+ connect(widget.buttonAutoAdjust, &QPushButton::clicked, this, &This::adjustView);
+#endif
+
+ connect(view.scene, &GraphScene::vertexSelected, this, &This::toggleVertexSelected);
+
+
+ // This -> This
+
+ // Intra-connections.
+ connect(this, &This::connected, this, &This::queryGraphs);
+ connect(this, &This::memoryDataChanged, this, &This::updateGraphList);
+
+ }
+
+
+ void LocationGraphEditorWidgetController::GraphView::setScene(GraphScene* scene)
+ {
+ this->scene = scene;
+ this->view->setScene(this->scene);
}
@@ -166,59 +231,57 @@ namespace armarx
QPointer<QDialog> LocationGraphEditorWidgetController::getConfigDialog(QWidget* parent)
{
- if (!dialog)
+ if (not dialog)
{
dialog = new SimpleConfigDialog(parent);
+ dialog->addProxyFinder<armem::mns::MemoryNameSystemInterfacePrx>("MemoryNameSystem", "Memory Name System", remote.memoryNameSystemName);
}
-
- 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");
+ remote.memoryNameSystemName = dialog->getProxyName("MemoryNameSystem");
}
void LocationGraphEditorWidgetController::loadSettings(QSettings* settings)
{
- memoryNameSystemName = settings->value("memoryNameSystemName", QString::fromStdString(memoryNameSystemName)).toString().toStdString();
- priorKnowledgeProxyName = settings->value("priorKnowledgeProxyName", QString::fromStdString(debugDrawerLayerName)).toString().toStdString();
+ remote.memoryNameSystemName = settings->value("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName)).toString().toStdString();
}
void LocationGraphEditorWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("memoryNameSystemName", QString::fromStdString(memoryNameSystemName));
- settings->setValue("priorKnowledgeProxyName", QString::fromStdString(priorKnowledgeProxyName));
+ settings->setValue("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName));
+ }
+
+
+ void LocationGraphEditorWidgetController::loadAutomaticSettings()
+ {
+ lastSelectedSceneName = settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
+ }
+
+
+ void LocationGraphEditorWidgetController::saveAutomaticSettings()
+ {
+ settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
}
void LocationGraphEditorWidgetController::onInitComponent()
{
- usingProxy(priorKnowledgeProxyName);
- usingProxy("GraphNodePoseResolver");
- offeringTopic(debugDrawerTopicName);
+ usingProxy(remote.memoryNameSystemName);
}
void LocationGraphEditorWidgetController::onConnectComponent()
{
- debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
-
- debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
- priorKnowledgePrx = getProxy<memoryx::PriorKnowledgeInterfacePrx>(priorKnowledgeProxyName);
- getProxy(gnpr, "GraphNodePoseResolver");
+ remote.connect(*this);
- if (priorKnowledgePrx->hasGraphSegment())
+ if (/* DISABLES CODE */ (true))
{
- ARMARX_VERBOSE << "get Proxy to graph segment";
- graphSeg = priorKnowledgePrx->getGraphSegment();
widget.sceneGroupBox->setEnabled(true);
widget.sceneGroupBox->setTitle("Scenes from graph memory segment");
}
@@ -228,435 +291,332 @@ namespace armarx
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();
+ emit connected();
}
- void LocationGraphEditorWidgetController::loadAutomaticSettings()
+ void LocationGraphEditorWidgetController::Remote::connect(Component& parent)
{
- lastSelectedSceneName = settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
- }
+ auto mnsProxy = parent.getProxy<armem::mns::MemoryNameSystemInterfacePrx>(memoryNameSystemName);
+ memoryNameSystem = armem::client::MemoryNameSystem(mnsProxy, &parent);
+ locationReader = memoryNameSystem.useReader(nav::loc::coreSegmentID);
+ locationWriter = memoryNameSystem.useWriter(nav::loc::coreSegmentID);
- void LocationGraphEditorWidgetController::saveAutomaticSettings()
- {
- settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
+ graphReader = memoryNameSystem.useReader(nav::graph::coreSegmentID);
+ graphWriter = memoryNameSystem.useWriter(nav::graph::coreSegmentID);
}
- void LocationGraphEditorWidgetController::addEdge(const std::string& node1Id, const std::string& node2Id)
+ void LocationGraphEditorWidgetController::queryGraphs()
{
- if (!hasNode(node1Id))
+ armem::client::QueryResult result = remote.graphReader.getLatestSnapshotsIn(nav::graph::coreSegmentID);
+ if (result.success)
{
- ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node1Id << " does not exist.";
- return;
+ data.memory = std::move(result.memory);
+ emit memoryDataChanged();
}
-
- if (!hasNode(node2Id))
+ else
{
- ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node2Id << " does not exist.";
- return;
+ widget.statusLabel->setText(QString::fromStdString(result.errorMessage));
}
+ }
- 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;
- }
+ void LocationGraphEditorWidgetController::updateGraphList()
+ {
+ widget.graphsComboBox->clear();
- 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
+ data.memory.forEachEntity([this](const armem::wm::Entity& entity)
{
- *this, edgeId,
- node1dat.pose->position->x, -node1dat.pose->position->y,
- node2dat.pose->position->x, -node2dat.pose->position->y
+ widget.graphsComboBox->addItem(QString::fromStdString(entity.id().str()));
});
- // 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)
+ void LocationGraphEditorWidgetController::loadGraph()
{
- ARMARX_CHECK_EXPRESSION(node);
- auto nodeId = node->getId();
+ // Store vertex highlighting.
+ std::vector<semrel::ShapeID> highlightedVertices;
+ for (auto vertex : data.graph.vertices())
+ {
+ if (vertex.attrib().highlighted)
+ {
+ highlightedVertices.push_back(vertex.objectID());
+ }
+ }
- armarx::FramedPosePtr globalNodePose;
- try
+ const armem::MemoryID entityID = armem::MemoryID::fromString(widget.graphsComboBox->currentText().toStdString());
+ const armem::wm::EntityInstance* instance = data.memory.findLatestInstance(entityID);
+ if (not instance)
{
- globalNodePose = armarx::FramedPosePtr::dynamicCast(gnpr->resolveToGlobalPose(node));
+ std::stringstream ss;
+ ss << "No latest instance of entity " << entityID << " in memory.";
+ widget.statusLabel->setText(QString::fromStdString(ss.str()));
}
- catch (...)
+
+ nav::graph::Graph nav;
{
- return;
+ nav::graph::arondto::Graph dto;
+ dto.fromAron(instance->data());
+ fromAron(dto, nav);
+ }
+
+ clearGraph();
+
+ // Add vertices
+ for (auto vertex : nav.vertices())
+ {
+ addVertex(vertex);
}
- if (hasNode(nodeId))
+ // Add edges
+ for (auto edge : nav.edges())
{
- 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)
+ addEdge(edge);
+ }
+
+ // Restore vertex highlighting.
+ for (const semrel::ShapeID& vertexID : highlightedVertices)
+ {
+ for (auto vertex : data.graph.vertices())
{
- if ((edge.first.first == nodeId) || (edge.first.second == nodeId))
+ if (vertex.objectID() == vertexID)
{
- updateEdge(edge.first);
+ vertex.attrib().highlighted = true;
+ updateVertex(vertex);
}
}
}
- 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);
+ emit graphChanged();
}
- void LocationGraphEditorWidgetController::clearEdges()
+ GuiGraph::Vertex
+ LocationGraphEditorWidgetController::addVertex(graph::Graph::Vertex vertex)
{
- 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));
- }
+ ARMARX_CHECK(not data.graph.hasVertex(vertex.objectID()));
+
+ VertexData attrib { vertex.attrib() };
+ attrib.graphicsItem = view.scene->addVertex(vertex);
+ attrib.tableWidgetItem = dataWidgets.vertexTable->addVertex(vertex);
+ auto guiVertex = data.graph.addVertex(vertex.objectID(), attrib);
- // clear table widget
- widget.tableWidgetEdges->clearContents();
- widget.tableWidgetEdges->setRowCount(0);
- // clear data structures
- edges.clear();
+ updateVertex(guiVertex);
+ return guiVertex;
}
- void LocationGraphEditorWidgetController::clearGraph()
+ GuiGraph::Edge
+ LocationGraphEditorWidgetController::addEdge(graph::Graph::Edge edge)
{
- // remove from debug layer
- for (auto& edge : edges)
- {
- debugDrawer->removeLineVisu(debugDrawerLayerName, iceName(edge.first));
- }
+ ARMARX_CHECK(not data.graph.hasEdge(edge.sourceObjectID(), edge.targetObjectID()))
+ << "Edge must not exist before being added: '"
+ << edge.source().attrib().getName() << "' -> '"
+ << edge.target().attrib().getName() << "'";
- for (auto& node : nodes)
- {
- debugDrawer->removeArrowVisu(debugDrawerLayerName, iceName(node.first));
- debugDrawer->removeTextVisu(debugDrawerLayerName, iceName(node.first) + "text");
- debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(node.first));
- }
+ auto source = data.graph.vertex(edge.sourceObjectID());
+ auto target = data.graph.vertex(edge.targetObjectID());
+
+ EdgeData attrib { edge.attrib() };
+ attrib.tableWidgetItem = dataWidgets.edgeTable->addEdge(edge);
+ attrib.graphicsItem = view.scene->addEdge(edge);
+ auto guiEdge = data.graph.addEdge(source, target, attrib);
- // 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();
+ updateEdge(guiEdge);
+ return guiEdge;
}
- void LocationGraphEditorWidgetController::resetHighlight()
+ void LocationGraphEditorWidgetController::updateVertex(GuiGraph::Vertex vertex)
{
- for (auto& edge : edges)
+ view.scene->updateVertex(vertex);
+ dataWidgets.vertexTable->updateVertex(vertex);
+
+ setEditFields(vertex);
+
{
- if (edge.second.highlighted)
+ // Highlight all edges between highlighted vertices
+ std::set<semrel::ShapeID> highlightedVertices;
+ for (auto v : data.graph.vertices())
{
- edge.second.highlighted = false;
- updateEdge(edge.first);
+ if (v.attrib().highlighted)
+ {
+ highlightedVertices.insert(v.objectID());
+ }
}
- }
- for (auto& node : nodes)
- {
- if (node.second.highlighted)
+ for (auto edge : data.graph.edges())
{
- node.second.highlighted = false;
- updateNode(node.first);
+ bool verticesHighlighted =
+ highlightedVertices.count(edge.sourceObjectID())
+ and highlightedVertices.count(edge.targetObjectID());
+
+ // Already highlighted but vertices not highlighted => to false, update
+ // Not highlighted but vertices highlighted => to true, update
+ if (edge.attrib().highlighted != verticesHighlighted)
+ {
+ edge.attrib().highlighted = verticesHighlighted;
+ updateEdge(edge);
+ }
}
}
+
+ // ToDo: Update ArViz
+#if 0
+ float yaw = getYawAngleDegree(attrib.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), attrib.pose->position,
+ new armarx::Vector3(dir),
+ armarx::DrawColor {0, 0, 1, 1},
+ 100,
+ lineWidth);
+ debugDrawer->setTextVisu(debugDrawerLayerName, iceName(id) + "text", attrib.vertex->getName(),
+ attrib.pose->position, armarx::DrawColor {0, 0, 1, 1}, 10);
+#endif
}
- void LocationGraphEditorWidgetController::updateEdge(const EdgeId& id)
+ void LocationGraphEditorWidgetController::updateEdge(GuiGraph::Edge edge)
{
- 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());
+ view.scene->updateEdge(edge);
+ dataWidgets.edgeTable->updateEdge(edge);
+
+#if 0
+ armarx::Vector3Ptr posStart = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(vertices.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());
+ armarx::Vector3Ptr posEnd = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(vertices.at(id.attrib()).pose->position)->toEigen());
posEnd->z += posEnd->z < 1 ? 10 : 0;
// debug layer
+ // ToDo: Update ArViz
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);
+#endif
}
- void LocationGraphEditorWidgetController::updateNode(const NodeId& id)
+ void LocationGraphEditorWidgetController::clearEdges()
{
- NodeData& data = nodes.at(id);
-
- if (editStartNodeNext)
+ // Remove from graphics scene
+ for (auto edge : data.graph.edges())
{
- widget.editStartNodeId->setText(QString::fromStdString(data.node->getId()));
- widget.editStartNodeName->setText(QString::fromStdString(data.node->getName()));
+ view.scene->removeEdge(edge.attrib().graphicsItem);
}
- else
+
+ // Clear table widget
+ dataWidgets.edgeTable->clearContents();
+ dataWidgets.edgeTable->setRowCount(0);
+
+ // Clear data structure
+ while (data.graph.edges().begin() != data.graph.edges().end())
{
- widget.editEndNodeId->setText(QString::fromStdString(data.node->getId()));
- widget.editEndNodeName->setText(QString::fromStdString(data.node->getName()));
+ data.graph.removeEdge(*data.graph.edges().begin());
}
- setEditFields(data);
+ // ToDo: Update ArViz
+ }
- editStartNodeNext = !editStartNodeNext;
- auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
- QColor qColor;
- qColor.setRedF(color.r);
- qColor.setGreenF(color.g);
- qColor.setBlueF(color.b);
+ void LocationGraphEditorWidgetController::clearGraph()
+ {
+ // Clear scene
+ view.scene->clear();
- auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
+ // Clear table widgets
+ dataWidgets.edgeTable->clearContents();
+ dataWidgets.edgeTable->setRowCount(0);
+ dataWidgets.vertexTable->clearContents();
+ dataWidgets.vertexTable->setRowCount(0);
- // 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);
- }
+ // Clear data structure
+ data.graph.clear();
+ }
+
+
+ void LocationGraphEditorWidgetController::toggleVertexSelected(const semrel::ShapeID& vertexID)
+ {
+ auto vertex = data.graph.vertex(vertexID);
+ vertex.attrib().highlighted ^= true;
+ updateVertex(vertex);
+ }
- // 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)
+
+ void LocationGraphEditorWidgetController::resetHighlighting()
+ {
+ for (auto vertex : data.graph.vertices())
{
- if (edge.second.highlighted)
+ if (vertex.attrib().highlighted)
{
- edge.second.highlighted = false;
- updateEdge(edge.first);
+ vertex.attrib().highlighted = false;
+ updateVertex(vertex);
}
}
-
- for (const auto& nodeFrom : highlightedNodes)
+ for (auto edge : data.graph.edges())
{
- for (const auto& nodeTo : highlightedNodes)
+ if (edge.attrib().highlighted)
{
- 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);
- }
+ edge.attrib().highlighted = false;
+ updateEdge(edge);
}
}
}
- 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]);
- }
+#if 0
- void LocationGraphEditorWidgetController::highlightEdge(const std::string& node1Id, const std::string& node2Id, bool highlighted)
+
+ void LocationGraphEditorWidgetController::highlightVertex(const std::string& vertexId, bool highlighted)
{
- if (!hasEdge(node1Id, node2Id))
+ if (!hasVertex(vertexId))
{
- ARMARX_WARNING << "No edge for: " << node1Id << " and " << node2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
+ ARMARX_WARNING << "No vertex: " << vertexId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
return;
}
- EdgeId edge = toEdge(node1Id, node2Id);
-
- if (edges.at(edge).highlighted != highlighted)
+ if (vertices.at(vertexId).highlighted != highlighted)
{
- edges.at(edge).highlighted = highlighted;
- updateEdge(edge);
+ vertices.at(vertexId).highlighted = highlighted;
+ updateVertex(vertexId);
}
}
- void LocationGraphEditorWidgetController::highlightNode(const std::string& nodeId, bool highlighted)
+ void LocationGraphEditorWidgetController::highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted)
{
- if (!hasNode(nodeId))
+ if (!hasEdge(vertex1Id, vertex2Id))
{
- ARMARX_WARNING << "No node: " << nodeId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
+ ARMARX_WARNING << "No edge for: " << vertex1Id << " and " << vertex2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
return;
}
- if (nodes.at(nodeId).highlighted != highlighted)
+ EdgeId edge = toEdge(vertex1Id, vertex2Id);
+
+ if (edges.at(edge).highlighted != highlighted)
{
- nodes.at(nodeId).highlighted = highlighted;
- updateNode(nodeId);
+ edges.at(edge).highlighted = highlighted;
+ updateEdge(edge);
}
}
- void LocationGraphEditorWidgetController::nodeTableDoubleClicked(int row, int)
+ void LocationGraphEditorWidgetController::vertexTableDoubleClicked(int row, int)
{
- auto nodeIt = std::find_if(nodes.cbegin(), nodes.cend(), [&](const std::pair<std::string, NodeData>& d)
+ auto vertexIt = std::find_if(vertices.cbegin(), vertices.cend(), [&](const std::pair<std::string, VertexData>& d)
{
- // return d.second.node->getName() == widget.tableWidgetNodes->item(row, 0)->text().toStdString();
- return d.second.tableWidgetNodesIndex == row;
+ // return d.attrib().vertex->getName() == dataWidgets.vertexTable->item(row, 0)->text().toStdString();
+ return d.attrib().tableWidgetVerticesIndex == row;
});
- auto nodeId = nodeIt->second.node->getId();
- nodeDoubleClicked(nodeId);
+ auto vertexId = vertexIt->second.vertex->getId();
+ highlightVertex(vertexId);
}
@@ -664,7 +624,7 @@ namespace armarx
{
auto edgeIt = std::find_if(edges.cbegin(), edges.cend(), [&](const std::pair<EdgeId, EdgeData>& d)
{
- return d.second.tableWidgetEdgesIndex == row;
+ return d.attrib().tableWidgetEdgesIndex == row;
});
edgeDoubleClicked(edgeIt->first);
@@ -672,65 +632,65 @@ namespace armarx
- void LocationGraphEditorWidgetController::nodeDoubleClicked(NodeId id)
- {
- nodes.at(id).highlighted ^= true;
- updateNode(id);
- }
-
-
- void LocationGraphEditorWidgetController::edgeDoubleClicked(EdgeId id)
+ void LocationGraphEditorWidgetController::edgeDoubleClicked(GuiGraph::Edge edge)
{
// 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);
+ bool highlight = !vertices.at(edge.first).highlighted && !vertices.at(edge.attrib()).highlighted;
+ vertices.at(edge.first).highlighted = highlight;
+ vertices.at(edge.attrib()).highlighted = highlight;
+ updateVertex(edge.first);
+ updateVertex(edge.attrib());
}
void LocationGraphEditorWidgetController::redraw()
{
- drawScene();
+ loadGraph();
}
+#endif
- void LocationGraphEditorWidgetController::setEditFields(const NodeData& nodeData)
+ void LocationGraphEditorWidgetController::setEditFields(GuiGraph::Vertex vertex)
{
- 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));
+#if 0
+ widget.editVertexId->setText(QString::fromStdString(vertexData.vertex->getId()));
+ widget.editSceneName->setText(QString::fromStdString(vertexData.vertex->getScene()));
+ widget.editVertexName->setText(QString::fromStdString(vertexData.vertex->getName()));
+ widget.editFrameName->setText(QString::fromStdString(vertexData.pose->frame));
+ widget.editAgentName->setText(QString::fromStdString(vertexData.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);
+ VirtualRobot::MathTools::eigen4f2rpy(vertexData.pose->toEigen(), rpy);
+ widget.spinBoxX->setValue(vertexData.pose->position->x);
+ widget.spinBoxY->setValue(vertexData.pose->position->y);
+ widget.spinBoxZ->setValue(vertexData.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]));
+#endif
}
-
+#if 0
void LocationGraphEditorWidgetController::refreshGraph()
{
clearGraph();
- drawScene();
+ loadGraph();
}
+#endif
void LocationGraphEditorWidgetController::transformView()
{
double d = widget.viewZoomFactor->value();
- widget.graphicsViewGraph->setTransform(QTransform::fromScale(d, d).rotate(viewAngle));
+ widget.graphicsViewGraph->setTransform(QTransform::fromScale(d, d).rotate(view.angle));
}
+#if 0
+
+
void LocationGraphEditorWidgetController::viewRotatedClock()
{
viewAngle = std::fmod(viewAngle + VIEW_ROTATE_STEP_SIZE_CC, 360);
@@ -743,22 +703,23 @@ namespace armarx
viewAngle = std::fmod(viewAngle + 360 - VIEW_ROTATE_STEP_SIZE_CC, 360);
transformView();
}
-
+#endif
void LocationGraphEditorWidgetController::adjustView()
{
+#if 0
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)
+ for (const auto& vertex : vertices)
{
- 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;
+ maxX = (maxX < vertex.attrib().pose->position->x) ? vertex.attrib().pose->position->x : maxX;
+ minX = (minX > vertex.attrib().pose->position->x) ? vertex.attrib().pose->position->x : minX;
+ maxY = (maxY < vertex.attrib().pose->position->y) ? vertex.attrib().pose->position->y : maxY;
+ minY = (minY > vertex.attrib().pose->position->y) ? vertex.attrib().pose->position->y : minY;
}
auto deltaX = maxX - minX; // >=0
@@ -781,35 +742,36 @@ namespace armarx
widget.viewZoomFactor->setValue(std::min(widget.graphicsViewGraph->width() / deltaY,
widget.graphicsViewGraph->height() / deltaX) * 0.9);
}
-
+#endif
}
+#if 0
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";
+ errorMsg = "start vertex with Id '" + fromId + "' not found in segment";
}
else if (!graphSeg->hasEntityById(toId))
{
- errorMsg = "end node with Id '" + toId + "' not found in segment";
+ errorMsg = "end vertex with Id '" + toId + "' not found in segment";
}
else if (fromId == toId)
{
- errorMsg = "starting and ending node are the same";
+ errorMsg = "starting and ending vertex are the same";
}
else
{
- auto fromNode = graphSeg->getNodeById(fromId);
- for (const auto& adjacent : fromNode->getAdjacentNodes())
+ auto fromVertex = graphSeg->getVertexById(fromId);
+ for (const auto& adjacent : fromVertex->getAdjacentVertices())
{
if (toId == adjacent->getId())
{
- errorMsg = "edge '" + fromNode->getName() + "' -> '" + adjacent->getName() + "' already exists";
+ errorMsg = "edge '" + fromVertex->getName() + "' -> '" + adjacent->getName() + "' already exists";
break;
}
}
@@ -819,11 +781,11 @@ namespace armarx
{
widget.labelAddEdgeStatus->setText(QString::fromStdString("Ok"));
graphSeg->addEdge(fromId, toId);
- gnpr->forceRefetch(fromId);
- gnpr->forceRefetch(toId);
+ graphVertexPoseResolver->forceRefetch(fromId);
+ graphVertexPoseResolver->forceRefetch(toId);
addEdge(fromId, toId);
- updateNode(fromId);
- updateNode(toId);
+ updateVertex(fromId);
+ updateVertex(toId);
widget.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : lime; }");
}
else
@@ -839,157 +801,29 @@ namespace armarx
void LocationGraphEditorWidgetController::addNewEdgeBoth()
{
- std::string startId = widget.editStartNodeId->text().toStdString();
- std::string endId = widget.editEndNodeId->text().toStdString();
+ std::string startId = widget.editStartVertexId->text().toStdString();
+ std::string endId = widget.editEndVertexId->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();
+ std::string startId = widget.editStartVertexId->text().toStdString();
+ std::string endId = widget.editEndVertexId->text().toStdString();
addNewEdge(startId, endId);
}
void LocationGraphEditorWidgetController::addNewEdgeEndStart()
{
- std::string startId = widget.editEndNodeId->text().toStdString();
- std::string endId = widget.editStartNodeId->text().toStdString();
+ std::string startId = widget.editEndVertexId->text().toStdString();
+ std::string endId = widget.editStartVertexId->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()
{
@@ -997,33 +831,33 @@ namespace armarx
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)
+ auto addVertex = [&](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)";
+ graphSeg->addVertex(new ::memoryx::GraphVertex {x, y, angle, name, scene});
+ ARMARX_INFO_S << "added vertex '" << name << "' at (" << x << ", " << y << ", " << angle << "rad)";
};
- auto addEdges = [&](const std::string & nodeFromName, const std::string & nodeToName, bool bidirectional)
+ auto addEdges = [&](const std::string & vertexFromName, const std::string & vertexToName, 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());
+ auto vertexFrom = graphSeg->getVertexFromSceneByName(scene, vertexFromName);
+ auto vertexTo = graphSeg->getVertexFromSceneByName(scene, vertexToName);
+ ARMARX_CHECK(vertexFrom);
+ ARMARX_CHECK(vertexTo);
+ ARMARX_INFO_S << "'" << vertexFrom->getName() << "' -> '" << vertexTo->getName() << "', status: " << graphSeg->addEdge(vertexFrom->getId(), vertexTo->getId());
if (bidirectional)
{
- ARMARX_INFO_S << "'" << nodeTo->getName() << "' -> '" << nodeFrom->getName() << "', status: " << graphSeg->addEdge(nodeTo->getId(), nodeFrom->getId());
+ ARMARX_INFO_S << "'" << vertexTo->getName() << "' -> '" << vertexFrom->getName() << "', status: " << graphSeg->addEdge(vertexTo->getId(), vertexFrom->getId());
}
};
// ex
- addNode("initialnode", 2900, 7000, 0);
- addNode("sideboard", 3400, 7000, 0);
- addEdges("initialnode", "sideboard", true);
+ addVertex("initialvertex", 2900, 7000, 0);
+ addVertex("sideboard", 3400, 7000, 0);
+ addEdges("initialvertex", "sideboard", true);
}
- void LocationGraphEditorWidgetController::addNewGraphNode()
+ void LocationGraphEditorWidgetController::addNewGraphVertex()
{
Eigen::Matrix4f mat;
Eigen::Vector3f rpy;
@@ -1031,28 +865,26 @@ namespace armarx
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();
+ 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(),
+ memoryx::GraphVertexPtr vertex = new memoryx::GraphVertex(pose,
+ widget.editVertexName->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())
+ auto entityId = graphSeg->addVertex(vertex);
+ graphVertexPoseResolver->forceRefetch(entityId);
+ vertex->setId(entityId);
+ if (widget.graphsComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
{
- widget.editNodeId->setText(QString::fromStdString(entityId));
- addNode(node);
+ widget.editVertexId->setText(QString::fromStdString(entityId));
+ addVertex(vertex);
}
}
- void LocationGraphEditorWidgetController::editGraphNode()
+ void LocationGraphEditorWidgetController::editGraphVertex()
{
Eigen::Matrix4f mat;
Eigen::Vector3f rpy;
@@ -1067,96 +899,59 @@ namespace armarx
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(),
+ memoryx::GraphVertexPtr vertex = new memoryx::GraphVertex(pose,
+ widget.editVertexName->text().toStdString(),
widget.editSceneName->text().toStdString());
- auto id = widget.editNodeId->text().toStdString();
+ auto id = widget.editVertexId->text().toStdString();
- node->setId(id);
- graphSeg->updateEntity(id, node);
- gnpr->forceRefetch(id);
- if (widget.scenesComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
+ vertex->setId(id);
+ graphSeg->updateEntity(id, vertex);
+ graphVertexPoseResolver->forceRefetch(id);
+ if (widget.graphsComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
{
- addNode(node);
+ addVertex(vertex);
}
}
- void LocationGraphEditorWidgetController::tableWidgetNodesCustomContextMenu(QPoint pos)
+ void LocationGraphEditorWidgetController::tableWidgetVerticesCustomContextMenu(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)
+ int row = dataWidgets.vertexTable->rowAt(pos.y());
+ auto vertexIt = std::find_if(vertices.begin(), vertices.end(), [&](const std::pair<std::string, VertexData>& d)
{
- return d.second.tableWidgetNodesIndex == row;
+ return d.attrib().tableWidgetVerticesIndex == row;
});
QMenu menu;
- QAction* deleteAction = menu.addAction("Delete Node");
+ QAction* deleteAction = menu.addAction("Delete Vertex");
if (menu.exec(QCursor::pos()) == deleteAction)
{
- ARMARX_CHECK_EXPRESSION(nodeIt != nodes.end());
- graphSeg->removeNode(nodeIt->second.node->getId());
- drawScene();
+ ARMARX_CHECK(vertexIt != vertices.end());
+ graphSeg->removeVertex(vertexIt->second.vertex->getId());
+ loadGraph();
}
}
void LocationGraphEditorWidgetController::tableWidgetEdgesCustomContextMenu(QPoint pos)
{
- int row = widget.tableWidgetEdges->rowAt(pos.y());
+ int row = dataWidgets.edgeTable->rowAt(pos.y());
auto edgeIt = std::find_if(edges.begin(), edges.end(), [&](const std::pair<EdgeId, EdgeData>& d)
{
- return d.second.tableWidgetEdgesIndex == row;
+ return d.attrib().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();
+ ARMARX_CHECK(edgeIt != edges.end());
+ graphSeg->removeEdge(edgeIt->first.first, edgeIt->first.attrib());
+ loadGraph();
}
}
- 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);
- }
+#endif
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index b87a5c2ab6a82461562a3c2f08903cbac9c2508f..d06303359ed86124fdd2f14fac4b3a8496eca329 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -21,18 +21,19 @@
*/
#pragma once
-#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
-
+#include "FunctionalEventFilter.h"
+#include "GraphScene.h"
+#include "GuiGraph.h"
-#include <MemoryX/components/PriorKnowledge/PriorKnowledge.h>
-#include <MemoryX/interface/gui/GraphVisualizerInterface.h>
-#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
+#include <armarx/navigation/graph/Graph.h>
+#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/client/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
@@ -40,27 +41,17 @@
#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 QDialog;
- class LocationGraphEditorWidgetController;
- class MouseEventProcessor;
+namespace armarx::nav::locgrapheditor
+{
+ class EdgeTableWidget;
+ class VertexDataWidget;
+ class VertexTableWidget;
/**
@@ -95,17 +86,11 @@ namespace armarx
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;
+ using This = LocationGraphEditorWidgetController;
- /// The type of edge ids. (This type implies the edge exists)
- using EdgeId = const std::pair<const std::string, const std::string>;
+ public:
static QString GetWidgetName();
static QIcon GetWidgetIcon();
@@ -134,354 +119,181 @@ namespace armarx
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();
+ signals:
- void tableWidgetNodesCustomContextMenu(QPoint pos);
- void tableWidgetEdgesCustomContextMenu(QPoint pos);
+ void connected();
+ void memoryDataChanged();
+ void graphChanged();
private slots:
- /**
- * @brief add kitchen graph (H2T Armar3a robot kitchen)
- */
- void addKitchenGraph();
-
- void selectedSceneChanged(int i);
- void updateSceneList();
- void drawScene();
+ // Data
+ void queryGraphs();
+ void updateGraphList();
+ void loadGraph();
- /**
- * @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);
+ GuiGraph::Vertex addVertex(graph::Graph::Vertex vertex);
+ GuiGraph::Edge addEdge(graph::Graph::Edge edge);
- /**
- * @brief Toggles the double clicked node's selection state.
- * @param id Identifies the node.
- */
- void nodeDoubleClicked(NodeId id);
+ void clearEdges();
+ void clearGraph();
- /**
- * @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();
+ // View & Tables
- /**
- * @brief Rotates the view counter clockwise.
- *
- * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
- */
- void viewRotatedCounterClock();
+ void updateVertex(GuiGraph::Vertex vertex);
+ void updateEdge(GuiGraph::Edge edge);
- /**
- * @brief Applies the current transforamtion to the view.
- */
+ /// Applies the current transforamtion to the view.
void transformView();
-
- /**
- * @brief Adjusts the view's zoom and rotation to display most of the graph.
- */
+ /// 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();
+ // Selection
+
+ void resetHighlighting();
+ void toggleVertexSelected(const semrel::ShapeID& vertexID);
private:
/// Widget Form
Ui::LocationGraphEditorWidget widget;
+ QPointer<SimpleConfigDialog> dialog;
- /**
- * @brief The NodeData struct holds data required for the node.
- * The name is stored in the key used in the map nodes.
- */
- struct NodeData
+ struct Remote
{
- /**
- * @brief The Entity of the graph segment this struct represents
- */
- memoryx::GraphNodeBasePtr node;
+ std::string memoryNameSystemName = "MemoryNameSystem";
+ armem::client::MemoryNameSystem memoryNameSystem;
- /**
- * @brief The pose drawn to debugDrawer.
- */
- armarx::FramedPosePtr pose;
+ armem::client::Reader locationReader;
+ armem::client::Writer locationWriter;
- /**
- * @brief The ellipse in the scene.
- */
- QGraphicsEllipseItem* graphicsItem;
+ armem::client::Reader graphReader;
+ armem::client::Writer graphWriter;
- /**
- * @brief The row in the table tableWidgetNodes.
- */
- int tableWidgetNodesIndex;
- /**
- * @brief Whether the node is highlighted.
- */
- bool highlighted;
+ void connect(Component& parent);
};
+ Remote remote;
- /**
- * @brief The EdgeData struct holds data required for the edge.
- * The name is stored in the key used in the map edges.
- */
- struct EdgeData
+
+ struct Data
{
- /**
- * @brief The line in the scene.
- */
- QGraphicsLineItem* graphicsItem;
- /**
- * @brief The row in the table tableWidgetEdges.
- */
- int tableWidgetEdgesIndex;
+ armem::wm::Memory memory;
+ nav::locgrapheditor::GuiGraph graph;
+ };
+ Data data;
- /**
- * @brief Whether the edge is highlighted.
- */
- bool highlighted;
+
+ struct DataWidgets
+ {
+ VertexTableWidget* vertexTable = nullptr;
+ EdgeTableWidget* edgeTable = nullptr;
+ VertexDataWidget* vertexData = nullptr;
};
+ DataWidgets dataWidgets;
- /**
- * @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)
+ struct GraphView
{
- return EdgeId {node1, node2};
- }
+ QGraphicsView* view = nullptr;
- /**
- * @brief Updates an edge.
- * @param The edge to update.
- */
- void updateEdge(const EdgeId& id);
+ /**
+ * @brief The scene displayed in the widget.
+ *
+ * For y coordinates (-y) is used to mirror the scene on the y axis.
+ * If (+y) would be used the graph displayed in the scene would not
+ * match the graph drawn to the debug layer.
+ */
+ GraphScene* scene;
- /**
- * @brief Updates a node.
- * @param The node to update.
- */
- void updateNode(const NodeId& id);
+ /// The view's rotation angle.
+ qreal angle = 0;
- void setEditFields(const NodeData& node);
+ void setScene(GraphScene* scene);
+ };
+ GraphView view;
- /**
- * @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;
+ // Non-refactored
- 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;
+#if 0
+ private slots:
- /**
- * @brief The nodes.
- */
- std::map<std::string, NodeData> nodes;
+ void highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted = true);
+ void highlightVertex(const std::string& vertexId, bool highlighted = true);
- /**
- * @brief The edges.
- */
- std::map<EdgeId, EdgeData> edges;
+ void redraw();
+ void refreshGraph();
- /**
- * @brief The view's rotation angle.
- */
- qreal viewAngle;
+ void tableWidgetVerticesCustomContextMenu(QPoint pos);
+ void tableWidgetEdgesCustomContextMenu(QPoint pos);
- /**
- * @brief The layer to draw on.
- */
- std::string debugDrawerLayerName;
- bool editStartNodeNext;
+ private slots:
- std::string priorKnowledgeProxyName;
- memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx;
- memoryx::GraphNodePoseResolverInterfacePrx gnpr;
- memoryx::GraphMemorySegmentBasePrx graphSeg;
- QSettings settings;
- QString lastSelectedSceneName;
+ /// Add kitchen graph (H2T Armar3a robot kitchen)
+ void addKitchenGraph();
/**
- * selectScene(): private function called when button load is pushed, and calls the function loadScene()
+ * @brief Toggles the double clicked vertex's selection state.
+ * @param row Identifies the vertex.
*/
- void selectScene();
-
+ void vertexTableDoubleClicked(int row, int);
/**
- * @brief loadScene Private function that parses XML file to load a scene
- * @param xmlFile
+ * @brief Toggles the double clicked edge's selection state.
+ * @param row Identifies the edge.
*/
- 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)
- {
- }
+ void edgeTableDoubleClicked(int row, int);
- ~GraphVisualizerGraphicsEllipseItem() override
- {
- }
- protected:
- void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override
- {
- parentVisuWidget.nodeDoubleClicked(id);
- }
- private:
/**
- * @brief Required to identify the element.
+ * @brief Toggles the double clicked edge's selection state.
+ * @param id Identifies the edge.
*/
- const NodeId id;
+ void edgeDoubleClicked(GuiGraph::Edge edge);
/**
- * @brief Required to call nodeDoubleClicked on it. (This class is no QObject so it does not support signals)
+ * @brief Rotates the view clockwise.
+ *
+ * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
*/
- 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)
- {
- }
+ void viewRotatedClock();
- ~GraphVisualizerGraphicsLineItem() override
- {
- }
- signals:
- protected:
- void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override
- {
- parentVisuWidget.edgeDoubleClicked(id);
- }
- private:
/**
- * @brief Required to identify the element.
+ * @brief Rotates the view counter clockwise.
+ *
+ * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
*/
- const EdgeId id;
-
- /// Required to call edgeDoubleClicked on it. (This class is no QObject so it does not support signals)
- LocationGraphEditorWidgetController& parentVisuWidget;
- };
-
+ void viewRotatedCounterClock();
- class MouseEventProcessor : public QObject
- {
- Q_OBJECT
- public:
+ bool addNewEdge(const std::string& from, const std::string& to);
+ void addNewEdgeBoth();
+ void addNewEdgeStartEnd();
+ void addNewEdgeEndStart();
- MouseEventProcessor(LocationGraphEditorWidgetController* gvw) : gvw(gvw) {}
+ void addNewGraphVertex();
+ void editGraphVertex();
+#endif
+ void setEditFields(GuiGraph::Vertex vertex);
- protected:
- LocationGraphEditorWidgetController* gvw;
- bool eventFilter(QObject* obj, QEvent* event) override;
+ private:
+ QSettings settings;
+ QString lastSelectedSceneName;
};
}
+
+namespace armarx
+{
+ using armarx::nav::locgrapheditor::LocationGraphEditorWidgetController;
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da5444cf3d46a7b94b382c9a9b63fed55d8fa22c
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -0,0 +1,75 @@
+/*
+ * 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/>.
+ *
+ * @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 "EdgeTableWidget.h"
+
+#include <QHeaderView>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ EdgeTableWidget::EdgeTableWidget()
+ {
+ QStringList columns{"Source", "Target"};
+ setColumnCount(columns.size());
+ setHorizontalHeaderLabels(columns);
+ horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
+ horizontalHeader()->setVisible(true);
+
+ setEditTriggers(QAbstractItemView::NoEditTriggers);
+ setSortingEnabled(true);
+
+ setContextMenuPolicy(Qt::CustomContextMenu);
+ }
+
+
+ QTableWidgetItem*
+ EdgeTableWidget::addEdge(graph::Graph::Edge edge)
+ {
+ int row = rowCount();
+ setRowCount(row + 1);
+ setItem(row, 0, new QTableWidgetItem {QString::fromStdString(edge.source().attrib().getName())});
+ setItem(row, 1, new QTableWidgetItem {QString::fromStdString(edge.target().attrib().getName())});
+
+ return item(row, 0);
+ }
+
+
+ void
+ EdgeTableWidget::updateEdge(GuiGraph::Edge edge)
+ {
+ QColor bgColor = edge.attrib().highlighted ? bgColorSelected : bgColorDefault;
+ QFont font;
+ font.setBold(edge.attrib().highlighted);
+
+ int row = this->row(edge.attrib().tableWidgetItem);
+ for (int col = 0; col < 2; ++col)
+ {
+ auto* item = this->item(row, col);
+ ARMARX_CHECK_NOT_NULL(item);
+
+ item->setData(Qt::BackgroundRole, bgColor);
+ item->setFont(font);
+ }
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..b42c448027ba1ddac6c4ee849449a90f8a029e4e
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -0,0 +1,57 @@
+/*
+ * 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/>.
+ *
+ * @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/gui-plugins/LocationGraphEditor/GuiGraph.h>
+
+#include <QTableWidget>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ class EdgeTableWidget : public QTableWidget
+ {
+ using This = EdgeTableWidget;
+
+
+ public:
+
+ EdgeTableWidget();
+
+ QTableWidgetItem* addEdge(graph::Graph::Edge edge);
+
+ void updateEdge(GuiGraph::Edge edge);
+
+
+ private slots:
+
+
+ public:
+
+ QColor bgColorDefault = QColor::fromRgb(255, 255, 255);
+ QColor bgColorSelected = QColor::fromRgb(0, 128, 255);
+
+
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b9df399353b2474dd47bac95f91a25792b2a04b0
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -0,0 +1,139 @@
+/*
+ * 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 "VertexDataWidget.h"
+
+#include <QDoubleSpinBox>
+#include <QFormLayout>
+#include <QHBoxLayout>
+#include <QLineEdit>
+#include <QRadioButton>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ VertexDataWidget::VertexDataWidget()
+ {
+ locationID = new QLineEdit(this);
+ frame = new QLineEdit(this);
+
+ x = new QDoubleSpinBox(this);
+ y = new QDoubleSpinBox(this);
+ z = new QDoubleSpinBox(this);
+
+ roll = new QDoubleSpinBox(this);
+ pitch = new QDoubleSpinBox(this);
+ yaw = new QDoubleSpinBox(this);
+
+ angleUnitDeg = new QRadioButton("Degrees");
+ angleUnitRad = new QRadioButton("Radians");
+
+
+ for (QDoubleSpinBox* pos : _positionSpinBoxes())
+ {
+ pos->setSuffix(" mm");
+ pos->setMinimum(-1e6);
+ pos->setMaximum(+1e6);
+ pos->setSingleStep(10);
+ pos->setValue(0);
+ }
+ for (QDoubleSpinBox* angle : _angleSpinBoxes())
+ {
+ angle->setSuffix("");
+ pos->setMinimum(-360);
+ pos->setMaximum(+360);
+ pos->setValue(0);
+ }
+ for (QDoubleSpinBox* spinBox : _allSpinBoxes())
+ {
+ spinBox->setAlignment(Qt::AlignRight);
+ }
+
+ QFormLayout* layout = new QFormLayout();
+ this->setLayout(layout);
+
+ layout->addRow("Location ID", locationID);
+ layout->addRow("Frame", frame);
+ layout->addRow("X", x);
+ layout->addRow("Y", y);
+ layout->addRow("Z", z);
+ layout->addRow("Roll", roll);
+ layout->addRow("Pitch", pitch);
+ layout->addRow("Yaw", yaw);
+
+ QHBoxLayout* angleUnitLayout = new QHBoxLayout();
+ for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
+ {
+ angleUnitLayout->addWidget(angleUnit);
+ }
+ layout->addRow("", angleUnitLayout);
+
+
+ // Connect
+ for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
+ {
+ connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
+ }
+
+ angleUnitDeg->click();
+ }
+
+
+ void VertexDataWidget::_updateAngleUnit()
+ {
+ QString suffix;
+ if (angleUnitDeg->isChecked())
+ {
+ suffix = " \u00b0";
+ }
+ else if (angleUnitRad->isChecked())
+ {
+ suffix = " rad";
+ }
+ else
+ {
+ return;
+ }
+ for (QDoubleSpinBox* angle : _angleSpinBoxes())
+ {
+ angle->setSuffix(suffix);
+ }
+ }
+
+
+ std::vector<QDoubleSpinBox*> VertexDataWidget::_positionSpinBoxes()
+ {
+ return { x, y, z };
+ }
+
+ std::vector<QDoubleSpinBox*> VertexDataWidget::_angleSpinBoxes()
+ {
+ return { roll, pitch, yaw };
+ }
+
+ std::vector<QDoubleSpinBox*> VertexDataWidget::_allSpinBoxes()
+ {
+ return { x, y, z, roll, pitch, yaw };
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..b96b8cbbe822574e9218d0191e1646b577dc4a26
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -0,0 +1,78 @@
+/*
+ * 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/>.
+ *
+ * @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/gui-plugins/LocationGraphEditor/GuiGraph.h>
+
+#include <QWidget>
+
+
+class QLineEdit;
+class QDoubleSpinBox;
+class QRadioButton;
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ class VertexDataWidget : public QWidget
+ {
+ using This = VertexDataWidget;
+
+
+ public:
+
+ VertexDataWidget();
+
+
+
+ private slots:
+
+ void _updateAngleUnit();
+
+
+ private:
+
+ std::vector<QDoubleSpinBox*> _positionSpinBoxes();
+ std::vector<QDoubleSpinBox*> _angleSpinBoxes();
+ std::vector<QDoubleSpinBox*> _allSpinBoxes();
+
+
+ private:
+
+ QLineEdit* locationID = nullptr;
+ QLineEdit* frame = nullptr;
+
+ QDoubleSpinBox* x = nullptr;
+ QDoubleSpinBox* y = nullptr;
+ QDoubleSpinBox* z = nullptr;
+
+ QDoubleSpinBox* roll = nullptr;
+ QDoubleSpinBox* pitch = nullptr;
+ QDoubleSpinBox* yaw = nullptr;
+
+ QRadioButton* angleUnitDeg = nullptr;
+ QRadioButton* angleUnitRad = nullptr;
+
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1906868bb6849768cce0becce8f43b37192dde01
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -0,0 +1,87 @@
+/*
+ * 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/>.
+ *
+ * @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 "VertexTableWidget.h"
+
+#include <QHeaderView>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ VertexTableWidget::VertexTableWidget()
+ {
+ QStringList columns{"Name", "X", "Y", "Yaw"};
+ setColumnCount(columns.size());
+ setHorizontalHeaderLabels(columns);
+ horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
+ horizontalHeader()->setVisible(true);
+
+ setEditTriggers(QAbstractItemView::NoEditTriggers);
+ setSortingEnabled(true);
+
+ setContextMenuPolicy(Qt::CustomContextMenu);
+ }
+
+
+ QTableWidgetItem*
+ VertexTableWidget::addVertex(graph::Graph::Vertex vertex)
+ {
+ Eigen::Matrix4f pose = vertex.attrib().getPose();
+
+ char format = 'f';
+ const int precision = 2;
+
+ int row = rowCount();
+ setRowCount(row + 1);
+ setItem(row, 0, new QTableWidgetItem {QString::fromStdString(vertex.attrib().getName())});
+ setItem(row, 1, new QTableWidgetItem {QString::number(qreal(pose(0, 3)), format, precision)});
+ setItem(row, 2, new QTableWidgetItem {QString::number(qreal(pose(1, 3)), format, precision)});
+ setItem(row, 3, new QTableWidgetItem {QString::number(qreal(getYawAngleDegree(pose)), format, precision)});
+
+ for (int col = 1; col <= 3; ++col)
+ {
+ item(row, col)->setTextAlignment(Qt::AlignRight);
+ }
+
+ return item(row, 0);
+ }
+
+
+ void
+ VertexTableWidget::updateVertex(GuiGraph::Vertex vertex)
+ {
+ QColor bgColor = vertex.attrib().highlighted ? bgColorSelected : bgColorDefault;
+ QFont font;
+ font.setBold(vertex.attrib().highlighted);
+
+ int row = this->row(vertex.attrib().tableWidgetItem);
+ for (int col = 0; col < 4; ++col)
+ {
+ auto* item = this->item(row, col);
+ ARMARX_CHECK_NOT_NULL(item);
+
+ item->setData(Qt::BackgroundRole, bgColor);
+ item->setFont(font);
+ }
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..31fbed28d2fd0319e3b0a6514ddded4552097932
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -0,0 +1,56 @@
+/*
+ * 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/>.
+ *
+ * @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/gui-plugins/LocationGraphEditor/GuiGraph.h>
+
+#include <QTableWidget>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ class VertexTableWidget : public QTableWidget
+ {
+ using This = VertexTableWidget;
+
+
+ public:
+
+ VertexTableWidget();
+
+ QTableWidgetItem* addVertex(graph::Graph::Vertex vertex);
+
+ void updateVertex(GuiGraph::Vertex vertex);
+
+
+ private slots:
+
+
+ public:
+
+ QColor bgColorDefault = QColor::fromRgb(255, 255, 255);
+ QColor bgColorSelected = QColor::fromRgb(0, 128, 255);
+
+ };
+
+}