temp

qgroundcontrol.pro

@@ -434,6 +434,7 @@ HEADERS += \
     src/Wima/Geometry/GenericPolygon.h \
     src/Wima/Geometry/GenericPolygonArray.h \
     src/Wima/Geometry/GeoPoint3D.h \
+    src/Wima/Snake/NemoInterface.h \
     src/Wima/Snake/QNemoHeartbeat.h \
     src/Wima/Snake/QNemoProgress.h \
     src/Wima/Snake/QNemoProgress.h \
@@ -500,6 +501,7 @@ SOURCES += \
     src/Snake/clipper/clipper.cpp \
     src/Snake/snake.cpp \
     src/Wima/Geometry/GeoPoint3D.cpp \
+    src/Wima/Snake/NemoInterface.cpp \
     src/Wima/Snake/QNemoProgress.cc \
     src/Wima/Snake/SnakeDataManager.cc \
     src/Wima/Snake/SnakeTile.cpp \

src/Snake/snake.cpp

@@ -454,7 +454,7 @@ bool Scenario::update() {
   return true;
 }
 
-bool Scenario::_calculateBoundingBox() {
+bool Scenario::_calculateBoundingBox() const {
   return minimalBoundingBox(_mArea, _mAreaBoundingBox);
 }
 
@@ -474,7 +474,7 @@ bool Scenario::_calculateBoundingBox() {
  *
  * @return Returns true if successful.
  */
-bool Scenario::_calculateTiles() {
+bool Scenario::_calculateTiles() const {
   _tiles.clear();
   _tileCenterPoints.clear();
 
@@ -571,7 +571,7 @@ bool Scenario::_calculateTiles() {
   return true;
 }
 
-bool Scenario::_calculateJoinedArea() {
+bool Scenario::_calculateJoinedArea() const {
   _jArea.clear();
   // Measurement area and service area overlapping?
   bool overlapingSerMeas = bg::intersects(_mArea, _sArea) ? true : false;
@@ -775,7 +775,7 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
   ClipperLib::PolyTree clippedTransecs;
   clipper.Execute(ClipperLib::ctIntersection, clippedTransecs,
                   ClipperLib::pftNonZero, ClipperLib::pftNonZero);
-  auto &transects = clippedTransecs;
+  const auto *transects = &clippedTransecs;
 
   bool ignoreProgress = p.size() != scenario.tiles().size();
   ClipperLib::PolyTree clippedTransecs2;
@@ -793,9 +793,9 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
 
     if (processedTiles.size() != numTiles) {
       vector<ClipperLib::Path> processedTilesClipper;
-      for (auto t : processedTiles) {
+      for (const auto &t : processedTiles) {
         ClipperLib::Path path;
-        for (auto vertex : t.outer()) {
+        for (const auto &vertex : t.outer()) {
           path.push_back(ClipperLib::IntPoint{
               static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
               static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
@@ -805,26 +805,31 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
 
       // Subtract holes (tiles with measurement_progress == 100) from transects.
       clipper.Clear();
-      for (auto &child : clippedTransecs.Childs)
+      for (const auto &child : clippedTransecs.Childs) {
         clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
+      }
       clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
       clipper.Execute(ClipperLib::ctDifference, clippedTransecs2,
                       ClipperLib::pftNonZero, ClipperLib::pftNonZero);
-      transects = clippedTransecs2;
+      transects = &clippedTransecs2;
+    } else {
+      // All tiles processed (t.size() not changed).
+      return true;
     }
   }
 
   // Extract transects from  PolyTree and convert them to BoostLineString
-  for (auto &child : transects.Childs) {
-    auto &clipperTransect = child->Contour;
+  for (const auto &child : transects->Childs) {
+    const auto &clipperTransect = child->Contour;
     BoostPoint v1{static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
                   static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
     BoostPoint v2{static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
                   static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};
 
     BoostLineString transect{v1, v2};
-    if (bg::length(transect) >= minLength.value())
+    if (bg::length(transect) >= minLength.value()) {
       t.push_back(transect);
+    }
   }
 
   if (t.size() == 0) {

src/Snake/snake.h

@@ -173,14 +173,14 @@ public:
   const BoundingBox &measurementAreaBBox() const;
   const BoostPoint &homePositon() const;
 
-  bool update();
+  bool update() const;
 
-  string errorString;
+  mutable string errorString;
 
 private:
-  bool _calculateBoundingBox();
-  bool _calculateTiles();
-  bool _calculateJoinedArea();
+  bool _calculateBoundingBox() const;
+  bool _calculateTiles() const;
+  bool _calculateJoinedArea() const;
 
   Length _tileWidth;
   Length _tileHeight;
@@ -191,12 +191,12 @@ private:
   BoostPolygon _mArea;
   BoostPolygon _sArea;
   BoostPolygon _corridor;
-  BoostPolygon _jArea;
+  mutable BoostPolygon _jArea;
 
-  BoundingBox _mAreaBoundingBox;
-  vector<BoostPolygon> _tiles;
-  BoostLineString _tileCenterPoints;
-  BoostPoint _homePosition;
+  mutable BoundingBox _mAreaBoundingBox;
+  mutable vector<BoostPolygon> _tiles;
+  mutable BoostLineString _tileCenterPoints;
+  mutable BoostPoint _homePosition;
 };
 
 template <class GeoPoint, template <class, class...> class Container>

src/Wima/Snake/NemoInterface.h

+#pragma once
+
+#include <QGeoCoordinate>
+#include <QObject>
+
+#include "SnakeTilesLocal.h"
+
+#include <memory>
+
+class NemoInterface : public QObject {
+  Q_OBJECT
+  class Impl;
+  using PImpl = std::unique_ptr<Impl>;
+
+public:
+  enum class NemoStatus {
+    NotConnected = 0,
+    Connected = 1,
+    Timeout = -1,
+    InvalidHeartbeat = -2
+  };
+
+  explicit NemoInterface(QObject *parent = nullptr);
+
+  void start();
+  void stop();
+
+  void setTilesENU(const SnakeTilesLocal &tilesENU);
+  void setENUOrigin(const QGeoCoordinate &ENUOrigin);
+
+  NemoStatus status();
+  QVector<int> progress();
+
+signals:
+  void statusChanged();
+  void progressChanged();
+
+private:
+  PImpl pImpl;
+};

src/Wima/Snake/SnakeDataManager.h

@@ -15,13 +15,6 @@ using namespace boost::units;
 using Length = quantity<si::length>;
 using Area = quantity<si::area>;
 
-enum class NemoStatus {
-  NotConnected = 0,
-  Connected = 1,
-  Timeout = -1,
-  InvalidHeartbeat = -2
-};
-
 class SnakeDataManager : public QThread {
   Q_OBJECT
 
@@ -39,7 +32,6 @@ public:
   const QmlObjectListModel *tiles() const;
   QVariantList tileCenterPoints() const;
   QNemoProgress nemoProgress() const;
-  int nemoStatus() const;
   bool calcInProgress() const;
   QString errorMessage() const;
   bool success() const;
@@ -63,12 +55,7 @@ public:
   Length tileWidth() const;
   void setTileWidth(Length tileWidth);
 
-  void enableRosBridge();
-  void disableRosBride();
-
 signals:
-  void nemoProgressChanged();
-  void nemoStatusChanged(int status);
   void calcInProgressChanged(bool inProgress);
 
 protected:

src/Wima/WimaController.cc

@@ -121,6 +121,8 @@ WimaController::WimaController(QObject *parent)
           &WimaController::nemoStatusChanged);
   connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
           &WimaController::nemoStatusStringChanged);
+  connect(_currentDM, &SnakeDataManager::calcInProgressChanged, this,
+          &WimaController::snakeCalcInProgressChanged);
 
   connect(this, &QObject::destroyed, &this->_snakeDM, &SnakeDataManager::quit);
   connect(this, &QObject::destroyed, &this->_emptyDM, &SnakeDataManager::quit);
@@ -726,15 +728,15 @@ void WimaController::_DMFinishedHandler() {
   }
 
   // Do update.
-  auto fut = QtConcurrent::run([this] {
-    this->_snakeWM.update(); // this can take a while (ca. 200ms)
+  this->_snakeWM.update(); // this can take a while (ca. 200ms)
 
-    emit this->missionItemsChanged();
-    emit this->currentMissionItemsChanged();
-    emit this->currentWaypointPathChanged();
-    emit this->waypointPathChanged();
-  });
-  (void)fut;
+  emit snakeTilesChanged();
+  emit snakeTileCenterPointsChanged();
+  emit nemoProgressChanged();
+  emit missionItemsChanged();
+  emit currentMissionItemsChanged();
+  emit currentWaypointPathChanged();
+  emit waypointPathChanged();
 }
 
 void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
@@ -756,6 +758,8 @@ void WimaController::_switchDataManager(SnakeDataManager &dataManager) {
                &WimaController::nemoStatusChanged);
     disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
                &WimaController::nemoStatusStringChanged);
+    disconnect(_currentDM, &SnakeDataManager::calcInProgressChanged, this,
+               &WimaController::snakeCalcInProgressChanged);
 
     _currentDM = &dataManager;
 
@@ -767,8 +771,9 @@ void WimaController::_switchDataManager(SnakeDataManager &dataManager) {
             &WimaController::nemoStatusChanged);
     connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
             &WimaController::nemoStatusStringChanged);
+    connect(_currentDM, &SnakeDataManager::calcInProgressChanged, this,
+            &WimaController::snakeCalcInProgressChanged);
 
-    emit snakeConnectionStatusChanged();
     emit snakeCalcInProgressChanged();
     emit snakeTilesChanged();
     emit snakeTileCenterPointsChanged();
@@ -778,16 +783,13 @@ void WimaController::_switchDataManager(SnakeDataManager &dataManager) {
   }
 }
 
-void WimaController::_progressChangedHandler() {
-  emit this->nemoProgressChanged();
-  this->_currentDM->start();
-}
+void WimaController::_progressChangedHandler() { _snakeDM.start(); }
 
 void WimaController::_enableSnakeChangedHandler() {
   if (this->_enableSnake.rawValue().toBool()) {
     qDebug() << "WimaController: enabling snake.";
+    _switchDataManager(this->_snakeDM);
     this->_snakeDM.enableRosBridge();
-    _switchDataManager(_snakeDM);
     _currentDM->start();
   } else {
     qDebug() << "WimaController: disabling snake.";

src/Wima/WimaController.h

@@ -205,7 +205,6 @@ signals:
   void phaseDistanceChanged(void);
   void phaseDurationChanged(void);
   // Snake.
-  void snakeConnectionStatusChanged(void);
   void snakeCalcInProgressChanged(void);
   void snakeTilesChanged(void);
   void snakeTileCenterPointsChanged(void);