AffordancePipelineGuiWidgetController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * \package    VisionX::gui-plugins::AffordancePipelineGuiWidgetController
 * \author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * \date       2016
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "AffordancePipelineGuiWidgetController.h"
#include "AffordancePipelineGuiConfigDialog.h"
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/application/Application.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <QPushButton>
#include <QCheckBox>
#include <QListWidget>
 
#include <string>
 
using namespace armarx;
 
AffordancePipelineGuiWidgetController::AffordancePipelineGuiWidgetController() :
    customToolbar(NULL)
{
    widget.setupUi(getWidget());
    updateTimer = new QTimer(this);
}
 
AffordancePipelineGuiWidgetController::~AffordancePipelineGuiWidgetController()
{
    delete updateTimer;
}
 
void AffordancePipelineGuiWidgetController::loadSettings(QSettings* settings)
{
    pointCloudProviderNames.clear();
    QList<QVariant> names = settings->value("pointCloudProviderNames", QList<QVariant>()).toList();
    for (int i = 0; i < names.count(); i++)
    {
        pointCloudProviderNames.push_back(names[i].toString().toStdString());
    }
 
    filteredPointCloudProviderNames.clear();
    names = settings->value("filteredPointCloudProviderNames", QList<QVariant>()).toList();
    for (int i = 0; i < names.count(); i++)
    {
        filteredPointCloudProviderNames.push_back(names[i].toString().toStdString());
    }
 
    pointCloudProviderDisplayNames.clear();
    names = settings->value("pointCloudProviderDisplayNames", QList<QVariant>()).toList();
    for (int i = 0; i < names.count(); i++)
    {
        pointCloudProviderDisplayNames.push_back(names[i].toString().toStdString());
    }
 
    pipelineConfigFiles.clear();
    names = settings->value("pipelineConfigFiles", QList<QVariant>()).toList();
    for (int i = 0; i < names.count(); i++)
    {
        pipelineConfigFiles.push_back(names[i].toString().toStdString());
    }
 
    pointCloudSegmenterName = settings->value("pointCloudSegmenterName", "").toString().toStdString();
    primitiveExtractorName = settings->value("primitiveExtractorName", "").toString().toStdString();
    affordanceExtractionName = settings->value("affordanceExtractionName", "").toString().toStdString();
    pipelineVisualizationName = settings->value("pipelineVisualizationName", "").toString().toStdString();
    robotStateComponentName = settings->value("robotStateComponentName", "").toString().toStdString();
    platform = settings->value("platform", "").toString().toStdString();
}
 
void AffordancePipelineGuiWidgetController::saveSettings(QSettings* settings)
{
    QList<QVariant> names;
    for (auto& name : pointCloudProviderNames)
    {
        names.append(QString::fromStdString(name));
    }
    settings->setValue("pointCloudProviderNames", names);
 
    names.clear();
    for (auto& name : filteredPointCloudProviderNames)
    {
        names.append(QString::fromStdString(name));
    }
    settings->setValue("filteredPointCloudProviderNames", names);
 
    names.clear();
    for (auto& name : pointCloudProviderDisplayNames)
    {
        names.append(QString::fromStdString(name));
    }
    settings->setValue("pointCloudProviderDisplayNames", names);
 
    names.clear();
    for (auto& filename : pipelineConfigFiles)
    {
        names.append(QString::fromStdString(filename));
    }
    settings->setValue("pipelineConfigFiles", names);
 
    settings->setValue("pointCloudSegmenterName", QString::fromStdString(pointCloudSegmenterName));
    settings->setValue("primitiveExtractorName", QString::fromStdString(primitiveExtractorName));
    settings->setValue("affordanceExtractionName", QString::fromStdString(affordanceExtractionName));
    settings->setValue("pipelineVisualizationName", QString::fromStdString(pipelineVisualizationName));
    settings->setValue("robotStateComponentName", QString::fromStdString(robotStateComponentName));
    settings->setValue("platform", QString::fromStdString(platform));
}
 
void AffordancePipelineGuiWidgetController::onInitComponent()
{
    for (auto& provider : pointCloudProviderNames)
    {
        usingProxy(provider);
    }
 
    for (auto& provider : filteredPointCloudProviderNames)
    {
        usingProxy(provider);
    }
 
    usingProxy(pointCloudSegmenterName);
    usingProxy(primitiveExtractorName);
    usingProxy(affordanceExtractionName);
    usingProxy(pipelineVisualizationName);
    usingProxy(robotStateComponentName);
}
 
void AffordancePipelineGuiWidgetController::onConnectComponent()
{
    pointCloudProviders.clear();
    for (auto& provider : pointCloudProviderNames)
    {
        pointCloudProviders.push_back(getProxy<visionx::CapturingPointCloudProviderInterfacePrx>(provider));
    }
 
    filteredPointCloudProviders.clear();
    for (auto& provider : filteredPointCloudProviderNames)
    {
        filteredPointCloudProviders.push_back(getProxy<armarx::PointCloudFilterInterfacePrx>(provider));
    }
 
    pointCloudSegmenter = getProxy<visionx::PointCloudSegmenterInterfacePrx>(pointCloudSegmenterName);
    primitiveExtractor = getProxy<visionx::PrimitiveMapperInterfacePrx>(primitiveExtractorName);
    affordanceExtraction = getProxy<AffordanceExtractionInterfacePrx>(affordanceExtractionName);
    pipelineVisualization = getProxy<AffordancePipelineVisualizationInterfacePrx>(pipelineVisualizationName);
    robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
 
    // Load pipeline config files
    widget.comboBoxSegmentation->clear();
    for (unsigned int i = 0; i < pipelineConfigFiles.size(); i++)
    {
        widget.comboBoxSegmentation->addItem(QString::fromStdString("C" + std::to_string(i)));
    }
 
    // Initialize provider list in UI
    widget.comboBoxPointCloudSource->clear();
    for (auto& provider : pointCloudProviderDisplayNames)
    {
        widget.comboBoxPointCloudSource->addItem(QString::fromStdString(provider));
    }
    widget.comboBoxPointCloudSource->setCurrentIndex(0);
    pointCloudSourceSelected(0);
 
    // Clear any previous visualization
    pipelineVisualization->clearVisualization();
 
    updatePipelineStatus();
 
    connect(widget.pushButtonStart, SIGNAL(clicked()), this, SLOT(startPipeline()));
    connect(widget.pushButtonPause, SIGNAL(clicked()), this, SLOT(pausePipeline()));
    connect(widget.pushButtonStep, SIGNAL(clicked()), this, SLOT(stepPipeline()));
 
    connect(widget.comboBoxCropping, SIGNAL(currentIndexChanged(int)), this, SLOT(croppingSelected(int)));
    connect(widget.comboBoxPointCloudSource, SIGNAL(currentIndexChanged(int)), this, SLOT(pointCloudSourceSelected(int)));
    connect(widget.comboBoxSegmentation, SIGNAL(currentIndexChanged(int)), this, SLOT(segmentationSetupSelected(int)));
    connect(widget.checkBoxShowPrimitives, SIGNAL(toggled(bool)), this, SLOT(visualizationOptionsChanged(bool)));
    connect(widget.checkBoxShowAffordances, SIGNAL(toggled(bool)), this, SLOT(visualizationOptionsChanged(bool)));
 
    connect(updateTimer, SIGNAL(timeout()), this, SLOT(updatePipelineStatus()));
    updateTimer->start(500);
 
    // Set default cropping setup
    widget.comboBoxCropping->setCurrentIndex(3);
 
    // Set default segmentation setup
    widget.comboBoxSegmentation->setCurrentIndex(0);
 
    // Initially start in paused mode
    pausePipeline();
 
    QMetaObject::invokeMethod(this, "visualizationOptionsChanged", Qt::QueuedConnection, Q_ARG(bool, true));
}
 
void AffordancePipelineGuiWidgetController::onDisconnectComponent()
{
    QObject::disconnect(this, SLOT(startPipeline()));
    QObject::disconnect(this, SLOT(pausePipeline()));
    QObject::disconnect(this, SLOT(stepPipeline()));
    QObject::disconnect(this, SLOT(croppingSelected(int)));
    QObject::disconnect(this, SLOT(segmentationSetupSelected(int)));
    QObject::disconnect(this, SLOT(pointCloudSourceSelected(int)));
    QObject::disconnect(this, SLOT(visualizationOptionsChanged(bool)));
 
    QObject::disconnect(this, SLOT(updatePipelineStatus()));
    updateTimer->stop();
 
    widget.labelProviderStatus->setText("Stopped");
    widget.labelSegmenterStatus->setText("Stopped");
    widget.labelPrimitivesStatus->setText("Stopped");
    widget.labelAffordancesStatus->setText("Stopped");
}
 
void AffordancePipelineGuiWidgetController::updatePipelineStatus()
{
    try
    {
        bool providerStatus = pointCloudProviders[currentPointCloudProvider]->isCaptureEnabled();
        bool segmenterStatus = pointCloudSegmenter->isPipelineStepEnabled();
        bool primitivesStatus = primitiveExtractor->isPipelineStepEnabled();
        bool affordancesStatus = affordanceExtraction->isPipelineStepEnabled();
 
        widget.labelProviderStatus->setText(providerStatus ? "Running" : "Paused");
        widget.labelSegmenterStatus->setText(segmenterStatus ? "Running" : "Paused");
        widget.labelPrimitivesStatus->setText(primitivesStatus ? "Running" : "Paused");
        widget.labelAffordancesStatus->setText(affordancesStatus ? "Running" : "Paused");
 
        widget.labelProviderTimestamp->setText(formatTimestamp(pointCloudProviders[currentPointCloudProvider]->getPointCloudFormat()->timeProvided));
        widget.labelSegmenterTimestamp->setText(formatTimestamp(pointCloudSegmenter->getLastProcessedTimestamp()->timestamp));
        widget.labelPrimitivesTimestamp->setText(formatTimestamp(primitiveExtractor->getLastProcessedTimestamp()->timestamp));
        widget.labelAffordancesTimestamp->setText(formatTimestamp(affordanceExtraction->getLastProcessedTimestamp()->timestamp));
 
        bool status = providerStatus && segmenterStatus && primitivesStatus && affordancesStatus;
        widget.pushButtonStart->setEnabled(!status);
        widget.pushButtonPause->setEnabled(status);
        widget.pushButtonStep->setEnabled(!status);
    }
    catch (const Ice::ConnectionRefusedException& e)
    {
        ARMARX_INFO << "The pipeline has been killed";
    }
}
 
void AffordancePipelineGuiWidgetController::startPipeline()
{
    ARMARX_INFO << "Starting perception pipeline";
 
    pointCloudSegmenter->enablePipelineStep();
    primitiveExtractor->enablePipelineStep();
    affordanceExtraction->enablePipelineStep();
    pointCloudProviders[currentPointCloudProvider]->begin_startCapture();
}
 
void AffordancePipelineGuiWidgetController::pausePipeline()
{
    ARMARX_INFO << "Pausing perception pipeline";
 
    pointCloudProviders[currentPointCloudProvider]->begin_stopCapture();
    pointCloudSegmenter->disablePipelineStep();
    primitiveExtractor->disablePipelineStep();
    affordanceExtraction->disablePipelineStep();
}
 
void AffordancePipelineGuiWidgetController::stepPipeline()
{
    ARMARX_INFO << "Stepping perception pipeline";
 
    pointCloudSegmenter->enablePipelineStep();
    primitiveExtractor->enablePipelineStep();
    affordanceExtraction->enablePipelineStep();
    pointCloudProviders[currentPointCloudProvider]->begin_startCaptureForNumFrames(1);
}
 
void AffordancePipelineGuiWidgetController::visualizationOptionsChanged(bool prm)
{
    pipelineVisualization->begin_enableVisualization(widget.checkBoxShowPrimitives->isChecked(), widget.checkBoxShowAffordances->isChecked(), false);
}
 
void AffordancePipelineGuiWidgetController::loadScene()
{
    std::string filename = QFileDialog::getOpenFileName(getWidget(), tr("Open Affordance Scene"), "~", tr("Scene Files (*.scene)")).toStdString();
    if (filename == "")
    {
        return;
    }
 
    affordanceExtraction->importScene(filename);
}
 
void AffordancePipelineGuiWidgetController::saveScene()
{
    std::string filename = QFileDialog::getSaveFileName(NULL, tr("Save Affordance Scene"), "", tr("Scene Files (*.scene)")).toStdString();
    if (filename == "")
    {
        return;
    }
 
    affordanceExtraction->exportScene(filename);
}
 
void AffordancePipelineGuiWidgetController::croppingSelected(int index)
{
    Eigen::Vector3f min, max;
 
    // Account for robots which don't have their root pose in the ground plane (e.g. ARMAR-4 as opposed to ARMAR-III)
    float root_z = PosePtr::dynamicCast(robotStateComponent->getSynchronizedRobot()->getGlobalPose())->toEigen()(2, 3);
 
    switch (index)
    {
        case 0:
            // Effectively disable cropping
            min << -10000, -10000, -10000;
            max <<  10000,  10000,  10000;
            break;
 
        case 1:
            min << -3000,  300,    0 - root_z;
            max <<  3000, 3000, 3000 - root_z;
            break;
 
        case 2:
            min << -2000,  300,  300 - root_z;
            max <<  2000, 3000, 2500 - root_z;
            break;
 
        case 3:
            min << -1000,  300,  600 - root_z;
            max <<  1000, 2000, 1500 - root_z;
            break;
 
        case 4:
            min << -500,  300, 1000 - root_z;
            max <<  500, 2000, 1500 - root_z;
            break;
 
        default:
            ARMARX_WARNING << "Unknown cropping setup selected";
            break;
    }
 
    for (auto& provider : filteredPointCloudProviders)
    {
        provider->setCroppingParameters(new armarx::Vector3(min), new armarx::Vector3(max), "Root");
    }
}
 
void AffordancePipelineGuiWidgetController::pointCloudSourceSelected(int index)
{
    currentPointCloudProvider = index;
    pointCloudSegmenter->begin_changePointCloudProvider(filteredPointCloudProviderNames[currentPointCloudProvider] + "Result");
 
    segmentationSetupSelected(widget.comboBoxSegmentation->currentIndex());
}
 
void AffordancePipelineGuiWidgetController::segmentationSetupSelected(int index)
{
    if (index < 0 || static_cast<std::size_t>(index) >= pipelineConfigFiles.size())
    {
        return;
    }
 
    primitiveExtractor->loadParametersFromFile(pipelineConfigFiles[index]);
    pointCloudSegmenter->loadLccpParametersFromFile(pipelineConfigFiles[index]);
}
 
QString AffordancePipelineGuiWidgetController::formatTimestamp(long timestamp)
{
    IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
    return QString(time.toDateTime().substr(time.toDateTime().find(' ') + 1).c_str());
}
 
QPointer<QDialog> armarx::AffordancePipelineGuiWidgetController::getConfigDialog(QWidget* parent)
{
    if (!configDialog)
    {
        configDialog = new AffordancePipelineGuiConfigDialog(parent);
    }
 
    return qobject_cast<AffordancePipelineGuiConfigDialog*>(configDialog);
}
 
void AffordancePipelineGuiWidgetController::configured()
{
    pointCloudProviderNames = configDialog->getProviderNames();
    pointCloudProviderDisplayNames = configDialog->getProviderDisplayNames();
    filteredPointCloudProviderNames = configDialog->getFilteredProviderNames();
    pipelineConfigFiles = configDialog->getPipelineConfigurationFiles();
 
    pointCloudSegmenterName = configDialog->pointCloudSegmenterProxyFinder->getSelectedProxyName().toStdString();
    primitiveExtractorName = configDialog->primitiveExtractorProxyFinder->getSelectedProxyName().toStdString();
    affordanceExtractionName = configDialog->affordanceExtractionProxyFinder->getSelectedProxyName().toStdString();
    pipelineVisualizationName = configDialog->pipelineVisualizationProxyFinder->getSelectedProxyName().toStdString();
    robotStateComponentName = configDialog->robotStateComponentProxyFinder->getSelectedProxyName().toStdString();
    platform = configDialog->getPlatform();
}
 
QPointer<QWidget> AffordancePipelineGuiWidgetController::getCustomTitlebarWidget(QWidget* parent)
{
    if (customToolbar)
    {
        if (parent != customToolbar->parent())
        {
            customToolbar->setParent(parent);
        }
 
        return customToolbar;
    }
 
    customToolbar = new QToolBar(parent);
    customToolbar->setIconSize(QSize(16, 16));
    customToolbar->addAction(QIcon(":/icons/document-open-folder.png"), "Load Scene File", this, SLOT(loadScene()));
    customToolbar->addAction(QIcon(":/icons/document-save-5.ico"), "Save Scene File", this, SLOT(saveScene()));
 
    return customToolbar;
}