diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
index 3513a795e4509898fb747d6653e0f92ccf87b30d..0b5fb30b720cdc5965d72622a244d84259a306ba 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
@@ -12,12 +12,11 @@ namespace simox::gui
class FunctionalEventFilter : public QObject
{
- Q_OBJECT
-
public:
using Function = std::function<bool(QObject* obj, QEvent* event)>;
+
FunctionalEventFilter(Function&& function);
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
index 514bd6a403e1d21b43e60bbfdf9d3c14988b8220..099f41583937ee78a669e006de963a9138dff8dd 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
@@ -17,6 +17,7 @@ namespace semrel
}
namespace armarx::nav::locgrapheditor
{
+
class GraphScene : public QGraphicsScene
{
Q_OBJECT
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index da5444cf3d46a7b94b382c9a9b63fed55d8fa22c..09a1b2b184244a0ceef0bd3e5078e8815aa15377 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -72,4 +72,5 @@ namespace armarx::nav::locgrapheditor
}
}
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index f0266930f4e3b1e9e295a6825c32848ebe7f977b..0991665da806feb0b0e3357ad945ce008be71a85 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -33,6 +33,7 @@ namespace armarx::nav::locgrapheditor
class EdgeTableWidget : public QTableWidget
{
+ Q_OBJECT
using This = EdgeTableWidget;
@@ -40,10 +41,15 @@ namespace armarx::nav::locgrapheditor
EdgeTableWidget();
+
QTableWidgetItem* addEdge(graph::Graph::Edge edge);
void updateEdge(GuiGraph::Edge edge);
+ void removeEdge(GuiGraph::Edge edge);
+
+ void clearEdges();
+
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 b9df399353b2474dd47bac95f91a25792b2a04b0..e79324ea503f9e76ca589167a0ac579ad3989c02 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -22,19 +22,33 @@
#include "VertexDataWidget.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+
#include <QDoubleSpinBox>
#include <QFormLayout>
#include <QHBoxLayout>
#include <QLineEdit>
#include <QRadioButton>
+#include <SimoxUtility/math/convert/deg_to_rad.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <SimoxUtility/math/convert/rad_to_deg.h>
+#include <SimoxUtility/math/convert/rpy_to_mat4f.h>
+#include <SimoxUtility/math/pose/pose.h>
+
namespace armarx::nav::locgrapheditor
{
VertexDataWidget::VertexDataWidget()
{
+ setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
+
locationID = new QLineEdit(this);
+ locationID->setReadOnly(true);
+
frame = new QLineEdit(this);
x = new QDoubleSpinBox(this);
@@ -60,9 +74,9 @@ namespace armarx::nav::locgrapheditor
for (QDoubleSpinBox* angle : _angleSpinBoxes())
{
angle->setSuffix("");
- pos->setMinimum(-360);
- pos->setMaximum(+360);
- pos->setValue(0);
+ angle->setMinimum(-360);
+ angle->setMaximum(+360);
+ angle->setValue(0);
}
for (QDoubleSpinBox* spinBox : _allSpinBoxes())
{
@@ -90,6 +104,7 @@ namespace armarx::nav::locgrapheditor
// Connect
+
for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
{
connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
@@ -99,6 +114,71 @@ namespace armarx::nav::locgrapheditor
}
+ void VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
+ {
+ setFromVertex(vertex);
+ setEnabled(true);
+ }
+
+
+ void VertexDataWidget::setFromVertex(const GuiGraph::Vertex& vertex)
+ {
+ const VertexData& attrib = vertex.attrib();
+ const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
+
+ locationID->setText(QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
+ frame->setText("<WIP>");
+ _setXyz(simox::math::position(pose));
+ _setRpyRad(simox::math::mat4f_to_rpy(pose));
+ }
+
+
+ void VertexDataWidget::getToVertex(GuiGraph::Vertex& vertex)
+ {
+ VertexData& attrib = vertex.attrib();
+
+ // locationID is read-only
+ // frame->setText("<WIP>"); // WIP
+
+ Eigen::Matrix4d pose = simox::math::pose(xyz(), simox::math::rpy_to_mat3f(rpyRad()));
+ attrib.setPose(pose.cast<float>());
+ }
+
+
+ Eigen::Vector3d VertexDataWidget::xyz() const
+ {
+ return { x->value(), y->value(), z->value() };
+ }
+
+
+ Eigen::Vector3d VertexDataWidget::rpyDeg() const
+ {
+ Eigen::Vector3d raw = _rpyRaw();
+ if (angleUnitRad->isChecked())
+ {
+ return {}; // return simox::math::rad_to_deg(raw);
+ }
+ else
+ {
+ return raw;
+ }
+ }
+
+
+ Eigen::Vector3d VertexDataWidget::rpyRad() const
+ {
+ Eigen::Vector3d raw = _rpyRaw();
+ if (angleUnitDeg->isChecked())
+ {
+ return simox::math::deg_to_rad(raw);
+ }
+ else
+ {
+ return raw;
+ }
+ }
+
+
void VertexDataWidget::_updateAngleUnit()
{
QString suffix;
@@ -121,19 +201,80 @@ namespace armarx::nav::locgrapheditor
}
+ void VertexDataWidget::_updateVertexAttribs()
+ {
+ if (vertex.has_value())
+ {
+ getToVertex(vertex.value());
+
+ emit vertexDataChanged(vertex.value());
+ }
+ }
+
+
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 };
}
+
+ void VertexDataWidget::_setXyz(const Eigen::Vector3d& xyz)
+ {
+ x->setValue(xyz.x());
+ y->setValue(xyz.y());
+ z->setValue(xyz.z());
+ }
+
+
+ Eigen::Vector3d VertexDataWidget::_rpyRaw() const
+ {
+ return { roll->value(), pitch->value(), yaw->value() };
+ }
+
+
+ void VertexDataWidget::_setRpyRaw(const Eigen::Vector3d& rpy) const
+ {
+ roll->setValue(rpy(0));
+ pitch->setValue(rpy(1));
+ yaw->setValue(rpy(2));
+ }
+
+
+ void VertexDataWidget::_setRpyDeg(const Eigen::Vector3d& rpyDeg) const
+ {
+ if (angleUnitDeg->isChecked())
+ {
+ _setRpyRaw(rpyDeg);
+ }
+ else if (angleUnitRad->isChecked())
+ {
+ _setRpyRaw(simox::math::deg_to_rad(rpyDeg));
+ }
+ }
+
+
+ void VertexDataWidget::_setRpyRad(const Eigen::Vector3d& rpyRad) const
+ {
+ if (angleUnitDeg->isChecked())
+ {
+ _setRpyRaw(simox::math::rad_to_deg(rpyRad));
+ }
+ else if (angleUnitRad->isChecked())
+ {
+ _setRpyRaw(rpyRad);
+ }
+ }
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
index b96b8cbbe822574e9218d0191e1646b577dc4a26..75d17e33b0bb8ff32314da64b30806338903d9d2 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -23,6 +23,8 @@
#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+#include <Eigen/Core>
+
#include <QWidget>
@@ -36,6 +38,7 @@ namespace armarx::nav::locgrapheditor
class VertexDataWidget : public QWidget
{
+ Q_OBJECT
using This = VertexDataWidget;
@@ -43,11 +46,26 @@ namespace armarx::nav::locgrapheditor
VertexDataWidget();
+ void setVertex(GuiGraph::Vertex vertex);
+
+ void setFromVertex(const GuiGraph::Vertex& vertex);
+ void getToVertex(GuiGraph::Vertex& vertex);
+
+
+ Eigen::Vector3d xyz() const;
+ Eigen::Vector3d rpyDeg() const;
+ Eigen::Vector3d rpyRad() const;
+
+
+ signals:
+
+ void vertexDataChanged(GuiGraph::Vertex vertex);
private slots:
void _updateAngleUnit();
+ void _updateVertexAttribs();
private:
@@ -56,9 +74,19 @@ namespace armarx::nav::locgrapheditor
std::vector<QDoubleSpinBox*> _angleSpinBoxes();
std::vector<QDoubleSpinBox*> _allSpinBoxes();
+ void _setXyz(const Eigen::Vector3d& xyz);
+
+ Eigen::Vector3d _rpyRaw() const;
+ void _setRpyRaw(const Eigen::Vector3d& rpy) const;
+ void _setRpyDeg(const Eigen::Vector3d& rpyDeg) const;
+ void _setRpyRad(const Eigen::Vector3d& rpyRad) const;
+
private:
+ std::optional<GuiGraph::Vertex> vertex;
+
+
QLineEdit* locationID = nullptr;
QLineEdit* frame = nullptr;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 12697de5958cbd8697e9cbe969bca0d7633f4126..7df6d1b71498ef961f14ef3996667f3e4205bf56 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -86,4 +86,28 @@ namespace armarx::nav::locgrapheditor
}
}
+
+
+ QList<QTableWidgetItem*>
+ VertexTableWidget::selectedEdgeItems()
+ {
+ // 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;
+ }
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index e00b356e3c51792bf0d71f3802aeaed94ece3d06..6a8f24a75da194f582619e0df9c4e02a31010e9f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -41,11 +41,18 @@ namespace armarx::nav::locgrapheditor
VertexTableWidget();
+
QTableWidgetItem* addVertex(graph::Graph::Vertex vertex);
void updateVertex(GuiGraph::Vertex vertex);
+ QList<QTableWidgetItem*> selectedEdgeItems();
+
+
+ public slots:
+
+
private slots: