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,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<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) {
-      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<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) {
+    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);
   }
 }

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);
   }