temp

src/Snake/snake.cpp

+#include <algorithm>
 #include <iostream>
 
 #include "snake.h"
@@ -52,7 +53,7 @@ void polygonCenter(const BoostPolygon &polygon, BoostPoint &center) {
   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<std::size_t>(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<TransectInfo> 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

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 <class GeoPoint>
@@ -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 &center);
-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
 

src/Wima/Snake/SnakeDataManager.cc

@@ -306,17 +306,26 @@ void SnakeDataManager::setMeasurementArea(
     const QList<QGeoCoordinate> &measurementArea) {
   UniqueLock lk(this->pImpl->mutex);
   this->pImpl->mArea = measurementArea;
+  for (auto &vertex : this->pImpl->mArea) {
+    vertex.setAltitude(0);
+  }
 }
 
 void SnakeDataManager::setServiceArea(
     const QList<QGeoCoordinate> &serviceArea) {
   UniqueLock lk(this->pImpl->mutex);
   this->pImpl->sArea = serviceArea;
+  for (auto &vertex : this->pImpl->sArea) {
+    vertex.setAltitude(0);
+  }
 }
 
 void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &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::milliseconds>(
+                    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,9 +501,38 @@ void SnakeDataManager::run() {
     return;
   }
 
-  // Asynchronously update flightplan.
+  // Store scenario data.
+  {
+    // 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->tilesQml.append(new SnakeTile(geoTile));
+      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);
+    }
+  }
+
+  // Create transects.
   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;
@@ -490,17 +542,19 @@ void SnakeDataManager::run() {
       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;
+    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 = "Routing error.";
-      return value;
+    this->pImpl->errorMessage = errorString.c_str();
+    return;
   }
 
   // Store arrival path.
@@ -542,46 +596,4 @@ void SnakeDataManager::run() {
     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->tilesQml.append(new SnakeTile(geoTile));
-      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!!!
-  }
 }

src/Wima/WimaController.cc

@@ -184,7 +184,7 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
 Fact *WimaController::altitude() { return &_altitude; }
 
 QmlObjectListModel *WimaController::snakeTiles() {
-  return const_cast<QmlObjectListModel *>(this->_snakeDM.tiles());
+  return const_cast<QmlObjectListModel *>(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);
   }