WidgetController.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    VisionX::gui-plugins::WidgetController
 * \author     Philipp Schmidt ( ufedv at student dot kit dot edu )
 * \date       2015
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "WidgetController.h"
 
#include <algorithm>
#include <type_traits>
 
// Ice
#include <Ice/Ice.h>
#include <IceUtil/Time.h>
 
// VisionX
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/CoinViewer.h>
 
#include <VisionX/components/pointcloud_core/PointCloudProvider.h>
#include <VisionX/interface/core/PointCloudProviderInterface.h>
 
// PCL tools
#include <QMainWindow>
 
#include <VisionX/components/pointcloud_core/PCLUtilities.h>
#include <VisionX/components/pointcloud_core/PointCloudConversions.h>
#include <VisionX/libraries/PointCloudTools/segments.h>
 
#include "CoinPointCloud.h"
 
namespace visionx
{
    WidgetController::WidgetController() : recenterCamera(false)
    {
        qRegisterMetaType<IceGrid::ObjectInfoSeq>("IceGrid::ObjectInfoSeq");
    }
 
    WidgetController::~WidgetController()
    {
    }
 
    void
    WidgetController::loadSettings(QSettings* settings)
    {
        (void)settings; // Unused.
    }
 
    void
    WidgetController::saveSettings(QSettings* settings)
    {
        (void)settings; // Unused.
    }
 
    void
    WidgetController::onInitPointCloudProcessor()
    {
        automaticConversion = true;
    }
 
    void
    WidgetController::onConnectPointCloudProcessor()
    {
        // Refresh list once.
        this->refreshProviderList();
 
        // Ice is ready, allow refreshing of list.
        QMetaObject::invokeMethod(widget.refreshButton, "setEnabled", Q_ARG(bool, true));
    }
 
    void
    WidgetController::onDisconnectPointCloudProcessor()
    {
        QMetaObject::invokeMethod(widget.refreshButton, "setEnabled", Q_ARG(bool, false));
        QMetaObject::invokeMethod(this, "disconnectQt");
    }
 
    void
    WidgetController::disconnectQt()
    {
        auto icemanager = this->getIceManager();
        if (!icemanager)
        {
            return;
        }
        // Let's check which providers just went offline
        for (int row = 0; row < widget.providerList->count(); row++)
        {
            QListWidgetItem* item = widget.providerList->item(row);
            const std::string subscriber = item->text().toStdString();
 
            ARMARX_LOG << subscriber << " has state "
                       << icemanager->getIceGridSession()->getComponentState(subscriber)
                       << std::endl;
 
            // Check component status
            if (icemanager->getIceGridSession()->getComponentState(subscriber) !=
                armarx::IceGridAdmin::eActivated)
            {
                // Unregister cloud
                manager->unregisterPointCloud(subscriber);
 
                // Remove item from provider list (and free memory)
                delete this->widget.providerList->takeItem(this->widget.providerList->row(item));
 
                // Indices got reduced by one
                row--;
 
                // Release point cloud provider
                this->releasePointCloudProvider(subscriber);
            }
        }
    }
 
    void
    WidgetController::onExitPointCloudProcessor()
    {
    }
 
    void
    WidgetController::postDocking()
    {
        // Setup user interface
        widget.setupUi(getWidget());
        //qRegisterMetaType<visionx::CoinPointCloud*>("visionx::CoinPointCloud*");
 
        // Connect update calls with process calls
        connect(this,
                SIGNAL(newPointCloudReceived(QString, CoinPointCloud*)),
                this,
                SLOT(processPointCloud(QString, CoinPointCloud*)));
 
        // Add manager
        manager = new Manager();
        widget.pointCloudDisplay->getDisplay()->getRootNode()->addChild(manager);
 
        // Remove line counter in table
        widget.tableWidget->verticalHeader()->setVisible(false);
 
        // Disable editing of table
        widget.tableWidget->setEditTriggers(QAbstractItemView::NoEditTriggers);
 
        // Button for manual refreshing
        connect(widget.refreshButton, SIGNAL(clicked()), this, SLOT(refreshProviderList()));
 
        // Do not allow refreshing until component connected and ice is ready
        // widget.refreshButton->setEnabled(false);
 
        // Allow to chose background color
        connect(widget.backgroundButton, SIGNAL(clicked()), this, SLOT(selectColor()));
 
        // Items get selected and deselected in list, show infos for pointcloud
        connect(widget.providerList,
                SIGNAL(itemPressed(QListWidgetItem*)),
                this,
                SLOT(showPointCloudInfo(QListWidgetItem*)));
 
        // Items get checked and unchecked in list, add to viewport
        connect(widget.providerList,
                SIGNAL(itemChanged(QListWidgetItem*)),
                this,
                SLOT(pointCloudChecked(QListWidgetItem*)));
 
        // Connect save point cloud button
        connect(widget.saveButton, SIGNAL(clicked()), this, SLOT(savePointCloud()));
 
        // Connect spin box to change point size
        connect(widget.doubleSpinBoxPointSize,
                SIGNAL(valueChanged(double)),
                this,
                SLOT(setPointSize(double)));
    }
 
    void
    WidgetController::process()
    {
        std::lock_guard<std::mutex> lock(mutex);
 
        // Fetch all point cloud providers for updates and save in buffer
        for (const std::string& providerName : manager->getRegisteredPointClouds())
        {
            if (!isPointCloudProviderKnown(providerName) || !pointCloudHasNewData(providerName))
            {
                // Skip this one (disconnected in mean time or no new data available).
                continue;
            }
 
            // Save in this buffer
            pcl::PointCloud<PointT>::Ptr providerBuffer(new pcl::PointCloud<PointT>());
 
            // Check point cloud type.
            const visionx::MetaPointCloudFormatPtr info = getPointCloudFormat(providerName);
            const visionx::PointContentType pointContentType = info->type;
 
            bool colorFromLabel =
                tools::isLabeled(pointContentType) && this->widget.colorMode_label->isChecked();
 
            switch (pointContentType)
            {
                case visionx::PointContentType::ePoints:
                    getPointCloudAs(providerName, *providerBuffer);
                    break;
                case visionx::PointContentType::eColoredPoints:
                    getPointCloudAs(providerName, *providerBuffer);
                    break;
                case visionx::PointContentType::eColoredLabeledPoints:
                    getPointCloudAs(providerName, *providerBuffer);
                    break;
                case visionx::PointContentType::eLabeledPoints:
                    getPointCloudAs(providerName, *providerBuffer);
                    break;
                default:
                    ARMARX_WARNING << deactivateSpam() << "Unexpected point cloud content type: "
                                   << tools::toString(pointContentType);
                    continue;
            }
 
            CoinPointCloud* cloud =
                new CoinPointCloud(providerBuffer, pointContentType, colorFromLabel, pointSize);
            emit newPointCloudReceived(QString::fromStdString(providerName), cloud);
        }
    }
 
    template <class PointCloudT>
    void
    WidgetController::getPointCloudAs(const std::string& providerName,
                                      PointCloudT& targetPointCloud)
    {
        using InputPointT = typename PointCloudT::PointType;
 
        typename pcl::PointCloud<InputPointT>::Ptr inputCloudPtr(
            new pcl::PointCloud<InputPointT>());
        if (getPointClouds(providerName, inputCloudPtr))
        {
            pcl::copyPointCloud(*inputCloudPtr, targetPointCloud);
        }
    }
 
    void
    WidgetController::processPointCloud(QString providerName, CoinPointCloud* cloud)
    {
        manager->updatePointCloud(providerName.toStdString(), *cloud);
        if (recenterCamera)
        {
            widget.pointCloudDisplay->getDisplay()->cameraViewAll();
            recenterCamera = false;
        }
 
        auto* item = widget.providerList->currentItem();
        if (item && providerName == item->text())
        {
            showPointCloudInfo(widget.providerList->currentItem());
        }
    }
 
    void
    WidgetController::updateProviderList(IceGrid::ObjectInfoSeq objects)
    {
        ARMARX_INFO << "Updating cloudlist.";
 
        // Signal user we are fetching providers
        widget.loadingStatusLabel->setText(QString::fromStdString("Loading..."));
 
        // Count how many providers are online
        int counter = 0;
 
        // Iterate over all objects
        for (const auto& object : objects)
        {
            const std::string name =
                this->getIceManager()->getCommunicator()->proxyToString(object.proxy);
            const std::string shortName = name.substr(0, name.find(' '));
 
            // Check if we didn't connect to this provider yet.
            // Invariant: Every item in list widget is a provider we already connected in the past.
            if (widget.providerList->findItems(QString::fromStdString(shortName), Qt::MatchExactly)
                    .count() == 0)
            {
                // Add new item and make it checkable.
                QListWidgetItem* item =
                    new QListWidgetItem(QString::fromStdString(shortName), widget.providerList);
                item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
                item->setCheckState(Qt::Unchecked);
 
                // Subscribe to topic, provider is fetchable after that.
                this->usingPointCloudProvider(shortName);
                this->getPointCloudProvider(shortName);
            }
 
            counter++;
        }
 
        // Signal user we are done and show how many providers are online
        widget.loadingStatusLabel->setText(QString::fromStdString(
            "Loading complete: " + std::to_string(counter) + " provider found"));
    }
 
    void
    WidgetController::refreshProviderList()
    {
        ARMARX_INFO << "Get cloud providers from ice.";
        // Get ice grid admin
        armarx::IceGridAdminPtr admin = this->getIceManager()->getIceGridSession();
        IceGrid::ObjectInfoSeq objects =
            admin->getRegisteredObjects<PointCloudProviderInterfacePrx>(
                widget.lineEditPointCloudFilter->text().toStdString());
        QMetaObject::invokeMethod(
            this, "updateProviderList", Q_ARG(IceGrid::ObjectInfoSeq, objects));
    }
 
    void
    WidgetController::showPointCloudInfo(QListWidgetItem* item)
    {
        const std::string providerName = item->text().toStdString();
        visionx::MetaPointCloudFormatPtr info = this->getPointCloudFormat(providerName);
 
        widget.tableWidget->setRowCount(8);
        widget.tableWidget->setColumnCount(2);
 
        // Name
        widget.tableWidget->setItem(0, 0, new QTableWidgetItem("Name"));
        widget.tableWidget->setItem(0, 1, new QTableWidgetItem(item->text()));
 
        // Type
        widget.tableWidget->setItem(1, 0, new QTableWidgetItem("Type"));
        widget.tableWidget->setItem(
            1,
            1,
            new QTableWidgetItem(QString::fromStdString(visionx::tools::toString(info->type))));
 
        // Width
        widget.tableWidget->setItem(2, 0, new QTableWidgetItem("Width"));
        widget.tableWidget->setItem(
            2, 1, new QTableWidgetItem(QString::fromStdString(std::to_string(info->width))));
 
        // Height
        widget.tableWidget->setItem(3, 0, new QTableWidgetItem("Height"));
        widget.tableWidget->setItem(
            3, 1, new QTableWidgetItem(QString::fromStdString(std::to_string(info->height))));
 
        // Size
        widget.tableWidget->setItem(4, 0, new QTableWidgetItem("Size"));
        widget.tableWidget->setItem(
            4, 1, new QTableWidgetItem(QString::fromStdString(std::to_string(info->size))));
 
        // Capacity
        widget.tableWidget->setItem(5, 0, new QTableWidgetItem("Capacity"));
        widget.tableWidget->setItem(
            5, 1, new QTableWidgetItem(QString::fromStdString(std::to_string(info->capacity))));
 
        // Sequence
        widget.tableWidget->setItem(6, 0, new QTableWidgetItem("Sequence"));
        widget.tableWidget->setItem(
            6, 1, new QTableWidgetItem(QString::fromStdString(std::to_string(info->seq))));
 
        // Time provided
        widget.tableWidget->setItem(7, 0, new QTableWidgetItem("Time Provided"));
 
        const bool showRawTimestamp = false;
        const std::string timeProvided = showRawTimestamp
                                             ? std::to_string(info->timeProvided) // Raw time stamp
                                             : IceUtil::Time::microSeconds(info->timeProvided)
                                                   .toDateTime(); // Readable time stamp.
        widget.tableWidget->setItem(
            7, 1, new QTableWidgetItem(QString::fromStdString(timeProvided)));
    }
 
    void
    WidgetController::pointCloudChecked(QListWidgetItem* item)
    {
        std::lock_guard<std::mutex> lock(mutex);
 
        const std::string providerName = item->text().toStdString();
 
        if (item->checkState())
        {
            manager->registerPointCloud(providerName);
        }
        else
        {
            manager->unregisterPointCloud(providerName);
        }
        recenterCamera = true;
    }
 
    void
    WidgetController::selectColor()
    {
        QColor selectedColor = this->colorDialog.getColor();
 
        widget.pointCloudDisplay->getDisplay()->setBackgroundColor(
            SbColor(selectedColor.red() / 255.0f,
                    selectedColor.green() / 255.0f,
                    selectedColor.blue() / 255.0f));
    }
 
    void
    WidgetController::savePointCloud()
    {
        if (widget.providerList->selectedItems().size() == 1)
        {
            SaveDialog saveDialog(this->getMainWindow());
            const std::string providerID =
                widget.providerList->selectedItems()[0]->text().toStdString();
            const std::vector<std::string> clouds = manager->getRegisteredPointClouds();
            if (std::find(clouds.begin(), clouds.end(), providerID) != clouds.end())
            {
                saveDialog.setPointCloudToSave(manager->getCloudByName(providerID));
                int result = saveDialog.exec();
 
                if (result == QDialog::Accepted)
                {
                    const CoinPointCloud& coinCloud = *manager->getCloudByName(providerID);
                    const pcl::PointCloud<PointT>& originalCloud = *coinCloud.getPCLCloud();
                    const std::string fileName = saveDialog.getResultFileName();
 
                    switch (coinCloud.getOriginalType())
                    {
                        case visionx::PointContentType::ePoints:
                            storePointCloudAs(fileName, originalCloud);
                            break;
                        case visionx::PointContentType::eColoredPoints:
                            storePointCloudAs(fileName, originalCloud);
                            break;
                        case visionx::PointContentType::eColoredLabeledPoints:
                            storePointCloudAs(fileName, originalCloud);
                            break;
                        case visionx::PointContentType::eLabeledPoints:
                            storePointCloudAs(fileName, originalCloud);
                            break;
                        default:
                            ARMARX_WARNING << deactivateSpam()
                                           << "Unexpected point cloud content type: "
                                           << tools::toString(coinCloud.getOriginalType());
                            return;
                    }
                }
 
                saveDialog.releaseBuffer();
            }
        }
    }
 
    template <class PointCloudT>
    void
    WidgetController::storePointCloudAs(const std::string& filename, const PointCloudT& pointCloud)
    {
        using StoredPointT = typename PointCloudT::PointType;
 
        if (std::is_same<StoredPointT, PointT>())
        {
            pcl::io::savePCDFile(filename, pointCloud);
        }
        else
        {
            pcl::PointCloud<StoredPointT> stored;
            pcl::copyPointCloud(pointCloud, stored);
            pcl::io::savePCDFile(filename, stored);
        }
    }
 
    void
    WidgetController::onLockWidget()
    {
        widget.configColumn->setVisible(false);
    }
 
    void
    WidgetController::onUnlockWidget()
    {
        widget.configColumn->setVisible(true);
    }
} // namespace visionx
 
void
visionx::WidgetController::setPointSize(double value)
{
    ARMARX_VERBOSE << "Using new point size: " << value;
    pointSize = static_cast<float>(value);
}