diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index d2661dbac93f5ecba03215cc9dfad01921beffa3..0820ee1bebc4508809c90bad440adc3131c5f299 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -15,8 +15,10 @@ set(SOURCES
FunctionalEventFilter.cpp
GuiGraph.cpp
+ Visu.cpp
widgets/default_colors.cpp
+ widgets/utils.cpp
widgets/EdgeTableWidget.cpp
widgets/VertexDataWidget.cpp
widgets/VertexTableWidget.cpp
@@ -30,8 +32,10 @@ set(HEADERS
FunctionalEventFilter.h
GuiGraph.h
+ Visu.h
widgets/default_colors.h
+ widgets/utils.h
widgets/EdgeTableWidget.h
widgets/VertexDataWidget.h
widgets/VertexTableWidget.h
@@ -52,6 +56,7 @@ set(COMPONENT_LIBS
SimpleConfigDialog
# RobotAPI
+ ArViz
armem
Navigation::Location
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
index f4fd3e1d100fe63545cb55c7fd80ae870fb52a2d..49361f44c32a3388337661f3380056021bbd4592 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
@@ -59,24 +59,47 @@ namespace armarx::nav::locgrapheditor
return map;
}
+
+ std::map<QTableWidgetItem*, GuiGraph::Edge>
+ GuiGraph::getTableItemToEdgeMap()
+ {
+ std::map<QTableWidgetItem*, GuiGraph::Edge> map;
+
+ for (auto edge : edges())
+ {
+ if (edge.attrib().tableWidgetItem != nullptr)
+ {
+ map[edge.attrib().tableWidgetItem] = edge;
+ }
+ }
+
+ return map;
+ }
+
}
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]);
-}
+ float locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
+ {
+ return simox::math::rad_to_deg(simox::math::mat4f_to_rpy(pose)(2));
+ }
-auto locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
-{
- GuiGraph gui;
- for (auto v : nav.vertices())
+
+ double locgrapheditor::getYawAngleDegree(const Eigen::Matrix4d& pose)
{
- gui.addVertex(v.objectID(), { v.attrib() });
+ return simox::math::rad_to_deg(simox::math::mat4f_to_rpy(pose)(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() });
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
index 87395a1fb13350128ea78fd1d5cffa6ed2f761e7..33f4ddf584347a65d167734778b05da43fd7033c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
@@ -86,7 +86,7 @@ namespace armarx::nav::locgrapheditor
std::optional<Vertex> getVertexFromTableItem(QTableWidgetItem* item);
std::map<QTableWidgetItem*, Vertex> getTableItemToVertexMap();
-
+ std::map<QTableWidgetItem*, Edge> getTableItemToEdgeMap();
};
@@ -96,5 +96,6 @@ namespace armarx::nav::locgrapheditor
float getYawAngleDegree(const Eigen::Matrix4f& pose);
+ double getYawAngleDegree(const Eigen::Matrix4d& pose);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index e23d674833f93d45484578ff73f493f7a6012746..59de014fbcfe3a4dafe8069aff80ae67732d9735 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -20,7 +20,10 @@
* GNU General Public License
*/
+#include <VirtualRobot/VirtualRobot.h>
+
#include "LocationGraphEditorWidgetController.h"
+#include "Visu.h"
#include "widgets/EdgeTableWidget.h"
#include "widgets/VertexDataWidget.h"
#include "widgets/VertexTableWidget.h"
@@ -35,6 +38,7 @@
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -54,17 +58,6 @@
#include <sstream>
-/**
- * @brief The increment used when a rotation button is pressed.
- *
- * A positive rotation is counter clockwise.
- * This value should be positive.
- *
- * rotation buttons: LocationGraphEditorWidgetController::widget.buttonRotateClock and
- * LocationGraphEditorWidgetController::widget.buttonRotateCounterClock
- */
-static const float VIEW_ROTATE_STEP_SIZE_CC = 45;
-
static const QString SETTING_LAST_SCENE = "lastScene";
@@ -125,58 +118,36 @@ namespace armarx::nav::locgrapheditor
view.graph = new GraphSceneWidget();
widget.graphSceneLayout->addWidget(view.graph);
-#if 0
- auto eventFilter = [this](QObject* obj, QEvent* event) -> bool
- {
- 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();
- }
- else
- {
- return false;
- }
- };
- view.view->installEventFilter(new simox::gui::FunctionalEventFilter(eventFilter));
-#endif
// Widgets -> This
// Memory Access
+ connect(this, &This::connected, this, &This::queryGraphs);
+ connect(this, &This::memoryDataChanged, this, &This::updateGraphList);
connect(widget.refreshGraphsButton, &QPushButton::pressed, this, &This::updateGraphList);
+
connect(widget.loadGraphButton, &QPushButton::pressed, this, &This::loadGraph);
+ // Update views
+ connect(this, &This::graphChanged, this, &This::updateGraphView);
+ connect(view.vertexData, &VertexDataWidget::vertexDataChanged,
+ this, &This::updateGraphView);
+
+ // Selection
+ connect(view.vertexTable, &VertexTableWidget::currentItemChanged,
+ [this](QTableWidgetItem* current, QTableWidgetItem* previous)
+ {
+ (void) previous;
+ this->selectVertex(current);
+ });
+
+ connect(view.vertexTable, &VertexTableWidget::itemSelectionChanged,
+ this, &This::updateVertexHighlighting);
+ connect(view.edgeTable, &EdgeTableWidget::itemSelectionChanged,
+ this, &This::updateEdgeHighlighting);
+
+
#if 0
// Tables
connect(view.vertexTable, &QTableWidget::cellDoubleClicked, this, &This::vertexTableDoubleClicked);
@@ -190,30 +161,6 @@ namespace armarx::nav::locgrapheditor
connect(widget.btnAddEdgeEndStart, &QPushButton::pressed, this, &This::addNewEdgeEndStart);
connect(widget.btnEdit, &QPushButton::pressed, this, &This::editGraphVertex);
#endif
-
- // View
-
- // connect(view.graph->scene(), &GraphScene::vertexSelected, this, &This::toggleVertexSelected);
-
- // This -> This
-
- // Intra-connections.
- connect(this, &This::connected, this, &This::queryGraphs);
- connect(this, &This::memoryDataChanged, this, &This::updateGraphList);
-
-
- connect(view.vertexData, &VertexDataWidget::vertexDataChanged,
- this, &This::updateVertexView);
-
- connect(view.vertexTable, &VertexTableWidget::currentItemChanged,
- [this](QTableWidgetItem* current, QTableWidgetItem* previous)
- {
- (void) previous;
- this->selectVertex(current);
- });
-
- connect(view.vertexTable, &VertexTableWidget::itemSelectionChanged,
- this, &This::updateVertexHighlighting);
}
@@ -299,6 +246,8 @@ namespace armarx::nav::locgrapheditor
graphReader = memoryNameSystem.useReader(nav::graph::coreSegmentID);
graphWriter = memoryNameSystem.useWriter(nav::graph::coreSegmentID);
+
+ arviz = std::make_unique<viz::Client>(viz::Client::createForGuiPlugin(parent));
}
@@ -307,7 +256,7 @@ namespace armarx::nav::locgrapheditor
armem::client::QueryResult result = remote.graphReader.getLatestSnapshotsIn(nav::graph::coreSegmentID);
if (result.success)
{
- data.memory = std::move(result.memory);
+ model.memory = std::move(result.memory);
emit memoryDataChanged();
}
else
@@ -322,7 +271,7 @@ namespace armarx::nav::locgrapheditor
widget.graphsComboBox->clear();
bool enable = false;
- data.memory.forEachEntity([this, &enable](const armem::wm::Entity& entity)
+ model.memory.forEachEntity([this, &enable](const armem::wm::Entity& entity)
{
widget.graphsComboBox->addItem(QString::fromStdString(entity.id().str()));
enable = true;
@@ -333,18 +282,8 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::loadGraph()
{
- // Store vertex highlighting.
- std::vector<semrel::ShapeID> highlightedVertices;
- for (auto vertex : data.graph.vertices())
- {
- if (vertex.attrib().highlighted)
- {
- highlightedVertices.push_back(vertex.objectID());
- }
- }
-
const armem::MemoryID entityID = armem::MemoryID::fromString(widget.graphsComboBox->currentText().toStdString());
- const armem::wm::EntityInstance* instance = data.memory.findLatestInstance(entityID);
+ const armem::wm::EntityInstance* instance = model.memory.findLatestInstance(entityID);
if (not instance)
{
std::stringstream ss;
@@ -359,30 +298,39 @@ namespace armarx::nav::locgrapheditor
fromAron(dto, nav);
}
- clearGraph();
+ setGraph(nav);
+ }
+
+
+ void LocationGraphEditorWidgetController::setGraph(graph::Graph& nav)
+ {
+ // Store vertex highlighting.
+ std::set<std::string> highlightedVertices;
+ for (auto vertex : model.graph.vertices())
+ {
+ if (vertex.attrib().highlighted)
+ {
+ highlightedVertices.insert(vertex.attrib().getName());
+ }
+ }
- // Add vertices
+ // Build graph.
+ clearGraph();
for (auto vertex : nav.vertices())
{
addVertex(vertex);
}
-
- // Add edges
for (auto edge : nav.edges())
{
addEdge(edge);
}
// Restore vertex highlighting.
- for (const semrel::ShapeID& vertexID : highlightedVertices)
+ for (auto vertex : model.graph.vertices())
{
- for (auto vertex : data.graph.vertices())
+ if (highlightedVertices.count(vertex.attrib().getName()))
{
- if (vertex.objectID() == vertexID)
- {
- vertex.attrib().highlighted = true;
- updateVertexView(vertex);
- }
+ vertex.attrib().highlighted = true;
}
}
@@ -391,42 +339,52 @@ namespace armarx::nav::locgrapheditor
GuiGraph::Vertex
- LocationGraphEditorWidgetController::addVertex(graph::Graph::Vertex vertex)
+ LocationGraphEditorWidgetController::addVertex(graph::Graph::ConstVertex vertex)
{
- ARMARX_CHECK(not data.graph.hasVertex(vertex.objectID()));
+ ARMARX_CHECK(not model.graph.hasVertex(vertex.objectID()));
VertexData attrib { vertex.attrib() };
attrib.graphicsItem = view.graph->scene()->addVertex(vertex);
attrib.tableWidgetItem = view.vertexTable->addVertex(vertex);
- auto guiVertex = data.graph.addVertex(vertex.objectID(), attrib);
-
- updateVertexView(guiVertex);
- return guiVertex;
+ return model.graph.addVertex(vertex.objectID(), attrib);
}
GuiGraph::Edge
- LocationGraphEditorWidgetController::addEdge(graph::Graph::Edge edge)
+ LocationGraphEditorWidgetController::addEdge(graph::Graph::ConstEdge edge)
{
- ARMARX_CHECK(not data.graph.hasEdge(edge.sourceObjectID(), edge.targetObjectID()))
+ ARMARX_CHECK(not model.graph.hasEdge(edge.sourceObjectID(), edge.targetObjectID()))
<< "Edge must not exist before being added: '"
<< edge.source().attrib().getName() << "' -> '"
<< edge.target().attrib().getName() << "'";
- auto source = data.graph.vertex(edge.sourceObjectID());
- auto target = data.graph.vertex(edge.targetObjectID());
+ auto source = model.graph.vertex(edge.sourceObjectID());
+ auto target = model.graph.vertex(edge.targetObjectID());
EdgeData attrib { edge.attrib() };
attrib.tableWidgetItem = view.edgeTable->addEdge(edge);
attrib.graphicsItem = view.graph->scene()->addEdge(edge);
- auto guiEdge = data.graph.addEdge(source, target, attrib);
+ return model.graph.addEdge(source, target, attrib);
+ }
+
- updateEdgeView(guiEdge);
- return guiEdge;
+
+ void LocationGraphEditorWidgetController::updateGraphView()
+ {
+ for (auto vertex : model.graph.vertices())
+ {
+ updateVertexView(vertex);
+ }
+ for (auto edge : model.graph.edges())
+ {
+ updateEdgeView(edge);
+ }
+ updateArViz();
}
- void LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
+ void
+ LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
{
view.graph->scene()->updateVertex(vertex);
view.vertexTable->updateVertex(vertex);
@@ -435,7 +393,7 @@ namespace armarx::nav::locgrapheditor
{
// Highlight all edges between highlighted vertices
std::set<semrel::ShapeID> highlightedVertices;
- for (auto v : data.graph.vertices())
+ for (auto v : model.graph.vertices())
{
if (v.attrib().highlighted)
{
@@ -443,7 +401,7 @@ namespace armarx::nav::locgrapheditor
}
}
- for (auto edge : data.graph.edges())
+ for (auto edge : model.graph.edges())
{
bool verticesHighlighted =
highlightedVertices.count(edge.sourceObjectID())
@@ -454,54 +412,36 @@ namespace armarx::nav::locgrapheditor
if (edge.attrib().highlighted != verticesHighlighted)
{
edge.attrib().highlighted = verticesHighlighted;
- updateEdgeView(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::updateEdgeView(GuiGraph::Edge edge)
+ void
+ LocationGraphEditorWidgetController::updateEdgeView(GuiGraph::Edge edge)
{
view.graph->scene()->updateEdge(edge);
view.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(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);
-#endif
+
+ void LocationGraphEditorWidgetController::updateArViz()
+ {
+ if (remote.arviz)
+ {
+ viz::Layer layer = remote.arviz->layer(widget.graphsComboBox->currentText().toStdString());
+ GraphVisu visu;
+ visu.draw(layer, model.graph);
+ remote.arviz->commit(layer);
+ }
}
void LocationGraphEditorWidgetController::clearEdges()
{
// Remove from graphics scene
- for (auto edge : data.graph.edges())
+ for (auto edge : model.graph.edges())
{
view.graph->scene()->removeEdge(edge.attrib().graphicsItem);
}
@@ -511,12 +451,12 @@ namespace armarx::nav::locgrapheditor
view.edgeTable->setRowCount(0);
// Clear data structure
- while (data.graph.edges().begin() != data.graph.edges().end())
+ while (model.graph.edges().begin() != model.graph.edges().end())
{
- data.graph.removeEdge(*data.graph.edges().begin());
+ model.graph.removeEdge(*model.graph.edges().begin());
}
- // ToDo: Update ArViz
+ emit graphChanged();
}
@@ -532,148 +472,71 @@ namespace armarx::nav::locgrapheditor
view.vertexTable->setRowCount(0);
// Clear data structure
- data.graph.clear();
- }
-
+ model.graph.clear();
- void LocationGraphEditorWidgetController::toggleVertexSelected(const semrel::ShapeID& vertexID)
- {
- auto vertex = data.graph.vertex(vertexID);
- vertex.attrib().highlighted ^= true;
- updateVertexView(vertex);
+ emit graphChanged();
}
void LocationGraphEditorWidgetController::resetHighlighting()
{
- for (auto vertex : data.graph.vertices())
+ for (auto vertex : model.graph.vertices())
{
if (vertex.attrib().highlighted)
{
vertex.attrib().highlighted = false;
- updateVertexView(vertex);
}
}
- for (auto edge : data.graph.edges())
+ for (auto edge : model.graph.edges())
{
if (edge.attrib().highlighted)
{
edge.attrib().highlighted = false;
- updateEdgeView(edge);
}
}
+
+ emit graphChanged();
}
- void LocationGraphEditorWidgetController::updateVertexHighlighting()
+ template <class T>
+ static
+ void updateElementHighlighting(
+ QList<QTableWidgetItem*> selectedItems,
+ std::map<QTableWidgetItem*, T>&& itemToElementMap)
{
- std::map<QTableWidgetItem*, GuiGraph::Vertex> map = data.graph.getTableItemToVertexMap();
-
- for (QTableWidgetItem* selected : view.vertexTable->selectedEdgeItems())
+ for (QTableWidgetItem* selected : selectedItems)
{
- if (auto it = map.find(selected); it != map.end())
+ if (auto it = itemToElementMap.find(selected); it != itemToElementMap.end())
{
it->second.attrib().highlighted = true;
- updateVertexView(it->second);
- map.erase(it);
+ itemToElementMap.erase(it);
}
}
- for (auto& [_, unselected] : map)
+ for (auto& [_, unselected] : itemToElementMap)
{
unselected.attrib().highlighted = false;
- updateVertexView(unselected);
}
}
-#if 0
-
-
- void LocationGraphEditorWidgetController::highlightVertex(const std::string& vertexId, bool highlighted)
- {
- if (!hasVertex(vertexId))
- {
- ARMARX_WARNING << "No vertex: " << vertexId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
- return;
- }
-
- if (vertices.at(vertexId).highlighted != highlighted)
- {
- vertices.at(vertexId).highlighted = highlighted;
- updateVertex(vertexId);
- }
- }
-
-
- void LocationGraphEditorWidgetController::highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted)
- {
- if (!hasEdge(vertex1Id, vertex2Id))
- {
- ARMARX_WARNING << "No edge for: " << vertex1Id << " and " << vertex2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
- return;
- }
-
- EdgeId edge = toEdge(vertex1Id, vertex2Id);
-
- if (edges.at(edge).highlighted != highlighted)
- {
- edges.at(edge).highlighted = highlighted;
- updateEdge(edge);
- }
- }
-
-
- void LocationGraphEditorWidgetController::vertexTableDoubleClicked(int row, int)
- {
- auto vertexIt = std::find_if(vertices.cbegin(), vertices.cend(), [&](const std::pair<std::string, VertexData>& d)
- {
- // return d.attrib().vertex->getName() == view.vertexTable->item(row, 0)->text().toStdString();
- return d.attrib().tableWidgetVerticesIndex == row;
- });
- auto vertexId = vertexIt->second.vertex->getId();
- highlightVertex(vertexId);
- }
-
-
- void LocationGraphEditorWidgetController::edgeTableDoubleClicked(int row, int)
- {
- auto edgeIt = std::find_if(edges.cbegin(), edges.cend(), [&](const std::pair<EdgeId, EdgeData>& d)
- {
- return d.attrib().tableWidgetEdgesIndex == row;
- });
-
- edgeDoubleClicked(edgeIt->first);
- }
-
-
-
- void LocationGraphEditorWidgetController::edgeDoubleClicked(GuiGraph::Edge edge)
+ void LocationGraphEditorWidgetController::updateVertexHighlighting()
{
- // edges.at(id).highlighted ^= true;
- // updateEdge(id);
- 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());
+ updateElementHighlighting(view.vertexTable->selectedVertexItems(), model.graph.getTableItemToVertexMap());
+ emit graphChanged();
}
-#endif
-
-#if 0
- void LocationGraphEditorWidgetController::refreshGraph()
+ void LocationGraphEditorWidgetController::updateEdgeHighlighting()
{
- clearGraph();
- loadGraph();
+ updateElementHighlighting(view.edgeTable->selectedEdgeItems(), model.graph.getTableItemToEdgeMap());
+ emit graphChanged();
}
-#endif
-
void LocationGraphEditorWidgetController::selectVertex(QTableWidgetItem* vertexItem)
{
- if (auto vertex = data.graph.getVertexFromTableItem(vertexItem))
+ if (auto vertex = model.graph.getVertexFromTableItem(vertexItem))
{
selectVertex(vertex.value());
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index ded79af18d0f42d05163b250f2138803822a32f4..fe1610651b5e2a24a7da2b70e69364dcf8d700b4 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -41,12 +41,17 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <memory>
#include <string>
class QDialog;
+namespace armarx::viz
+{
+ class Client;
+}
namespace armarx::nav::locgrapheditor
{
class EdgeTableWidget;
@@ -133,8 +138,9 @@ namespace armarx::nav::locgrapheditor
void updateGraphList();
void loadGraph();
- GuiGraph::Vertex addVertex(graph::Graph::Vertex vertex);
- GuiGraph::Edge addEdge(graph::Graph::Edge edge);
+ void setGraph(graph::Graph& nav);
+ GuiGraph::Vertex addVertex(graph::Graph::ConstVertex vertex);
+ GuiGraph::Edge addEdge(graph::Graph::ConstEdge edge);
void clearEdges();
void clearGraph();
@@ -142,8 +148,10 @@ namespace armarx::nav::locgrapheditor
// View & Tables
+ void updateGraphView();
void updateVertexView(GuiGraph::Vertex vertex);
void updateEdgeView(GuiGraph::Edge edge);
+ void updateArViz();
// Selection
@@ -153,7 +161,7 @@ namespace armarx::nav::locgrapheditor
void resetHighlighting();
void updateVertexHighlighting();
- void toggleVertexSelected(const semrel::ShapeID& vertexID);
+ void updateEdgeHighlighting();
private:
@@ -174,18 +182,19 @@ namespace armarx::nav::locgrapheditor
armem::client::Reader graphReader;
armem::client::Writer graphWriter;
+ std::unique_ptr<viz::Client> arviz;
void connect(Component& parent);
};
Remote remote;
- struct Data
+ struct Model
{
armem::wm::Memory memory;
GuiGraph graph;
};
- Data data;
+ Model model;
struct View
@@ -204,11 +213,6 @@ namespace armarx::nav::locgrapheditor
#if 0
private slots:
- void highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted = true);
- void highlightVertex(const std::string& vertexId, bool highlighted = true);
-
- void refreshGraph();
-
void tableWidgetVerticesCustomContextMenu(QPoint pos);
void tableWidgetEdgesCustomContextMenu(QPoint pos);
@@ -218,37 +222,6 @@ namespace armarx::nav::locgrapheditor
/// Add kitchen graph (H2T Armar3a robot kitchen)
void addKitchenGraph();
- /**
- * @brief Toggles the double clicked vertex's selection state.
- * @param row Identifies the vertex.
- */
- void vertexTableDoubleClicked(int row, int);
- /**
- * @brief Toggles the double clicked edge's selection state.
- * @param row Identifies the edge.
- */
- void edgeTableDoubleClicked(int row, int);
-
- /**
- * @brief Toggles the double clicked edge's selection state.
- * @param id Identifies the edge.
- */
- void edgeDoubleClicked(GuiGraph::Edge edge);
-
- /**
- * @brief Rotates the view clockwise.
- *
- * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
- */
- void viewRotatedClock();
-
- /**
- * @brief Rotates the view counter clockwise.
- *
- * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
- */
- void viewRotatedCounterClock();
-
bool addNewEdge(const std::string& from, const std::string& to);
void addNewEdgeBoth();
void addNewEdgeStartEnd();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d978ffb66a7618c6f13af5f49b195fb72c0756b2
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
@@ -0,0 +1,121 @@
+/*
+ * 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 <VirtualRobot/VirtualRobot.h>
+
+#include "Visu.h"
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose.h>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ const simox::Color visu::defaultColorHighlighted = simox::Color::orange();
+
+
+ viz::Pose VertexVisu::Pose::draw(const VertexData& attribs) const
+ {
+ viz::Pose pose = nav::graph::VertexVisu::Pose::draw(attribs);
+ if (attribs.highlighted)
+ {
+ pose.scale(scale * scaleFactorHighlighted);
+ }
+ return pose;
+ }
+
+
+ viz::Arrow VertexVisu::ForwardArrow::draw(const VertexData& attribs) const
+ {
+ viz::Arrow arrow = nav::graph::VertexVisu::ForwardArrow::draw(attribs);
+ if (attribs.highlighted)
+ {
+ arrow.color(colorHighlighted);
+ arrow.width(width * 1.5f);
+ }
+ return arrow;
+ }
+
+
+ void VertexVisu::draw(viz::Layer& layer, const VertexData& attribs) const
+ {
+ if (pose.has_value())
+ {
+ layer.add(pose->draw(attribs));
+ }
+ if (forwardArrow.has_value())
+ {
+ layer.add(forwardArrow->draw(attribs));
+ }
+ }
+
+
+ viz::Arrow EdgeVisu::Arrow::draw(GuiGraph::ConstEdge edge) const
+ {
+ return draw(edge.attrib(), edge.source().attrib(), edge.target().attrib());
+ }
+
+
+ viz::Arrow EdgeVisu::Arrow::draw(
+ const EdgeData& edge,
+ const VertexData& source,
+ const VertexData& target) const
+ {
+ viz::Arrow arrow = nav::graph::EdgeVisu::Arrow::draw(edge, source, target);
+ if (edge.highlighted)
+ {
+ arrow.color(colorHighlighted);
+ }
+ return arrow;
+ }
+
+
+ void EdgeVisu::draw(viz::Layer& layer, GuiGraph::ConstEdge edge) const
+ {
+ if (arrow.has_value())
+ {
+ layer.add(arrow->draw(edge));
+ }
+ }
+
+
+ void GraphVisu::draw(viz::Layer& layer, const GuiGraph& graph)
+ {
+ if (vertex.has_value())
+ {
+ for (GuiGraph::ConstVertex v : graph.vertices())
+ {
+ vertex->draw(layer, v.attrib());
+ }
+ }
+ if (edge.has_value())
+ {
+ for (GuiGraph::ConstEdge e : graph.edges())
+ {
+ edge->draw(layer, e);
+ }
+ }
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e77c8bef52fb3eced552a3515a554e48c7e2a49
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
@@ -0,0 +1,88 @@
+/*
+ * 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 "GuiGraph.h"
+
+#include <armarx/navigation/graph/Visu.h>
+
+#include <SimoxUtility/color/Color.h>
+
+#include <optional>
+
+
+namespace armarx::nav::locgrapheditor::visu
+{
+ extern const simox::Color defaultColorHighlighted;
+}
+namespace armarx::nav::locgrapheditor
+{
+
+ struct VertexVisu
+ {
+ struct Pose : public nav::graph::VertexVisu::Pose
+ {
+ float scaleFactorHighlighted = 1.5;
+
+ viz::Pose draw(const VertexData& attribs) const;
+ };
+ std::optional<Pose> pose = Pose {};
+
+ struct ForwardArrow : public nav::graph::VertexVisu::ForwardArrow
+ {
+ simox::Color colorHighlighted = visu::defaultColorHighlighted;
+
+ viz::Arrow draw(const VertexData& attribs) const;
+ };
+ std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
+
+
+ void draw(viz::Layer& layer, const VertexData& attribs) const;
+ };
+
+
+ struct EdgeVisu
+ {
+ struct Arrow : public nav::graph::EdgeVisu::Arrow
+ {
+ simox::Color colorHighlighted = visu::defaultColorHighlighted;
+
+ viz::Arrow draw(GuiGraph::ConstEdge edge) const;
+ viz::Arrow draw(const EdgeData& edge, const VertexData& source, const VertexData& target) const;
+ };
+ std::optional<Arrow> arrow = Arrow {};
+
+
+ void draw(viz::Layer& layer, GuiGraph::ConstEdge edge) const;
+ };
+
+
+ struct GraphVisu
+ {
+ std::optional<VertexVisu> vertex = VertexVisu {};
+ std::optional<EdgeVisu> edge = EdgeVisu {};
+
+
+ void draw(viz::Layer& layer, const GuiGraph& graph);
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index 09a1b2b184244a0ceef0bd3e5078e8815aa15377..6c60fac3a54c34e66e07081d6ce6299a3052fc43 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -21,6 +21,10 @@
#include "EdgeTableWidget.h"
+#include "utils.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <QHeaderView>
@@ -33,17 +37,23 @@ namespace armarx::nav::locgrapheditor
setColumnCount(columns.size());
setHorizontalHeaderLabels(columns);
horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
+ horizontalHeader()->setResizeMode(1, QHeaderView::Stretch);
horizontalHeader()->setVisible(true);
setEditTriggers(QAbstractItemView::NoEditTriggers);
setSortingEnabled(true);
+ QString styleSheet = this->styleSheet();
+ ARMARX_IMPORTANT << VAROUT(styleSheet.toStdString());
+ styleSheet = styleSheet + "\n" + "selection-background-color: orange;";
+ this->setStyleSheet(styleSheet);
+
setContextMenuPolicy(Qt::CustomContextMenu);
}
QTableWidgetItem*
- EdgeTableWidget::addEdge(graph::Graph::Edge edge)
+ EdgeTableWidget::addEdge(graph::Graph::ConstEdge edge)
{
int row = rowCount();
setRowCount(row + 1);
@@ -73,4 +83,11 @@ namespace armarx::nav::locgrapheditor
}
+ QList<QTableWidgetItem*>
+ EdgeTableWidget::selectedEdgeItems()
+ {
+ return utils::getSelectedItemsOfColumn(this, 0);
+ }
+
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index 0991665da806feb0b0e3357ad945ce008be71a85..bf6a496fbb2276fd4bdca3e7a58b3979f4b0804a 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -42,7 +42,7 @@ namespace armarx::nav::locgrapheditor
EdgeTableWidget();
- QTableWidgetItem* addEdge(graph::Graph::Edge edge);
+ QTableWidgetItem* addEdge(graph::Graph::ConstEdge edge);
void updateEdge(GuiGraph::Edge edge);
@@ -51,6 +51,9 @@ namespace armarx::nav::locgrapheditor
void clearEdges();
+ QList<QTableWidgetItem*> selectedEdgeItems();
+
+
private slots:
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
index e79324ea503f9e76ca589167a0ac579ad3989c02..d34dab046971d80dd22b8846d81f3fb1d459a573 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -68,18 +68,11 @@ namespace armarx::nav::locgrapheditor
pos->setSuffix(" mm");
pos->setMinimum(-1e6);
pos->setMaximum(+1e6);
- pos->setSingleStep(10);
- pos->setValue(0);
- }
- for (QDoubleSpinBox* angle : _angleSpinBoxes())
- {
- angle->setSuffix("");
- angle->setMinimum(-360);
- angle->setMaximum(+360);
- angle->setValue(0);
+ pos->setSingleStep(50);
}
for (QDoubleSpinBox* spinBox : _allSpinBoxes())
{
+ spinBox->setValue(0);
spinBox->setAlignment(Qt::AlignRight);
}
@@ -107,21 +100,38 @@ namespace armarx::nav::locgrapheditor
for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
{
- connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
+ connect(angleUnit, &QRadioButton::toggled,
+ this, &This::_updateAngleUnit);
+ }
+
+ connect(frame, &QLineEdit::editingFinished,
+ this, &This::_updateVertexAttribs);
+ for (QDoubleSpinBox* spinBox : _allSpinBoxes())
+ {
+ connect(spinBox, QOverload<qreal>::of(&QDoubleSpinBox::valueChanged),
+ this, &This::_updateVertexAttribs);
}
+
angleUnitDeg->click();
}
+ std::optional<GuiGraph::Vertex> VertexDataWidget::vertex()
+ {
+ return _vertex;
+ }
+
+
void VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
{
- setFromVertex(vertex);
+ _vertex = vertex;
+ _setFromVertex(vertex);
setEnabled(true);
}
- void VertexDataWidget::setFromVertex(const GuiGraph::Vertex& vertex)
+ void VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
{
const VertexData& attrib = vertex.attrib();
const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
@@ -133,7 +143,7 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::getToVertex(GuiGraph::Vertex& vertex)
+ void VertexDataWidget::_getToVertex(GuiGraph::Vertex& vertex)
{
VertexData& attrib = vertex.attrib();
@@ -181,33 +191,60 @@ namespace armarx::nav::locgrapheditor
void VertexDataWidget::_updateAngleUnit()
{
- QString suffix;
- if (angleUnitDeg->isChecked())
- {
- suffix = " \u00b0";
- }
- else if (angleUnitRad->isChecked())
+ std::function<double(double)> convertValue;
+ QString suffix = " \u00b0";
+ double min = -360;
+ double max = +360;
+ double step = 5.;
+ int decimals = 2;
+
+ if (angleUnitRad->isChecked())
{
+ convertValue = [](double deg)
+ {
+ return simox::math::deg_to_rad(deg);
+ };
suffix = " rad";
+ min = simox::math::deg_to_rad(min);
+ max = simox::math::deg_to_rad(max);
+ step = simox::math::deg_to_rad(step);
+ decimals = 3;
}
else
{
- return;
+ ARMARX_CHECK(angleUnitDeg->isChecked());
+ convertValue = [](double rad)
+ {
+ return simox::math::rad_to_deg(rad);
+ };
}
for (QDoubleSpinBox* angle : _angleSpinBoxes())
{
+ angle->blockSignals(true);
+
angle->setSuffix(suffix);
+ angle->setMinimum(min);
+ angle->setMaximum(max);
+ angle->setSingleStep(step);
+ angle->setDecimals(decimals);
+ }
+ if (_vertex.has_value())
+ {
+ _setRpyRad(simox::math::mat4f_to_rpy(_vertex->attrib().getPose().cast<qreal>()));
+ }
+ for (QDoubleSpinBox* angle : _angleSpinBoxes())
+ {
+ angle->blockSignals(false);
}
}
void VertexDataWidget::_updateVertexAttribs()
{
- if (vertex.has_value())
+ if (_vertex.has_value())
{
- getToVertex(vertex.value());
-
- emit vertexDataChanged(vertex.value());
+ _getToVertex(_vertex.value());
+ emit vertexDataChanged();
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
index 75d17e33b0bb8ff32314da64b30806338903d9d2..64b7971e236749b0a5c2f51e440e12c34d88dd7c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -46,11 +46,9 @@ namespace armarx::nav::locgrapheditor
VertexDataWidget();
- void setVertex(GuiGraph::Vertex vertex);
-
- void setFromVertex(const GuiGraph::Vertex& vertex);
- void getToVertex(GuiGraph::Vertex& vertex);
+ std::optional<GuiGraph::Vertex> vertex();
+ void setVertex(GuiGraph::Vertex vertex);
Eigen::Vector3d xyz() const;
Eigen::Vector3d rpyDeg() const;
@@ -59,7 +57,7 @@ namespace armarx::nav::locgrapheditor
signals:
- void vertexDataChanged(GuiGraph::Vertex vertex);
+ void vertexDataChanged();
private slots:
@@ -70,6 +68,9 @@ namespace armarx::nav::locgrapheditor
private:
+ void _setFromVertex(const GuiGraph::Vertex& vertex);
+ void _getToVertex(GuiGraph::Vertex& vertex);
+
std::vector<QDoubleSpinBox*> _positionSpinBoxes();
std::vector<QDoubleSpinBox*> _angleSpinBoxes();
std::vector<QDoubleSpinBox*> _allSpinBoxes();
@@ -84,7 +85,7 @@ namespace armarx::nav::locgrapheditor
private:
- std::optional<GuiGraph::Vertex> vertex;
+ std::optional<GuiGraph::Vertex> _vertex;
QLineEdit* locationID = nullptr;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 7df6d1b71498ef961f14ef3996667f3e4205bf56..c06f36de491895a6a86694b9ad07f5806dc1a376 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -20,6 +20,7 @@
*/
#include "VertexTableWidget.h"
+#include "utils.h"
#include <QHeaderView>
@@ -29,7 +30,7 @@ namespace armarx::nav::locgrapheditor
VertexTableWidget::VertexTableWidget()
{
- QStringList columns{"Name", "X", "Y", "Yaw"};
+ QStringList columns{"Name", "X [mm]", "Y [mm]", "Yaw [\u00b0]"};
setColumnCount(columns.size());
setHorizontalHeaderLabels(columns);
horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
@@ -45,20 +46,18 @@ namespace armarx::nav::locgrapheditor
QTableWidgetItem*
- VertexTableWidget::addVertex(graph::Graph::Vertex vertex)
+ VertexTableWidget::addVertex(graph::Graph::ConstVertex vertex)
{
- Eigen::Matrix4f pose = vertex.attrib().getPose();
-
- char format = 'f';
- const int precision = 2;
+ (void) vertex;
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)});
+ // Just fill with vanilla items, they will get values in the update.
+ for (int col = 0; col < 4; ++col)
+ {
+ setItem(row, col, new QTableWidgetItem {});
+ }
for (int col = 1; col <= 3; ++col)
{
item(row, col)->setTextAlignment(Qt::AlignRight | Qt::AlignVCenter);
@@ -71,11 +70,20 @@ namespace armarx::nav::locgrapheditor
void
VertexTableWidget::updateVertex(GuiGraph::Vertex vertex)
{
+ const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
+ char format = 'f';
+ const int precision = 2;
+
+ int row = this->row(vertex.attrib().tableWidgetItem);
+ item(row, 0)->setText(QString::fromStdString(vertex.attrib().getName()));
+ item(row, 1)->setText(QString::number(pose(0, 3), format, precision));
+ item(row, 2)->setText(QString::number(pose(1, 3), format, precision));
+ item(row, 3)->setText(QString::number(getYawAngleDegree(pose), format, precision));
+
+
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);
@@ -87,27 +95,10 @@ namespace armarx::nav::locgrapheditor
}
-
QList<QTableWidgetItem*>
- VertexTableWidget::selectedEdgeItems()
+ VertexTableWidget::selectedVertexItems()
{
- // Only return items from the first column (and avoid duplicates).
- std::set<QTableWidgetItem*> set;
- for (QTableWidgetItem* selected : selectedItems())
- {
- if (column(selected) != 0)
- {
- selected = item(row(selected), 0);
- }
- set.insert(selected);
- }
-
- QList<QTableWidgetItem*> list;
- for (auto i : set)
- {
- list.append(i);
- }
- return list;
+ return utils::getSelectedItemsOfColumn(this, 0);
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index 6a8f24a75da194f582619e0df9c4e02a31010e9f..7fea8530d486ba05e3d9bf328653bc5462728da9 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -42,12 +42,12 @@ namespace armarx::nav::locgrapheditor
VertexTableWidget();
- QTableWidgetItem* addVertex(graph::Graph::Vertex vertex);
+ QTableWidgetItem* addVertex(graph::Graph::ConstVertex vertex);
void updateVertex(GuiGraph::Vertex vertex);
- QList<QTableWidgetItem*> selectedEdgeItems();
+ QList<QTableWidgetItem*> selectedVertexItems();
public slots:
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
index 059cb71ceaa32b1218b3d1245ea89fb357e61b88..fd0262011963248aee416e40e08cf352a8aee016 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
@@ -28,6 +28,6 @@ namespace armarx::nav::locgrapheditor
{
const QColor default_colors::tableBackgroundDefault = QColor::fromRgb(255, 255, 255);
- const QColor default_colors::tableBackgroundSelected = QColor::fromRgb(128, 196, 255);
+ const QColor default_colors::tableBackgroundSelected = QColor::fromRgb(255, 210, 160);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
index d66a24e55615b8b86d8597b09c56c4a61a1db486..32c3338219fb5595799e820f2bc54d214d408ca6 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
@@ -11,7 +11,7 @@ namespace armarx::nav::locgrapheditor::graph_scene
{
QGraphicsEllipseItem*
- Scene::addVertex(const graph::Graph::Vertex& vertex)
+ Scene::addVertex(graph::Graph::ConstVertex vertex)
{
const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
@@ -36,7 +36,7 @@ namespace armarx::nav::locgrapheditor::graph_scene
QGraphicsLineItem*
- Scene::addEdge(const graph::Graph::Edge& edge)
+ Scene::addEdge(graph::Graph::ConstEdge edge)
{
semrel::ShapeID sourceID = edge.sourceObjectID();
semrel::ShapeID targetID = edge.targetObjectID();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
index f1b1ac37a186e289b5cea0cbcd23ced1b410f117..95965d34cf09eb2d1bd9a9d390c00ba2fa24c44e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
@@ -24,8 +24,8 @@ namespace armarx::nav::locgrapheditor::graph_scene
using QGraphicsScene::QGraphicsScene;
- QGraphicsEllipseItem* addVertex(const graph::Graph::Vertex& vertex);
- QGraphicsLineItem* addEdge(const graph::Graph::Edge& Edge);
+ QGraphicsEllipseItem* addVertex(graph::Graph::ConstVertex vertex);
+ QGraphicsLineItem* addEdge(graph::Graph::ConstEdge Edge);
void updateVertex(GuiGraph::Vertex& vertex);
void updateEdge(GuiGraph::Edge& edge);
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
index e05f7e6be1fb604701496271d440503085161655..7cdcbcd84bb6641f82020281428990c328c3625d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
@@ -48,6 +48,51 @@ namespace armarx::nav::locgrapheditor::graph_scene
// Initial transform.
transform();
+
+#if 0
+ auto eventFilter = [this](QObject* obj, QEvent* event) -> bool
+ {
+ 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();
+ }
+ else
+ {
+ return false;
+ }
+ };
+ _view->installEventFilter(new simox::gui::FunctionalEventFilter(eventFilter));
+#endif
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c81e5509a60eae4024a635bcf1ee72705b84d732
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
@@ -0,0 +1,53 @@
+/*
+ * 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 "utils.h"
+
+#include <QTableWidget>
+
+#include <set>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ QList<QTableWidgetItem*>
+ utils::getSelectedItemsOfColumn(QTableWidget* widget, int column)
+ {
+ std::set<QTableWidgetItem*> set;
+ for (QTableWidgetItem* selected : widget->selectedItems())
+ {
+ if (widget->column(selected) != column)
+ {
+ selected = widget->item(widget->row(selected), 0);
+ }
+ set.insert(selected);
+ }
+
+ QList<QTableWidgetItem*> list;
+ for (auto i : set)
+ {
+ list.append(i);
+ }
+ return list;
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c344f01a5320f5d92e3cb12d52ec4a99df103e2
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
@@ -0,0 +1,35 @@
+/*
+ * 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 <QList>
+
+class QTableWidget;
+class QTableWidgetItem;
+
+
+namespace armarx::nav::locgrapheditor::utils
+{
+
+ QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
+
+}