GraphVisualizerGuiPlugin.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <VirtualRobot/MathTools.h>
 
#include "GraphVisualizerGuiPlugin.h"
#include "GraphVisualizerConfigDialog.h"
 
 
//armar
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <MemoryX/libraries/memorytypes/variants/GraphNode/GraphNode.h>
 
// Qt headers
#include <Qt>
#include <QtGlobal>
#include <QPushButton>
#include <QLabel>
#include <QLineEdit>
#include <QHBoxLayout>
#include <QMenu>
 
 
//std
#include <memory>
#include <sstream>
#include <unordered_set>
#include <tuple>
#include <filesystem>
#include <fstream>
 
#include <MemoryX/gui-plugins/GraphVisualizerPlugin/ui_GraphVisualizerConfigDialog.h>
 
//#include <VirtualRobot/VirtualRobotCommon.h>
 
using namespace memoryx;
 
using namespace VirtualRobot;
 
//static values and helper functions
#define DEFAULT_PRIOR_KNOWLEDGE_NAME "PriorKnowledge"
#define DEFAULT_DEBUG_DRAWER_NAME "DebugDrawerUpdates"
#define DEFAULT_DEBUG_DRAWER_LAYER_NAME "DebugDrawerUpdates_Graph"
 
/**
 * @brief The default width of lines drawn onto the debug layer and scene.
 */
static const ::Ice::Float LINE_WIDTH_DEFAULT = 5;
/**
 * @brief The width of selected lines drawn onto the debug layer and scene.
 */
static const ::Ice::Float LINE_WIDTH_SELECTED = 10;
 
/**
 * @brief The default color of lines drawn onto the debug layer and scene.
 */
static const ::armarx::DrawColor COLOR_DEFAULT = {0.5f, 0.5f, 1.f, 0.2f};
/**
 * @brief The color of highlighted lines drawn onto the debug layer and scene.
 */
static const ::armarx::DrawColor COLOR_HIGHLIGHT = {1.f, 0.0f, 0.f, 1.f};
 
/**
 * @brief The scale factor for elements drawn onto the scene.
 */
static const float SCENE_SCALE_FACTOR = 2;
/**
 * @brief The scale factor for nodes drawn onto the scene.
 */
static const float SCENE_NODES_SCALE_FACTOR = 3 * SCENE_SCALE_FACTOR;
/**
 * @brief The scale factor for edges drawn onto the scene.
 */
static const float SCENE_LINE_SCALE_FACTOR = SCENE_SCALE_FACTOR;
 
/**
 * @brief The increment used when a rotation button is pressed.
 *
 * A positive rotation is counter clockwise.
 * This value should be positive.
 *
 * rotation buttons: GraphVisualizerWidget::ui.buttonRotateClock and
 * GraphVisualizerWidget::ui.buttonRotateCounterClock
 */
static const float VIEW_ROTATE_STEP_SIZE_CC = 45;
 
static const QString SETTING_LAST_SCENE = "lastScene";
 
/**
 * @brief Returns the name used on the debug layer.
 * @param edge The edge.
 * @return The name used on the debug layer.
 */
inline std::string iceName(const GraphVisualizerWidget::EdgeId& edge)
{
    std::stringstream s;
    s << "edge_" << edge.first << "_" << edge.second;
    return s.str();
}
 
/**
 * @brief iceName Returns the name used on the debug layer.
 * @param nodeName The node.
 * @return The name used on the debug layer.
 */
inline std::string iceName(const GraphVisualizerWidget::NodeId& nodeName)
{
    return nodeName;
}
 
//gui plugin
GraphVisualizerGuiPlugin::GraphVisualizerGuiPlugin()
{
    addWidget<GraphVisualizerWidget>();
}
 
//widget
 
GraphVisualizerWidget::GraphVisualizerWidget() :
    debugDrawerTopicName {DEFAULT_DEBUG_DRAWER_NAME},
    viewAngle {0},
    debugDrawerLayerName {DEFAULT_DEBUG_DRAWER_LAYER_NAME},
    priorKnowledgeProxyName {DEFAULT_PRIOR_KNOWLEDGE_NAME},
    settings {"KIT", "GraphVisualizerGuiPlugin"}
{
    loadAutomaticSettings();
    editStartNodeNext = true;
 
    // init gui
    ui.setupUi(getWidget());
    //add scene
    std::unique_ptr<QGraphicsScene> scenePtr{new QGraphicsScene};
    scene = scenePtr.get();
    ui.graphicsViewGraph->setScene(scenePtr.release());
    MouseEventProcessor* mep = new MouseEventProcessor(this);
    ui.graphicsViewGraph->installEventFilter(mep);
    //transform view
    transformView();
}
 
GraphVisualizerWidget::~GraphVisualizerWidget()
{
    saveAutomaticSettings();
}
 
void GraphVisualizerWidget::onInitComponent()
{
    ARMARX_VERBOSE << "init start";
    usingProxy(priorKnowledgeProxyName);
    usingProxy("GraphNodePoseResolver");
    offeringTopic(debugDrawerTopicName);
    ARMARX_VERBOSE << "init done";
}
 
void GraphVisualizerWidget::onConnectComponent()
{
    ARMARX_VERBOSE << "connecting";
    debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
    priorKnowledgePrx = getProxy<memoryx::PriorKnowledgeInterfacePrx>(priorKnowledgeProxyName);
    getProxy(gnpr, "GraphNodePoseResolver");
 
    if (priorKnowledgePrx->hasGraphSegment())
    {
        ARMARX_VERBOSE << "get Proxy to graph segment";
        graphSeg = priorKnowledgePrx->getGraphSegment();
        ui.sceneGroupBox->setEnabled(true);
        ui.sceneGroupBox->setTitle("Scenes from graph memory segment");
 
        ui.b->setEnabled(true);
    }
    else
    {
        ui.sceneGroupBox->setEnabled(false);
        ui.sceneGroupBox->setTitle("Scenes from graph memory segment (No graph memory segment available)");
 
        ui.b->setEnabled(false);
    }
 
    ui.tableWidgetNodes->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
    ui.tableWidgetEdges->horizontalHeader()->setResizeMode(QHeaderView::Stretch);
 
    ui.tableWidgetNodes->setContextMenuPolicy(Qt::CustomContextMenu);
    ui.tableWidgetEdges->setContextMenuPolicy(Qt::CustomContextMenu);
 
    //todo remove test function
    QObject::connect(ui.b, SIGNAL(clicked()), this, SLOT(addKitchenGraph()), Qt::UniqueConnection);
 
    //tables
    QObject::connect(ui.tableWidgetNodes, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(nodeTableDoubleClicked(int, int)), Qt::UniqueConnection);
    QObject::connect(ui.tableWidgetNodes, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(tableWidgetNodesCustomContextMenu(QPoint)), Qt::UniqueConnection);
    QObject::connect(ui.tableWidgetEdges, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(edgeTableDoubleClicked(int, int)), Qt::UniqueConnection);
    QObject::connect(ui.tableWidgetEdges, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(tableWidgetEdgesCustomContextMenu(QPoint)), Qt::UniqueConnection);
 
    QObject::connect(ui.btnAdd, SIGNAL(clicked()), this, SLOT(addNewGraphNode()), Qt::UniqueConnection);
    QObject::connect(ui.btnAddEdge, SIGNAL(clicked()), this, SLOT(addNewEdgeBoth()), Qt::UniqueConnection);
    QObject::connect(ui.btnAddEdgeStartEnd, SIGNAL(clicked()), this, SLOT(addNewEdgeStartEnd()), Qt::UniqueConnection);
    QObject::connect(ui.btnAddEdgeEndStart, SIGNAL(clicked()), this, SLOT(addNewEdgeEndStart()), Qt::UniqueConnection);
    QObject::connect(ui.btnEdit, SIGNAL(clicked()), this, SLOT(editGraphNode()), Qt::UniqueConnection);
 
    //zoom
    QObject::connect(ui.viewZoomFactor, SIGNAL(valueChanged(double)), this, SLOT(transformView()), Qt::UniqueConnection);
    //rota
    QObject::connect(ui.buttonRotateClock, SIGNAL(clicked()), this, SLOT(viewRotatedClock()), Qt::UniqueConnection);
    QObject::connect(ui.buttonRotateCounterClock, SIGNAL(clicked()), this, SLOT(viewRotatedCounterClock()), Qt::UniqueConnection);
    //redraw+clear
    QObject::connect(ui.buttonRedraw, SIGNAL(clicked()), this, SLOT(redraw()), Qt::UniqueConnection);
    QObject::connect(ui.buttonClear, SIGNAL(clicked()), this, SLOT(clearGraph()), Qt::UniqueConnection);
    //auto adjust
    QObject::connect(ui.buttonAutoAdjust, SIGNAL(clicked()), this, SLOT(adjustView()), Qt::UniqueConnection);
    //memory
    QObject::connect(ui.refreshScenesButton, SIGNAL(clicked()), this, SLOT(updateSceneList()), Qt::UniqueConnection);
    QObject::connect(ui.drawSceneButton, SIGNAL(clicked()), this, SLOT(drawScene()), Qt::UniqueConnection); //BUTTON LOAD
    QObject::connect(ui.scenesComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectedSceneChanged(int)), Qt::UniqueConnection);
 
    ARMARX_VERBOSE << "connected";
 
    updateSceneList();
}
 
void GraphVisualizerWidget::onExitComponent()
{
}
 
QPointer<QDialog> GraphVisualizerWidget::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new GraphVisualizerConfigDialog(parent);
    }
 
    dialog->ui->editDebugDrawerProxyName->setText(QString::fromStdString(debugDrawerTopicName));
    dialog->ui->editDebugDrawerLayerName->setText(QString::fromStdString(debugDrawerLayerName));
    dialog->ui->editPriorKnowledgeProxyName->setText(QString::fromStdString(priorKnowledgeProxyName));
    return qobject_cast<GraphVisualizerConfigDialog*>(dialog);
}
 
void GraphVisualizerWidget::configured()
{
    debugDrawerTopicName = dialog->ui->editDebugDrawerProxyName->text().toStdString();
    debugDrawerLayerName = dialog->ui->editDebugDrawerLayerName->text().toStdString();
    priorKnowledgeProxyName = dialog->ui->editPriorKnowledgeProxyName->text().toStdString();
}
 
 
void GraphVisualizerWidget::loadSettings(QSettings* settings)
{
    debugDrawerTopicName = settings->value("debugDrawerTopicName", QString::fromStdString(debugDrawerTopicName)).toString().toStdString();
    debugDrawerLayerName = settings->value("debugDrawerLayerName", QString::fromStdString(debugDrawerLayerName)).toString().toStdString();
}
 
void GraphVisualizerWidget::saveSettings(QSettings* settings)
{
    settings->setValue("debugDrawerTopicName", QString::fromStdString(debugDrawerTopicName));
    settings->setValue("debugDrawerLayerName", QString::fromStdString(debugDrawerLayerName));
}
 
void GraphVisualizerWidget::loadAutomaticSettings()
{
    lastSelectedSceneName = settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
}
 
void GraphVisualizerWidget::saveAutomaticSettings()
{
    settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
}
 
void GraphVisualizerWidget::addEdge(const std::string& node1Id, const std::string& node2Id, const Ice::Current&)
{
    if (!hasNode(node1Id))
    {
        ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node1Id << " does not exist.";
        return;
    }
 
    if (!hasNode(node2Id))
    {
        ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node2Id << " does not exist.";
        return;
    }
 
    auto node1dat = nodes.at(node1Id);
    auto node2dat = nodes.at(node2Id);
 
    if (hasEdge(node1Id, node2Id))
    {
        //nothing needs to be updated
        ARMARX_WARNING << "Edge: '" << node1dat.node->getName() << "' -> '" << node2dat.node->getName() << "' already exists.";
        return;
    }
 
    auto edgeId = toEdge(node1Id, node2Id);
 
    //add
    //table
    int row = ui.tableWidgetEdges->rowCount();
    ui.tableWidgetEdges->setRowCount(row + 1);
    ui.tableWidgetEdges->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node1dat.node->getName())});
    ui.tableWidgetEdges->setItem(row, 1, new QTableWidgetItem {QString::fromStdString(node2dat.node->getName())});
    //debug layer will be done later
    //scene
    QGraphicsLineItem* graphicsItem = dynamic_cast<QGraphicsLineItem*>(new GraphVisualizerGraphicsLineItem
    {
        *this, edgeId,
        node1dat.pose->position->x, -node1dat.pose->position->y,
        node2dat.pose->position->x, -node2dat.pose->position->y
    });
    //auto graphicsItem= scene->addLine(node1dat.pos->x,-node1dat.pos->y,
    //                                  node2dat.pos->x,-node2dat.pos->y);
    scene->addItem(graphicsItem);
    //setToolTip on graphicsItem does not work
    dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString {"Edge:"} +QString::fromStdString(node1dat.node->getName()) +
            QString {" <-> "} +QString::fromStdString(node2dat.node->getName()));
    //data
    EdgeData data {graphicsItem, row, false};
    edges[edgeId] = data;
 
    updateEdge(edgeId);
}
 
void GraphVisualizerWidget::addNode(const GraphNodeBasePtr& node, const Ice::Current&)
{
    ARMARX_CHECK_EXPRESSION(node);
    auto nodeId = node->getId();
 
    armarx::FramedPosePtr globalNodePose;
    try
    {
        globalNodePose = armarx::FramedPosePtr::dynamicCast(gnpr->resolveToGlobalPose(node));
    }
    catch (...)
    {
        return;
    }
 
    if (hasNode(nodeId))
    {
        NodeData& oldNode = nodes.at(nodeId);
        ARMARX_VERBOSE << "Node: " << nodeId << " was overwritten! Old: "
                       << oldNode.pose->position->x << ", " << oldNode.pose->position->y << ", " << getYawAngle(oldNode.pose) << "| New: "
                       << globalNodePose->position->x << ", " << globalNodePose->position->y << ", " << getYawAngle(globalNodePose);
        //update node data
        //table
        ui.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 1, new QTableWidgetItem {QString::number(globalNodePose->position->x)});
        ui.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 2, new QTableWidgetItem {QString::number(globalNodePose->position->y)});
        ui.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 3, new QTableWidgetItem {QString::number(getYawAngle(globalNodePose))});
 
        //data
        oldNode.pose = globalNodePose;
 
        //update connected edges
        for (const auto& edge : edges)
        {
            if ((edge.first.first == nodeId) || (edge.first.second == nodeId))
            {
                updateEdge(edge.first);
            }
        }
    }
    else
    {
        //add&draw node
        //table
        int row = ui.tableWidgetNodes->rowCount();
        ui.tableWidgetNodes->setRowCount(row + 1);
        ui.tableWidgetNodes->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node->getName())});
        ui.tableWidgetNodes->setItem(row, 1, new QTableWidgetItem {QString::number(globalNodePose->position->x)});
        ui.tableWidgetNodes->setItem(row, 2, new QTableWidgetItem {QString::number(globalNodePose->position->y)});
        ui.tableWidgetNodes->setItem(row, 3, new QTableWidgetItem {QString::number(getYawAngle(globalNodePose))});
        //scene
        QGraphicsEllipseItem* graphicsItem = dynamic_cast<QGraphicsEllipseItem*>(new GraphVisualizerGraphicsEllipseItem
        {
            *this, node->getName(),
            globalNodePose->position->x, -globalNodePose->position->y, 0, 0
        });
        //auto graphicsItem= scene->addEllipse(node->x,-node->y,0,0);
        scene->addItem(graphicsItem);
        //setToolTip on graphicsItem does not work
        graphicsItem->setZValue(std::numeric_limits<qreal>::max());
        dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString {"Node:"} +QString::fromStdString(node->getName()));
 
        //data
        NodeData data {node, globalNodePose, graphicsItem, row, false};
        nodes.insert({nodeId, data});
    }
 
    updateNode(nodeId);
}
 
void GraphVisualizerWidget::clearEdges(const Ice::Current&)
{
    for (auto& edge : edges)
    {
        //remove from graphics scene
        scene->removeItem(edge.second.graphicsItem);
        delete edge.second.graphicsItem;
        //remove from debug layer
        debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(edge.first));
    }
 
    //clear table widget
    ui.tableWidgetEdges->clearContents();
    ui.tableWidgetEdges->setRowCount(0);
    //clear data structures
    edges.clear();
}
 
void GraphVisualizerWidget::clearGraph(const Ice::Current&)
{
    //remove from debug layer
    for (auto& edge : edges)
    {
        debugDrawer->removeLineVisu(debugDrawerLayerName, iceName(edge.first));
    }
 
    for (auto& node : nodes)
    {
        debugDrawer->removeArrowVisu(debugDrawerLayerName, iceName(node.first));
        debugDrawer->removeTextVisu(debugDrawerLayerName, iceName(node.first) + "text");
        debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(node.first));
    }
 
    //clear scene
    scene->clear();
    //clear table widgets
    ui.tableWidgetEdges->clearContents();
    ui.tableWidgetEdges->setRowCount(0);
    ui.tableWidgetNodes->clearContents();
    ui.tableWidgetNodes->setRowCount(0);
    //clear data structures
 
    edges.clear();
    nodes.clear();
}
 
void GraphVisualizerWidget::resetHighlight(const Ice::Current&)
{
    for (auto& edge : edges)
    {
        if (edge.second.highlighted)
        {
            edge.second.highlighted = false;
            updateEdge(edge.first);
        }
    }
 
    for (auto& node : nodes)
    {
        if (node.second.highlighted)
        {
            node.second.highlighted = false;
            updateNode(node.first);
        }
    }
}
 
void GraphVisualizerWidget::updateEdge(const EdgeId& id)
{
    const EdgeData& data = edges.at(id);
    auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
    QColor qColor;
    qColor.setRedF(color.r);
    qColor.setGreenF(color.g);
    qColor.setBlueF(color.b);
 
    auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
    armarx::Vector3Ptr posStart = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(nodes.at(id.first).pose->position)->toEigen());
    posStart->z += posStart->z < 1 ? 10 : 0;
    armarx::Vector3Ptr posEnd = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(nodes.at(id.second).pose->position)->toEigen());
    posEnd->z += posEnd->z < 1 ? 10 : 0;
    //debug layer
    debugDrawer->setLineVisu(debugDrawerLayerName,
                             iceName(id),
                             posStart,
                             posEnd,
                             lineWidth,
                             color);
 
    data.graphicsItem->setLine(nodes[id.first].pose->position->x, -nodes[id.first].pose->position->y,
                               nodes[id.second].pose->position->x, -nodes[id.second].pose->position->y);
 
    //scene
    QPen pen {qColor};
    pen.setWidthF(lineWidth * SCENE_LINE_SCALE_FACTOR);
    data.graphicsItem->setPen(pen);
    //table
    QFont font {};
    font.setBold(data.highlighted);
    ui.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 0)->setData(Qt::BackgroundRole, qColor);
    ui.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 0)->setFont(font);
    ui.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 1)->setData(Qt::BackgroundRole, qColor);
    ui.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 1)->setFont(font);
}
 
void GraphVisualizerWidget::updateNode(const NodeId& id)
{
    NodeData& data = nodes.at(id);
 
    if (editStartNodeNext)
    {
        ui.editStartNodeId->setText(QString::fromStdString(data.node->getId()));
        ui.editStartNodeName->setText(QString::fromStdString(data.node->getName()));
    }
    else
    {
        ui.editEndNodeId->setText(QString::fromStdString(data.node->getId()));
        ui.editEndNodeName->setText(QString::fromStdString(data.node->getName()));
    }
 
    setEditFields(data);
 
    editStartNodeNext = !editStartNodeNext;
 
    auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
    QColor qColor;
    qColor.setRedF(color.r);
    qColor.setGreenF(color.g);
    qColor.setBlueF(color.b);
 
    auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
 
    //debug layer
    float yaw = getYawAngle(data.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), data.pose->position,
                              new armarx::Vector3(dir),
                              armarx::DrawColor {0, 0, 1, 1},
                              100,
                              lineWidth);
    debugDrawer->setTextVisu(debugDrawerLayerName, iceName(id) + "text", data.node->getName(), data.pose->position, armarx::DrawColor {0, 0, 1, 1}, 10);
 
    //scene
    data.graphicsItem->setPen(QPen {qColor});
    data.graphicsItem->setBrush(QBrush {qColor});
    data.graphicsItem->setRect(data.pose->position->x - lineWidth * SCENE_NODES_SCALE_FACTOR / 2,
                               -data.pose->position->y - lineWidth * SCENE_NODES_SCALE_FACTOR / 2,
                               lineWidth * SCENE_NODES_SCALE_FACTOR,
                               lineWidth * SCENE_NODES_SCALE_FACTOR);
    //table
    QFont font {};
    font.setBold(data.highlighted);
    ui.tableWidgetNodes->item(data.tableWidgetNodesIndex, 0)->setData(Qt::BackgroundRole, qColor);
    ui.tableWidgetNodes->item(data.tableWidgetNodesIndex, 0)->setFont(font);
    ui.tableWidgetNodes->item(data.tableWidgetNodesIndex, 1)->setData(Qt::BackgroundRole, qColor);
    ui.tableWidgetNodes->item(data.tableWidgetNodesIndex, 1)->setFont(font);
    ui.tableWidgetNodes->item(data.tableWidgetNodesIndex, 2)->setData(Qt::BackgroundRole, qColor);
    ui.tableWidgetNodes->item(data.tableWidgetNodesIndex, 2)->setFont(font);
    ui.tableWidgetNodes->item(data.tableWidgetNodesIndex, 3)->setData(Qt::BackgroundRole, qColor);
    ui.tableWidgetNodes->item(data.tableWidgetNodesIndex, 3)->setFont(font);
 
    //highlight all edges between highlighted nodes
    std::vector<GraphNodeBasePtr> highlightedNodes;
    for (const auto& nodeData : nodes)
    {
        if (nodeData.second.highlighted)
        {
            highlightedNodes.push_back(nodeData.second.node);
        }
    }
 
    for (auto& edge : edges)
    {
        if (edge.second.highlighted)
        {
            edge.second.highlighted = false;
            updateEdge(edge.first);
        }
    }
 
    for (const auto& nodeFrom : highlightedNodes)
    {
        for (const auto& nodeTo : highlightedNodes)
        {
            const auto nodeFromId = nodeFrom->getId();
            const auto nodeToId = nodeTo->getId();
            if (hasEdge(nodeFromId, nodeToId))
            {
                auto edgeId = toEdge(nodeFromId, nodeToId);
                auto& edge = edges[edgeId];
                edge.highlighted = true;
                updateEdge(edgeId);
            }
        }
    }
}
 
float GraphVisualizerWidget::getYawAngle(const armarx::PoseBasePtr& pose) const
{
    Eigen::Vector3f rpy;
    armarx::PosePtr p = armarx::PosePtr::dynamicCast(pose);
    VirtualRobot::MathTools::eigen4f2rpy(p->toEigen(), rpy);
    return VirtualRobot::MathTools::rad2deg(rpy[2]);
}
 
void GraphVisualizerWidget::highlightEdge(const std::string& node1Id, const std::string& node2Id, bool highlighted, const Ice::Current&)
{
    if (!hasEdge(node1Id, node2Id))
    {
        ARMARX_WARNING << "No edge for: " << node1Id << " and " << node2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
        return;
    }
 
    EdgeId edge = toEdge(node1Id, node2Id);
 
    if (edges.at(edge).highlighted != highlighted)
    {
        edges.at(edge).highlighted = highlighted;
        updateEdge(edge);
    }
}
 
void GraphVisualizerWidget::highlightNode(const std::string& nodeId, bool highlighted, const Ice::Current&)
{
    if (!hasNode(nodeId))
    {
        ARMARX_WARNING << "No node: " << nodeId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
        return;
    }
 
    if (nodes.at(nodeId).highlighted != highlighted)
    {
        nodes.at(nodeId).highlighted = highlighted;
        updateNode(nodeId);
    }
}
 
 
void GraphVisualizerWidget::nodeTableDoubleClicked(int row, int)
{
    auto nodeIt = std::find_if(nodes.cbegin(), nodes.cend(), [&](const std::pair<std::string, NodeData>& d)
    {
        //        return d.second.node->getName() == ui.tableWidgetNodes->item(row, 0)->text().toStdString();
        return d.second.tableWidgetNodesIndex == row;
    });
    auto nodeId = nodeIt->second.node->getId();
    nodeDoubleClicked(nodeId);
}
 
void GraphVisualizerWidget::edgeTableDoubleClicked(int row, int)
{
    auto edgeIt = std::find_if(edges.cbegin(), edges.cend(), [&](const std::pair<EdgeId, EdgeData>& d)
    {
        return d.second.tableWidgetEdgesIndex == row;
    });
 
    edgeDoubleClicked(edgeIt->first);
}
 
void GraphVisualizerWidget::nodeDoubleClicked(NodeId id)
{
    nodes.at(id).highlighted ^= true;
    updateNode(id);
}
 
void GraphVisualizerWidget::edgeDoubleClicked(EdgeId id)
{
    //    edges.at(id).highlighted ^= true;
    //    updateEdge(id);
    bool highlight = !nodes.at(id.first).highlighted && !nodes.at(id.second).highlighted;
    nodes.at(id.first).highlighted = highlight;
    nodes.at(id.second).highlighted = highlight;
    updateNode(id.first);
    updateNode(id.second);
}
 
void GraphVisualizerWidget::redraw(const Ice::Current&)
{
    drawScene();
}
 
void GraphVisualizerWidget::setEditFields(const NodeData& nodeData)
{
    ui.editNodeId->setText(QString::fromStdString(nodeData.node->getId()));
    ui.editSceneName->setText(QString::fromStdString(nodeData.node->getScene()));
    ui.editNodeName->setText(QString::fromStdString(nodeData.node->getName()));
    ui.editFrameName->setText(QString::fromStdString(nodeData.pose->frame));
    ui.editAgentName->setText(QString::fromStdString(nodeData.pose->agent));
 
    Eigen::Vector3f rpy;
    VirtualRobot::MathTools::eigen4f2rpy(nodeData.pose->toEigen(), rpy);
    ui.spinBoxX->setValue(nodeData.pose->position->x);
    ui.spinBoxY->setValue(nodeData.pose->position->y);
    ui.spinBoxZ->setValue(nodeData.pose->position->z);
 
    ui.spinBoxRoll->setValue(VirtualRobot::MathTools::rad2deg(rpy[0]));
    ui.spinBoxPitch->setValue(VirtualRobot::MathTools::rad2deg(rpy[1]));
    ui.spinBoxYaw->setValue(VirtualRobot::MathTools::rad2deg(rpy[2]));
}
 
void GraphVisualizerWidget::refreshGraph()
{
    clearGraph();
    drawScene();
}
 
void GraphVisualizerWidget::transformView()
{
    double d = ui.viewZoomFactor->value();
    ui.graphicsViewGraph->setTransform(QTransform::fromScale(d, d).rotate(viewAngle));
}
 
void GraphVisualizerWidget::viewRotatedClock()
{
    viewAngle = std::fmod(viewAngle + VIEW_ROTATE_STEP_SIZE_CC, 360);
    transformView();
}
 
void GraphVisualizerWidget::viewRotatedCounterClock()
{
    viewAngle = std::fmod(viewAngle + 360 - VIEW_ROTATE_STEP_SIZE_CC, 360);
    transformView();
}
 
void GraphVisualizerWidget::adjustView()
{
    float maxX = std::numeric_limits<float>::min();
    float minX = std::numeric_limits<float>::max();
    float maxY = std::numeric_limits<float>::min();
    float minY = std::numeric_limits<float>::max();
 
    //search bounding box
    for (const auto& node : nodes)
    {
        maxX = (maxX < node.second.pose->position->x) ? node.second.pose->position->x : maxX;
        minX = (minX > node.second.pose->position->x) ? node.second.pose->position->x : minX;
        maxY = (maxY < node.second.pose->position->y) ? node.second.pose->position->y : maxY;
        minY = (minY > node.second.pose->position->y) ? node.second.pose->position->y : minY;
    }
 
    auto deltaX = maxX - minX; //>=0
    auto deltaY = maxY - minY; //>=0
 
    //compare ratio of graph and view. if both horizontal (vertical) ->rotate to 0 or 180 (90,270)
    if (std::signbit(deltaX / deltaY - 1) == std::signbit(ui.graphicsViewGraph->width() / ui.graphicsViewGraph->height() - 1))
    {
        //same => rotate to 0 or 180
        viewAngle = (viewAngle < std::abs(180 - viewAngle)) ? 0 : 180;
        //set zoom => update
        ui.viewZoomFactor->setValue(std::min(ui.graphicsViewGraph->width() / deltaX,
                                             ui.graphicsViewGraph->height() / deltaY) * 0.9);
    }
    else
    {
        //different rotate to 90 or 270
        viewAngle = (std::abs(90 - viewAngle) < std::abs(270 - viewAngle)) ? 90 : 270;
        //set zoom => update
        ui.viewZoomFactor->setValue(std::min(ui.graphicsViewGraph->width() / deltaY,
                                             ui.graphicsViewGraph->height() / deltaX) * 0.9);
    }
 
}
 
bool GraphVisualizerWidget::addNewEdge(const std::string& fromId, const std::string& toId)
{
    std::string errorMsg;
 
    if (!graphSeg->hasEntityById(fromId))
    {
        errorMsg = "start node with Id '" + fromId + "' not found in segment";
    }
    else if (!graphSeg->hasEntityById(toId))
    {
        errorMsg = "end node with Id '" + toId + "' not found in segment";
 
    }
    else if (fromId == toId)
    {
        errorMsg = "starting and ending node are the same";
    }
    else
    {
        auto fromNode = graphSeg->getNodeById(fromId);
        for (const auto& adjacent : fromNode->getAdjacentNodes())
        {
            if (toId == adjacent->getId())
            {
                errorMsg = "edge '" + fromNode->getName() + "' -> '" + adjacent->getName() + "' already exists";
                break;
            }
        }
    }
 
    if (errorMsg.empty())
    {
        ui.labelAddEdgeStatus->setText(QString::fromStdString("Ok"));
        graphSeg->addEdge(fromId, toId);
        gnpr->forceRefetch(fromId);
        gnpr->forceRefetch(toId);
        addEdge(fromId, toId);
        updateNode(fromId);
        updateNode(toId);
        ui.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : lime; }");
    }
    else
    {
        ARMARX_WARNING << errorMsg;
        ui.labelAddEdgeStatus->setText(QString::fromStdString(errorMsg));
        ui.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : orange; }");
    }
 
    return errorMsg.empty();
}
 
void GraphVisualizerWidget::addNewEdgeBoth()
{
    std::string startId = ui.editStartNodeId->text().toStdString();
    std::string endId = ui.editEndNodeId->text().toStdString();
    addNewEdge(startId, endId);
    addNewEdge(endId, startId);
}
 
void GraphVisualizerWidget::addNewEdgeStartEnd()
{
    std::string startId = ui.editStartNodeId->text().toStdString();
    std::string endId = ui.editEndNodeId->text().toStdString();
    addNewEdge(startId, endId);
}
 
void GraphVisualizerWidget::addNewEdgeEndStart()
{
    std::string startId = ui.editEndNodeId->text().toStdString();
    std::string endId = ui.editStartNodeId->text().toStdString();
    addNewEdge(startId, endId);
}
 
void GraphVisualizerWidget::selectedSceneChanged(int i)
{
    const auto current = ui.scenesComboBox->currentText();
    if (!current.isEmpty())
    {
        lastSelectedSceneName = current;
    }
}
 
void GraphVisualizerWidget::updateSceneList()
{
    auto scenes = graphSeg->getScenes();
    ui.scenesComboBox->clear();
    int idx = -1;
 
    for (std::size_t i = 0; i < scenes.size(); i++)
    {
        const auto currentScene = QString::fromStdString(scenes[i]);
        ui.scenesComboBox->addItem(currentScene);
 
        if (currentScene == lastSelectedSceneName)
        {
            idx = i;
        }
    }
 
    ui.scenesComboBox->setCurrentIndex(idx);
}
 
void GraphVisualizerWidget::drawScene()
{
    std::vector<std::string> highlightedNodes;
    for (const auto& nodeData : nodes)
    {
        bool sameScene = ui.scenesComboBox->currentText().toStdString() == nodeData.second.node->getScene();
        if (nodeData.second.highlighted && sameScene)
        {
            highlightedNodes.push_back(nodeData.first);
        }
    }
 
    clearGraph();
    auto graphNodes = graphSeg->getNodesByScene(ui.scenesComboBox->currentText().toStdString());
 
    //add nodes
    for (auto& node : graphNodes)
    {
        auto pos = armarx::FramedPosePtr::dynamicCast(node->getPose());
 
        if (!pos || node->isMetaEntity())
        {
            continue;
        }
 
        addNode(node);
    }
 
    //add edges
    for (auto& node : graphNodes)
    {
        auto nodeId = node->getId();
 
        for (int i = 0; i < node->getOutdegree(); i++)
        {
            auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
            ARMARX_CHECK_EXPRESSION(adjacent);
            auto adjacentId = adjacent->getId();
 
            addEdge(nodeId, adjacentId);
        }
    }
 
    for (const auto& nodeId : highlightedNodes)
    {
        auto nodeIt = std::find_if(nodes.begin(), nodes.end(), [&](const std::pair<std::string, NodeData>& d)
        {
            return d.first == nodeId;
        });
 
        if (nodeIt != nodes.end())
        {
            nodeIt->second.highlighted = true;
            updateNode(nodeIt->first);
        }
    }
 
    adjustView();
}
 
void GraphVisualizerWidget::selectScene()
{
    //QString fi = QFileDialog::getOpenFileName(this, tr("Open Scene File"), QString(), tr("XML Files (*.xml)"));
    //    std::string xmlSceneFile = std::string(fi.toLatin1());
    //    loadScene(xmlSceneFile);
}
 
void GraphVisualizerWidget::loadScene(const std::string& xmlFile)
{
    //    VirtualRobot::ScenePtr SceneIO::loadScene(const std::string& xmlFile)
    //    {
    // load file
    std::ifstream in(xmlFile.c_str());
 
    if (!in.is_open())
    {
        ARMARX_WARNING << "Could not open XML file:" << xmlFile;
        return;
    }
 
    std::stringstream buffer;
    buffer << in.rdbuf();
    std::string sceneXML(buffer.str());
    std::filesystem::path filenameBaseComplete(xmlFile);
    std::filesystem::path filenameBasePath = filenameBaseComplete.parent_path();
    std::string basePath = filenameBasePath.string();
 
    in.close();
 
    //        VirtualRobot::ScenePtr res = createSceneFromString(robotXML, basePath);
    //        THROW_VR_EXCEPTION_IF(!res, "Error while parsing file " << xmlFile);
 
    //        return res;
    //    }
}
 
void GraphVisualizerWidget::addKitchenGraph()
{
    std::string scene {"GraphKitchen"};
    graphSeg->clearScene(scene);
 
    //if you insist on hardcoding scenes, use these convenience functions for improved readability:
    auto addNode = [&](const std::string & name, float x, float y, float angle)
    {
        graphSeg->addNode(new ::memoryx::GraphNode {x, y, angle, name, scene});
        ARMARX_INFO_S << "added node '" << name << "' at (" << x << ", " << y << ", " << angle << "rad)";
    };
 
    auto addEdges = [&](const std::string & nodeFromName, const std::string & nodeToName, bool bidirectional)
    {
        auto nodeFrom = graphSeg->getNodeFromSceneByName(scene, nodeFromName);
        auto nodeTo = graphSeg->getNodeFromSceneByName(scene, nodeToName);
        ARMARX_CHECK_EXPRESSION(nodeFrom);
        ARMARX_CHECK_EXPRESSION(nodeTo);
        ARMARX_INFO_S << "'" << nodeFrom->getName() << "' -> '" << nodeTo->getName() << "', status: " << graphSeg->addEdge(nodeFrom->getId(), nodeTo->getId());
        if (bidirectional)
        {
            ARMARX_INFO_S << "'" << nodeTo->getName() << "' -> '" << nodeFrom->getName() << "', status: " << graphSeg->addEdge(nodeTo->getId(), nodeFrom->getId());
        }
    };
 
    //ex
    addNode("initialnode", 2900, 7000, 0);
    addNode("sideboard", 3400, 7000, 0);
    addEdges("initialnode", "sideboard", true);
}
 
void GraphVisualizerWidget::addNewGraphNode()
{
    Eigen::Matrix4f mat;
    Eigen::Vector3f rpy;
    Eigen::Vector3f pos;
    rpy << VirtualRobot::MathTools::deg2rad(ui.spinBoxRoll->value()),
        VirtualRobot::MathTools::deg2rad(ui.spinBoxPitch->value()),
        VirtualRobot::MathTools::deg2rad(ui.spinBoxYaw->value());
    pos << ui.spinBoxX->value(),
        ui.spinBoxY->value(),
        ui.spinBoxZ->value();
    VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
    armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
            ui.editFrameName->text().toStdString(),
            ui.editAgentName->text().toStdString());
    GraphNodePtr node = new GraphNode(pose,
                                      ui.editNodeName->text().toStdString(),
                                      ui.editSceneName->text().toStdString());
    auto entityId = graphSeg->addNode(node);
    gnpr->forceRefetch(entityId);
    node->setId(entityId);
    if (ui.scenesComboBox->currentText().toStdString() == ui.editSceneName->text().toStdString())
    {
        ui.editNodeId->setText(QString::fromStdString(entityId));
        addNode(node);
    }
}
 
void GraphVisualizerWidget::editGraphNode()
{
    Eigen::Matrix4f mat;
    Eigen::Vector3f rpy;
    Eigen::Vector3f pos;
    rpy << VirtualRobot::MathTools::deg2rad(ui.spinBoxRoll->value()),
        VirtualRobot::MathTools::deg2rad(ui.spinBoxPitch->value()),
        VirtualRobot::MathTools::deg2rad(ui.spinBoxYaw->value());
    pos << ui.spinBoxX->value(),
        ui.spinBoxY->value(),
        ui.spinBoxZ->value();
    VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
    armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
            ui.editFrameName->text().toStdString(),
            ui.editAgentName->text().toStdString());
    GraphNodePtr node = new GraphNode(pose,
                                      ui.editNodeName->text().toStdString(),
                                      ui.editSceneName->text().toStdString());
    auto id = ui.editNodeId->text().toStdString();
 
    node->setId(id);
    graphSeg->updateEntity(id, node);
    gnpr->forceRefetch(id);
    if (ui.scenesComboBox->currentText().toStdString() == ui.editSceneName->text().toStdString())
    {
        addNode(node);
    }
}
 
void GraphVisualizerWidget::tableWidgetNodesCustomContextMenu(QPoint pos)
{
    int row = ui.tableWidgetNodes->rowAt(pos.y());
    auto nodeIt = std::find_if(nodes.begin(), nodes.end(), [&](const std::pair<std::string, NodeData>& d)
    {
        return d.second.tableWidgetNodesIndex == row;
    });
    QMenu menu;
    QAction* deleteAction = menu.addAction("Delete Node");
 
    if (menu.exec(QCursor::pos()) == deleteAction)
    {
        ARMARX_CHECK_EXPRESSION(nodeIt != nodes.end());
        graphSeg->removeNode(nodeIt->second.node->getId());
        drawScene();
    }
}
 
void GraphVisualizerWidget::tableWidgetEdgesCustomContextMenu(QPoint pos)
{
    int row = ui.tableWidgetEdges->rowAt(pos.y());
    auto edgeIt = std::find_if(edges.begin(), edges.end(), [&](const std::pair<EdgeId, EdgeData>& d)
    {
        return d.second.tableWidgetEdgesIndex == row;
    });
    QMenu menu;
    QAction* deleteAction = menu.addAction("Delete Edge");
 
    if (menu.exec(QCursor::pos()) == deleteAction)
    {
        ARMARX_CHECK_EXPRESSION(edgeIt != edges.end());
        graphSeg->removeEdge(edgeIt->first.first, edgeIt->first.second);
        drawScene();
    }
}
 
bool MouseEventProcessor::eventFilter(QObject* obj, QEvent* event)
{
    if (obj == gvw->ui.graphicsViewGraph && event->type() == QEvent::MouseButtonPress)
    {
        QMouseEvent* me = static_cast<QMouseEvent*>(event);
        if (me->button() == Qt::LeftButton)
        {
            QPointF scenePoint = gvw->ui.graphicsViewGraph->mapToScene(me->pos());
            scenePoint.setY(-scenePoint.y()); //not sure why
 
            float minDist = std::numeric_limits<float>::max();
            auto bestIt = gvw->nodes.cend();
 
            for (auto it = gvw->nodes.cbegin(); it != gvw->nodes.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 != gvw->nodes.cend())
            {
                gvw->nodeDoubleClicked(bestIt->first);
            }
        }
    }
    else if (event->type() == QEvent::Resize)
    {
        gvw->adjustView();
    }
    return QObject::eventFilter(obj, event);
}