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 "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"


#define EVENT_TIMER_INTERVAL 500 // ms
#define TIMEOUT 3000 // ms


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>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;

class SnakeImpl : public QObject{
    Q_OBJECT
public:
    SnakeImpl(SnakeDataManager* p);

    bool precondition() const;
    void resetWaypointData();
    bool doTopicServiceSetup();

    // Private data.
    ROSBridgePtr                        pRosBridge;
    bool                                rosBridgeEnabeled;
    bool                                topicServiceSetupDone;
    QTimer                              eventTimer;
    QTimer                              timeout;
    mutable std::shared_timed_mutex     mutex;

    // Scenario
    snake::Scenario       scenario;
    Length                lineDistance;
    Length                minTransectLength;
    QList<QGeoCoordinate> mArea;
    QList<QGeoCoordinate> sArea;
    QList<QGeoCoordinate> corridor;
    QGeoCoordinate        ENUOrigin;
    SnakeTiles            tiles;
    QVariantList          tileCenterPoints;
    SnakeTilesLocal       tilesENU;
    QVector<QPointF>      tileCenterPointsENU;

    // Waypoints
    QVector<QGeoCoordinate>     waypoints;
    QVector<QGeoCoordinate>     arrivalPath;
    QVector<QGeoCoordinate>     returnPath;
    QVector<QPointF>            waypointsENU;
    QVector<QPointF>            arrivalPathENU;
    QVector<QPointF>            returnPathENU;
    QString                     errorMessage;


    // Other
    std::atomic_bool            calcInProgress;
    QNemoProgress               qProgress;
    std::vector<int>            progress;
    QNemoHeartbeat              heartbeat;
    SnakeDataManager           *parent;

};

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>(this))

{
}

SnakeDataManager::~SnakeDataManager()
{

}

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

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

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

QNemoProgress SnakeDataManager::nemoProgress()
{
    SharedLock lk(this->pImpl->mutex);
    return this->pImpl->qProgress;
}

int SnakeDataManager::nemoStatus()
{
    SharedLock lk(this->pImpl->mutex);
    return this->pImpl->heartbeat.status();
}

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

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

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

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

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

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

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

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

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

void SnakeDataManager::enableRosBridge()
{
    this->pImpl->rosBridgeEnabeled = true;
}

void SnakeDataManager::disableRosBride()
{
    this->pImpl->rosBridgeEnabeled = false;
}

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

//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void SnakeImpl::resetWaypointData()
{
    this->waypoints.clear();
    this->arrivalPath.clear();
    this->returnPath.clear();
    this->ENUOrigin = QGeoCoordinate(0.0,0.0,0.0);
    this->waypointsENU.clear();
    this->arrivalPathENU.clear();
    this->returnPathENU.clear();
    this->tiles.polygons().clear();
    this->tileCenterPoints.clear();
    this->tilesENU.polygons().clear();
    this->tileCenterPointsENU.clear();
    this->errorMessage.clear();
}

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

    UniqueLock lk(this->pImpl->mutex);
    this->pImpl->resetWaypointData();

    if ( !this->pImpl->precondition() )
        return;

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


    this->pImpl->ENUOrigin = this->pImpl->mArea.front();
    auto &origin = this->pImpl->ENUOrigin;
    // Update measurement area.
    auto &mAreaEnu = this->pImpl->scenario.measurementArea();
    auto &mArea = this->pImpl->mArea;
    mAreaEnu.clear();
    for (auto geoVertex : mArea){
        snake::BoostPoint p;
        snake::toENU(origin, geoVertex, p);
        mAreaEnu.outer().push_back(p);
    }

    // Update service area.
    auto &sAreaEnu = this->pImpl->scenario.serviceArea();
    auto &sArea = this->pImpl->sArea;
    sAreaEnu.clear();
    for (auto geoVertex : sArea){
        snake::BoostPoint p;
        snake::toENU(origin, geoVertex, p);
        sAreaEnu.outer().push_back(p);
    }

    // Update corridor.
    auto &corridorEnu = this->pImpl->scenario.corridor();
    auto &corridor = this->pImpl->corridor;
    corridor.clear();
    for (auto geoVertex : corridor){
        snake::BoostPoint p;
        snake::toENU(origin, geoVertex, p);
        corridorEnu.outer().push_back(p);
    }

    if ( !this->pImpl->scenario.update() ){
        this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
        return;
    }

    // Asynchronously update flightplan.
    qWarning() << "route calculation missing";
    std::string errorString;
    auto future = std::async([this, &errorString, &origin]{
        snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle*degree::degree);
        snake::flight_plan::Transects transects;
        transects.push_back(bg::model::linestring<snake::BoostPoint>{
                                this->pImpl->scenario.homePositon()
                            });
        bool value = snake::flight_plan::transectsFromScenario(this->pImpl->lineDistance,
                                                             this->pImpl->minTransectLength,
                                                             alpha,
                                                             this->pImpl->scenario,
                                                             this->pImpl->progress,
                                                             transects,
                                                             errorString);
        if ( !value )
            return value;
        snake::flight_plan::Transects transectsRouted;
        snake::flight_plan::Route route;
        value = snake::flight_plan::route(this->pImpl->scenario.joinedArea(),
                                          transects,
                                          transectsRouted,
                                          route,
                                          errorString);
        if ( !value )
            return value;

        // Store arrival path.
        const auto &firstWaypoint = transectsRouted.front().front();
        long startIdx = 0;
        for (long i = 0; i < route.size(); ++i){
            const auto &boostVertex = route[i];
            if ( boostVertex == firstWaypoint){
                startIdx = i;
                break;
            }
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, boostVertex, geoVertex);
            this->pImpl->arrivalPathENU.push_back(enuVertex);
            this->pImpl->arrivalPath.push_back(geoVertex);
        }
        // Store return path.
        long endIdx = 0;
        const auto &lastWaypoint = transectsRouted.back().back();
        for (long i = route.size()-1; i >= 0; --i){
            const auto &boostVertex = route[i];
            if ( boostVertex == lastWaypoint){
                endIdx = i;
                break;
            }
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, boostVertex, geoVertex);
            this->pImpl->returnPathENU.push_back(enuVertex);
            this->pImpl->returnPath.push_back(geoVertex);
        }
        // Store waypoints.
        for (long i = startIdx; i <= endIdx; ++i){
            const auto &boostVertex = route[i];
            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
            QGeoCoordinate geoVertex;
            snake::fromENU(origin, boostVertex, geoVertex);
            this->pImpl->waypointsENU.push_back(enuVertex);
            this->pImpl->waypoints.push_back(geoVertex);
        }

        return true;

    });

    // Continue with storing scenario data in the mean time.
    {
        // Get tiles.
        const auto &tiles = this->pImpl->scenario.tiles();
        const auto &centerPoints = this->pImpl->scenario.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);
            this->pImpl->tiles.polygons().push_back(geoTile);
            this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
            this->pImpl->tilesENU.polygons().push_back(enuTile);
            this->pImpl->tileCenterPointsENU.push_back(enuVertex);
         }
    }

    future.wait();
    // Trying to generate flight plan.
    if ( !future.get() ){
        // error
        this->pImpl->errorMessage = errorString.c_str();
    } else {
        // Success!!!

    }
}

bool SnakeImpl::doTopicServiceSetup()
{
    using namespace ros_bridge::messages;
    UniqueLock lk(this->mutex);

    if ( this->tilesENU.polygons().size() == 0)
        return false;

    // Publish snake origin.
    this->pRosBridge->advertiseTopic("/snake/origin",
                                geographic_msgs::geo_point::messageType().c_str());
    JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
    bool ret = geographic_msgs::geo_point::toJson(
                this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
    Q_ASSERT(ret);
    (void)ret;
    this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");


    // Publish snake tiles.
    this->pRosBridge->advertiseTopic("/snake/tiles",
                                jsk_recognition_msgs::polygon_array::messageType().c_str());
    JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
    ret = jsk_recognition_msgs::polygon_array::toJson(
                this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
    Q_ASSERT(ret);
    (void)ret;
    this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");


    // Subscribe nemo progress.
    this->pRosBridge->subscribe("/nemo/progress",
                           /* callback */ [this](JsonDocUPtr pDoc){
        UniqueLock lk(this->mutex);
        int requiredSize = this->tilesENU.polygons().size();
        auto& progressMsg = this->qProgress;
        if ( !nemo_msgs::progress::fromJson(*pDoc, progressMsg)
             || progressMsg.progress().size() != requiredSize ) { // Some error occured.
            // Set progress to default.
            progressMsg.progress().fill(0, requiredSize);

            // Publish snake origin.
            JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
            bool ret = geographic_msgs::geo_point::toJson(
                         this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
            Q_ASSERT(ret);
            (void)ret;
            this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");

            // Publish snake tiles.
            JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
            ret = jsk_recognition_msgs::polygon_array::toJson(
                         this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
            Q_ASSERT(ret);
            (void)ret;
            this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
        }
        this->progress.clear();
        for (const auto&p : progressMsg.progress()){
            this->progress.push_back(p);
        }
        lk.unlock();

        emit this->parent->nemoProgressChanged();
    });


    // Subscribe /nemo/heartbeat.
    this->pRosBridge->subscribe("/nemo/heartbeat",
                           /* callback */ [this](JsonDocUPtr pDoc){
        UniqueLock lk(this->mutex);
        if ( this->timeout.isActive() ) {
            this->timeout.stop();
        }
        auto &heartbeatMsg = this->heartbeat;
        if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg) ) {
            heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
        }
        this->timeout.singleShot(TIMEOUT, [this]{
            UniqueLock lk(this->mutex);
            this->heartbeat.setStatus(integral(NemoStatus::Timeout));
            emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout));
        });
        emit this->parent->nemoStatusChanged(heartbeatMsg.status());
    });


    // Advertise /snake/get_origin.
    this->pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
                                  [this](JsonDocUPtr) -> JsonDocUPtr
    {
        using namespace ros_bridge::messages;
        SharedLock lk(this->mutex);

        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
        auto &origin = this->ENUOrigin;
        rapidjson::Value jOrigin(rapidjson::kObjectType);
        bool ret = geographic_msgs::geo_point::toJson(
                    origin, jOrigin, pDoc->GetAllocator());
        lk.unlock();
        Q_ASSERT(ret);
        (void)ret;
        pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
        return pDoc;
    });


    // Advertise /snake/get_tiles.
    this->pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
                                  [this](JsonDocUPtr) -> JsonDocUPtr
    {
        SharedLock lk(this->mutex);

        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
        bool ret = jsk_recognition_msgs::polygon_array::toJson(
                    this->tilesENU, jSnakeTiles, pDoc->GetAllocator());
        Q_ASSERT(ret);
        (void)ret;
        pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
        return pDoc;
    });

    return true;
}



SnakeImpl::SnakeImpl(SnakeDataManager *p) :
    rosBridgeEnabeled(false)
  , topicServiceSetupDone(false)
  , lineDistance(1*si::meter)
  , minTransectLength(1*si::meter)
  , calcInProgress(false)
  , parent(p)
{
    // 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 {
            QString defaultString("localhost:9090");
            qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
            qgcApp()->showMessage("Resetting connection string to: " + defaultString);
            connectionStringFact->setRawValue(QVariant(defaultString)); // calls this function recursively
        }
    };
    connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString);
    setConnectionString();

    // Periodic.
    connect(&this->eventTimer, &QTimer::timeout, [this]{
        if ( this->rosBridgeEnabeled ) {
            if ( !this->pRosBridge->isRunning()) {
                this->pRosBridge->start();
            } else if ( this->pRosBridge->isRunning()
                        && this->pRosBridge->connected()
                        && !this->topicServiceSetupDone) {
                if ( this->doTopicServiceSetup() )
                    this->topicServiceSetupDone = true;
            } else if ( this->pRosBridge->isRunning()
                        && !this->pRosBridge->connected()
                        && this->topicServiceSetupDone){
                this->pRosBridge->reset();
                this->pRosBridge->start();
                this->topicServiceSetupDone = false;
            }
        } else  if ( this->pRosBridge->isRunning() ) {
            this->pRosBridge->reset();
            this->topicServiceSetupDone = false;
        }
    });
    this->eventTimer.start(EVENT_TIMER_INTERVAL);
}