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 "SimoxCSpaceVisualizerWidgetController.h"
 
#include <unordered_set>
#include <utility>
 
#include <QComboBox>
#include <QDoubleSpinBox>
#include <QFileDialog>
#include <QGroupBox>
#include <QLabel>
#include <QPushButton>
#include <QSlider>
#include <QSpinBox>
#include <QString>
#include <QTableWidget>
#include <QTableWidgetItem>
#include <QWidget>
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
#include <VirtualRobot/Visualization/Visualization.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
 
#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
 
#include <MotionPlanning/CSpace/CSpaceSampled.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();
        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);
    }
}