LaserScannerSelfLocalisationWidgetController.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    RobotComponents::gui-plugins::LaserScannerSelfLocalisationWidgetController
 * \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 "LaserScannerSelfLocalisationWidgetController.h"
 
#include <QSpinBox>
#include <QDoubleSpinBox>
#include <QCheckBox>
#include <QTimer>
 
#include <string>
#include <cfloat>
 
using namespace armarx;
 
LaserScannerSelfLocalisationWidgetController::LaserScannerSelfLocalisationWidgetController()
    : setPoseState(SetPoseState::None)
{
    widget.setupUi(getWidget());
    painterWidget = new QPainterWidget();
    painterWidget->setupUi(widget.frame);
    painterWidget->setPaintCallback([this](QPainter & painter)
    {
        onPaintCanvas(painter, painterWidget->size());
    });
 
    paintTimerId = startTimer(16);
}
 
 
LaserScannerSelfLocalisationWidgetController::~LaserScannerSelfLocalisationWidgetController()
{
 
}
 
 
void LaserScannerSelfLocalisationWidgetController::loadSettings(QSettings* settings)
{
 
}
 
void LaserScannerSelfLocalisationWidgetController::saveSettings(QSettings* settings)
{
 
}
 
 
void LaserScannerSelfLocalisationWidgetController::onInitComponent()
{
    usingProxy(localisationName);
}
 
 
void LaserScannerSelfLocalisationWidgetController::onConnectComponent()
{
    localisation = getProxy<LaserScannerSelfLocalisationInterfacePrx>(localisationName);
    localisationManager = getProxy<armarx::ArmarXManagerInterfacePrx>(localisationName + "Manager");
    localisationAdmin = localisationManager->getPropertiesAdmin();
    readProperties();
 
    map = localisation->getMap();
 
    connect(widget.smoothingFrameSize, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
    connect(widget.smoothingMergeDistance, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
    connect(widget.matchingMaxDistance, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
    connect(widget.matchingMinPoints, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
    connect(widget.matchingCorrectionFactor, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
    connect(widget.edgeMaxDistance, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
    connect(widget.edgeMaxDeltaAngle, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
    connect(widget.edgePointAddingThreshold, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
    connect(widget.edgeEpsilon, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
    connect(widget.edgeMinPoints, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
    connect(widget.useMapCorrection, SIGNAL(stateChanged(int)), this, SLOT(onSpinBoxChanged(int)));
    connect(widget.useOdometry, SIGNAL(stateChanged(int)), this, SLOT(onSpinBoxChanged(int)));
    connect(widget.reportPoints, SIGNAL(stateChanged(int)), this, SLOT(onSpinBoxChanged(int)));
    connect(widget.reportEdges, SIGNAL(stateChanged(int)), this, SLOT(onSpinBoxChanged(int)));
    connect(widget.setPoseButton, SIGNAL(clicked()), this, SLOT(onSetPoseClick()));
    connect(widget.sensorStdDev, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
    connect(widget.velSensorStdDev, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
 
    connect(painterWidget, SIGNAL(mousePressed(QPoint)), this, SLOT(onCanvasClick(QPoint)));
 
    connect(this, SIGNAL(newDataReported()), this, SLOT(onNewDataReported()), Qt::QueuedConnection);
 
    usingTopic(localisation->getReportTopicName());
}
 
 
QPointer<QDialog> armarx::LaserScannerSelfLocalisationWidgetController::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new SimpleConfigDialog(parent);
        dialog->addProxyFinder<LaserScannerSelfLocalisationInterfacePrx>({"LaserScannerSelfLocalisation", "", "*"});
    }
    return qobject_cast<SimpleConfigDialog*>(dialog);
}
 
void armarx::LaserScannerSelfLocalisationWidgetController::configured()
{
    if (dialog)
    {
        localisationName = dialog->getProxyName("LaserScannerSelfLocalisation");
    }
}
 
void LaserScannerSelfLocalisationWidgetController::onPaintCanvas(QPainter& painter, QSize size)
{
    float maxX = 0.0f;
    float minX = FLT_MAX;
    float maxY = 0.0f;
    float minY = FLT_MAX;
    for (LineSegment2D& s : map)
    {
        maxX = std::max(maxX, s.start.e0);
        maxY = std::max(maxY, s.start.e1);
        minX = std::min(minX, s.start.e0);
        minY = std::min(minY, s.start.e1);
        maxX = std::max(maxX, s.end.e0);
        maxY = std::max(maxY, s.end.e1);
        minX = std::min(minX, s.end.e0);
        minY = std::min(minY, s.end.e1);
    }
 
    float normX = 1.0f / (maxX - minX);
    float normY = 1.0f / (maxY - minY);
    float scaleFactorX = size.width() * normX;
    float scaleFactorY = size.height() * normY;
    float scaleFactor = 0.95f * std::min(scaleFactorX, scaleFactorY);
    float invScaleFactor = 1.0f / scaleFactor;
 
    auto toCanvas = [ = ](armarx::Vector2f p)
    {
        float xCoord = scaleFactor * (p.e0 - minX);
        float yCoord = scaleFactor * (p.e1 - minY);
        return QPoint((int)xCoord, (int)(size.height() - 1 - yCoord));
    };
 
    auto toCanvasE = [ = ](Eigen::Vector2f p)
    {
        float xCoord = scaleFactor * (p.x() - minX);
        float yCoord = scaleFactor * (p.y() - minY);
        return QPoint((int)xCoord, (int)(size.height() - 1 - yCoord));
    };
 
    auto fromCanvas = [ = ](QPoint p)
    {
        float globalX = (invScaleFactor * p.x()) + minX;
        float globalY = invScaleFactor * (size.height() - 1 - p.y()) + minY;
        return Eigen::Vector2f(globalX, globalY);
    };
 
    if (setPoseState == SetPoseState::Wait)
    {
        setPoseState = SetPoseState::Orientation;
    }
    else if (setPoseState == SetPoseState::Finished)
    {
        // Transform click points into global coordinates
        Eigen::Vector2f globalPos = fromCanvas(setPosition);
        Eigen::Vector2f globalOr = fromCanvas(setOrientation);
        Eigen::Vector2f fromPosToOr = globalOr - globalPos;
        float theta = std::atan2(fromPosToOr.y(), fromPosToOr.x()) - M_PI_2;
 
        try
        {
            ARMARX_INFO << "Set global pose to (" << globalPos.x() << ", " << globalPos.y() << ", " << theta << ")";
            localisation->setAbsolutePose(globalPos.x(), globalPos.y(), theta);
        }
        catch (std::exception const& ex)
        {
            ARMARX_WARNING << "Could not set absolute pose: " << ex.what();
        }
 
        setPoseState = SetPoseState::None;
    }
 
    QBrush whiteBrush(QColor(240, 240, 240));
    QPen blackPen(QColor(0, 0, 0));
    blackPen.setWidth(1);
    painter.setBrush(whiteBrush);
    painter.setPen(blackPen);
 
    lines.clear();
    lines.reserve(map.size());
    for (LineSegment2D const& s : map)
    {
        lines.emplace_back(toCanvas(s.start), toCanvas(s.end));
    }
    painter.drawLines(lines.data(), lines.size());
 
    if (setPoseState == SetPoseState::Orientation)
    {
        QPoint setOrientation = painterWidget->mapFromGlobal(QCursor::pos());
        Eigen::Vector2f globalPos = fromCanvas(setPosition);
        Eigen::Vector2f globalOr = fromCanvas(setOrientation);
 
        painter.setBrush(QColor(255, 0, 0));
        painter.drawEllipse(toCanvasE(globalPos), 10, 10);
        painter.drawLine(toCanvasE(globalPos), toCanvasE(globalOr));
    }
 
    // Draw the robot pose
    Eigen::Vector2f robotPos(poseX, poseY);
    float robotTheta = poseTheta;
 
    armarx::Vector2f estRobotPos {robotPos.x(), robotPos.y()};
    Eigen::Vector2f estRobotEnd = robotPos + 500.0f * Eigen::Vector2f(Eigen::Rotation2Df(robotTheta) * Eigen::Vector2f::UnitY());
    armarx::Vector2f estRobotEnda {estRobotEnd.x(), estRobotEnd.y()};
    painter.setBrush(QColor(0, 255, 255));
    painter.drawEllipse(toCanvas(estRobotPos), 10, 10);
    painter.drawLine(toCanvas(estRobotPos), toCanvas(estRobotEnda));
    painter.setBrush(QColor(255, 255, 0, 25));
    float posUncertaintyAverage = (poseUncertaintyX + poseUncertaintyY) * 0.5;
    painter.drawEllipse(toCanvas(estRobotPos), (int)(posUncertaintyAverage * scaleFactor), (int)(posUncertaintyAverage * scaleFactor));
 
    qpoints.clear();
    qedges.clear();
    {
 
        if (widget.reportPoints->checkState() == Qt::Checked)
        {
            ScopedLock lock(pointsMutex);
            qpoints.reserve(points.size());
            for (armarx::Vector2f const& point : points)
            {
                qpoints.push_back(toCanvas(point));
            }
        }
 
        if (widget.reportEdges->checkState() == Qt::Checked)
        {
            ScopedLock lock(pointsMutex);
            qedges.reserve(edges.size());
            for (armarx::LineSegment2D const& s : edges)
            {
                QPoint start = toCanvas(s.start);
                QPoint end = toCanvas(s.end);
                qedges.push_back(QLine(start, end));
            }
        }
    }
 
    // Draw the laser scanner points
    float lineWidth = 1.0f;
    QPen pointPen(QColor(0, 0, 255), lineWidth);
    painter.setPen(pointPen);
 
    painter.drawPoints(qpoints.data(), qpoints.size());
 
    // Draw extracted edges
    QPen edgePen(QColor(0, 255, 0), lineWidth);
    painter.setPen(edgePen);
    painter.drawLines(qedges.data(), qedges.size());
}
 
 
void armarx::LaserScannerSelfLocalisationWidgetController::reportCorrectedPose(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&)
{
    poseX = x;
    poseY = y;
    poseTheta = theta;
 
    emit newDataReported();
}
 
void LaserScannerSelfLocalisationWidgetController::reportPoseUncertainty(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&)
{
    poseUncertaintyX = x;
    poseUncertaintyY = y;
    poseUncertaintyTheta = theta;
 
    emit newDataReported();
 
}
 
 
void armarx::LaserScannerSelfLocalisationWidgetController::reportLaserScanPoints(const Vector2fSeq& globalPoints, const Ice::Current&)
{
    {
        ScopedLock guard(pointsMutex);
        points = globalPoints;
    }
    emit newDataReported();
}
 
void armarx::LaserScannerSelfLocalisationWidgetController::reportExtractedEdges(const LineSegment2DSeq& globalEdges, const Ice::Current&)
{
    {
        ScopedLock guard(pointsMutex);
        edges = globalEdges;
    }
    emit newDataReported();
}
 
void LaserScannerSelfLocalisationWidgetController::onSpinBoxChanged(int value)
{
    updateProperties();
}
 
void LaserScannerSelfLocalisationWidgetController::onSpinBoxChanged(double value)
{
    updateProperties();
}
 
void LaserScannerSelfLocalisationWidgetController::onNewDataReported()
{
    QString poseString = QString("Pose: ") + QString::number(poseX) + ", " + QString::number(poseY)
                         + ", " + QString::number(poseTheta) + "\nUncertainty: " +  QString::number(poseUncertaintyX, 'g', 4) + ", " + QString::number(poseUncertaintyY, 'g', 4)
                         + ", " + QString::number(poseUncertaintyTheta, 'g', 4);
    widget.poseLabel->setText(poseString);
}
 
void LaserScannerSelfLocalisationWidgetController::onSetPoseClick()
{
    setPoseState = SetPoseState::Position;
    ARMARX_INFO << "Starting to set new position";
}
 
void LaserScannerSelfLocalisationWidgetController::onCanvasClick(QPoint p)
{
    if (setPoseState == SetPoseState::Position)
    {
        setPosition = setOrientation = p;
        setPoseState = SetPoseState::Wait;
        ARMARX_INFO << "Poistion set, now setting orientation";
    }
    else if (setPoseState == SetPoseState::Orientation)
    {
        setOrientation = p;
        setPoseState = SetPoseState::Finished;
        ARMARX_INFO << "Orientation set, waiting for finish";
    }
}
 
static std::string floatToString(float v)
{
    std::ostringstream s;
    s.imbue(std::locale::classic());
    s << v;
    return s.str();
}
 
static float stringToFloat(std::string const& string)
{
    std::istringstream s(string);
    s.imbue(std::locale::classic());
    float v = 0.0f;
    s >> v;
    return v;
}
 
const std::string PROPERTIES_PREFIX = "ArmarX.LaserScannerSelfLocalisation.";
 
void LaserScannerSelfLocalisationWidgetController::updateProperties()
{
    if (!localisationAdmin)
    {
        ARMARX_WARNING << "Could not set properties because no admin proxy is set";
        return;
    }
 
    ARMARX_INFO << "start updating LaserScannerSelfLocalisationWidget properties";
    int smoothingFrameSize = widget.smoothingFrameSize->value();
    int smoothingMergeDistance = widget.smoothingMergeDistance->value();
    float matchingMaxDistance = widget.matchingMaxDistance->value();
    float matchingMinPoints = widget.matchingMinPoints->value();
    float matchingCorrectionFactor = widget.matchingCorrectionFactor->value();
    float edgeMaxDistance = widget.edgeMaxDistance->value();
    float edgeMaxDeltaAngle = widget.edgeMaxDeltaAngle->value();
    float edgePointAddingThreshold = widget.edgePointAddingThreshold->value();
    int edgeEpsilon = widget.edgeEpsilon->value();
    int edgeMinPoints = widget.edgeMinPoints->value();
    bool useMapCorrection = widget.useMapCorrection->checkState() == Qt::Checked;
    bool useOdometry = widget.useOdometry->checkState() == Qt::Checked;
    bool reportPoints = widget.reportPoints->checkState() == Qt::Checked;
    bool reportEdges = widget.reportEdges->checkState() == Qt::Checked;
    float sensorStdDev = widget.sensorStdDev->value();
    float velSensorStdDev = widget.velSensorStdDev->value();
 
    Ice::PropertyDict properties;
    properties[PROPERTIES_PREFIX + "SmoothFrameSize"] = to_string(smoothingFrameSize);
    properties[PROPERTIES_PREFIX + "SmoothMergeDistance"] = to_string(smoothingMergeDistance);
    properties[PROPERTIES_PREFIX + "MatchingMaxDistance"] = floatToString(matchingMaxDistance);
    properties[PROPERTIES_PREFIX + "MatchingMinPoints"] = floatToString(matchingMinPoints);
    properties[PROPERTIES_PREFIX + "MatchingCorrectionFactor"] = floatToString(matchingCorrectionFactor);
    properties[PROPERTIES_PREFIX + "EdgeMaxDistance"] = floatToString(edgeMaxDistance);
    properties[PROPERTIES_PREFIX + "EdgeMaxDeltaAngle"] = floatToString(edgeMaxDeltaAngle * M_PI / 180.0);
    properties[PROPERTIES_PREFIX + "EdgePointAddingThreshold"] = floatToString(edgePointAddingThreshold);
    properties[PROPERTIES_PREFIX + "EdgeEpsilon"] = to_string(edgeEpsilon);
    properties[PROPERTIES_PREFIX + "EdgeMinPoints"] = to_string(edgeMinPoints);
    properties[PROPERTIES_PREFIX + "UseMapCorrection"] = useMapCorrection ? "1" : "0";
    properties[PROPERTIES_PREFIX + "UseOdometry"] = useOdometry ? "1" : "0";
    properties[PROPERTIES_PREFIX + "ReportPoints"] = reportPoints ? "1" : "0";
    properties[PROPERTIES_PREFIX + "ReportEdges"] = reportEdges ? "1" : "0";
    properties[PROPERTIES_PREFIX + "SensorStdDev"] = floatToString(sensorStdDev);
    properties[PROPERTIES_PREFIX + "VelSensorStdDev"] = floatToString(velSensorStdDev);
 
    ARMARX_INFO << "setting properties on local admin";
    localisationAdmin->setProperties(properties);
    ARMARX_INFO << "done updating LaserScannerSelfLocalisationWidget properties";
}
 
void LaserScannerSelfLocalisationWidgetController::readProperties()
{
    ARMARX_IMPORTANT << "Trying to read properties";
    ObjectPropertyInfos properties = localisationManager->getObjectPropertyInfos(localisationName);
 
    int smoothingFrameSize = std::stoi(properties[PROPERTIES_PREFIX + "SmoothFrameSize"].value);
    int smoothingMergeDistance = std::stoi(properties[PROPERTIES_PREFIX + "SmoothMergeDistance"].value);
    float matchingMaxDistance = stringToFloat(properties[PROPERTIES_PREFIX + "MatchingMaxDistance"].value);
    float matchingMinPoints = stringToFloat(properties[PROPERTIES_PREFIX + "MatchingMinPoints"].value);
    float matchingCorrectionFactor = stringToFloat(properties[PROPERTIES_PREFIX + "MatchingCorrectionFactor"].value);
    float edgeMaxDistance = stringToFloat(properties[PROPERTIES_PREFIX + "EdgeMaxDistance"].value);
    float edgeMaxDeltaAngle = 180.0 / M_PI * stringToFloat(properties[PROPERTIES_PREFIX + "EdgeMaxDeltaAngle"].value);
    float edgePointAddingThreshold = stringToFloat(properties[PROPERTIES_PREFIX + "EdgePointAddingThreshold"].value);
    int edgeEpsilon = std::stoi(properties[PROPERTIES_PREFIX + "EdgeEpsilon"].value);
    int edgeMinPoints = std::stoi(properties[PROPERTIES_PREFIX + "EdgeMinPoints"].value);
    bool useMapCorrection = properties[PROPERTIES_PREFIX + "UseMapCorrection"].value == "true";
    bool useOdometry = properties[PROPERTIES_PREFIX + "UseOdometry"].value == "true";
    bool reportPoints = properties[PROPERTIES_PREFIX + "ReportPoints"].value == "true";
    bool reportEdges = properties[PROPERTIES_PREFIX + "ReportEdges"].value == "true";
    float sensorStdDev = stringToFloat(properties[PROPERTIES_PREFIX + "SensorStdDev"].value);
    float velSensorStdDev = stringToFloat(properties[PROPERTIES_PREFIX + "VelSensorStdDev"].value);
 
    widget.smoothingFrameSize->setValue(smoothingFrameSize);
    widget.smoothingMergeDistance->setValue(smoothingMergeDistance);
    widget.matchingMaxDistance->setValue(matchingMaxDistance);
    widget.matchingMinPoints->setValue(matchingMinPoints);
    widget.matchingCorrectionFactor->setValue(matchingCorrectionFactor);
    widget.edgeMaxDistance->setValue(edgeMaxDistance);
    widget.edgeMaxDeltaAngle->setValue(edgeMaxDeltaAngle);
    widget.edgePointAddingThreshold->setValue(edgePointAddingThreshold);
    widget.edgeEpsilon->setValue(edgeEpsilon);
    widget.edgeMinPoints->setValue(edgeMinPoints);
    widget.useMapCorrection->setChecked(useMapCorrection);
    widget.useOdometry->setChecked(useOdometry);
    widget.reportPoints->setChecked(reportPoints);
    widget.reportEdges->setChecked(reportEdges);
    widget.sensorStdDev->setValue(sensorStdDev);
    widget.velSensorStdDev->setValue(velSensorStdDev);
}
 
void LaserScannerSelfLocalisationWidgetController::timerEvent(QTimerEvent*)
{
    painterWidget->update();
}