LaserScannerPluginWidgetController.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    RobotAPI::gui-plugins::LaserScannerPluginWidgetController
 * \author     Fabian Paus ( fabian dot paus at kit dot edu )
 * \date       2017
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LaserScannerPluginWidgetController.h"
 
#include <RobotAPI/gui-plugins/LaserScannerPlugin/ui_LaserScannerPluginWidget.h>
 
#include <string>
 
#include <QGraphicsLineItem>
#include <QGraphicsView>
 
using namespace armarx;
 
LaserScannerPluginWidgetController::LaserScannerPluginWidgetController()
{
    widget = std::make_unique<Ui::LaserScannerPluginWidget>();
    widget->setupUi(getWidget());
}
 
LaserScannerPluginWidgetController::~LaserScannerPluginWidgetController()
{
}
 
void
LaserScannerPluginWidgetController::loadSettings(QSettings* settings)
{
}
 
void
LaserScannerPluginWidgetController::saveSettings(QSettings* settings)
{
}
 
void
LaserScannerPluginWidgetController::onInitComponent()
{
    usingProxy(laserScannerUnitName);
 
    connect(this,
            SIGNAL(newSensorValuesReported()),
            this,
            SLOT(onNewSensorValuesReported()),
            Qt::QueuedConnection);
    connect(
        widget->deviceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(onDeviceSelected(int)));
}
 
void
LaserScannerPluginWidgetController::onConnectComponent()
{
    laserScannerUnit = getProxy<LaserScannerUnitInterfacePrx>(laserScannerUnitName);
    std::string topicName = laserScannerUnit->getReportTopicName();
    usingTopic(topicName);
 
    laserScanners = laserScannerUnit->getConnectedDevices();
}
 
QPointer<QDialog>
LaserScannerPluginWidgetController::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new SimpleConfigDialog(parent);
        dialog->addProxyFinder<LaserScannerUnitInterfacePrx>({"LaserScannerUnit", "", "*"});
    }
    return qobject_cast<SimpleConfigDialog*>(dialog);
}
 
void
LaserScannerPluginWidgetController::configured()
{
    if (dialog)
    {
        laserScannerUnitName = dialog->getProxyName("LaserScannerUnit");
    }
}
 
void
LaserScannerPluginWidgetController::reportSensorValues(const std::string& device,
                                                       const std::string& name,
                                                       const LaserScan& newScan,
                                                       const TimestampBasePtr& timestamp,
                                                       const Ice::Current& c)
{
    {
        std::unique_lock lock(scanMutex);
 
        LaserScan& scan = scans[device];
        // TODO: Do some filtering? aggregation?
        scan = newScan;
    }
 
    emit newSensorValuesReported();
}
 
void
LaserScannerPluginWidgetController::onNewSensorValuesReported()
{
    QComboBox* deviceBox = widget->deviceComboBox;
 
    std::unique_lock lock(scanMutex);
    for (auto& pair : scans)
    {
        QString deviceName(QString::fromStdString(pair.first.c_str()));
        if (deviceBox->findText(deviceName) < 0)
        {
            deviceBox->addItem(deviceName);
        }
    }
 
    std::string deviceName(deviceBox->currentText().toUtf8().data());
    float stepSize = 0.25f;
    for (LaserScannerInfo const& scanner : laserScanners)
    {
        if (scanner.device == deviceName)
        {
            stepSize = scanner.stepSize;
        }
    }
 
    QGraphicsView* view = widget->graphicsView;
 
    scene.reset(new QGraphicsScene());
    widget->graphicsView->setScene(scene.get());
    int outerR = std::min(view->width() / 2, view->height() / 2);
    scene->addEllipse(-outerR, -outerR, 2 * outerR, 2 * outerR, QPen(QColor(255, 255, 255)));
    int r = outerR - 10;
    //scene.addEllipse(-r, -r, 2 * r, 2 * r, QPen(QColor(200, 200, 200)));
    QColor stepColor(QColor::fromRgb(100, 100, 255));
    QPen stepPen(stepColor);
    QBrush stepBrush(stepColor);
    auto line = [&](float angle, float d)
    {
        float di = d * r;
        QGraphicsEllipseItem* item =
            scene->addEllipse(-di, -di, 2 * di, 2 * di, stepPen, stepBrush);
        // Angles for Qt ellipse are in 16th of degree (who thought that would be a great idea?)
        item->setStartAngle(std::round(16.0f * angle * 180.0 / M_PI) + 90 * 16);
        item->setSpanAngle(std::round(stepSize * 16.0f * 180.0 / M_PI));
    };
 
    LaserScan& scan = scans[deviceName];
    float maxDistance = 1000.0f;
    for (LaserScanStep& step : scan)
    {
        if (step.distance > maxDistance)
        {
            maxDistance = step.distance;
        }
    }
    float ringDistance = 1000.0f;
    int numberOfRings = (std::size_t)std::ceil(maxDistance / ringDistance);
    std::deque<int>& history = numberOfRingsHistory[deviceName];
    history.push_back(numberOfRings);
    if (history.size() > 256)
    {
        history.pop_front();
    }
    int maxNumberOfRings = *std::max_element(history.begin(), history.end());
    float outerRadius = maxNumberOfRings * ringDistance;
 
    for (LaserScanStep& step : scan)
    {
        line(step.angle, step.distance / outerRadius);
    }
 
    for (int ringIndex = 1; ringIndex <= maxNumberOfRings; ++ringIndex)
    {
        float ri = 1.0f * ringIndex / maxNumberOfRings * r;
        scene->addEllipse(-ri, -ri, 2 * ri, 2 * ri, QPen(QColor(200, 200, 200)));
    }
 
    view->fitInView(scene->itemsBoundingRect(), Qt::KeepAspectRatio);
}
 
void
LaserScannerPluginWidgetController::onDeviceSelected(int index)
{
    if (index < 0 && index >= widget->deviceComboBox->count())
    {
        return;
    }
    std::string deviceName = widget->deviceComboBox->itemText(index).toStdString();
    for (LaserScannerInfo const& scanner : laserScanners)
    {
        if (scanner.device == deviceName)
        {
            widget->frameLabel->setText(QString::fromStdString(scanner.frame));
            widget->minAngleLabel->setText(QString::number(scanner.minAngle));
            widget->maxAngleLabel->setText(QString::number(scanner.minAngle));
            widget->stepSizeLabel->setText(QString::number(scanner.stepSize));
        }
    }
}