#include "SnakeThread.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" template class CommandRAII { public: CommandRAII(Callable &fun) : _fun(fun) {} ~CommandRAII() { _fun(); } private: Callable _fun; }; using QVariantList = QList; using UniqueLock = std::unique_lock; using SharedLock = std::shared_lock; class SnakeThread::Impl { public: struct Input { Input() : tileWidth(5 * si::meter), tileHeight(5 * si::meter), minTileArea(1 * si::meter * si::meter), lineDistance(2 * si::meter), minTransectLength(1 * si::meter), scenarioChanged(true), progressChanged(true), routeParametersChanged(true) {} QList mArea; QGeoCoordinate ENUOrigin; QList sArea; QList corridor; Length tileWidth; Length tileHeight; Area minTileArea; Length lineDistance; Length minTransectLength; QNemoProgress progress; std::atomic_bool scenarioChanged; std::atomic_bool progressChanged; std::atomic_bool routeParametersChanged; mutable std::shared_timed_mutex mutex; }; struct Output { Output() : calcInProgress(false), tilesUpdated(false), waypointsUpdated(false), progressUpdated(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; bool tilesUpdated; bool waypointsUpdated; bool progressUpdated; mutable std::shared_timed_mutex mutex; }; Impl(SnakeThread *p); bool precondition() const; bool doTopicServiceSetup(); // Internal data. snake::Scenario scenario; SnakeThread *parent; // Input Input input; // Output Output output; }; SnakeThread::Impl::Impl(SnakeThread *p) : parent(p) {} SnakeThread::SnakeThread(QObject *parent) : QThread(parent), pImpl(std::make_unique(this)) {} SnakeThread::~SnakeThread() {} void SnakeThread::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 SnakeThread::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 SnakeThread::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); } } void SnakeThread::setProgress(const QVector &progress) { this->pImpl->input.progressChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->input.progress.progress() = progress; } SnakeTiles SnakeThread::tiles() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.tiles; } SnakeTilesLocal SnakeThread::tilesENU() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.tilesENU; } QGeoCoordinate SnakeThread::ENUOrigin() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.ENUOrigin; } QVariantList SnakeThread::tileCenterPoints() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.tileCenterPoints; } QVector SnakeThread::progress() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->input.progress.progress(); } bool SnakeThread::calcInProgress() const { return this->pImpl->output.calcInProgress.load(); } QString SnakeThread::errorMessage() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.errorMessage; } bool SnakeThread::tilesUpdated() { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.tilesUpdated; } bool SnakeThread::waypointsUpdated() { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.waypointsUpdated; } bool SnakeThread::progressUpdated() { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.progressUpdated; } QVector SnakeThread::waypoints() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.waypoints; } QVector SnakeThread::arrivalPath() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.arrivalPath; } QVector SnakeThread::returnPath() const { SharedLock lk(this->pImpl->output.mutex); return this->pImpl->output.returnPath; } Length SnakeThread::lineDistance() const { SharedLock lk(this->pImpl->input.mutex); return this->pImpl->input.lineDistance; } void SnakeThread::setLineDistance(Length lineDistance) { this->pImpl->input.routeParametersChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->input.routeParametersChanged = true; this->pImpl->input.lineDistance = lineDistance; } Area SnakeThread::minTileArea() const { SharedLock lk(this->pImpl->input.mutex); return this->pImpl->scenario.minTileArea(); } void SnakeThread::setMinTileArea(Area minTileArea) { this->pImpl->input.scenarioChanged = true; this->pImpl->input.scenarioChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->scenario.setMinTileArea(minTileArea); } Length SnakeThread::tileHeight() const { SharedLock lk(this->pImpl->input.mutex); return this->pImpl->scenario.tileHeight(); } void SnakeThread::setTileHeight(Length tileHeight) { this->pImpl->input.scenarioChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->scenario.setTileHeight(tileHeight); } Length SnakeThread::tileWidth() const { SharedLock lk(this->pImpl->input.mutex); return this->pImpl->scenario.tileWidth(); } void SnakeThread::setTileWidth(Length tileWidth) { this->pImpl->input.scenarioChanged = true; UniqueLock lk(this->pImpl->input.mutex); this->pImpl->scenario.setTileWidth(tileWidth); } void SnakeThread::run() { #ifndef NDEBUG auto startTime = std::chrono::high_resolution_clock::now(); #endif // Signal that calculation is in progress. this->pImpl->output.calcInProgress.store(true); emit calcInProgressChanged(this->pImpl->output.calcInProgress.load()); #ifndef NDEBUG auto onExit = [this, &startTime] { #else auto onExit = [this] { #endif #ifndef NDEBUG qDebug() << "SnakeThread::run() execution time: " << std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - startTime) .count() << " ms"; #endif this->pImpl->output.calcInProgress.store(false); emit calcInProgressChanged(this->pImpl->output.calcInProgress.load()); }; CommandRAII onExitRAII(onExit); bool tilesValid = true; QGeoCoordinate origin; { // Check preconditions. SharedLock lk(this->pImpl->input.mutex); if (this->pImpl->input.mArea.size() < 3) { UniqueLock uLock(this->pImpl->output.mutex); this->pImpl->output.errorMessage = "Measurement area invalid: size < 3."; tilesValid = false; } else if (this->pImpl->input.sArea.size() < 3) { UniqueLock uLock(this->pImpl->output.mutex); this->pImpl->output.errorMessage = "Service area invalid: size < 3."; tilesValid = false; } else { // Set Origin origin = this->pImpl->input.mArea.front(); // Update Scenario if necessary if (this->pImpl->input.scenarioChanged) { // 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; corridorEnu.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); if (!this->pImpl->scenario.update()) { this->pImpl->output.errorMessage = this->pImpl->scenario.errorString.c_str(); tilesValid = false; } } } } bool waypointsValid = tilesValid; bool progressValid = tilesValid; snake::flight_plan::Transects transects; snake::flight_plan::Transects transectsRouted; snake::flight_plan::Route route; std::vector progress; bool needWaypointUpdate = this->pImpl->input.scenarioChanged || this->pImpl->input.routeParametersChanged || this->pImpl->input.progressChanged; if (tilesValid) { if (needWaypointUpdate) { // Sample data SharedLock inputLock(this->pImpl->input.mutex); // Verify progress. size_t nTiles = this->pImpl->scenario.tiles().size(); if (size_t(this->pImpl->input.progress.progress().size()) != nTiles) { for (size_t i = 0; i < nTiles; ++i) { progress.push_back(0); } } else { for (size_t i = 0; i < nTiles; ++i) { progress.push_back(this->pImpl->input.progress.progress()[i]); } } // Copy remaining parameters and release lock. 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 * si::radian); std::cout << "Transects angle: " << alpha << std::endl; transects.push_back( bg::model::linestring{scenario.homePositon()}); bool success = snake::flight_plan::transectsFromScenario( lineDistance, minTransectLength, alpha, scenario, progress, transects, errorString); if (!success) { UniqueLock lk(this->pImpl->output.mutex); this->pImpl->output.errorMessage = errorString.c_str(); waypointsValid = false; progressValid = true; } else if (transects.size() > 1) { success = snake::flight_plan::route(scenario.joinedArea(), transects, transectsRouted, route, errorString); if (!success) { UniqueLock lk(this->pImpl->output.mutex); this->pImpl->output.errorMessage = errorString.c_str(); waypointsValid = false; progressValid = true; } else { waypointsValid = true; progressValid = true; } } else { // No transects waypointsValid = false; progressValid = true; } } else { // No update necessary waypointsValid = true; progressValid = true; } } UniqueLock lk(this->pImpl->output.mutex); // Store tiles. this->pImpl->output.tilesUpdated = false; if (!tilesValid) { // 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(); this->pImpl->output.ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0); this->pImpl->output.tilesUpdated = true; } else if (this->pImpl->input.scenarioChanged) { this->pImpl->input.scenarioChanged = false; // 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(); this->pImpl->output.ENUOrigin = origin; // Convert and store scenario data. 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); } this->pImpl->output.tilesUpdated = true; } // Store progress. this->pImpl->output.progressUpdated = false; if (!progressValid) { this->pImpl->input.progress.progress().clear(); this->pImpl->output.progressUpdated = true; } else if (this->pImpl->input.progressChanged) { if (progress.size() == this->pImpl->scenario.tiles().size()) { auto &qProgress = this->pImpl->input.progress; qProgress.progress().clear(); for (const auto &p : progress) { qProgress.progress().push_back(p); } } this->pImpl->output.progressUpdated = true; } // Store waypoints. if (!waypointsValid) { // Clear waypoints. this->pImpl->output.arrivalPath.clear(); this->pImpl->output.returnPath.clear(); this->pImpl->output.arrivalPathENU.clear(); this->pImpl->output.returnPathENU.clear(); this->pImpl->output.waypoints.clear(); this->pImpl->output.waypointsENU.clear(); this->pImpl->output.waypointsUpdated = true; } else if (needWaypointUpdate) { // Clear waypoints. this->pImpl->output.arrivalPath.clear(); this->pImpl->output.returnPath.clear(); this->pImpl->output.arrivalPathENU.clear(); this->pImpl->output.returnPathENU.clear(); this->pImpl->output.waypoints.clear(); this->pImpl->output.waypointsENU.clear(); // // Store arrival path. // 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); // } for (const auto &t : transects) { for (const auto &v : t) { QPointF enuVertex{v.get<0>(), v.get<1>()}; QGeoCoordinate geoVertex; snake::fromENU(origin, v, geoVertex); this->pImpl->output.waypointsENU.push_back(enuVertex); this->pImpl->output.waypoints.push_back(geoVertex); } } this->pImpl->output.waypointsUpdated = true; } }