WidgetController.cpp

Go to the documentation of this file.

/*
 * 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    Navigation::gui-plugins::WidgetController
 * \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 "WidgetController.h"
 
#include <map>
#include <memory>
#include <set>
#include <sstream>
#include <utility>
#include <vector>
 
#include <QDialog>
#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
#include <QMenu>
#include <QMessageBox>
#include <QMouseEvent>
#include <QObject>
#include <QPushButton>
#include <QSignalBlocker>
#include <qboxlayout.h>
#include <qhashfunctions.h>
#include <qicon.h>
#include <qlist.h>
#include <qmessagebox.h>
#include <qobject.h>
#include <qobjectdefs.h>
#include <qpair.h>
#include <qpointer.h>
#include <qsettings.h>
#include <qwidget.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <armarx/navigation/components/navigation_memory/ComponentInterface.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/LocationUtils.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/graph/Visu.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
#include <armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h>
#include <armarx/navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
#include <armarx/navigation/location/constants.h>
 
#include "widgets/EdgeTableWidget.h"
#include "widgets/NewEntityIdDialog.h"
#include "widgets/ObjectPoseClientWidget.h"
#include "widgets/RobotVisuWidget.h"
#include "widgets/VertexDataWidget.h"
#include "widgets/VertexTableWidget.h"
#include "widgets/utils.h"
#include <SemanticObjectRelations/Shapes/Shape.h>
 
 
static const QString SETTING_LAST_SCENE = "lastScene";
 
namespace armarx::navigation::qt_plugins::location_graph_editor
{
 
    QString
    WidgetController::GetWidgetName()
    {
        return "Navigation.LocationGraphEditor";
    }
 
    QIcon
    WidgetController::GetWidgetIcon()
    {
        return QIcon{"://icons/location_graph_editor.svg"};
    }
 
    WidgetController::WidgetController() : settings{"KIT", "WidgetController"}
    {
        widget.setupUi(getWidget());
 
        loadAutomaticSettings();
 
        widget.loadGraphButton->setEnabled(false);
 
        view.vertexData = new VertexDataWidget();
        view.vertexData->setEnabled(false); // Enable on first selection of vertex.
        widget.locationDataGroupBox->layout()->addWidget(view.vertexData);
        if (QBoxLayout* layout = dynamic_cast<QBoxLayout*>(widget.locationDataGroupBox->layout()))
        {
            layout->addStretch();
        }
 
        view.vertexTable = new VertexTableWidget();
        widget.locationsTableGroupBox->layout()->addWidget(view.vertexTable);
 
        view.edgeTable = new EdgeTableWidget();
        widget.edgesTableGroupBox->layout()->addWidget(view.edgeTable);
 
        view.robotVisu = new RobotVisuWidget(*this);
        widget.robotVisuGroupBox->layout()->addWidget(view.robotVisu);
 
        view.objectPoses = new ObjectPoseClientWidget(*this);
        widget.objectPoseClientGroupBox->layout()->addWidget(view.objectPoses);
 
 
        connect(this, &This::connected, this, &This::queryGraphs);
        connect(this, &This::connected, this, &This::graphChanged);
 
        connect(widget.createGraphButton, &QPushButton::pressed, this, &This::createGraphDialog);
 
        connect(widget.queryGraphsButton, &QPushButton::pressed, this, &This::queryGraphs);
 
        connect(this, &This::locationMemoryChanged, this, &This::memoryChanged);
        connect(this, &This::graphMemoryChanged, this, &This::memoryChanged);
 
        connect(this, &This::graphMemoryChanged, this, &This::updateGraphList);
 
        connect(widget.loadGraphButton, &QPushButton::pressed, this, &This::loadGraph);
        connect(widget.commitGraphButton, &QPushButton::pressed, this, &This::commit);
 
        // View updates
        connect(this, &This::graphChanged, this, &This::updateGraphView);
        connect(
            view.vertexData, &VertexDataWidget::vertexDataChanged, this, &This::updateGraphView);
        connect(view.robotVisu, &RobotVisuWidget::connected, this, &This::updateGraphView);
        connect(view.robotVisu,
                &RobotVisuWidget::connected,
                this,
                [this]() { view.vertexData->setRobotConnection(&view.robotVisu->connection()); });
        connect(view.robotVisu, &RobotVisuWidget::settingsChanged, this, &This::updateGraphView);
 
        connect(view.objectPoses,
                &ObjectPoseClientWidget::connected,
                this,
                [this]()
                { view.vertexData->setObjectPoseConnection(&view.objectPoses->connection()); });
 
        // 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);
 
        // Graph modification
        connect(view.vertexTable,
                &VertexTableWidget::newVertexRequested,
                this,
                &This::createVertexDialog);
        connect(view.vertexTable, &VertexTableWidget::newEdgesRequested, this, &This::addEdges);
        connect(view.vertexTable,
                &VertexTableWidget::edgeRemovalRequested,
                this,
                &This::removeEdgesOfVertex);
 
        connect(view.edgeTable, &EdgeTableWidget::edgeRemovalRequested, this, &This::removeEdges);
 
        // export
        connect(widget.exportButton, &QPushButton::clicked, this, &This::exportLocationGraph);
    }
 
    void
    WidgetController::exportLocationGraph()
    {
        QString labelText;
 
        auto navigationMemoryPrx =
            memory::NavigationMemoryInterfacePrx::uncheckedCast(remote.locationReader.readingPrx);
        if (navigationMemoryPrx)
        {
            const ::armarx::PackagePath packagePath(
                widget.packageNameEdit->text().toStdString(),
                widget.packageDirectoryEdit->text().toStdString());
 
            navigationMemoryPrx->storeLocationGraph(packagePath.serialize());
            labelText = "Exported locations and graph.";
        }
        else
        {
            labelText = "Export failed: The connected navigation memory does not implement the "
                        "NavigationMemoryInterface.";
        }
 
        widget.exportLabel->setText(labelText);
    }
 
    WidgetController::~WidgetController()
    {
        saveAutomaticSettings();
    }
 
    QPointer<QDialog>
    WidgetController::getConfigDialog(QWidget* parent)
    {
        if (not configDialog)
        {
            configDialog = new SimpleConfigDialog(parent);
            configDialog->addProxyFinder<armem::mns::MemoryNameSystemInterfacePrx>(
                "MemoryNameSystem", "Memory Name System", remote.memoryNameSystemName);
        }
        return qobject_cast<SimpleConfigDialog*>(configDialog);
    }
 
    void
    WidgetController::configured()
    {
        remote.memoryNameSystemName = configDialog->getProxyName("MemoryNameSystem");
    }
 
    void
    WidgetController::loadSettings(QSettings* settings)
    {
        remote.memoryNameSystemName =
            settings
                ->value("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName))
                .toString()
                .toStdString();
    }
 
    void
    WidgetController::saveSettings(QSettings* settings)
    {
        settings->setValue("memoryNameSystemName",
                           QString::fromStdString(remote.memoryNameSystemName));
    }
 
    void
    WidgetController::loadAutomaticSettings()
    {
        lastSelectedSceneName =
            settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
    }
 
    void
    WidgetController::saveAutomaticSettings()
    {
        settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
    }
 
    void
    WidgetController::onInitComponent()
    {
        usingProxy(remote.memoryNameSystemName);
    }
 
    void
    WidgetController::onConnectComponent()
    {
        remote.connect(*this);
        {
            std::stringstream ss;
            ss << "Navigation Graphs (Entities from Core Segment "
               << navigation::graph::coreSegmentID << ")";
            widget.graphGroupBox->setTitle(QString::fromStdString(ss.str()));
            widget.graphGroupBox->setEnabled(true);
        }
 
        emit connected();
    }
 
    void
    WidgetController::Remote::connect(Component& parent)
    {
        auto mnsProxy =
            parent.getProxy<armem::mns::MemoryNameSystemInterfacePrx>(memoryNameSystemName);
        memoryNameSystem = armem::client::MemoryNameSystem(mnsProxy, &parent);
 
        locationReader = memoryNameSystem.useReader(navigation::location::coreSegmentID);
        locationWriter = memoryNameSystem.useWriter(navigation::location::coreSegmentID);
 
        graphReader = memoryNameSystem.useReader(navigation::graph::coreSegmentID);
        graphWriter = memoryNameSystem.useWriter(navigation::graph::coreSegmentID);
 
        arviz = std::make_unique<viz::Client>(viz::Client::createForGuiPlugin(parent));
    }
 
    void
    WidgetController::queryMemory()
    {
        armem::client::QueryResult locResult = queryLocations();
        armem::client::QueryResult graphResult = queryGraphs();
 
        if (not(locResult.success and graphResult.success))
        {
            QStringList status;
            std::stringstream ss;
            if (not locResult.success)
            {
                status.append(QString::fromStdString(locResult.errorMessage));
            }
            if (not graphResult.success)
            {
                status.append(QString::fromStdString(graphResult.errorMessage));
            }
            widget.statusLabel->setText(status.join("\n"));
        }
    }
 
    armem::client::QueryResult
    WidgetController::queryLocations()
    {
        armem::client::QueryResult result =
            remote.locationReader.getLatestSnapshotsIn(navigation::location::coreSegmentID);
        if (result.success)
        {
            model.locationsMemory = std::move(result.memory);
            emit locationMemoryChanged();
        }
        return result;
    }
 
    armem::client::QueryResult
    WidgetController::queryGraphs()
    {
        armem::client::QueryResult result =
            remote.graphReader.getLatestSnapshotsIn(navigation::graph::coreSegmentID);
        if (result.success)
        {
            model.graphMemory = std::move(result.memory);
            emit graphMemoryChanged();
        }
        return result;
    }
 
    void
    WidgetController::updateGraphList()
    {
        QString previousText = widget.graphsComboBox->currentText();
        widget.graphsComboBox->clear();
 
        int i = 0;
        int previousIndex = -1; // To keep selection.
        model.graphMemory.forEachEntity(
            [&](const armem::wm::Entity& entity)
            {
                bool hasChanged =
                    (entity.id() == model.graphEntityID ? model.graph.hasChanged() : false);
                QString text = getGraphDisplayName(entity.id(), hasChanged);
                QString id = QString::fromStdString(entity.id().str());
 
                widget.graphsComboBox->addItem(text, id);
                if (previousIndex < 0 and text == previousText)
                {
                    previousIndex = i;
                }
                ++i;
            });
        if (previousIndex >= 0)
        {
            widget.graphsComboBox->setCurrentIndex(previousIndex);
        }
        widget.loadGraphButton->setEnabled(widget.graphsComboBox->count() > 0);
    }
 
    void
    WidgetController::loadGraph()
    {
        if (model.graph.hasChanged())
        {
            if (not loadGraphDialog())
            {
                return;
            }
        }
 
        const armem::MemoryID entityID = armem::MemoryID::fromString(
            widget.graphsComboBox->currentData().toString().toStdString());
 
        // Refresh local memory.
        queryMemory();
 
        const armem::wm::EntityInstance* instance = model.graphMemory.findLatestInstance(entityID);
        if (instance)
        {
            widget.statusLabel->setText(QString::fromStdString(
                "Loaded snapshot " + instance->id().getEntitySnapshotID().str()));
        }
        else
        {
            std::stringstream ss;
            ss << "No latest instance of entity " << entityID << " in memory.";
            widget.statusLabel->setText(QString::fromStdString(ss.str()));
            return;
        }
 
        navigation::core::Graph nav;
        {
            navigation::core::arondto::Graph dto;
            ARMARX_CHECK_NOT_NULL(instance->data());
            dto.fromAron(instance->data());
            fromAron(dto, nav);
        }
        // Resolve locations and remove vertices which could not be resolved.
        {
            resolveLocations(nav, model.locationsMemory);
            std::vector<semrel::ShapeID> remove;
            for (navigation::core::Graph::Vertex vertex : nav.vertices())
            {
                if (not vertex.attrib().hasPose())
                {
                    remove.push_back(vertex.objectID());
                }
            }
            if (not remove.empty())
            {
                for (semrel::ShapeID vertexID : remove)
                {
                    nav.removeVertex(nav.vertex(vertexID));
                }
                std::stringstream ss;
                ss << "Dropped " << remove.size() << " locations which could not be resolved.";
                widget.statusLabel->setText(QString::fromStdString(ss.str()));
            }
        }
 
        ARMARX_VERBOSE << "Loading graph " << nav.str();
        setGraph(entityID, nav);
    }
 
    bool
    WidgetController::loadGraphDialog()
    {
        ARMARX_CHECK(model.graph.hasChanged());
 
        QMessageBox msgBox;
        msgBox.setText("The current graph and/or locations have uncommitted changes. ");
        msgBox.setInformativeText("Do you want to discard them?");
        QStringList detailLines;
        if (model.graph.attrib().edgesChanged)
        {
            detailLines.append("Graph edges have changed.");
        }
        for (auto vertex : model.graph.vertices())
        {
            if (vertex.attrib().changed)
            {
                detailLines.append("Location " + QString::fromStdString(vertex.attrib().getName()) +
                                   " has changed.");
            }
        }
        msgBox.setDetailedText(detailLines.join("\n"));
        msgBox.setStandardButtons(QMessageBox::Discard | QMessageBox::Cancel);
        msgBox.setDefaultButton(QMessageBox::Cancel);
 
        switch (msgBox.exec())
        {
            case QMessageBox::Discard:
                return true; // Ok go.
            case QMessageBox::Cancel:
                return false; // Abort loading.
            default:
                return false; // Something went wrong.
        }
    }
 
    void
    WidgetController::commit()
    {
        armem::CommitResult locResults = commitLocations();
        armem::EntityUpdateResult graphResult = commitGraph();
 
        {
            std::stringstream ss;
            if (locResults.allSuccess())
            {
                ss << "Committed " << locResults.results.size() << " location snapshot";
                if (locResults.results.size() != 1)
                {
                    ss << "s"; // plural
                }
                if (graphResult.success)
                {
                    ss << " and 1 graph snapshot " << graphResult.snapshotID;
                }
                else
                {
                    ss << " but failed to commit graph: \n" << graphResult.errorMessage;
                }
            }
            else
            {
                int numLocs = static_cast<int>(locResults.results.size());
                int numSuccess = 0;
                for (const auto& r : locResults.results)
                {
                    numSuccess += int(r.success);
                }
                int numFailed = numLocs - numSuccess;
 
                if (graphResult.success)
                {
                    ss << "Committed 1 graph snapshot " << graphResult.snapshotID;
                    ss << " and " << numSuccess << " locations, but failed to commit " << numFailed
                       << " locations: \n"
                       << locResults.allErrorMessages();
                }
                else
                {
                    ss << "Failed to commit graph and " << numFailed << " of " << numLocs
                       << " locations: \n";
                    ss << graphResult.errorMessage << "\n";
                    ss << locResults.allErrorMessages();
                }
            }
 
            widget.statusLabel->setText(QString::fromStdString(ss.str()));
        }
 
        // `changed` flags may have changed
        emit graphChanged();
    }
 
    armem::CommitResult
    WidgetController::commitLocations()
    {
        armem::Commit commit;
 
        for (auto vertex : model.graph.vertices())
        {
            if (vertex.attrib().changed)
            {
                armem::EntityUpdate& update = commit.add();
                fromAron(vertex.attrib().aron.locationID, update.entityID);
                update.referencedTime = armem::Time::Now();
 
                navigation::location::arondto::Location dto;
                toAron(dto.framedPose, vertex.attrib().getPose());
                update.instancesData = {dto.toAron()};
            }
        }
 
        armem::CommitResult result = remote.locationWriter.commit(commit);
        auto it = result.results.begin();
        for (auto vertex : model.graph.vertices())
        {
            if (vertex.attrib().changed)
            {
                ARMARX_CHECK(it != result.results.end());
                if (it->success)
                {
                    // Only clear dirty flag when update was successful.
                    vertex.attrib().changed = false;
                }
                ++it;
            }
        }
        return result;
    }
 
    armem::EntityUpdateResult
    WidgetController::commitGraph()
    {
        navigation::core::arondto::Graph dto;
        {
            navigation::core::Graph nav = fromGuiGraph(model.graph);
            toAron(dto, nav);
        }
 
        armem::EntityUpdate update;
        update.entityID = model.graphEntityID;
        update.referencedTime = armem::Time::Now();
        update.instancesData = {dto.toAron()};
 
        armem::EntityUpdateResult result = remote.graphWriter.commit(update);
        if (result.success)
        {
            // Clear dirty flag
            model.graph.attrib().edgesChanged = false;
        }
        return result;
    }
 
    template <class GraphT>
    semrel::ShapeID
    findNextFreeVertexID(const GraphT& graph, semrel::ShapeID vertexID)
    {
        while (graph.hasVertex(vertexID))
        {
            ++vertexID;
        }
        return vertexID;
    }
 
    void
    WidgetController::setGraph(const armem::MemoryID& entityID, const core::Graph& nav)
    {
        model.graphEntityID = entityID;
 
        // Build the gui graph (model).
        {
            QSignalBlocker blocker(this);
 
            clearGraph();
            model.graph.attrib().edgesChanged = false;
 
            std::set<armem::MemoryID> coveredLocationIDs;
            for (auto vertex : nav.vertices())
            {
                ARMARX_CHECK(vertex.attrib().hasPose());
                addVertex(vertex.objectID(), {vertex.attrib()});
 
                armem::MemoryID id =
                    aron::fromAron<armem::MemoryID>(vertex.attrib().aron.locationID).getEntityID();
                if (coveredLocationIDs.count(id) > 0)
                {
                    ARMARX_WARNING << "Two vertices have the same location!";
                }
                else
                {
                    coveredLocationIDs.insert(id);
                }
            }
            // Add locations which have not been part of graph.
            // ToDo: This should be an explicit step in the GUI.
            {
                semrel::ShapeID vertexID{0};
                model.locationsMemory.forEachInstance(
                    [&](const armem::wm::EntityInstance& instance)
                    {
                        if (coveredLocationIDs.count(instance.id().getEntityID()) == 0)
                        {
                            vertexID = findNextFreeVertexID(model.graph, vertexID);
                            addVertex(vertexID, instance);
                        }
                    });
            }
 
 
            for (auto edge : nav.edges())
            {
                addEdge(model.graph.vertex(edge.sourceObjectID()),
                        model.graph.vertex(edge.targetObjectID()),
                        {edge.attrib()});
            }
        }
 
        // Trigger a view update.
        emit graphChanged();
    }
 
    void
    WidgetController::setEmptyGraph(const armem::MemoryID& entityID)
    {
        model.graphEntityID = entityID;
 
        {
            QSignalBlocker blocker(this);
            clearGraph();
 
            semrel::ShapeID id{0};
            queryLocations();
            model.locationsMemory.forEachInstance(
                [this, &id](const armem::wm::EntityInstance& instance)
                {
                    addVertex(id, instance);
                    ++id;
                });
        }
 
        // Mark graph as changed.
        model.graph.attrib().edgesChanged = true;
 
        emit graphChanged();
    }
 
    GuiGraph::Vertex
    WidgetController::addVertex(semrel::ShapeID vertexID, const VertexData& defaultAttribs)
    {
        ARMARX_CHECK(not model.graph.hasVertex(vertexID));
 
        GuiGraph::Vertex vertex = model.graph.addVertex(vertexID, defaultAttribs);
        vertex.attrib().tableWidgetItem = view.vertexTable->addVertex();
 
        emit graphChanged();
 
        return vertex;
    }
 
    GuiGraph::Vertex
    WidgetController::addVertex(semrel::ShapeID vertexID,
                                const armem::wm::EntityInstance& locationInstance)
    {
        navigation::location::arondto::Location location;
        location.fromAron(locationInstance.data());
 
        VertexData attrib;
        toAron(attrib.aron.locationID, locationInstance.id().getEntityID());
        FramedPose pose;
        fromAron(location.framedPose, pose);
        attrib.setPose(pose);
        return addVertex(vertexID, attrib);
    }
 
    GuiGraph::Edge
    WidgetController::addEdge(GuiGraph::ConstVertex source,
                              GuiGraph::ConstVertex target,
                              const EdgeData& defaultAttribs)
    {
        ARMARX_CHECK(not model.graph.hasEdge(source.objectID(), target.objectID()))
            << "Edge must not exist before being added: '" << source.attrib().getName() << "' -> '"
            << target.attrib().getName() << "'";
 
        GuiGraph::Edge edge = model.graph.addEdge(source, target, defaultAttribs);
        edge.attrib().tableWidgetItem = view.edgeTable->addEdge(edge);
 
        emit graphChanged();
 
        return edge;
    }
 
    void
    WidgetController::updateGraphView()
    {
        for (auto vertex : model.graph.vertices())
        {
            updateVertexView(vertex);
        }
        for (auto edge : model.graph.edges())
        {
            updateEdgeView(edge);
        }
 
        int index =
            widget.graphsComboBox->findData(QString::fromStdString(model.graphEntityID.str()));
        if (index >= 0)
        {
            widget.graphsComboBox->setItemText(
                index, getGraphDisplayName(model.graphEntityID, model.graph.hasChanged()));
        }
 
        updateArViz();
    }
 
    void
    WidgetController::updateVertexView(GuiGraph::Vertex vertex)
    {
        view.vertexTable->updateVertex(vertex);
 
        if (/* DISABLES CODE */ (false)) // Disable this for the moment.
        {
            // Highlight all edges between highlighted vertices
            std::set<semrel::ShapeID> highlightedVertices;
            for (auto v : model.graph.vertices())
            {
                if (v.attrib().highlighted)
                {
                    highlightedVertices.insert(v.objectID());
                }
            }
 
            for (auto edge : model.graph.edges())
            {
                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;
                }
            }
        }
    }
 
    void
    WidgetController::updateEdgeView(GuiGraph::Edge edge)
    {
        view.edgeTable->updateEdge(edge);
    }
 
    void
    WidgetController::updateArViz()
    {
        if (remote.arviz)
        {
            objpose::ObjectPoseMap objectMap;
            std::vector<ObjectInfo> objectInfo;
            graph::visu::ObjectParserInfo info{objectMap, objectInfo};
 
            // if ObjectPoseClient is connected, load objects to resolve graph locations
            if (view.objectPoses->isConnected())
            {
                objectMap = view.objectPoses->connection().getObjectPoseMap();
                objectInfo = view.objectPoses->connection().getObjectInfo();
            }
 
            std::vector<viz::Layer> layers;
            {
                viz::Layer& vertices = layers.emplace_back(remote.arviz->layer("Locations"));
                applyVisu(vertices, model.visu.vertex, model.graph.vertices(), info);
            }
            {
                viz::Layer& edges = layers.emplace_back(remote.arviz->layer("Graph Edges"));
                applyVisu(edges, model.visu.edge, model.graph.edges(), info);
            }
            {
                viz::Layer& robot = layers.emplace_back(remote.arviz->layer("Robot"));
                if (view.vertexData->vertex().has_value() and view.robotVisu->isEnabled())
                {
                    const auto res = core::resolveLocation(
                        objectMap, objectInfo, view.vertexData->vertex()->attrib().getPose());
                    if (res.pose.has_value())
                    {
                        robot.add(
                            view.robotVisu->connection().vizRobot("robot").pose(res.pose.value()));
                    }
                }
            }
            remote.arviz->commit(layers);
        }
    }
 
    template <class T>
    static void
    updateElementHighlighting(QList<QTableWidgetItem*> selectedItems,
                              std::map<QTableWidgetItem*, T>&& itemToElementMap)
    {
        for (QTableWidgetItem* selected : selectedItems)
        {
            if (auto it = itemToElementMap.find(selected); it != itemToElementMap.end())
            {
                it->second.attrib().highlighted = true;
                itemToElementMap.erase(it);
            }
        }
        for (auto& [_, unselected] : itemToElementMap)
        {
            unselected.attrib().highlighted = false;
        }
    }
 
    void
    WidgetController::updateVertexHighlighting()
    {
        updateElementHighlighting(view.vertexTable->selectedVertexItems(),
                                  model.graph.getTableItemToVertexMap());
        emit graphChanged();
    }
 
    void
    WidgetController::updateEdgeHighlighting()
    {
        updateElementHighlighting(view.edgeTable->selectedEdgeItems(),
                                  model.graph.getTableItemToEdgeMap());
        emit graphChanged();
    }
 
    void
    WidgetController::selectVertex(QTableWidgetItem* vertexItem)
    {
        if (vertexItem == nullptr)
        {
            view.vertexData->clearVertex();
        }
        if (auto vertex = model.graph.getVertexFromTableItem(vertexItem))
        {
            selectVertex(vertex.value());
        }
    }
 
    void
    WidgetController::selectVertex(GuiGraph::Vertex vertex)
    {
        view.vertexData->setVertex(vertex);
    }
 
    void
    WidgetController::clearGraph()
    {
        {
            QSignalBlocker blocker(this);
            clearEdges();
            clearVertices();
        }
 
        // Clear data structure
        ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0);
        ARMARX_CHECK_EQUAL(model.graph.numVertices(), 0);
 
        emit graphChanged();
    }
 
    void
    WidgetController::clearEdges()
    {
        // Remove in reverse order to be more array-friendly.
        std::vector<GuiGraph::Edge> edges{model.graph.edges().begin(), model.graph.edges().end()};
        {
            QSignalBlocker blocker(this);
            for (auto it = edges.rbegin(); it != edges.rend(); ++it)
            {
                GuiGraph::Edge edge = *it;
                removeEdge(edge);
            }
        }
 
        ARMARX_CHECK_EQUAL(view.edgeTable->rowCount(), 0);
        ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0);
 
        emit graphChanged();
    }
 
    void
    WidgetController::clearVertices()
    {
        ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0)
            << "The graph may not have any edges when clearing the vertices.";
 
        // Remove in reverse order to be more array-friendly.
        std::vector<GuiGraph::Vertex> vertices{model.graph.vertices().begin(),
                                               model.graph.vertices().end()};
        {
            QSignalBlocker blocker(this);
            for (auto it = vertices.rbegin(); it != vertices.rend(); ++it)
            {
                GuiGraph::Vertex vertex = *it;
                removeVertex(vertex);
            }
        }
 
        ARMARX_CHECK_EQUAL(view.vertexTable->rowCount(), 0);
        ARMARX_CHECK_EQUAL(model.graph.numVertices(), 0);
 
        emit graphChanged();
    }
 
    void
    WidgetController::removeEdge(GuiGraph::Edge& edge)
    {
        ARMARX_CHECK(model.graph.hasEdge(edge.sourceDescriptor(), edge.targetDescriptor()))
            << "Cannot remove edge that does not exist.";
 
        // Remove view elements
        {
            QSignalBlocker blocker(view.edgeTable);
            view.edgeTable->removeEdge(edge);
        }
 
        // Remove from model
        model.graph.removeEdge(edge);
 
        ARMARX_CHECK(not model.graph.hasEdge(edge.sourceDescriptor(), edge.targetDescriptor()))
            << edge.sourceDescriptor() << " -> " << edge.targetDescriptor();
 
        emit graphChanged();
    }
 
    void
    WidgetController::removeVertex(GuiGraph::Vertex& vertex)
    {
        ARMARX_CHECK_EQUAL(vertex.inDegree(), 0)
            << "A vertex must not have any edges before being removed. "
            << vertex.attrib().getName();
        ARMARX_CHECK_EQUAL(vertex.outDegree(), 0)
            << "A vertex must not have any edges before being removed. "
            << vertex.attrib().getName();
 
        // Remove view elements
        {
            QSignalBlocker blocker(view.vertexTable);
            view.vertexTable->removeVertex(vertex);
        }
        if (view.vertexData->vertex().has_value() and view.vertexData->vertex().value() == vertex)
        {
            view.vertexData->clearVertex();
        }
 
        // Remove from model
        model.graph.removeVertex(vertex);
 
        emit graphChanged();
    }
 
    void
    WidgetController::createVertexDialog()
    {
        NewEntityIdDialog dialog(navigation::location::coreSegmentID, nullptr);
        switch (dialog.exec())
        {
            case QDialog::Accepted:
                // Ok go.
                break;
            case QDialog::Rejected:
                // Abort creation.
                return;
        }
 
        // Find free vertex ID
        const semrel::ShapeID vertexID = findNextFreeVertexID(model.graph, semrel::ShapeID{0});
 
        // Initialize attributes
        VertexData attribs;
        toAron(attribs.aron.locationID, dialog.entityID());
        attribs.setPose(FramedPose(core::Pose::Identity().matrix(), armarx::GlobalFrame, ""));
        attribs.changed = true;
 
        addVertex(vertexID, attribs);
    }
 
    void
    WidgetController::addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
    {
        const auto itemToVertexMap = model.graph.getTableItemToVertexMap();
        {
            QSignalBlocker blocker(this);
 
            for (const auto& [sourceItem, targetItem] : vertexItems)
            {
                GuiGraph::Vertex source = itemToVertexMap.at(sourceItem);
                GuiGraph::Vertex target = itemToVertexMap.at(targetItem);
                if (not model.graph.hasEdge(source, target))
                {
                    addEdge(source, target, {});
                }
            }
        }
 
        model.graph.attrib().edgesChanged = true;
        emit graphChanged();
    }
 
    void
    WidgetController::removeEdges(QList<QTableWidgetItem*> edgeItems)
    {
        const auto itemToEdgeMap = model.graph.getTableItemToEdgeMap();
        {
            QSignalBlocker blocker(this);
 
            for (const auto& edgeItem : edgeItems)
            {
                GuiGraph::Edge edge = itemToEdgeMap.at(edgeItem);
                removeEdge(edge);
            }
        }
 
        model.graph.attrib().edgesChanged = true;
        emit graphChanged();
    }
 
    void
    WidgetController::removeEdgesOfVertex(QList<QTableWidgetItem*> vertexItems,
                                          utils::EdgeDirection direction)
    {
        auto removeEdges = [this](auto&& range)
        {
            for (auto edge : range)
            {
                removeEdge(edge);
            }
        };
        {
            QSignalBlocker blocker(this);
 
            const auto itemToVertexMap = model.graph.getTableItemToVertexMap();
            for (const auto& vertexItem : vertexItems)
            {
                GuiGraph::Vertex vertex = itemToVertexMap.at(vertexItem);
                switch (direction)
                {
                    case utils::EdgeDirection::To:
                        removeEdges(vertex.inEdges());
                        break;
 
                    case utils::EdgeDirection::From:
                        removeEdges(vertex.outEdges());
                        break;
 
                    case utils::EdgeDirection::Bidirectional:
                        removeEdges(vertex.inEdges());
                        removeEdges(vertex.outEdges());
                        break;
                }
            }
        }
 
        model.graph.attrib().edgesChanged = true;
        emit graphChanged();
    }
 
    void
    WidgetController::createGraphDialog()
    {
        NewEntityIdDialog dialog(navigation::graph::coreSegmentID, nullptr);
        switch (dialog.exec())
        {
            case QDialog::Accepted:
                break;
            case QDialog::Rejected:
                return;
        }
 
        const armem::MemoryID entityID = dialog.entityID();
 
        const bool hasChanged = true;
        QString text = getGraphDisplayName(entityID, hasChanged);
        QString data = QString::fromStdString(entityID.str());
        widget.graphsComboBox->addItem(text, data);
        widget.graphsComboBox->setCurrentIndex(widget.graphsComboBox->count() - 1);
 
        setEmptyGraph(entityID);
    }
 
    QString
    WidgetController::getGraphDisplayName(const armem::MemoryID& entityID, bool changed) const
    {
        QString name =
            QString::fromStdString(entityID.providerSegmentName + "/" + entityID.entityName);
        if (changed)
        {
            name += "*";
        }
        return name;
    }
 
} // namespace armarx::navigation::qt_plugins::location_graph_editor