diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp index 352b206abe6db55aacd7f15760c676883bbdb823..c0c9ecd4eff9c19ecf3d2bbca7750718e82a2477 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -1,3 +1,4 @@ +#include #include #include "snake.h" @@ -52,7 +53,7 @@ void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) { center.set<1>(c.y); } -void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { +bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { /* Find the minimum-area bounding box of a set of 2D points @@ -94,8 +95,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { POSSIBILITY OF SUCH DAMAGE. */ - if (polygon.outer().empty()) - return; + if (polygon.outer().empty() || polygon.outer().size() < 3) + return false; BoostPolygon convex_hull; bg::convex_hull(polygon, convex_hull); @@ -198,6 +199,8 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { BoostPolygon rotated_polygon; bg::transform(minBBox.corners, rotated_polygon, rotate); minBBox.corners = rotated_polygon; + + return true; } void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, @@ -438,19 +441,21 @@ const BoostPoint &Scenario::homePositon() const { return _homePosition; } bool Scenario::update() { if (!_needsUpdate) return true; + bg::correct(_mArea); + bg::correct(_sArea); + bg::correct(_corridor); + if (!_calculateJoinedArea()) + return false; if (!_calculateBoundingBox()) return false; if (!_calculateTiles()) return false; - if (!_calculateJoinedArea()) - return false; _needsUpdate = false; return true; } bool Scenario::_calculateBoundingBox() { - minimalBoundingBox(_mArea, _mAreaBoundingBox); - return true; + return minimalBoundingBox(_mArea, _mAreaBoundingBox); } /** @@ -474,9 +479,12 @@ bool Scenario::_calculateTiles() { _tileCenterPoints.clear(); if (_tileWidth <= 0 * bu::si::meter || _tileHeight <= 0 * bu::si::meter || - _minTileArea <= 0 * bu::si::meter * bu::si::meter) { - errorString = - "Parameters tileWidth, tileHeight, minTileArea must be positive."; + _minTileArea < 0 * bu::si::meter * bu::si::meter) { + std::stringstream ss; + ss << "Parameters tileWidth (" << _tileWidth << "), tileHeight (" + << _tileHeight << "), minTileArea (" << _minTileArea + << ") must be positive."; + errorString = ss.str(); return false; } @@ -504,9 +512,8 @@ bool Scenario::_calculateTiles() { if (iMax < 1 || jMax < 1) { std::stringstream ss; - ss << "Tile width or Tile height to large. " - << "tile width = " << _tileWidth << ", " - << "tile height = " << _tileHeight << "." << std::endl; + ss << "Tile width (" << _tileWidth << ") or tile height (" << _tileHeight + << ") to large for measurement area."; errorString = ss.str(); return false; } @@ -554,8 +561,10 @@ bool Scenario::_calculateTiles() { } if (_tiles.size() < 1) { - errorString = - "No tiles calculated. Is the minTileArea parameter large enough?"; + std::stringstream ss; + ss << "No tiles calculated. Is the minTileArea (" << _minTileArea + << ") parameter large enough?"; + errorString = ss.str(); return false; } @@ -590,7 +599,21 @@ bool Scenario::_calculateJoinedArea() { } else if (corridor_is_connection) { bg::union_(partialArea, _corridor, sol); } else { - errorString = "Areas are not overlapping"; + std::stringstream ss; + auto printPoint = [&ss](const BoostPoint &p) { + ss << " (" << p.get<0>() << ", " << p.get<1>() << ")"; + }; + ss << "Areas are not overlapping." << std::endl; + ss << "Measurement area:"; + bg::for_each_point(_mArea, printPoint); + ss << std::endl; + ss << "Service area:"; + bg::for_each_point(_sArea, printPoint); + ss << std::endl; + ss << "Corridor:"; + bg::for_each_point(_corridor, printPoint); + ss << std::endl; + errorString = ss.str(); return false; } @@ -872,21 +895,16 @@ bool flight_plan::route(const BoostPolygon &area, BoostLineString vertices; size_t n0 = 0; for (const auto &t : transects) { - if (t.size() >= 2) { - n0 += 2; - } else { - n0 += 1; - } + n0 += std::min(t.size(), 2); } vertices.reserve(n0); + struct TransectInfo { TransectInfo(size_t n, bool f) : index(n), front(f) {} - size_t index; bool front; }; std::vector transectInfoList; - size_t idx = 0; for (size_t i = 0; i < transects.size(); ++i) { const auto &t = transects[i]; vertices.push_back(t.front()); @@ -1038,6 +1056,7 @@ bool flight_plan::route(const BoostPolygon &area, idx2Vertex(idxList, route); } } + return true; } } // namespace snake diff --git a/src/Snake/snake.h b/src/Snake/snake.h index 5e4704edef9892c11ca028b3e90791fedc17b5c6..2e77bf521e2043f44f338d394c5b862761c873dc 100644 --- a/src/Snake/snake.h +++ b/src/Snake/snake.h @@ -97,10 +97,10 @@ void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) { GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), origin.altitude(), earth); - double x, y, z; + double x = 0, y = 0, z = 0; proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z); out.set<0>(x); - out.set<0>(y); + out.set<1>(y); (void)z; } template @@ -110,7 +110,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) { GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), origin.altitude(), earth); - double lat, lon, alt; + double lat = 0, lon = 0, alt = 0; proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt); out.setLatitude(lat); out.setLongitude(lon); @@ -118,7 +118,7 @@ void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) { } void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er); -void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox); +bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox); void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset); @@ -242,7 +242,7 @@ bool route(const BoostPolygon &area, const Transects &transects, namespace detail { const double offsetConstant = - 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. + 0.5; // meter, polygon offset to compenstate for numerical inaccurracies. } // namespace detail } // namespace snake diff --git a/src/Wima/Snake/SnakeDataManager.cc b/src/Wima/Snake/SnakeDataManager.cc index 0ebc71ef2f8dd8cfa32e6f302d5322fea5b1574e..1e9cfd0a1b71fc10c9ee54b55d42640565257d0f 100644 --- a/src/Wima/Snake/SnakeDataManager.cc +++ b/src/Wima/Snake/SnakeDataManager.cc @@ -306,17 +306,26 @@ void SnakeDataManager::setMeasurementArea( const QList &measurementArea) { UniqueLock lk(this->pImpl->mutex); this->pImpl->mArea = measurementArea; + for (auto &vertex : this->pImpl->mArea) { + vertex.setAltitude(0); + } } void SnakeDataManager::setServiceArea( const QList &serviceArea) { UniqueLock lk(this->pImpl->mutex); this->pImpl->sArea = serviceArea; + for (auto &vertex : this->pImpl->sArea) { + vertex.setAltitude(0); + } } void SnakeDataManager::setCorridor(const QList &corridor) { UniqueLock lk(this->pImpl->mutex); this->pImpl->corridor = corridor; + for (auto &vertex : this->pImpl->corridor) { + vertex.setAltitude(0); + } } const QmlObjectListModel *SnakeDataManager::tiles() const { @@ -418,9 +427,21 @@ void SnakeDataManager::disableRosBride() { } void SnakeDataManager::run() { +#ifndef NDEBUG + auto startTime = std::chrono::high_resolution_clock::now(); + qDebug() << "SnakeDataManager::run()"; +#endif + this->pImpl->calcInProgress.store(true); emit calcInProgressChanged(this->pImpl->calcInProgress.load()); - auto onExit = [this] { + 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->calcInProgress.store(false); emit calcInProgressChanged(this->pImpl->calcInProgress.load()); }; @@ -443,6 +464,8 @@ void SnakeDataManager::run() { this->pImpl->ENUOrigin = this->pImpl->mArea.front(); auto &origin = this->pImpl->ENUOrigin; + qDebug() << "SnakeDataManager::run(): origin: " << origin.latitude() << " " + << origin.longitude() << " " << origin.altitude(); // Update measurement area. auto &mAreaEnu = this->pImpl->scenario.measurementArea(); auto &mArea = this->pImpl->mArea; @@ -478,75 +501,7 @@ void SnakeDataManager::run() { return; } - // Asynchronously update flightplan. - 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{ - 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) { - this->pImpl->errorMessage = "Not able to generate transects."; - 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) { - this->pImpl->errorMessage = "Routing error."; - return value; - } - - // 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->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. + // Store scenario data. { // Get tiles. const auto &tiles = this->pImpl->scenario.tiles(); @@ -576,12 +531,69 @@ void SnakeDataManager::run() { } } - future.wait(); - // Trying to generate flight plan. - if (!future.get()) { - // error + // Create transects. + std::string errorString; + snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle * + degree::degree); + snake::flight_plan::Transects transects; + transects.push_back(bg::model::linestring{ + 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) { + this->pImpl->errorMessage = errorString.c_str(); + return; + } + + // Route transects + 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) { this->pImpl->errorMessage = errorString.c_str(); - } else { - // Success!!! + return; + } + + // 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->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); } } diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index e1ab2b4528de232b1f0957903236d9af2473f977..5fbc7ae66ce853e8968b857ce9ab62a2406939a9 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -184,7 +184,7 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; } Fact *WimaController::altitude() { return &_altitude; } QmlObjectListModel *WimaController::snakeTiles() { - return const_cast(this->_snakeDM.tiles()); + return const_cast(this->_currentDM->tiles()); } QVariantList WimaController::snakeTileCenterPoints() { @@ -710,7 +710,8 @@ void WimaController::_initSmartRTL() { void WimaController::_DMFinishedHandler() { if (!_snakeDM.success()) { - // qgcApp()->showMessage(r.errorMessage); + qDebug() << _snakeDM.errorMessage(); + // qgcApp()->showMessage(_snakeDM.errorMessage()); return; } @@ -784,10 +785,12 @@ void WimaController::_progressChangedHandler() { void WimaController::_enableSnakeChangedHandler() { if (this->_enableSnake.rawValue().toBool()) { + qDebug() << "WimaController: enabling snake."; this->_snakeDM.enableRosBridge(); _switchDataManager(_snakeDM); _currentDM->start(); } else { + qDebug() << "WimaController: disabling snake."; this->_snakeDM.disableRosBride(); _switchDataManager(_emptyDM); }