Extend / fix table/data widgets

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);
 
 

source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h

@@ -17,6 +17,7 @@ namespace semrel
 }
 namespace armarx::nav::locgrapheditor
 {
+
     class GraphScene : public QGraphicsScene
     {
         Q_OBJECT

source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp

@@ -72,4 +72,5 @@ namespace armarx::nav::locgrapheditor
         }
     }
 
+
 }

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:
 

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);
+        }
+    }
+
 }

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;
 

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;
+    }
+
 }

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: