#include "SnakeDataManager.h" #include #include #include "QGCApplication.h" #include "QGCToolbox.h" #include "SettingsFact.h" #include "SettingsManager.h" #include "WimaSettings.h" #include #include #include #include #include "Wima/Snake/SnakeTiles.h" #include "Wima/Snake/SnakeTilesLocal.h" #include "snake.h" using QVariantList = QList; using UniqueLock = std::unique_lock; using SharedLock = std::shared_lock; class SnakeDataManager::Impl { public: struct Input { Input() : tileWidth(5 * si::meter), tileHeight(5 * si::meter), minTileArea(1 * si::meter * si::meter), scenarioChanged(true), lineDistance(2 * si::meter), minTransectLength(1 * si::meter), routeParametersChanged(true) {} QList mArea; QGeoCoordinate ENUOrigin; QList sArea; QList corridor; Length tileWidth; Length tileHeight; Area minTileArea; std::atomic_bool scenarioChanged; Length lineDistance; Length minTransectLength; std::atomic_bool routeParametersChanged; mutable std::shared_timed_mutex mutex; }; struct Output { Output() : calcInProgress(false) {} SnakeTiles tiles; QVariantList tileCenterPoints; SnakeTilesLocal tilesENU; QVector tileCenterPointsENU; QGeoCoordinate ENUOrigin; QVector waypoints; QVector arrivalPath; QVector returnPath; QVector waypointsENU; QVector arrivalPathENU; QVector returnPathENU; QString errorMessage; std::atomic_bool calcInProgress; std::atomic_bool tilesUpdated; std::atomic_bool waypointsUpdated; std::atomic_bool progressUpdated; QNemoProgress progress; mutable std::shared_timed_mutex mutex; }; Impl(SnakeDataManager *p); bool precondition() const; bool doTopicServiceSetup(); // Internal data. snake::Scenario scenario; SnakeDataManager *parent; // Input Input input; // Output Output output; }; SnakeDataManager::Impl::Impl(SnakeDataManager *p) : parent(p) {} bool SnakeDataManager::Impl::precondition() const { return true; } template class CommandRAII { public: CommandRAII(Callable &fun) : _fun(fun) {} ~CommandRAII() { _fun(); } private: Callable _fun; }; SnakeDataManager::SnakeDataManager(QObject *parent) : QThread(parent), pImpl(std::make_unique(this)) {} void SnakeDataManager::setMeasurementArea( const QList &measurementArea) { this->pImpl->input.scenarioChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->input.mArea = measurementArea; for (auto &vertex : this->pImpl->input.mArea) { vertex.setAltitude(0); } } void SnakeDataManager::setServiceArea( const QList &serviceArea) { this->pImpl->input.scenarioChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->input.sArea = serviceArea; for (auto &vertex : this->pImpl->input.sArea) { vertex.setAltitude(0); } } void SnakeDataManager::setCorridor(const QList &corridor) { this->pImpl->input.scenarioChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->input.corridor = corridor; for (auto &vertex : this->pImpl->input.corridor) { vertex.setAltitude(0); } } SnakeTiles SnakeDataManager::tiles() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.tiles; } QVariantList SnakeDataManager::tileCenterPoints() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.tileCenterPoints; } QNemoProgress SnakeDataManager::progress() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.progress; } bool SnakeDataManager::calcInProgress() const { return this->pImpl->output.calcInProgress.load(); } QString SnakeDataManager::errorMessage() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.errorMessage; } bool SnakeDataManager::success() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.errorMessage.isEmpty() ? true : false; } QVector SnakeDataManager::waypoints() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.waypoints; } QVector SnakeDataManager::arrivalPath() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.arrivalPath; } QVector SnakeDataManager::returnPath() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.returnPath; } Length SnakeDataManager::lineDistance() const { SharedLock lk(this->pImpl->input.mutex); return this->pImpl->input.lineDistance; } void SnakeDataManager::setLineDistance(Length lineDistance) { UniqueLock lk(this->pImpl->input.mutex); this->pImpl->input.routeParametersChanged = true; this->pImpl->input.lineDistance = lineDistance; } Area SnakeDataManager::minTileArea() const { SharedLock lk(this->pImpl->input.mutex); return this->pImpl->scenario.minTileArea(); } void SnakeDataManager::setMinTileArea(Area minTileArea) { this->pImpl->input.scenarioChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->scenario.setMinTileArea(minTileArea); } Length SnakeDataManager::tileHeight() const { SharedLock lk(this->pImpl->input.mutex); return this->pImpl->scenario.tileHeight(); } void SnakeDataManager::setTileHeight(Length tileHeight) { this->pImpl->input.scenarioChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->scenario.setTileHeight(tileHeight); } Length SnakeDataManager::tileWidth() const { SharedLock lk(this->pImpl->input.mutex); return this->pImpl->scenario.tileWidth(); } void SnakeDataManager::setTileWidth(Length tileWidth) { UniqueLock lk(this->pImpl->input.mutex); this->pImpl->input.scenarioChanged = true; this->pImpl->scenario.setTileWidth(tileWidth); } void SnakeDataManager::run() { #ifndef NDEBUG auto startTime = std::chrono::high_resolution_clock::now(); #endif this->pImpl->output.calcInProgress.store(true); emit calcInProgressChanged(this->pImpl->output.calcInProgress.load()); auto onExit = [this, &startTime] { #ifndef NDEBUG qDebug() << "SnakeDataManager::run() execution time: " << std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - startTime) .count() << " ms"; #endif this->pImpl->output.calcInProgress.store(true); emit calcInProgressChanged(this->pImpl->output.calcInProgress.load()); }; CommandRAII onExitRAII(onExit); this->pImpl->output.tilesUpdated = false; this->pImpl->output.waypointsUpdated = false; this->pImpl->output.progressUpdated = false; { // Check preconditions. SharedLock lk(this->pImpl->input.mutex); if (!this->pImpl->precondition()) return; if (this->pImpl->input.mArea.size() < 3) { UniqueLock uLock(this->pImpl->output.mutex); this->pImpl->output.errorMessage = "Measurement area invalid: size < 3."; return; } if (this->pImpl->input.sArea.size() < 3) { UniqueLock uLock(this->pImpl->output.mutex); this->pImpl->output.errorMessage = "Service area invalid: size < 3."; return; } // Update Scenario if necessary if (this->pImpl->input.scenarioChanged) { // Set Origin QGeoCoordinate origin; { UniqueLock uLock(this->pImpl->output.mutex); this->pImpl->output.ENUOrigin = this->pImpl->input.mArea.front(); origin = this->pImpl->output.ENUOrigin; } // Update measurement area. auto &mAreaEnu = this->pImpl->scenario.measurementArea(); auto &mArea = this->pImpl->input.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->input.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->input.corridor; corridor.clear(); for (auto geoVertex : corridor) { snake::BoostPoint p; snake::toENU(origin, geoVertex, p); corridorEnu.outer().push_back(p); } // Update parametes. this->pImpl->scenario.setTileHeight(this->pImpl->input.tileHeight); this->pImpl->scenario.setTileWidth(this->pImpl->input.tileWidth); this->pImpl->scenario.setMinTileArea(this->pImpl->input.minTileArea); this->pImpl->input.scenarioChanged = false; if (!this->pImpl->scenario.update()) { this->pImpl->output.errorMessage = this->pImpl->scenario.errorString.c_str(); return; } this->pImpl->output.tilesUpdated = true; } } snake::flight_plan::Transects transectsRouted; snake::flight_plan::Route route; std::vector progress; if (this->pImpl->input.scenarioChanged || this->pImpl->input.routeParametersChanged) { // Sample data SharedLock inputLock(this->pImpl->input.mutex); SharedLock outputLock(this->pImpl->output.mutex); // Verify progress. size_t nTiles = this->pImpl->scenario.tiles().size(); if (this->pImpl->output.progress.progress().size() != nTiles) { outputLock.unlock(); for (size_t i = 0; i < nTiles; ++i) { progress.push_back(0); } this->pImpl->output.progressUpdated = true; } const auto &scenario = this->pImpl->scenario; const auto lineDistance = this->pImpl->input.lineDistance; const auto minTransectLength = this->pImpl->input.minTransectLength; inputLock.unlock(); // Create transects. std::string errorString; snake::Angle alpha(-scenario.mAreaBoundingBox().angle * degree::degree); std::cout << "Transects angle: " << alpha << std::endl; snake::flight_plan::Transects transects; transects.push_back( bg::model::linestring{scenario.homePositon()}); bool value = snake::flight_plan::transectsFromScenario( lineDistance, minTransectLength, alpha, scenario, progress, transects, errorString); if (!value) { UniqueLock lk(this->pImpl->output.mutex); this->pImpl->output.errorMessage = errorString.c_str(); } else if (transects.size() > 1) { value = snake::flight_plan::route(scenario.joinedArea(), transects, transectsRouted, route, errorString); if (!value) { UniqueLock lk(this->pImpl->output.mutex); this->pImpl->output.errorMessage = errorString.c_str(); } else { this->pImpl->output.waypointsUpdated = true; } } } UniqueLock lk(this->pImpl->output.mutex); // Store tiles. if (this->pImpl->output.tilesUpdated) { // Clear some output data. this->pImpl->output.tiles.polygons().clear(); this->pImpl->output.tileCenterPoints.clear(); this->pImpl->output.tilesENU.polygons().clear(); this->pImpl->output.tileCenterPointsENU.clear(); // Convert and store scenario data. const auto &origin = this->pImpl->output.ENUOrigin; const auto &tiles = this->pImpl->scenario.tiles(); const auto ¢erPoints = 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 = 0; 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->output.tiles.polygons().push_back(geoTile); this->pImpl->output.tileCenterPoints.push_back( QVariant::fromValue(geoVertex)); this->pImpl->output.tilesENU.polygons().push_back(enuTile); this->pImpl->output.tileCenterPointsENU.push_back(enuVertex); } } // Store progress. if (this->pImpl->output.progressUpdated) { size_t nTiles = progress.size(); this->pImpl->output.progress.progress().clear(); this->pImpl->output.progress.progress().reserve(nTiles); for (const auto &p : progress) { this->pImpl->output.progress.progress().push_back(p); } } // Store waypoints. if (this->pImpl->output.waypointsUpdated) { // Store arrival path. const auto &origin = this->pImpl->output.ENUOrigin; const auto &firstWaypoint = transectsRouted.front().front(); long startIdx = 0; for (long i = 0; i < long(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->output.arrivalPathENU.push_back(enuVertex); this->pImpl->output.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->output.returnPathENU.push_back(enuVertex); this->pImpl->output.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->output.waypointsENU.push_back(enuVertex); this->pImpl->output.waypoints.push_back(geoVertex); } } }