Skip to content
Snippets Groups Projects
Commit 1dd5dc6e authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Implement adding vertices and creating new graphs; remove robot pose from graph aron

parent c96e887a
No related branches found
No related tags found
1 merge request!1Add Locations and Graph to Navigation Memory and Redesign Location Graph Editor
Showing
with 748 additions and 305 deletions
......@@ -26,7 +26,6 @@
#include <armarx/navigation/location/aron/Location.aron.generated.h>
#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/graph/Writer.h>
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
......@@ -373,7 +372,7 @@ namespace armarx::nav
nav::graph::Graph::Vertex& vertex = vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
toAron(vertex.attrib().aron.locationID, getLocationProviderSegmentID().withEntityName(name));
vertex.attrib().aron.globalRobotPose = globalNodePose->toEigen();
vertex.attrib().setPose(globalNodePose->toEigen());
nextVertexID++;
}
......
......@@ -22,8 +22,14 @@
#include "Graph.h"
#include <armarx/navigation/location/aron/Location.aron.generated.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::nav::graph
{
......@@ -33,14 +39,20 @@ namespace armarx::nav::graph
return aron.locationID.providerSegmentName + "/" + aron.locationID.entityName;
}
bool VertexAttribs::hasPose() const
{
return _pose.has_value();
}
Eigen::Matrix4f VertexAttribs::getPose() const
{
return aron.globalRobotPose;
ARMARX_CHECK(_pose.has_value());
return _pose.value();
}
void VertexAttribs::setPose(const Eigen::Matrix4f& pose)
{
this->aron.globalRobotPose = pose;
this->_pose = pose;
}
}
......@@ -87,4 +99,22 @@ namespace armarx::nav
ARMARX_CHECK_EQUAL(bo.numEdges(), dto.edges.size());
}
void graph::resolveLocations(Graph& bo, armem::wm::Memory& locationMemory)
{
for (graph::Graph::Vertex vertex : bo.vertices())
{
armem::MemoryID locationID;
fromAron(vertex.attrib().aron.locationID, locationID);
armem::wm::EntityInstance* instance = locationMemory.findLatestInstance(locationID);
if (instance)
{
nav::loc::arondto::Location dto;
dto.fromAron(instance->data());
vertex.attrib().setPose(dto.globalRobotPose);
}
}
}
}
......@@ -24,6 +24,8 @@
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
......@@ -36,13 +38,21 @@ namespace armarx::nav::graph
std::string getName() const;
bool hasPose() const;
Eigen::Matrix4f getPose() const;
void setPose(const Eigen::Matrix4f& pose);
private:
std::optional<Eigen::Matrix4f> _pose;
};
struct EdgeAttribs
{
armarx::nav::graph::arondto::Edge aron;
};
struct GraphAttribs
{
};
......@@ -52,5 +62,6 @@ namespace armarx::nav::graph
void toAron(arondto::Graph& dto, const Graph& bo);
void fromAron(const arondto::Graph& dto, Graph& bo);
void resolveLocations(Graph& bo, armem::wm::Memory& locationMemory);
}
......@@ -19,9 +19,10 @@
<armarx::armem::arondto::MemoryID />
</ObjectChild>
<ObjectChild key='globalRobotPose'>
<!-- Only stored in Location -->
<!--ObjectChild key='globalRobotPose'>
<Pose />
</ObjectChild>
</ObjectChild-->
</Object>
......
......@@ -20,6 +20,7 @@ set(SOURCES
widgets/default_colors.cpp
widgets/utils.cpp
widgets/EdgeTableWidget.cpp
widgets/NewEntityIdDialog.cpp
widgets/VertexDataWidget.cpp
widgets/VertexTableWidget.cpp
......@@ -37,6 +38,7 @@ set(HEADERS
widgets/default_colors.h
widgets/utils.h
widgets/EdgeTableWidget.h
widgets/NewEntityIdDialog.h
widgets/VertexDataWidget.h
widgets/VertexTableWidget.h
......
......@@ -15,18 +15,28 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QGroupBox" name="sceneGroupBox">
<widget class="QGroupBox" name="graphGroupBox">
<property name="title">
<string>Navigation Graphs from Navigation Graph Segment</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
<widget class="QPushButton" name="refreshGraphsButton">
<widget class="QPushButton" name="createGraphButton">
<property name="toolTip">
<string>Reloads the list of scenes from the memory</string>
</property>
<property name="text">
<string>Refresh List</string>
<string>Create New Graph</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="queryGraphsButton">
<property name="toolTip">
<string>Reloads the list of scenes from the memory</string>
</property>
<property name="text">
<string>Query</string>
</property>
</widget>
</item>
......@@ -110,6 +120,14 @@
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="robotVisuGroupBox">
<property name="title">
<string>Robot Visu</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_5"/>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="verticalLayout_13Widget">
......@@ -117,7 +135,7 @@
<item>
<widget class="QGroupBox" name="locationsTableGroupBox">
<property name="title">
<string>Locations (Graph Vertices)</string>
<string>Locations (right click for options)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_7">
<property name="leftMargin">
......@@ -142,7 +160,7 @@
<item>
<widget class="QGroupBox" name="edgesTableGroupBox">
<property name="title">
<string>Graph Edges</string>
<string>Graph Edges (right click for options)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_8">
<property name="leftMargin">
......
......@@ -21,8 +21,8 @@
*/
#pragma once
#include "FunctionalEventFilter.h"
#include "GuiGraph.h"
#include "Visu.h"
#include "widgets/graph_scene.h"
#include <armarx/navigation/graph/Graph.h>
......@@ -52,6 +52,10 @@ namespace armarx::viz
{
class Client;
}
namespace armarx::nav::locgrapheditor::utils
{
enum class EdgeDirection;
}
namespace armarx::nav::locgrapheditor
{
class EdgeTableWidget;
......@@ -127,59 +131,73 @@ namespace armarx::nav::locgrapheditor
signals:
void connected();
void memoryDataChanged();
void locationMemoryChanged();
void graphMemoryChanged();
void memoryChanged();
void graphChanged();
private slots:
// Model
// Load
void queryMemory();
armem::client::QueryResult queryLocations();
armem::client::QueryResult queryGraphs();
void queryGraphs();
void updateGraphList();
void loadGraph();
bool loadGraphDialog();
void setGraph(graph::Graph& nav);
GuiGraph::Vertex addVertex(graph::Graph::ConstVertex vertex);
GuiGraph::Edge addEdge(graph::Graph::ConstEdge edge);
GuiGraph::Edge addEdge(GuiGraph::ConstVertex source, GuiGraph::ConstVertex target,
const EdgeData& defaultAttribs);
void clearGraph();
void clearEdges();
void clearVertices();
void removeEdge(GuiGraph::Edge& edge);
void removeVertex(GuiGraph::Vertex& vertex);
void addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems);
void removeEdges(QList<QTableWidgetItem*> edgeItems);
// Commit
void commit();
armem::CommitResult commitLocations();
armem::EntityUpdateResult commitGraph();
// View & Tables
// View & Tables
void updateGraphList();
void updateGraphView();
void updateVertexView(GuiGraph::Vertex vertex);
void updateEdgeView(GuiGraph::Edge edge);
void updateArViz();
// Selection
// Selection & Highlighting
void selectVertex(QTableWidgetItem* vertexItem);
void selectVertex(GuiGraph::Vertex vertex);
void resetHighlighting();
void updateVertexHighlighting();
void updateEdgeHighlighting();
// Graph modification triggered by user interaction
void createVertexDialog();
void addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems);
void removeEdges(QList<QTableWidgetItem*> edgeItems);
void removeEdgesOfVertex(QList<QTableWidgetItem*> vertexItems, utils::EdgeDirection direction);
void createGraphDialog();
private:
void setGraph(const graph::Graph& nav);
GuiGraph::Vertex
addVertex(semrel::ShapeID vertexID, const VertexData& defaultAttribs);
GuiGraph::Edge
addEdge(GuiGraph::ConstVertex source, GuiGraph::ConstVertex target, const EdgeData& defaultAttribs);
void removeVertex(GuiGraph::Vertex& vertex);
void removeEdge(GuiGraph::Edge& edge);
void clearGraph();
void clearEdges();
void clearVertices();
QString getGraphDisplayName(const armem::MemoryID& entityID, bool changed = false) const;
......@@ -187,7 +205,7 @@ namespace armarx::nav::locgrapheditor
/// Widget Form
Ui::LocationGraphEditorWidget widget;
QPointer<SimpleConfigDialog> dialog;
QPointer<SimpleConfigDialog> configDialog;
struct Remote
......@@ -210,9 +228,13 @@ namespace armarx::nav::locgrapheditor
struct Model
{
armem::wm::Memory memory;
armem::wm::Memory locationsMemory;
armem::wm::Memory graphMemory;
armem::MemoryID graphEntityID;
GuiGraph graph;
GraphVisu visu;
};
Model model;
......@@ -228,30 +250,6 @@ namespace armarx::nav::locgrapheditor
View view;
// Non-refactored
#if 0
private slots:
void tableWidgetVerticesCustomContextMenu(QPoint pos);
void tableWidgetEdgesCustomContextMenu(QPoint pos);
private slots:
/// Add kitchen graph (H2T Armar3a robot kitchen)
void addKitchenGraph();
bool addNewEdge(const std::string& from, const std::string& to);
void addNewEdgeBoth();
void addNewEdgeStartEnd();
void addNewEdgeEndStart();
void addNewGraphVertex();
void editGraphVertex();
#endif
private:
QSettings settings;
......
......@@ -100,9 +100,15 @@ namespace armarx::nav::locgrapheditor
}
void EdgeTableWidget::removeEdge(GuiGraph::Edge& edge)
void
EdgeTableWidget::removeEdge(GuiGraph::Edge& edge)
{
this->removeRow(row(edge.attrib().tableWidgetItem));
if (currentItem() == edge.attrib().tableWidgetItem)
{
setCurrentItem(nullptr);
}
removeRow(row(edge.attrib().tableWidgetItem));
edge.attrib().tableWidgetItem = nullptr;
}
......@@ -114,7 +120,8 @@ namespace armarx::nav::locgrapheditor
}
void EdgeTableWidget::makeContextMenu(QPoint pos)
void
EdgeTableWidget::makeContextMenu(QPoint pos)
{
QList<QTableWidgetItem*> items = selectedEdgeItems();
......
/*
* 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 "NewEntityIdDialog.h"
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <QDialogButtonBox>
#include <QGridLayout>
#include <QLineEdit>
#include <QLabel>
#include <QPushButton>
#include <QVBoxLayout>
namespace armarx::nav::locgrapheditor
{
NewEntityIdDialog::NewEntityIdDialog(const armem::MemoryID& coreSegmentID, QWidget* parent) :
QDialog(parent),
coreSegmentID(std::make_unique<armem::MemoryID>(coreSegmentID))
{
_providerSegmentName = new QLineEdit(this);
_entityName = new QLineEdit(this);
_buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok
| QDialogButtonBox::Cancel, this);
QLabel* instruction = new QLabel("Enter provider segment name and entity name for new entity:");
QFont font = instruction->font();
font.setBold(true);
instruction->setFont(font);
QGridLayout* grid = new QGridLayout();
int col = 0;
grid->addWidget(new QLabel("Core Segment ID"), 0, col);
grid->addWidget(new QLabel(QString::fromStdString(coreSegmentID.str()) + "/"), 1, col);
++col;
grid->addWidget(new QLabel("Provider Segment Name"), 0, col);
grid->addWidget(_providerSegmentName, 1, col);
++col;
grid->addWidget(new QLabel("/"), 1, col);
++col;
grid->addWidget(new QLabel("Entity Name"), 0, col);
grid->addWidget(_entityName, 1, col);
++col;
QVBoxLayout* layout = new QVBoxLayout();
setLayout(layout);
layout->addWidget(instruction);
layout->addLayout(grid);
layout->addWidget(_buttonBox);
connect(_buttonBox, &QDialogButtonBox::accepted, this, &QDialog::accept);
connect(_buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject);
auto enableOkIfReady = [this]()
{
_buttonBox->button(QDialogButtonBox::Ok)
->setEnabled(_providerSegmentName->text().size() > 0 and _entityName->text().size() > 0);
};
connect(_providerSegmentName, &QLineEdit::textChanged, this, enableOkIfReady);
connect(_entityName, &QLineEdit::textChanged, this, enableOkIfReady);
enableOkIfReady();
}
NewEntityIdDialog::~NewEntityIdDialog()
{
}
QString NewEntityIdDialog::providerSegmentName() const
{
return _providerSegmentName->text();
}
QString NewEntityIdDialog::entityName() const
{
return _entityName->text();
}
armem::MemoryID NewEntityIdDialog::entityIDWithoutCoreSegmentID() const
{
armem::MemoryID id;
id.providerSegmentName = providerSegmentName().toStdString();
id.entityName = entityName().toStdString();
return id;
}
armem::MemoryID NewEntityIdDialog::entityID() const
{
armem::MemoryID entityID = entityIDWithoutCoreSegmentID();
ARMARX_CHECK_NOT_NULL(coreSegmentID);
entityID.setCoreSegmentID(*coreSegmentID);
return entityID;
}
}
/*
* 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 <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <QDialog>
#include <memory>
class QLineEdit;
class QDialogButtonBox;
namespace armarx::nav::locgrapheditor
{
class NewEntityIdDialog : public QDialog
{
public:
NewEntityIdDialog(const armem::MemoryID& coreSegmentID, QWidget* parent = nullptr);
virtual ~NewEntityIdDialog() override;
QString providerSegmentName() const;
QString entityName() const;
armem::MemoryID entityIDWithoutCoreSegmentID() const;
armem::MemoryID entityID() const;
private:
QLineEdit* _providerSegmentName = nullptr;
QLineEdit* _entityName = nullptr;
QDialogButtonBox* _buttonBox = nullptr;
std::unique_ptr<armem::MemoryID> coreSegmentID;
};
}
......@@ -57,10 +57,8 @@ namespace armarx::nav::locgrapheditor
QTableWidgetItem*
VertexTableWidget::addVertex(graph::Graph::ConstVertex vertex)
VertexTableWidget::addVertex()
{
(void) vertex;
// We need to disable sorting to prevent the new row from being moved away.
setSortingEnabled(false);
......@@ -142,12 +140,8 @@ namespace armarx::nav::locgrapheditor
setCurrentItem(nullptr);
}
const int numRows = rowCount();
int row = this->row(vertex.attrib().tableWidgetItem);
removeRow(row);
removeRow(row(vertex.attrib().tableWidgetItem));
vertex.attrib().tableWidgetItem = nullptr;
ARMARX_CHECK_EQUAL(rowCount(), numRows - 1);
}
......@@ -158,20 +152,22 @@ namespace armarx::nav::locgrapheditor
}
QString VertexTableWidget::_nameOf(QTableWidgetItem* item)
QString
VertexTableWidget::_nameOf(QTableWidgetItem* item)
{
return item->data(Qt::UserRole).toString();
}
void VertexTableWidget::makeContextMenu(QPoint pos)
void
VertexTableWidget::makeContextMenu(QPoint pos)
{
QList<QTableWidgetItem*> items = selectedVertexItems();
QMenu menu;
if (items.size() == 0)
{
QAction* action = menu.addAction("No locations selected");
QAction* action = menu.addSection("No locations selected");
action->setEnabled(false);
}
......@@ -229,7 +225,7 @@ namespace armarx::nav::locgrapheditor
using ListOfEdges = QList<QPair<Item*, Item*>>;
using AppendFunc = std::function<void(ListOfEdges& edges, Item* selected, Item* action)>;
auto addBulkActions = [this, &items](QMenu* submenu, AppendFunc appendFunc)
auto addBulkAddEdgeActions = [this, &items](QMenu* submenu, AppendFunc appendFunc)
{
if (items.size() == rowCount())
{
......@@ -242,8 +238,8 @@ namespace armarx::nav::locgrapheditor
if (items.count(action) == 0) // Do no generate self-edges
{
QAction* a = submenu->addAction(_nameOf(action));
connect(a, &QAction::triggered,
[this, &items, action, appendFunc]()
connect(a, &QAction::triggered, this,
[this, items, action, appendFunc]()
{
QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges;
for (auto* selected : items)
......@@ -256,24 +252,52 @@ namespace armarx::nav::locgrapheditor
}
};
addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowRight + " ..."),
addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowRight + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({selected, action});
});
addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowLeft + " ..."),
addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowLeft + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({action, selected});
});
addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowBoth + " ..."),
addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowBoth + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({selected, action});
edges.append({action, selected});
});
auto connectBulkRemoveEdgeAction = [this, &items](
QAction* action, utils::EdgeDirection edgeDirection)
{
connect(action, &QAction::triggered, this, [this, items, edgeDirection]()
{
emit edgeRemovalRequested(items, edgeDirection);
});
};
connectBulkRemoveEdgeAction(
menu.addAction("Remove all edges " + utils::arrowRight + " " + desc),
utils::EdgeDirection::To);
connectBulkRemoveEdgeAction(
menu.addAction("Remove all edges " + utils::arrowLeft + " " + desc),
utils::EdgeDirection::From);
connectBulkRemoveEdgeAction(
menu.addAction("Remove all edges " + utils::arrowBoth + " " + desc),
utils::EdgeDirection::Bidirectional);
}
menu.addSection("Manage Locations");
connect(menu.addAction("Create new location ..."), &QAction::triggered, this,
[this]()
{
emit newVertexRequested();
});
menu.exec(mapToGlobal(pos));
}
......
......@@ -30,9 +30,12 @@
#include <QTableWidget>
namespace armarx::nav::locgrapheditor::utils
{
enum class EdgeDirection;
}
namespace armarx::nav::locgrapheditor
{
class VertexTableWidget : public QTableWidget
{
Q_OBJECT
......@@ -44,7 +47,7 @@ namespace armarx::nav::locgrapheditor
VertexTableWidget();
QTableWidgetItem* addVertex(graph::Graph::ConstVertex vertex);
QTableWidgetItem* addVertex();
void updateVertex(GuiGraph::Vertex vertex);
......@@ -56,7 +59,10 @@ namespace armarx::nav::locgrapheditor
signals:
void newVertexRequested();
void newEdgesRequested(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges);
void edgeRemovalRequested(QList<QTableWidgetItem*> vertexItems, utils::EdgeDirection direction);
public slots:
......
......@@ -37,6 +37,7 @@ namespace armarx::nav::locgrapheditor
const QString utils::arrowBoth = QStringLiteral("\u21C4");
QList<QTableWidgetItem*>
utils::getSelectedItemsOfColumn(QTableWidget* widget, int column)
{
......
......@@ -22,6 +22,7 @@
#pragma once
#include <QList>
#include <QString>
class QTableWidget;
class QTableWidgetItem;
......@@ -30,9 +31,6 @@ class QTableWidgetItem;
namespace armarx::nav::locgrapheditor::utils
{
QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
/// <-
extern const QString arrowLeft;
/// ->
......@@ -41,4 +39,12 @@ namespace armarx::nav::locgrapheditor::utils
extern const QString arrowBoth;
enum class EdgeDirection
{
From, To, Bidirectional
};
QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment