SimoxCSpaceVisualizerWidgetController.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    RobotComponents
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#include <utility>
#include <unordered_set>
 
#include <QWidget>
#include <QString>
#include <QSlider>
#include <QLabel>
#include <QDoubleSpinBox>
#include <QSpinBox>
#include <QComboBox>
#include <QPushButton>
#include <QGroupBox>
#include <QTableWidget>
#include <QTableWidgetItem>
#include <QFileDialog>
 
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXCore/core/ArmarXManager.h>
 
#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
#include <RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h>
#include <VirtualRobot/Visualization/Visualization.h>
#include <MotionPlanning/CSpace/CSpaceSampled.h>
 
#include "SimoxCSpaceVisualizerWidgetController.h"
#include <VirtualRobot/Robot.h>
 
void clearLayout(QLayout* layout, bool deleteWidgets = true)
{
    while (QLayoutItem* item = layout->takeAt(0))
    {
        if (deleteWidgets)
        {
            if (QWidget* widget = item->widget())
            {
                widget->deleteLater();
            }
        }
        if (QLayout* childLayout = item->layout())
        {
            clearLayout(childLayout, deleteWidgets);
        }
        delete item;
    }
}
 
using namespace armarx;
 
static const float EDGE_WIDTH
{
    1.f
};
static const armarx::DrawColor EDGE_COLOR_TREE
{
    0.f, 1.f, 0.f, 1.f
};
static const armarx::DrawColor EDGE_COLOR_PATH
{
    0.f, 0.f, 1.f, 1.f
};
 
SimoxCSpaceVisualizerWidgetController::SimoxCSpaceVisualizerWidgetController()
{
    widget.setupUi(getWidget());
 
    visuRoot = nullptr;
    mutex3D.reset(new RecursiveMutex());
 
    //global widget settings
    widget.sliderPathPosition->setScalePosition(QwtSlider::LeadingScale);
    widget.sliderPathPosition->setScaleStepSize(0.001);
 
    //add list for server tasks
 
    motionPlanningServerTaskList = new MotionPlanningServerTaskList {};
    widget.loadFromServerLayout->addWidget(motionPlanningServerTaskList);
 
    updateCollisionStateTimer.setSingleShot(true);
 
    connect(&updateCollisionStateTimer, SIGNAL(timeout()), this, SLOT(updateCollisionState()));
}
 
void SimoxCSpaceVisualizerWidgetController::loadSettings(QSettings* settings)
{
    motionPlanningServerProxyName = settings->value("motionPlanningServerProxyName", QString::fromStdString(motionPlanningServerProxyName)).toString().toStdString();
}
 
void SimoxCSpaceVisualizerWidgetController::saveSettings(QSettings* settings)
{
    settings->setValue("motionPlanningServerProxyName", QString::fromStdString(motionPlanningServerProxyName));
}
 
QString SimoxCSpaceVisualizerWidgetController::getWidgetName() const
{
    return "MotionPlanning.SimoxCSpaceVisualizer";
}
 
void SimoxCSpaceVisualizerWidgetController::onInitComponent()
{
    visuRoot = new SoSeparator();
    visuRoot->ref();
 
    // visu agent needs to be at top of tree if coin so that its selection node is found first
    visuAgent = new SoSeparator();
    visuAgent->ref();
    visuRoot->addChild(visuAgent);
 
    // create the debugdrawer
    std::string debugDrawerComponentName = generateSubObjectName("DebugDrawer");
    ARMARX_INFO << "Creating debug drawer component " << debugDrawerComponentName;
    debugDrawer = Component::create<armarx::DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
 
    if (mutex3D)
    {
        debugDrawer->setMutex(mutex3D);
    }
    else
    {
        ARMARX_ERROR << " No 3d mutex available...";
    }
 
    getArmarXManager()->addObject(debugDrawer);
 
    {
        std::unique_lock lock(*mutex3D);
        visuRoot->addChild(debugDrawer->getVisualization());
    }
 
    setEnabledSimoxCSpace(false);
    connectSlots();
 
    usingProxy(motionPlanningServerProxyName);
}
 
QPointer<QDialog> SimoxCSpaceVisualizerWidgetController::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new SimpleConfigDialog(parent);
        dialog->addProxyFinder<MotionPlanningServerInterfacePrx>({"MotionPlanningServer", "The MotionPlanningServer (empty for none)", "*Server"});
    }
    return qobject_cast<SimpleConfigDialog*>(dialog);
}
 
void SimoxCSpaceVisualizerWidgetController::onConnectComponent()
{
    motionPlanningServerProxy = getProxy<MotionPlanningServerInterfacePrx>(motionPlanningServerProxyName);
    motionPlanningServerTaskList->setMotionPlanningServer(motionPlanningServerProxy);
    motionPlanningServerTaskList->enableAutoUpdate(true);
    resetSimoxCSpace();
}
 
SoNode* SimoxCSpaceVisualizerWidgetController::getScene()
{
    if (visuRoot)
    {
        std::cout << "Returning scene = " << visuRoot->getName() << std::endl;
    }
 
    return visuRoot;
}
 
void SimoxCSpaceVisualizerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
{
    this->mutex3D = mutex3D;
 
    if (debugDrawer)
    {
        debugDrawer->setMutex(mutex3D);
    }
}
 
void SimoxCSpaceVisualizerWidgetController::connectSlots()
{
    connect(widget.pushButtonLoadTask, SIGNAL(clicked()), this, SLOT(loadTask()));
 
    connect(widget.checkBoxShowTreeEdges, SIGNAL(clicked()), this, SLOT(updateEdgeVisibility()));
    connect(widget.checkBoxDrawTrace, SIGNAL(clicked(bool)), this, SLOT(drawEdges(bool)));
 
    connect(widget.doubleSpinBoxTraceStep, SIGNAL(valueChanged(double)), this, SLOT(traceStepChanged()));
    connect(widget.pushButtonHideAllPaths, SIGNAL(clicked()), this, SLOT(hideAllPaths()));
 
    connect(widget.pushButtonSetConfig, SIGNAL(clicked()), this, SLOT(setCurrentConfig()));
    connect(widget.spinBoxPathNumber, SIGNAL(valueChanged(int)), this, SLOT(setCurrentPath()));
    connect(widget.sliderPathPosition, SIGNAL(valueChanged(double)), this, SLOT(setCurrentPathPosition()));
    connect(widget.sliderPathPosition, SIGNAL(valueChanged(double)), this, SLOT(setCurrentPathPosition()));
    connect(widget.checkBoxCollisionNodeHighlighting, SIGNAL(toggled(bool)), this, SLOT(highlightCollisionNodes(bool)));
    startTimer(timerPeriod);
}
 
void SimoxCSpaceVisualizerWidgetController::traceStepChanged()
{
    if (widget.checkBoxDrawTrace->isChecked())
    {
        drawEdges(true);
    }
}
 
void SimoxCSpaceVisualizerWidgetController::updateEdgeVisibility()
{
    for (std::size_t pIdx = 0; pIdx < paths.size(); ++pIdx)
    {
        auto&& path = paths.at(pIdx);
        debugDrawer->enableLayerVisu(PathData::getEdgeLayerName(pIdx), path.visible && widget.checkBoxShowTreeEdges->isChecked());
    }
}
 
void SimoxCSpaceVisualizerWidgetController::loadTask()
{
    //switch here if new sources are added
    loadTaskFromServer();
}
 
void SimoxCSpaceVisualizerWidgetController::setCurrentPath()
{
    resetCurrentPath();
    int newPath = widget.spinBoxPathNumber->value() - 1;
 
    if (newPath < 0)
    {
        return;
    }
 
    widget.sliderPathPosition->setScale(0.0, getCurrentPath().getLength());
    widget.sliderPathPosition->setValue(0);
    widget.sliderPathPosition->setScaleStepSize(getCurrentPath().getLength() * 0.3);
    widget.sliderPathPosition->setTotalSteps(100);
    widget.labelCurrentPathNodeCount->setText(QString::number(getCurrentPath().path.size()));
    widget.labelCurrentPathTotalLen->setText(QString::number(getCurrentPath().getLength()));
    setCurrentPathPosition();
}
 
void SimoxCSpaceVisualizerWidgetController::setCurrentPathPosition()
{
    if (!widget.spinBoxPathNumber->value())
    {
        widget.labelCurrentPathCurrentLen->setText(QString::number(0));
        return;
    }
    const auto& path = getCurrentPath().path;
    const auto& pathAcc = getCurrentPath().accPathLength;
    const auto val = widget.sliderPathPosition->value();
    widget.labelCurrentPathCurrentLen->setText(QString::number(val));
    auto indexTo = std::distance(pathAcc.begin(), std::upper_bound(pathAcc.begin(), pathAcc.end(), val));
    widget.labelCurrentPathCurrentNode->setText(QString::number(indexTo));
 
    assert(indexTo >= 0);
    assert(static_cast<std::size_t>(indexTo) <= path.size());
 
    if (indexTo == 0)
    {
        setAndApplyConfig(path.front());
        return;
    }
 
    if (static_cast<std::size_t>(indexTo) == path.size())
    {
        setAndApplyConfig(path.back());
        return;
    }
 
    auto indexFrom = indexTo - 1;
    auto cfgFrom = path.at(indexFrom);
    auto cfgTo   = path.at(indexTo);
 
    auto t = (val - pathAcc.at(indexFrom)) / (pathAcc.at(indexTo) - pathAcc.at(indexFrom));
    VectorXf cfgToSet;
 
 
    SimoxCSpacePtr simoxcspace = SimoxCSpacePtr::dynamicCast(cspaceUtil);
    if (!simoxcspace)
    {
        CSpaceAdaptorBasePtr simoxcspaceadapter = CSpaceAdaptorBasePtr::dynamicCast(cspaceUtil);
        if (simoxcspaceadapter)
        {
            simoxcspace = SimoxCSpacePtr::dynamicCast(simoxcspaceadapter->getOriginalCSpace());
        }
    }
    if (simoxcspace)
    {
        // do robot specific interpolation
        VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(simoxcspace->getAgentSceneObj(), "tmp",
                                            simoxcspace->getAgentJointNames());
        Saba::CSpaceSampled tmpCSpace(simoxcspace->getAgentSceneObj(), VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(simoxcspace->getCD())), rns);
        Eigen::VectorXf from = Eigen::Map<Eigen::VectorXf>(cfgFrom.data(), cfgFrom.size());
        Eigen::VectorXf to = Eigen::Map<Eigen::VectorXf>(cfgTo.data(), cfgTo.size());
        auto config = tmpCSpace.interpolate(from, to, t);
        cfgToSet = (VectorXf {config.data(), config.data() + config.rows()});
 
    }
    else
    {
        std::transform(cfgFrom.begin(), cfgFrom.end(), cfgTo.begin(), std::back_inserter(cfgToSet),
                       [t](float from, float to)
        {
            return t * to + (1 - t) * from;
        }
                      );
    }
    setAndApplyConfig(cfgToSet);
}
 
void SimoxCSpaceVisualizerWidgetController::setCurrentConfig()
{
    if (!cspaceVisu)
    {
        return;
    }
 
    VectorXf cfg(cspaceVisu->getDimensionality());
 
    for (std::size_t i = 0; i < cfg.size(); ++i)
    {
        cfg.at(i) = dynamic_cast<QDoubleSpinBox*>(widget.tableWidgetDims->cellWidget(i, 2))->value();
    }
 
    ARMARX_DEBUG_S << "Setting visu config to: " << cfg;
 
    const float* it = cfg.data();
    cspaceVisu->setConfig(it);
    widget.labelCurrentMinObjDist->setText("loading...");
    widget.labelCurrentCollisionState->setText("loading...");
    updateCollisionStateTimer.start(100);
}
 
void SimoxCSpaceVisualizerWidgetController::updateCollisionState()
{
    widget.labelCurrentMinObjDist->setText(QString::number(cspaceVisu->getCD().getDistance()));
    widget.labelCurrentCollisionState->setText(cspaceVisu->getCD().isInCollision() ? QString {"in collision"}: QString {"no collision"});
 
}
 
void SimoxCSpaceVisualizerWidgetController::drawEdges(bool drawTrace)
{
    std::vector<std::string> nodeNames;
    nodeNames.reserve(robotNodes.size());
    for (const auto& node : robotNodes)
    {
        if (node.second)
        {
            nodeNames.emplace_back(node.first->getName());
        }
    }
 
    for (std::size_t idxPath = 0; idxPath < paths.size(); ++idxPath)
    {
        const auto& pDat = paths.at(idxPath);
        const auto& p = pDat.path;
 
        const auto layer = PathData::getEdgeLayerName(idxPath);
 
        if (p.empty())
        {
            continue;
        }
 
        //extract poses for each node and cfg
        const float traceStep = drawTrace ? static_cast<float>(widget.doubleSpinBoxTraceStep->value()) : std::numeric_limits<float>::infinity();
        auto traces = cspaceUtil->getTraceVisu(p, nodeNames, traceStep);
 
        //draw
        for (std::size_t i = 0; i < traces.size(); ++i)
        {
            DebugDrawerLineSet& trace = traces.at(i);
            trace.colorFullIntensity = EDGE_COLOR_PATH;
            trace.colorNoIntensity = EDGE_COLOR_PATH;
            trace.lineWidth = EDGE_WIDTH;
            trace.intensities.assign(trace.points.size() / 2, 1);
            debugDrawer->setLineSetVisu(layer, "trace:" + nodeNames.at(i), trace);
        }
    }
}
 
void SimoxCSpaceVisualizerWidgetController::setVisibilityPath(int index, Qt::CheckState state)
{
    auto&& path = paths.at(index);
    const bool vis = (state == Qt::Checked);
    path.visible = vis;
    debugDrawer->enableLayerVisu(PathData::getEdgeLayerName(index), vis && widget.checkBoxShowTreeEdges->isChecked());
}
 
void SimoxCSpaceVisualizerWidgetController::hideAllPaths()
{
    for (int i = 0; static_cast<std::size_t>(i) < paths.size(); ++i)
    {
        dynamic_cast<QCheckBox*>(widget.tableWidgetPaths->cellWidget(i, 0))->setChecked(false);
    }
}
 
void SimoxCSpaceVisualizerWidgetController::loadTaskFromServer()
{
    if (!motionPlanningServerProxy)
    {
        return;
    }
    const auto taskId = motionPlanningServerTaskList->getSelectedId();
 
    if (!taskId)
    {
        return;
    }
 
    auto taskCSpaceAndPaths = motionPlanningServerProxy->getTaskCSpaceAndPathsById(*taskId);
 
    auto& taskCSpace = taskCSpaceAndPaths.cspace;
 
    if (!taskCSpace)
    {
        ARMARX_ERROR_S << "Task with id " << *taskId << " returned an empty cspace!";
        return;
    }
 
    //unwrap cspace adaptors
    while (taskCSpace->ice_isA(CSpaceAdaptor::ice_staticId()))
    {
        if (taskCSpace->ice_isA(ScaledCSpace::ice_staticId()))
        {
            auto scalingCSpace = ScaledCSpacePtr::dynamicCast(taskCSpace);
            for (auto& path : taskCSpaceAndPaths.paths)
            {
                scalingCSpace->unscalePath(path);
            }
        }
 
        taskCSpace = CSpaceAdaptorPtr::dynamicCast(taskCSpace)->getOriginalCSpace();
    }
 
    if (taskCSpace->ice_isA(SimoxCSpace::ice_staticId()))
    {
        SimoxCSpacePtr cspace = SimoxCSpacePtr::dynamicCast(taskCSpace);
        setSimoxCSpace(cspace);
        setPaths(std::move(taskCSpaceAndPaths.paths));
    }
}
 
void SimoxCSpaceVisualizerWidgetController::soSeparatorCleanUpAndRemoveFromRoot(SoSeparator*& toRemove)
{
    soSeparatorRemoveFromRoot(toRemove);
    soSeparatorCleanUp(toRemove);
}
 
void SimoxCSpaceVisualizerWidgetController::soSeparatorCleanUp(SoSeparator*& toClear)
{
    std::unique_lock lock(*mutex3D);
 
    if (toClear)
    {
        toClear->removeAllChildren();
        toClear->unref();
        toClear = nullptr;
    }
}
 
void SimoxCSpaceVisualizerWidgetController::soSeparatorRemoveFromRoot(SoSeparator* toRemove)
{
    std::unique_lock lock(*mutex3D);
 
    if (!visuRoot || !toRemove)
    {
        return;
    }
 
    if (visuRoot->findChild(toRemove) >= 0)
    {
        visuRoot->removeChild(toRemove);
    }
}
 
void SimoxCSpaceVisualizerWidgetController::soSeparatorAddToRoot(SoSeparator* toAdd)
{
    std::unique_lock lock(*mutex3D);
 
    if (!visuRoot || !toAdd)
    {
        return;
    }
 
    if (visuRoot->findChild(toAdd) < 0)
    {
        visuRoot->addChild(toAdd);
    }
}
 
void SimoxCSpaceVisualizerWidgetController::soSeparatorToggleRootChild(SoSeparator* toToggle)
{
    std::unique_lock lock(*mutex3D);
 
    if (!visuRoot || !toToggle)
    {
        return;
    }
 
    if (visuRoot->findChild(toToggle) < 0)
    {
        visuRoot->addChild(toToggle);
    }
    else
    {
        visuRoot->removeChild(toToggle);
    }
}
 
void SimoxCSpaceVisualizerWidgetController::soSeparatorRootChildVis(SoSeparator* child, bool visible)
{
    std::unique_lock lock(*mutex3D);
 
    if (!visuRoot || !child)
    {
        return;
    }
 
    if (visible)
    {
        if (visuRoot->findChild(child) < 0)
        {
            visuRoot->addChild(child);
        }
    }
    if (!visible)
    {
        if (!(visuRoot->findChild(child) < 0))
        {
            visuRoot->removeChild(child);
        }
    }
}
 
void SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSpace)
{
    resetSimoxCSpace();
    cspaceUtil = SimoxCSpacePtr::dynamicCast(newCSpace->clone());
    cspaceVisu = SimoxCSpacePtr::dynamicCast(newCSpace->clone(true));
    cspaceUtil->initCollisionTest();
    cspaceVisu->initCollisionTest();
 
    //set group box "current config"
    const auto&& dimensions = cspaceUtil->getDimensionsBounds();
    auto jointNames = cspaceUtil->getAgentJointNames();
    assert(jointNames.size() == dimensions.size());
 
    widget.tableWidgetDims->setRowCount(dimensions.size());
 
    for (std::size_t i = 0; i < dimensions.size(); ++i)
    {
        const std::string& name = jointNames.at(i);
        const FloatRange& bounds = dimensions.at(i);
 
        widget.tableWidgetDims->setItem(i, 0, new QTableWidgetItem {QString::fromStdString(name)});
 
        widget.tableWidgetDims->setItem(i, 1, new QTableWidgetItem {QString::number(bounds.min)});
        auto spinbox = new QDoubleSpinBox {};
        spinbox->setRange(bounds.min, bounds.max);
        widget.tableWidgetDims->setCellWidget(i, 2, spinbox);
        widget.tableWidgetDims->setItem(i, 3, new QTableWidgetItem {QString::number(bounds.max)});
        spinbox->setSingleStep((bounds.max - bounds.min) / 10000.0);
        connect(spinbox, SIGNAL(valueChanged(double)), widget.pushButtonSetConfig, SIGNAL(clicked()));
    }
 
    visuObjStat.resize(cspaceVisu->getStationaryObjectSet()->getSize());
    //add visu agent
    {
        std::unique_lock lock(*mutex3D);
        auto robot = cspaceVisu->getAgentSceneObj();
        robot->setUpdateVisualization(true);
        robotVisu = robot->getVisualization<VirtualRobot::CoinVisualization>();
        ARMARX_IMPORTANT << "Setting visu of robot " << robot.get();
        auto robotCoinVisu = robotVisu->getCoinVisualization(true);
        highlightCollisionNodes(widget.checkBoxCollisionNodeHighlighting->checkState() == Qt::Checked);
        visuAgent->removeAllChildren();
        visuAgent->addChild(robotCoinVisu);
        //add to table
        auto boxVis = new QCheckBox {QString::fromStdString(robot->getName())};
        boxVis->setChecked(true);
        connect(boxVis, SIGNAL(stateChanged(int)), this, SLOT(setVisibilityAgent(int)));
 
        widget.scrollAreaWidgetContentsObj->layout()->addWidget(boxVis);
    }
 
 
    //add visu stat obj
    for (int i = 0; i < static_cast<int>(visuObjStat.size()); ++i)
    {
        auto sceneObj = cspaceVisu->getStationaryObjectSet()->getSceneObject(i);
        //        auto& obj = cspaceVisu->getStationaryObjects().at(i);
        auto& sep = visuObjStat.at(i);
        //add visu
        sep = new SoSeparator();
        sep->ref();
        sceneObj->setUpdateVisualization(true);
        sceneObj->setUpdateCollisionModel(true);
        auto* visu = dynamic_cast<VirtualRobot::CoinVisualizationNode*>(sceneObj->getVisualization(VirtualRobot::SceneObject::Collision).get());
        auto coin = visu->getCoinVisualization();
        ARMARX_INFO << sceneObj->getName() << " visu: " << visu << " coin: " << coin;
        sep->addChild(coin);
        soSeparatorAddToRoot(sep);
        //add to table
        auto boxVis = new IndexedQCheckBox {i, QString::fromStdString(sceneObj->getName())};
        boxVis->setChecked(true);
        connect(boxVis, SIGNAL(stateChangedIndex(int, Qt::CheckState)), this, SLOT(setVisibilityObjectStationary(int, Qt::CheckState)));
        widget.scrollAreaWidgetContentsObj->layout()->addWidget(boxVis);
    }
 
 
    //add list of nodes
    auto nodes = cspaceUtil->getAgentSceneObj()->getRobotNodes();
    robotNodes.clear();
    std::transform(
        nodes.begin(), nodes.end(), std::back_inserter(robotNodes),
        [](VirtualRobot::RobotNodePtr & nptr)
    {
        return std::pair<VirtualRobot::RobotNodePtr, bool> {std::move(nptr), false};
    }
    );
 
    //tcps of used kinematic chains are visible per default
    std::set<std::string> visTcps;
    const auto& kinematicChainNames = cspaceUtil->getAgent().kinemaicChainNames;
    std::transform(
        kinematicChainNames.begin(), kinematicChainNames.end(), std::inserter(visTcps, visTcps.begin()),
        [this](const std::string & name)
    {
        return cspaceUtil->getAgentSceneObj()->getRobotNodeSet(name)->getTCP()->getName();
    }
    );
 
    for (int i = 0; i < static_cast<int>(robotNodes.size()); ++i)
    {
        const auto& node = robotNodes.at(i).first;
        const auto& name = node->getName();
        bool visible = visTcps.find(name) != visTcps.end();
        robotNodes.at(i).second = visible;
 
        auto boxVis = new IndexedQCheckBox {i, QString::fromStdString(node->getName())};
        boxVis->setChecked(visible);
        connect(boxVis, SIGNAL(stateChangedIndex(int, Qt::CheckState)), this, SLOT(toggleRobotNodePath(int, Qt::CheckState)));
        widget.scrollAreaWidgetContentsJoints->layout()->addWidget(boxVis);
    }
 
    setCurrentConfig();
    setEnabledSimoxCSpace();
}
 
void SimoxCSpaceVisualizerWidgetController::toggleRobotNodePath(int index, Qt::CheckState)
{
    robotNodes.at(index).second ^= 1;
    drawEdges();
}
 
void SimoxCSpaceVisualizerWidgetController::setPaths(std::vector<Path> newPaths)
{
    resetPaths();
 
    assert(paths.empty());
 
    std::transform(
        newPaths.begin(), newPaths.end(), std::back_inserter(paths),
        [](Path & p)
    {
        PathData dat;
        dat.path = std::move(p.nodes);
 
        float lengthAcc = 0;
        dat.accPathLength.emplace_back(0);
        for (std::size_t i = 1; i < dat.path.size(); ++i)
        {
            const auto& cfg1 = dat.path.at(i - 1);
            const auto& cfg2 = dat.path.at(i);
 
            lengthAcc += euclideanDistance(cfg1.begin(), cfg1.end(), cfg2.begin());
            dat.accPathLength.emplace_back(lengthAcc);
        }
 
        return dat;
    }
    );
 
    widget.spinBoxPathNumber->setMaximum(paths.size());
 
    widget.tableWidgetPaths->setRowCount(paths.size());
    for (std::size_t i = 0; i < paths.size(); ++i)
    {
        const auto& p = paths.at(i);
        widget.tableWidgetPaths->setItem(i, 1, new QTableWidgetItem {QString::number(p.path.size())});
        widget.tableWidgetPaths->setItem(i, 2, new QTableWidgetItem {QString::number(p.getLength())});
 
        auto boxVis = new IndexedQCheckBox {static_cast<int>(i), QString::fromStdString(newPaths.at(i).pathName)};
        connect(boxVis, SIGNAL(stateChangedIndex(int, Qt::CheckState)), this, SLOT(setVisibilityPath(int, Qt::CheckState)));
        boxVis->setChecked(true);
        widget.tableWidgetPaths->setCellWidget(i, 0, boxVis);
    }
 
    setEnabledPaths(true);
    //draw
    drawEdges();
    updateEdgeVisibility();
}
 
void SimoxCSpaceVisualizerWidgetController::setAndApplyConfig(const VectorXf& cfg)
{
    if (widget.tableWidgetDims->columnCount() < 2)
    {
        return;
    }
    for (std::size_t i = 0; i < cfg.size() && (int)i < widget.tableWidgetDims->rowCount(); ++i)
    {
        auto spinbox = dynamic_cast<QDoubleSpinBox*>(widget.tableWidgetDims->cellWidget(i, 2));
        if (spinbox)
        {
            spinbox->setValue(cfg.at(i));
        }
    }
    setCurrentConfig();
}
 
void SimoxCSpaceVisualizerWidgetController::resetSimoxCSpace()
{
    resetPaths();
    setEnabledSimoxCSpace(false);
 
    widget.labelCurrentMinObjDist->setText("");
    widget.tableWidgetDims->clearContents();
    widget.tableWidgetDims->setRowCount(0);
 
    clearLayout(widget.scrollAreaWidgetContentsJoints->layout());
    clearLayout(widget.scrollAreaWidgetContentsObj->layout());
 
    for (auto& sep : visuObjStat)
    {
        soSeparatorCleanUpAndRemoveFromRoot(sep);
    }
    visuObjStat.clear();
    robotVisu.reset();
    visuAgent->removeAllChildren();
    debugDrawer->clearAll();
    cspaceUtil = nullptr;
    cspaceVisu = nullptr;
}
 
void SimoxCSpaceVisualizerWidgetController::resetPaths()
{
    setEnabledPaths(false);
    resetCurrentPath();
    widget.tableWidgetPaths->clearContents();
    widget.spinBoxPathNumber->setMaximum(0);
    widget.doubleSpinBoxMovePerMs->setValue(0);
    paths.clear();
}
 
void SimoxCSpaceVisualizerWidgetController::setEnabledSimoxCSpace(bool active)
{
    widget.groupBoxCurCfg->setEnabled(active);
    widget.groupBoxVisPath->setEnabled(active);
    widget.groupBoxVisObj->setEnabled(active);
}
 
void SimoxCSpaceVisualizerWidgetController::setEnabledPaths(bool active)
{
    widget.groupBoxPaths->setEnabled(active);
    widget.groupBoxPathOpt->setEnabled(active);
}
 
void SimoxCSpaceVisualizerWidgetController::timerEvent(QTimerEvent*)
{
    const auto step = widget.doubleSpinBoxMovePerMs->value() * timerPeriod;
 
    if (widget.checkBoxAnimate->isChecked() && step != 0)
    {
        widget.sliderPathPosition->setValue(widget.sliderPathPosition->value() + step);
    }
}
 
void SimoxCSpaceVisualizerWidgetController::highlightCollisionNodes(bool enabled)
{
    if (!cspaceVisu)
    {
        return;
    }
    if (!robotVisu)
    {
        return;
    }
    auto robot = cspaceVisu->getAgentSceneObj();
    //add visu
    std::unique_lock lock(*mutex3D);
    robot->setUpdateVisualization(true);
 
 
    std::set<std::string> nodesToHighlight;
    for (auto& setName : cspaceVisu->getAgent().collisionSetNames)
    {
        auto set = robot->getRobotNodeSet(setName);
        for (size_t i = 0; i < set->getSize(); ++i)
        {
            VirtualRobot::RobotNodePtr node = set->getNode(i);
            nodesToHighlight.insert(node->getName());
        }
    }
 
    for (auto& name : nodesToHighlight)
    {
        auto node = robot->getRobotNode(name);
        ARMARX_INFO << "highlighting: " << node->getName() <<  ": " << enabled;
        node->highlight(robotVisu, enabled);
 
    }
}