SnakeDataManager.cc

#include "SnakeDataManager.h"

#include <QGeoCoordinate>
#include <QMutexLocker>

#include "QGCApplication.h"
#include "SettingsManager.h"
#include "QGCToolbox.h"
#include "WimaSettings.h"
#include "SettingsFact.h"

#include <memory>
#include <shared_mutex>
#include <mutex>

#include "snake.h"
#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"

#include "comm/ros_bridge/include/ros_bridge.h"

using QVariantList = QList<QVariant>;
using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;

using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;

class SnakeImpl : public QObject{
    Q_OBJECT
public:
    SnakeImpl() :
        lineDistance(1*si::meter)
      , minTransectLength(1*si::meter)
      , calcInProgress(false)
      , topicServiceSetupDone(false)
    {
        // ROS Bridge.
        WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
        auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
        auto setConnectionString = [connectionStringFact, this]{
            auto connectionString = connectionStringFact->rawValue().toString();
            if ( ros_bridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){
                this->pRosBridge.reset(new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
            } else {
                qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
                this->pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090"));
            }
        };
        setConnectionString();
        connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString);
    }

    // Private data.
    ROSBridgePtr                pRosBridge;
    bool                        topicServiceSetupDone;
    mutable std::shared_timed_mutex   mutex;

    // Input
    snake::Scenario scenario;
    Length lineDistance;
    Length minTransectLength;
    QList<QGeoCoordinate> mArea;
    QList<QGeoCoordinate> sArea;
    QList<QGeoCoordinate> corridor;


    // Output
    std::atomic_bool            calcInProgress;
    QNemoProgress               progress;
    QNemoHeartbeat              heartbeat;

    QVector<QGeoCoordinate>     waypoints;
    QVector<QGeoCoordinate>     arrivalPath;
    QVector<QGeoCoordinate>     returnPath;
    QGeoCoordinate              ENUorigin;
    QVector<QPointF>            waypointsENU;
    QVector<QPointF>            arrivalPathENU;
    QVector<QPointF>            returnPathENU;
    SnakeTiles                  tiles;
    QVariantList                tileCenterPoints;
    SnakeTilesLocal             tilesENU;
    QVector<QPointF>            tileCenterPointsENU;
    QString                     errorMessage;
};

template<class AtomicType>
class ToggleRAII{
public:
    ToggleRAII(AtomicType &t)
        : _t(t)
    {}

    ~ToggleRAII()
    {
        if ( _t.load() ){
            _t.store(false);
        } else {
            _t.store(true);
        }
    }
private:
    AtomicType &_t;
};



SnakeDataManager::SnakeDataManager(QObject *parent)
    : QThread(parent)
    , _pImpl(std::make_unique<SnakeImpl>())

{
}

SnakeDataManager::~SnakeDataManager()
{

}

void SnakeDataManager::setMeasurementArea(const QList<QGeoCoordinate> &measurementArea)
{
    UniqueLock lk(this->_pImpl->m);
    this->_pImpl->mArea = measurementArea;
}

void SnakeDataManager::setServiceArea(const QList<QGeoCoordinate> &serviceArea)
{
    UniqueLock lk(this->_pImpl->m);
    this->_pImpl->sArea = serviceArea;
}

void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
{
    UniqueLock lk(this->_pImpl->m);
    this->_pImpl->corridor = corridor;
}

QNemoProgress SnakeDataManager::progress()
{
    SharedLock lk(this->_pImpl->m);
    return this->_pImpl->progress;
}

QNemoHeartbeat SnakeDataManager::heartbeat()
{
    SharedLock lk(this->_pImpl->m);
    return this->_pImpl->heartbeat;
}

bool SnakeDataManager::calcInProgress()
{
    return this->_pImpl->calcInProgress.load();
}

Length SnakeDataManager::lineDistance() const
{
    SharedLock lk(this->_pImpl->m);
    return this->_pImpl->lineDistance;
}

void SnakeDataManager::setLineDistance(Length lineDistance)
{
    UniqueLock lk(this->_pImpl->m);
    this->_pImpl->lineDistance = lineDistance;
}

Area SnakeDataManager::minTileArea() const
{
    SharedLock lk(this->_pImpl->m);
    return this->_pImpl->scenario.minTileArea();
}

void SnakeDataManager::setMinTileArea(Area minTileArea)
{
    UniqueLock lk(this->_pImpl->m);
    this->_pImpl->scenario.setMinTileArea(minTileArea);
}

Length SnakeDataManager::tileHeight() const
{
    SharedLock lk(this->_pImpl->m);
    return this->_pImpl->scenario.tileHeight();
}

void SnakeDataManager::setTileHeight(Length tileHeight)
{
    UniqueLock lk(this->_pImpl->m);
    this->_pImpl->scenario.setTileHeight(tileHeight);
}

Length SnakeDataManager::tileWidth() const
{
    SharedLock lk(this->_pImpl->m);
    return this->_pImpl->scenario.tileWidth();
}

void SnakeDataManager::setTileWidth(Length tileWidth)
{
    UniqueLock lk(this->_pImpl->m);
    this->_pImpl->scenario.setTileWidth(tileWidth);
}

bool SnakeDataManager::precondition() const
{
    return true;
}

void SnakeDataManager::run()
{
    this->_pImpl->calcInProgress.store(true);
    ToggleRAII<std::atomic_bool> tr(this->_pImpl->calcInProgress);

    UniqueLock lk(&_pImpl->m);
    _pImpl->clear();

    if ( !precondition() )
        return;

    if ( this->_pImpl->mArea.size() < 3) {
        _pImpl->errorMessage = "Measurement area invalid: size < 3.";
        return;
    }
    if ( this->_pImpl->sArea.size() < 3) {
        _pImpl->errorMessage = "Service area invalid: size < 3.";
        return;
    }


         || _sArea.size() <= 0
         || _corridor.size() <= 0 ){

    auto origin = _mArea.front();
    // Measurement area update necessary.
    if ( _mArea.size() != _pScenario->measurementArea().outer().size() ){
        {
            QMutexLocker lk(&_pImpl->m);
            _pImpl->ENUorigin = origin;
        }
        for (auto geoVertex : _mArea){
            snake::BoostPoint p;
            snake::toENU(origin, geoVertex, p);
            _pScenario->measurementArea().outer().push_back(p);
        }
    }

    // Service area update necessary.
    if ( _sArea.size() != _pScenario->serviceArea().outer().size() ){
        for (auto geoVertex : _sArea){
            snake::BoostPoint p;
            snake::toENU(origin, geoVertex, p);
            _pScenario->serviceArea().outer().push_back(p);
        }
    }

    // Service area update necessary.
    if ( _corridor.size() != _pScenario->corridor().outer().size() ){
        for (auto geoVertex : _corridor){
            snake::BoostPoint p;
            snake::toENU(origin, geoVertex, p);
            _pScenario->corridor().outer().push_back(p);
        }
    }

    if ( !_pScenario->update() ){
        {
            QMutexLocker lk(&_pImpl->m);
            for (auto c : _pScenario->errorString){
                _pImpl->errorMessage.push_back(QChar(c));
            }
        }
        return;
    }

    // Asynchronously update flightplan.
    auto future = std::async(std::bind(&snake::Flightplan::update, _pFlightplan));

    // Continue with storing scenario data in the mean time.
    {
        QMutexLocker lk(&_pImpl->m);
        // Get tiles.
        const auto &tiles = _pScenario->tiles();
        const auto &centerPoints = _pScenario->tileCenterPoints();
        for ( unsigned int i=0; i < tiles.size(); ++i ) {
            const auto &tile = tiles[i];
            SnakeTile geoTile;
            SnakeTileLocal enuTile;
            for ( size_t i = tile.outer().size(); i < tile.outer().size()-1; ++i) {
                auto &p = tile.outer()[i];
                QPointF enuVertex(p.get<0>(), p.get<1>());
                QGeoCoordinate geoVertex;
                snake::fromENU(origin, p, geoVertex);
                enuTile.polygon().points().push_back(enuVertex);
                geoTile.push_back(geoVertex);
            }
            const auto &boostPoint = centerPoints[i];
            QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>());
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, boostPoint, geoVertex);
            geoTile.setCenter(geoVertex);
            _pImpl->tiles.polygons().push_back(geoTile);
            _pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
            _pImpl->tilesENU.polygons().push_back(enuTile);
            _pImpl->tileCenterPointsENU.push_back(enuVertex);
         }
    }

    future.wait();
    // Trying to generate flight plan.
    if ( !future.get() ){
        // error
        QMutexLocker lk(&_pImpl->m);
        for (auto c : _pFlightplan->errorString){
            _pImpl->errorMessage.push_back(QChar(c));
        }
    } else {
        //success!!!
        QMutexLocker lk(&_pImpl->m);
        // Store waypoints.
        for (auto boostVertex : _pFlightplan->waypoints()){
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, boostVertex, geoVertex);
            _pImpl->waypointsENU.push_back(enuVertex);
            _pImpl->waypoints.push_back(geoVertex);
        }
        // Store arrival path.
        for (auto boostVertex : _pFlightplan->arrivalPath()){
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, boostVertex, geoVertex);
            _pImpl->arrivalPathENU.push_back(enuVertex);
            _pImpl->arrivalPath.push_back(geoVertex);
        }
        // Store return path.
        for (auto boostVertex : _pFlightplan->returnPath()){
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, boostVertex, geoVertex);
            _pImpl->returnPathENU.push_back(enuVertex);
            _pImpl->returnPath.push_back(geoVertex);
        }
    }
}