1243

qgroundcontrol.pro

@@ -28,11 +28,13 @@ QGCROOT  = $$PWD
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
     DEFINES += DEBUG
+    DEFINES += SNAKE_SHOW_TIME
     #DEFINES += ROS_BRIDGE_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
     #DEFINES += ROS_BRIDGE_DEBUG
+    DEFINES += SNAKE_SHOW_TIME
     DEFINES += NDEBUG
 }
 
@@ -438,11 +440,11 @@ HEADERS += \
     src/Wima/Snake/QNemoHeartbeat.h \
     src/Wima/Snake/QNemoProgress.h \
     src/Wima/Snake/QNemoProgress.h \
+    src/Wima/Snake/SnakeThread.h \
     src/Wima/Snake/SnakeTile.h \
     src/Wima/Snake/SnakeTileLocal.h \
     src/Wima/Snake/SnakeTiles.h \
     src/Wima/Snake/SnakeTilesLocal.h \
-    src/Wima/Snake/SnakeDataManager.h \
     src/Wima/WaypointManager/AreaInterface.h \
     src/Wima/WaypointManager/DefaultManager.h \
     src/Wima/WaypointManager/GenericWaypointManager.h \
@@ -503,7 +505,7 @@ SOURCES += \
     src/Wima/Geometry/GeoPoint3D.cpp \
     src/Wima/Snake/NemoInterface.cpp \
     src/Wima/Snake/QNemoProgress.cc \
-    src/Wima/Snake/SnakeDataManager.cc \
+    src/Wima/Snake/SnakeThread.cc \
     src/Wima/Snake/SnakeTile.cpp \
     src/Wima/WaypointManager/AreaInterface.cpp \
     src/Wima/WaypointManager/DefaultManager.cpp \

src/Snake/snake.cpp

@@ -22,7 +22,7 @@
 using namespace operations_research;
 
 #ifndef NDEBUG
-//#define SHOW_TIME
+//#define SNAKE_SHOW_TIME
 #endif
 
 namespace bg = boost::geometry;
@@ -104,7 +104,7 @@ bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) {
 
   //# Compute edges (x2-x1,y2-y1)
   std::vector<BoostPoint> edges;
-  auto convex_hull_outer = convex_hull.outer();
+  const auto &convex_hull_outer = convex_hull.outer();
   for (long i = 0; i < long(convex_hull_outer.size()) - 1; ++i) {
     BoostPoint p1 = convex_hull_outer.at(i);
     BoostPoint p2 = convex_hull_outer.at(i + 1);
@@ -711,13 +711,12 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
   // Rotate measurement area by angle and calculate bounding box.
   auto &mArea = scenario.measurementArea();
   BoostPolygon mAreaRotated;
-  trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-angle.value() *
+  trans::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
                                                              180 / M_PI);
   bg::transform(mArea, mAreaRotated, rotate);
 
   BoostBox box;
   boost::geometry::envelope(mAreaRotated, box);
-
   double x0 = box.min_corner().get<0>();
   double y0 = box.min_corner().get<1>();
   double x1 = box.max_corner().get<0>();
@@ -725,8 +724,6 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
 
   // Generate transects and convert them to clipper path.
   size_t num_t = int(ceil((y1 - y0) / distance.value())); // number of transects
-  trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
-      angle.value() * 180 / M_PI);
   vector<ClipperLib::Path> transectsClipper;
   transectsClipper.reserve(num_t);
   for (size_t i = 0; i < num_t; ++i) {
@@ -736,17 +733,20 @@ bool flight_plan::transectsFromScenario(Length distance, Length minLength,
     BoostLineString transect;
     transect.push_back(v1);
     transect.push_back(v2);
-
     // transform back
     BoostLineString temp_transect;
+    trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
+        -angle.value() * 180 / M_PI);
     bg::transform(transect, temp_transect, rotate_back);
-
-    ClipperLib::IntPoint c1{
-        static_cast<ClipperLib::cInt>(transect[0].get<0>() * CLIPPER_SCALE),
-        static_cast<ClipperLib::cInt>(transect[0].get<1>() * CLIPPER_SCALE)};
-    ClipperLib::IntPoint c2{
-        static_cast<ClipperLib::cInt>(transect[1].get<0>() * CLIPPER_SCALE),
-        static_cast<ClipperLib::cInt>(transect[1].get<1>() * CLIPPER_SCALE)};
+    // to clipper
+    ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
+                                temp_transect[0].get<0>() * CLIPPER_SCALE),
+                            static_cast<ClipperLib::cInt>(
+                                temp_transect[0].get<1>() * CLIPPER_SCALE)};
+    ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
+                                temp_transect[1].get<0>() * CLIPPER_SCALE),
+                            static_cast<ClipperLib::cInt>(
+                                temp_transect[1].get<1>() * CLIPPER_SCALE)};
     ClipperLib::Path path{c1, c2};
     transectsClipper.push_back(path);
   }
@@ -852,22 +852,22 @@ void generateRoutingModel(const BoostLineString &vertices,
                           const BoostPolygon &polygonOffset, size_t n0,
                           RoutingDataModel &dataModel, Matrix<double> &graph) {
 
-#ifdef SHOW_TIME
+#ifdef SNAKE_SHOW_TIME
   auto start = std::chrono::high_resolution_clock::now();
 #endif
   graphFromPolygon(polygonOffset, vertices, graph);
-#ifdef SHOW_TIME
+#ifdef SNAKE_SHOW_TIME
   auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
       std::chrono::high_resolution_clock::now() - start);
   cout << "Execution time graphFromPolygon(): " << delta.count() << " ms"
        << endl;
 #endif
   Matrix<double> distanceMatrix(graph);
-#ifdef SHOW_TIME
+#ifdef SNAKE_SHOW_TIME
   start = std::chrono::high_resolution_clock::now();
 #endif
   toDistanceMatrix(distanceMatrix);
-#ifdef SHOW_TIME
+#ifdef SNAKE_SHOW_TIME
   delta = std::chrono::duration_cast<std::chrono::milliseconds>(
       std::chrono::high_resolution_clock::now() - start);
   cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms"
@@ -934,12 +934,12 @@ bool flight_plan::route(const BoostPolygon &area,
   // Offset joined area.
   BoostPolygon areaOffset;
   offsetPolygon(area, areaOffset, detail::offsetConstant);
-#ifdef SHOW_TIME
-  start = std::chrono::high_resolution_clock::now();
+#ifdef SNAKE_SHOW_TIME
+  auto start = std::chrono::high_resolution_clock::now();
 #endif
   generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph);
-#ifdef SHOW_TIME
-  delta = std::chrono::duration_cast<std::chrono::milliseconds>(
+#ifdef SNAKE_SHOW_TIME
+  auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
       std::chrono::high_resolution_clock::now() - start);
   cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms"
        << endl;
@@ -995,11 +995,11 @@ bool flight_plan::route(const BoostPolygon &area,
   searchParameters.set_allocated_time_limit(tMax);
 
   // Solve the problem.
-#ifdef SHOW_TIME
+#ifdef SNAKE_SHOW_TIME
   start = std::chrono::high_resolution_clock::now();
 #endif
   const Assignment *solution = routing.SolveWithParameters(searchParameters);
-#ifdef SHOW_TIME
+#ifdef SNAKE_SHOW_TIME
   delta = std::chrono::duration_cast<std::chrono::milliseconds>(
       std::chrono::high_resolution_clock::now() - start);
   cout << "Execution time routing.SolveWithParameters(): " << delta.count()

src/Snake/snake.h

@@ -173,7 +173,7 @@ public:
   const BoundingBox &measurementAreaBBox() const;
   const BoostPoint &homePositon() const;
 
-  bool update() const;
+  bool update();
 
   mutable string errorString;
 

src/Wima/Snake/NemoInterface.cpp

@@ -213,6 +213,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
   this->pRosBridge->subscribe(
       "/nemo/heartbeat",
       /* callback */ [this](JsonDocUPtr pDoc) {
+        //        auto start = std::chrono::high_resolution_clock::now();
         UniqueLock lk(this->heartbeatMutex);
 
         auto &heartbeatMsg = this->heartbeat;
@@ -223,8 +224,15 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
           this->nextTimeout =
               std::chrono::high_resolution_clock::now() + timeoutInterval;
         }
+        lk.unlock();
 
         emit this->parent->statusChanged();
+        //        auto delta =
+        //        std::chrono::duration_cast<std::chrono::milliseconds>(
+        //            std::chrono::high_resolution_clock::now() - start);
+        //        std::cout << "/nemo/heartbeat callback time: " <<
+        //        delta.count() << " ms"
+        //                  << std::endl;
       });
 
   // Advertise /snake/get_origin.
@@ -289,11 +297,12 @@ void NemoInterface::Impl::loop() {
   }
 
   // Check if heartbeat timeout occured.
-  if (this->running && this->topicServiceSetupDone &&
-      this->nextTimeout != TimePoint::max()) {
-    if (this->nextTimeout < std::chrono::high_resolution_clock::now()) {
+  if (this->running && this->topicServiceSetupDone) {
     UniqueLock lk(this->heartbeatMutex);
+    if (this->nextTimeout != TimePoint::max() &&
+        this->nextTimeout < std::chrono::high_resolution_clock::now()) {
       this->heartbeat.setStatus(integral(NemoStatus::Timeout));
+      lk.unlock();
       emit this->parent->statusChanged();
     }
   }
@@ -321,6 +330,13 @@ void NemoInterface::Impl::publishENUOrigin() {
   this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
 }
 
+// ===============================================================
+// NemoInterface
+NemoInterface::NemoInterface(QObject *parent)
+    : QObject(parent), pImpl(std::make_unique<NemoInterface::Impl>(this)) {}
+
+NemoInterface::~NemoInterface() {}
+
 void NemoInterface::start() { this->pImpl->start(); }
 
 void NemoInterface::stop() { this->pImpl->stop(); }
@@ -333,8 +349,8 @@ void NemoInterface::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
   this->pImpl->setENUOrigin(ENUOrigin);
 }
 
-NemoInterface::NemoStatus NemoInterface::status() {
+NemoInterface::NemoStatus NemoInterface::status() const {
   return this->pImpl->status();
 }
 
-QVector<int> NemoInterface::progress() { return this->pImpl->progress(); }
+QVector<int> NemoInterface::progress() const { return this->pImpl->progress(); }

src/Wima/Snake/NemoInterface.h

@@ -21,6 +21,7 @@ public:
   };
 
   explicit NemoInterface(QObject *parent = nullptr);
+  ~NemoInterface() override;
 
   void start();
   void stop();
@@ -28,8 +29,8 @@ public:
   void setTilesENU(const SnakeTilesLocal &tilesENU);
   void setENUOrigin(const QGeoCoordinate &ENUOrigin);
 
-  NemoStatus status();
-  QVector<int> progress();
+  NemoStatus status() const;
+  QVector<int> progress() const;
 
 signals:
   void statusChanged();

src/Wima/Snake/SnakeDataManager.h → src/Wima/Snake/SnakeThread.h

@@ -15,29 +15,34 @@ using namespace boost::units;
 using Length = quantity<si::length>;
 using Area = quantity<si::area>;
 
-class SnakeDataManager : public QThread {
+class SnakeThread : public QThread {
   Q_OBJECT
 
   class Impl;
   using PImpl = std::unique_ptr<Impl>;
 
 public:
-  SnakeDataManager(QObject *parent = nullptr);
-  ~SnakeDataManager() override;
+  SnakeThread(QObject *parent = nullptr);
+  ~SnakeThread() override;
 
   void setMeasurementArea(const QList<QGeoCoordinate> &measurementArea);
   void setServiceArea(const QList<QGeoCoordinate> &serviceArea);
   void setCorridor(const QList<QGeoCoordinate> &corridor);
+  void setProgress(const QVector<int> &progress);
 
   SnakeTiles tiles() const;
   SnakeTilesLocal tilesENU() const;
   QGeoCoordinate ENUOrigin() const;
   QVariantList tileCenterPoints() const;
-  QNemoProgress progress() const;
+  QVector<int> progress() const;
   bool calcInProgress() const;
   QString errorMessage() const;
   bool success() const;
 
+  bool tilesUpdated();
+  bool waypointsUpdated();
+  bool progressUpdated();
+
   QVector<QGeoCoordinate> waypoints() const;
   QVector<QGeoCoordinate> arrivalPath() const;
   QVector<QGeoCoordinate> returnPath() const;

src/Wima/Snake/SnakeTile.cpp

 #include "SnakeTile.h"
 
-SnakeTile::SnakeTile() : WimaAreaData()
-{
+SnakeTile::SnakeTile() : WimaAreaData() {}
 
-}
-
-SnakeTile::SnakeTile(const SnakeTile &other) : WimaAreaData()
-{
+SnakeTile::SnakeTile(const SnakeTile &other, QObject *parent)
+    : WimaAreaData(parent) {
   *this = other;
 }
 
-SnakeTile &SnakeTile::operator=(const SnakeTile &other)
-{
+SnakeTile &SnakeTile::operator=(const SnakeTile &other) {
   this->assign(other);
   return *this;
-
 }
 
-void SnakeTile::assign(const SnakeTile &other)
-{
-    WimaAreaData::assign(other);
-}
+void SnakeTile::assign(const SnakeTile &other) { WimaAreaData::assign(other); }

src/Wima/Snake/SnakeTile.h

@@ -2,18 +2,16 @@
 
 #include "Wima/Geometry/WimaAreaData.h"
 
-class SnakeTile : public WimaAreaData
-{
+class SnakeTile : public WimaAreaData {
 public:
   SnakeTile();
-    SnakeTile(const SnakeTile&other);
+  SnakeTile(const SnakeTile &other, QObject *parent = nullptr);
 
-    QString type() const {return "Tile";}
-    SnakeTile* Clone() const {return new SnakeTile(*this);}
+  QString type() const { return "Tile"; }
+  SnakeTile *Clone() const { return new SnakeTile(*this); }
 
-    SnakeTile& operator=(const SnakeTile &other);
+  SnakeTile &operator=(const SnakeTile &other);
 
 protected:
   void assign(const SnakeTile &other);
 };
-

src/Wima/WimaController.cc

@@ -11,6 +11,11 @@
 
 #include <memory>
 
+template <typename T>
+constexpr typename std::underlying_type<T>::type integral(T value) {
+  return static_cast<typename std::underlying_type<T>::type>(value);
+}
+
 #define SMART_RTL_MAX_ATTEMPTS 3       // times
 #define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
 #define EVENT_TIMER_INTERVAL 50        // ms
@@ -73,10 +78,11 @@ WimaController::WimaController(QObject *parent)
       _snakeMinTransectLength(settingsGroup,
                               _metaDataMap[snakeMinTransectLengthName]),
       _lowBatteryHandlingTriggered(false), _measurementPathLength(-1),
-      _currentDM(&_emptyDM),
+      _snakeThread(this), _emptyThread(this), _currentThread(&_emptyThread),
+      _nemoInterface(this),
       _batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) {
 
-  // Set up facts.
+  // Set up facts for waypoint manager.
   _showAllMissionItems.setRawValue(true);
   _showCurrentMissionItems.setRawValue(true);
   connect(&_overlapWaypoints, &Fact::rawValueChanged, this,
@@ -112,21 +118,23 @@ WimaController::WimaController(QObject *parent)
           &WimaController::_eventTimerHandler);
   _eventTimer.start(EVENT_TIMER_INTERVAL);
 
-  // Snake Data Manager.
-  connect(_currentDM, &SnakeDataManager::finished, this,
-          &WimaController::_DMFinishedHandler);
-  connect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
+  // SnakeThread.
+  connect(_currentThread, &SnakeThread::finished, this,
+          &WimaController::_threadFinishedHandler);
+  connect(_currentThread, &SnakeThread::calcInProgressChanged, this,
+          &WimaController::snakeCalcInProgressChanged);
+  connect(this, &QObject::destroyed, &this->_snakeThread, &SnakeThread::quit);
+  connect(this, &QObject::destroyed, &this->_emptyThread, &SnakeThread::quit);
+
+  // NemoInterface.
+  connect(&_nemoInterface, &NemoInterface::progressChanged, this,
           &WimaController::_progressChangedHandler);
-  connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
+  connect(&_nemoInterface, &NemoInterface::statusChanged, this,
           &WimaController::nemoStatusChanged);
-  connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
+  connect(&_nemoInterface, &NemoInterface::statusChanged, 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);
 
+  // Enable/disable snake.
   connect(&_enableSnake, &Fact::rawValueChanged, this,
           &WimaController::_enableSnakeChangedHandler);
   _enableSnakeChangedHandler();
@@ -185,16 +193,14 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
 
 Fact *WimaController::altitude() { return &_altitude; }
 
-QmlObjectListModel *WimaController::snakeTiles() {
-  return const_cast<QmlObjectListModel *>(this->_currentDM->tiles());
-}
+QmlObjectListModel *WimaController::snakeTiles() { return &this->tiles; }
 
 QVariantList WimaController::snakeTileCenterPoints() {
-  return _currentDM->tileCenterPoints();
+  return _currentThread->tileCenterPoints();
 }
 
 QVector<int> WimaController::nemoProgress() {
-  return _currentDM->progress().progress();
+  return _currentThread->progress();
 }
 
 double WimaController::phaseDistance() const {
@@ -207,14 +213,16 @@ double WimaController::phaseDuration() const {
   return 0.0;
 }
 
-int WimaController::nemoStatus() const { return _currentDM->nemoStatus(); }
+int WimaController::nemoStatus() const {
+  return integral(_nemoInterface.status());
+}
 
 QString WimaController::nemoStatusString() const {
   return _nemoStatusMap.at(nemoStatus());
 }
 
 bool WimaController::snakeCalcInProgress() const {
-  return _currentDM->calcInProgress();
+  return _currentThread->calcInProgress();
 }
 
 void WimaController::setMasterController(PlanMasterController *masterC) {
@@ -432,10 +440,10 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData) {
   emit currentWaypointPathChanged();
 
   // Update Snake Data Manager
-  _snakeDM.setMeasurementArea(_measurementArea.coordinateList());
-  _snakeDM.setServiceArea(_serviceArea.coordinateList());
-  _snakeDM.setCorridor(_corridor.coordinateList());
-  _currentDM->start();
+  _snakeThread.setMeasurementArea(_measurementArea.coordinateList());
+  _snakeThread.setServiceArea(_serviceArea.coordinateList());
+  _snakeThread.setCorridor(_corridor.coordinateList());
+  _currentThread->start();
 
   _localPlanDataValid = true;
   return true;
@@ -710,16 +718,32 @@ void WimaController::_initSmartRTL() {
   }
 }
 
-void WimaController::_DMFinishedHandler() {
-  if (!_snakeDM.success()) {
-    qDebug() << _snakeDM.errorMessage();
-    // qgcApp()->showMessage(_snakeDM.errorMessage());
-    return;
+void WimaController::_threadFinishedHandler() {
+  if (!_snakeThread.errorMessage().isEmpty()) {
+    qDebug() << _snakeThread.errorMessage();
+  }
+
+  if (_snakeThread.tilesUpdated()) {
+    this->tiles.clearAndDeleteContents();
+    auto tls = _snakeThread.tiles().polygons();
+    for (const auto &t : tls) {
+      this->tiles.append(new SnakeTile(t, &tiles));
+    }
+    emit snakeTilesChanged();
+    emit snakeTileCenterPointsChanged();
+
+    _nemoInterface.setTilesENU(_snakeThread.tilesENU());
+    _nemoInterface.setENUOrigin(_snakeThread.ENUOrigin());
+  }
+
+  if (_snakeThread.progressUpdated()) {
+    emit nemoProgressChanged();
   }
 
+  if (_snakeThread.waypointsUpdated()) {
     // Copy waypoints to waypoint manager.
     _snakeWM.clear();
-  auto waypoints = _snakeDM.waypoints();
+    auto waypoints = _snakeThread.waypoints();
     if (waypoints.size() < 1) {
       return;
     }
@@ -730,13 +754,11 @@ void WimaController::_DMFinishedHandler() {
     // Do update.
     this->_snakeWM.update(); // this can take a while (ca. 200ms)
 
-  emit snakeTilesChanged();
-  emit snakeTileCenterPointsChanged();
-  emit nemoProgressChanged();
     emit missionItemsChanged();
     emit currentMissionItemsChanged();
     emit currentWaypointPathChanged();
     emit waypointPathChanged();
+  }
 }
 
 void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
@@ -747,32 +769,35 @@ void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
   }
 }
 
-void WimaController::_switchDataManager(SnakeDataManager &dataManager) {
-  if (_currentDM != &dataManager) {
-
-    disconnect(_currentDM, &SnakeDataManager::finished, this,
-               &WimaController::_DMFinishedHandler);
-    disconnect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
+void WimaController::_switchThreadObject(SnakeThread &thread) {
+  if (_currentThread != &thread) {
+    // SnakeThread.
+    disconnect(_currentThread, &SnakeThread::finished, this,
+               &WimaController::_threadFinishedHandler);
+    disconnect(_currentThread, &SnakeThread::calcInProgressChanged, this,
+               &WimaController::snakeCalcInProgressChanged);
+    // NemoInterface.
+    disconnect(&_nemoInterface, &NemoInterface::progressChanged, this,
                &WimaController::_progressChangedHandler);
-    disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
+    disconnect(&_nemoInterface, &NemoInterface::statusChanged, this,
                &WimaController::nemoStatusChanged);
-    disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
+    disconnect(&_nemoInterface, &NemoInterface::statusChanged, this,
                &WimaController::nemoStatusStringChanged);
-    disconnect(_currentDM, &SnakeDataManager::calcInProgressChanged, this,
-               &WimaController::snakeCalcInProgressChanged);
 
-    _currentDM = &dataManager;
+    _currentThread = &thread;
 
-    connect(_currentDM, &SnakeDataManager::finished, this,
-            &WimaController::_DMFinishedHandler);
-    connect(_currentDM, &SnakeDataManager::nemoProgressChanged, this,
+    // SnakeThread.
+    connect(_currentThread, &SnakeThread::finished, this,
+            &WimaController::_threadFinishedHandler);
+    connect(_currentThread, &SnakeThread::calcInProgressChanged, this,
+            &WimaController::snakeCalcInProgressChanged);
+    // NemoInterface.
+    connect(&_nemoInterface, &NemoInterface::progressChanged, this,
             &WimaController::_progressChangedHandler);
-    connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
+    connect(&_nemoInterface, &NemoInterface::statusChanged, this,
             &WimaController::nemoStatusChanged);
-    connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this,
+    connect(&_nemoInterface, &NemoInterface::statusChanged, this,
             &WimaController::nemoStatusStringChanged);
-    connect(_currentDM, &SnakeDataManager::calcInProgressChanged, this,
-            &WimaController::snakeCalcInProgressChanged);
 
     emit snakeCalcInProgressChanged();
     emit snakeTilesChanged();
@@ -783,17 +808,21 @@ void WimaController::_switchDataManager(SnakeDataManager &dataManager) {
   }
 }
 
-void WimaController::_progressChangedHandler() { _snakeDM.start(); }
+void WimaController::_progressChangedHandler() {
+  this->_snakeThread.setProgress(this->_nemoInterface.progress());
+  this->_snakeThread.start();
+}
 
 void WimaController::_enableSnakeChangedHandler() {
   if (this->_enableSnake.rawValue().toBool()) {
     qDebug() << "WimaController: enabling snake.";
-    _switchDataManager(this->_snakeDM);
-    this->_snakeDM.enableRosBridge();
-    _currentDM->start();
+    _switchThreadObject(this->_snakeThread);
+    this->_nemoInterface.start();
+    this->_snakeThread.start();
   } else {
     qDebug() << "WimaController: disabling snake.";
-    this->_snakeDM.disableRosBride();
-    _switchDataManager(_emptyDM);
+    this->_nemoInterface.stop();
+    this->_snakeThread.quit();
+    _switchThreadObject(_emptyThread);
   }
 }

src/Wima/WimaController.h

@@ -27,7 +27,8 @@
 #include "SurveyComplexItem.h"
 
 #include "Geometry/GeoPoint3D.h"
-#include "Snake/SnakeDataManager.h"
+#include "Snake/NemoInterface.h"
+#include "Snake/SnakeThread.h"
 #include "Snake/SnakeTiles.h"
 #include "Snake/SnakeTilesLocal.h"
 
@@ -237,19 +238,16 @@ private slots:
   void _initSmartRTL();
   void _smartRTLCleanUp(bool flying);
   // Snake.
-  void _DMFinishedHandler();
+  void _threadFinishedHandler();
+  void _switchWaypointManager(WaypointManager::ManagerBase &manager);
   void _switchToSnakeWaypointManager(QVariant variant);
-  void _switchDataManager(SnakeDataManager &dataManager);
+  void _switchThreadObject(SnakeThread &thread);
   void _progressChangedHandler();
   void _enableSnakeChangedHandler();
   // Periodic tasks.
   void _eventTimerHandler(void);
-  // Waypoint manager handling.
-  void _switchWaypointManager(WaypointManager::ManagerBase &manager);
 
 private:
-  using StatusMap = std::map<int, QString>;
-
   // Controllers.
   PlanMasterController *_masterController;
   MissionController *_missionController;
@@ -310,9 +308,12 @@ private:
   double _measurementPathLength; // the lenght of the phase in meters
 
   // Snake
-  SnakeDataManager _snakeDM; // Snake Data Manager
-  SnakeDataManager _emptyDM;
-  SnakeDataManager *_currentDM;
+  QmlObjectListModel tiles;
+  SnakeThread _snakeThread; // Snake Data Manager
+  SnakeThread _emptyThread;
+  SnakeThread *_currentThread;
+  NemoInterface _nemoInterface;
+  using StatusMap = std::map<int, QString>;
   static StatusMap _nemoStatusMap;
 
   // Periodic tasks.

src/comm/ros_bridge/include/com_private.h

@@ -2,8 +2,8 @@
 
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
 
-#include <memory>
 #include <deque>
+#include <memory>
 #include <unordered_map>
 
 namespace ros_bridge {
@@ -16,11 +16,6 @@ typedef std::size_t HashType;
 
 using ClientMap = std::unordered_map<HashType, std::string>;
 
-static const char* _topicAdvertiserKey = "topic_advertiser";
-static const char* _topicPublisherKey =  "topic_publisher";
-static const char* _serviceAdvertiserKey = "service_advertiser";
-static const char* _topicSubscriberKey = "topic_subscriber";
-
 std::size_t getHash(const std::string &str);
 std::size_t getHash(const char *str);
 
@@ -30,5 +25,5 @@ bool getType(const JsonDoc &doc, std::string &type);
 bool removeTopic(JsonDoc &doc);
 bool removeType(JsonDoc &doc);
 
-}
-}
+} // namespace com_private
+} // namespace ros_bridge

src/comm/ros_bridge/include/server.cpp

@@ -2,18 +2,15 @@
 
 #include "rapidjson/include/rapidjson/ostreamwrapper.h"
 
-ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc) :
-    _rbc(rbc)
-  , _stopped(std::make_shared<std::atomic_bool>(true))
-{
+static const char *serviceAdvertiserKey = "service_advertiser";
 
-}
+ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc)
+    : _rbc(rbc), _stopped(std::make_shared<std::atomic_bool>(true)) {}
 
-void ros_bridge::com_private::Server::advertiseService(const std::string &service,
-                                                     const std::string &type,
-                                                     const Server::CallbackType &userCallback)
-{
-    std::string clientName = _serviceAdvertiserKey + service;
+void ros_bridge::com_private::Server::advertiseService(
+    const std::string &service, const std::string &type,
+    const Server::CallbackType &userCallback) {
+  std::string clientName = serviceAdvertiserKey + service;
   auto it = _serviceMap.find(clientName);
   if (it != _serviceMap.end())
     return; // Service allready advertised.
@@ -21,74 +18,66 @@ void ros_bridge::com_private::Server::advertiseService(const std::string &servic
   _rbc.addClient(clientName);
 
   auto rbc = &_rbc;
-    _rbc.advertiseService(clientName, service, type,
-                          [userCallback, rbc](
-                          std::shared_ptr<WsClient::Connection>,
-                          std::shared_ptr<WsClient::InMessage> in_message){
+  _rbc.advertiseService(
+      clientName, service, type,
+      [userCallback, rbc](std::shared_ptr<WsClient::Connection>,
+                          std::shared_ptr<WsClient::InMessage> in_message) {
         // message->string() is destructive, so we have to buffer it first
         std::string messagebuf = in_message->string();
-        //std::cout << "advertiseServiceCallback(): Message Received: "
+        // std::cout << "advertiseServiceCallback(): Message Received: "
         // << messagebuf << std::endl;
 
         rapidjson::Document document;
-        if (document.Parse(messagebuf.c_str()).HasParseError())
-        {
-          std::cerr << "advertiseServiceCallback(): Error in parsing service request message: "
+        if (document.Parse(messagebuf.c_str()).HasParseError()) {
+          std::cerr << "advertiseServiceCallback(): Error in parsing service "
+                       "request message: "
                     << messagebuf << std::endl;
           return;
         }
 
-        if ( !document.HasMember("args") || !document["args"].IsObject()){
-            std::cerr << "advertiseServiceCallback(): message has no args member: "
+        if (!document.HasMember("args") || !document["args"].IsObject()) {
+          std::cerr
+              << "advertiseServiceCallback(): message has no args member: "
               << messagebuf << std::endl;
           return;
         }
 
-        if ( !document.HasMember("service") || !document["service"].IsString()){
-            std::cerr << "advertiseServiceCallback(): message has no service member: "
+        if (!document.HasMember("service") || !document["service"].IsString()) {
+          std::cerr
+              << "advertiseServiceCallback(): message has no service member: "
               << messagebuf << std::endl;
           return;
         }
 
-        if ( !document.HasMember("id") || !document["id"].IsString()){
+        if (!document.HasMember("id") || !document["id"].IsString()) {
           std::cerr << "advertiseServiceCallback(): message has no id member: "
                     << messagebuf << std::endl;
           return;
         }
         JsonDocUPtr pDoc(new JsonDoc());
         std::cout << "args member count: " << document["args"].MemberCount();
-        if ( document["args"].MemberCount() > 0)
+        if (document["args"].MemberCount() > 0)
           pDoc->CopyFrom(document["args"].Move(), document.GetAllocator());
         JsonDocUPtr pDocResponse = userCallback(std::move(pDoc));
 
         rbc->serviceResponse(
-                    document["service"].GetString(),
-                    document["id"].GetString(),
-                    pDocResponse->MemberCount() > 0 ? true : false,
-                    *pDocResponse);
+            document["service"].GetString(), document["id"].GetString(),
+            pDocResponse->MemberCount() > 0 ? true : false, *pDocResponse);
 
         return;
-
       });
 }
 
-ros_bridge::com_private::Server::~Server()
-{
-    this->reset();
-}
+ros_bridge::com_private::Server::~Server() { this->reset(); }
 
-void ros_bridge::com_private::Server::start()
-{
-    _stopped->store(false);
-}
+void ros_bridge::com_private::Server::start() { _stopped->store(false); }
 
-void ros_bridge::com_private::Server::reset()
-{
-    if ( _stopped->load() )
+void ros_bridge::com_private::Server::reset() {
+  if (_stopped->load())
     return;
   std::cout << "Server: _stopped->store(true)" << std::endl;
   _stopped->store(true);
-    for (const auto& item : _serviceMap){
+  for (const auto &item : _serviceMap) {
     std::cout << "Server: unadvertiseService " << item.second << std::endl;
     _rbc.unadvertiseService(item.second);
     std::cout << "Server: removeClient " << item.first << std::endl;

src/comm/ros_bridge/include/topic_publisher.cpp

 #include "topic_publisher.h"
 
-#include <unordered_map>
+static const char *topicAdvertiserKey = "topic_advertiser";
 
-ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc) :
-      _stopped(std::make_shared<std::atomic_bool>(true))
-    , _rbc(rbc)
-{
+#include <unordered_map>
 
-}
+ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc)
+    : _stopped(std::make_shared<std::atomic_bool>(true)), _rbc(rbc) {}
 
-ros_bridge::com_private::TopicPublisher::~TopicPublisher()
-{
-    this->reset();
-}
+ros_bridge::com_private::TopicPublisher::~TopicPublisher() { this->reset(); }
 
-void ros_bridge::com_private::TopicPublisher::start()
-{
-    if ( !_stopped->load() )  // start called while thread running.
+void ros_bridge::com_private::TopicPublisher::start() {
+  if (!_stopped->load()) // start called while thread running.
     return;
   _stopped->store(false);
-    _pThread = std::make_unique<std::thread>([this]{
+  _pThread = std::make_unique<std::thread>([this] {
     // Main Loop.
-        while( !this->_stopped->load() ){
+    while (!this->_stopped->load()) {
       std::unique_lock<std::mutex> lk(this->_mutex);
       // Check if new data available, wait if not.
-            if (this->_queue.empty()){
-                if ( _stopped->load() ) // Check condition again while holding the lock.
+      if (this->_queue.empty()) {
+        if (_stopped->load()) // Check condition again while holding the lock.
           break;
-                this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
+        this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter
+                            // in this case.
         continue;
       }
       // Pop message from queue.
@@ -41,7 +36,6 @@ void ros_bridge::com_private::TopicPublisher::start()
       assert(ret);
       (void)ret;
 
-
       // Debug
       rapidjson::StringBuffer sb;
       rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
@@ -49,18 +43,16 @@ void ros_bridge::com_private::TopicPublisher::start()
       std::cout << "message: " << sb.GetString() << std::endl;
       std::cout << "topic: " << topic << std::endl;
 
-
       ret = com_private::removeTopic(*pJsonDoc);
       assert(ret);
       (void)ret;
 
-            // Wait for topic (Rosbridge needs some time to process a advertise() request).
-            this->_rbc.waitForTopic(topic, [this]{
-                return this->_stopped->load();
-            });
+      // Wait for topic (Rosbridge needs some time to process a advertise()
+      // request).
+      this->_rbc.waitForTopic(topic, [this] { return this->_stopped->load(); });
 
       // Publish Json message.
-            if ( !this->_stopped->load() )
+      if (!this->_stopped->load())
         this->_rbc.publish(topic, *pJsonDoc);
     } // while loop
 #ifdef ROS_BRIDGE_DEBUG
@@ -69,22 +61,21 @@ void ros_bridge::com_private::TopicPublisher::start()
   });
 }
 
-void ros_bridge::com_private::TopicPublisher::reset()
-{
-    if ( _stopped->load() )  // stop called while thread not running.
+void ros_bridge::com_private::TopicPublisher::reset() {
+  if (_stopped->load()) // stop called while thread not running.
     return;
   std::unique_lock<std::mutex> lk(_mutex);
   _stopped->store(true);
   _cv.notify_one(); // Wake publisher thread.
   lk.unlock();
 
-    if ( !_pThread )
+  if (!_pThread)
     return;
   _pThread->join();
 
   lk.lock();
   // Tidy up.
-    for (auto& it :  this->_topicMap){
+  for (auto &it : this->_topicMap) {
     this->_rbc.unadvertise(it.first);
     this->_rbc.removeClient(it.second);
   }
@@ -93,18 +84,16 @@ void ros_bridge::com_private::TopicPublisher::reset()
 }
 
 void ros_bridge::com_private::TopicPublisher::publish(
-        ros_bridge::com_private::JsonDocUPtr docPtr,
-        const char *topic)
-{
+    ros_bridge::com_private::JsonDocUPtr docPtr, const char *topic) {
   // Check if topic was advertised.
   std::string t(topic);
   std::unique_lock<std::mutex> lk(this->_mutex);
   auto it = this->_topicMap.find(t);
-    if ( it ==  this->_topicMap.end()) { // Not yet advertised?
+  if (it == this->_topicMap.end()) { // Not yet advertised?
     lk.unlock();
 #ifdef ROS_BRIDGE_DEBUG
-        std::cerr << "TopicPublisher: topic "
-                  << t << " not yet advertised" << std::endl;
+    std::cerr << "TopicPublisher: topic " << t << " not yet advertised"
+              << std::endl;
 #endif
     return;
   }
@@ -122,15 +111,13 @@ void ros_bridge::com_private::TopicPublisher::publish(
   _cv.notify_one(); // Wake publisher thread.
 }
 
-bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic, const char *type)
-{
+bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic,
+                                                        const char *type) {
   std::unique_lock<std::mutex> lk(this->_mutex);
   std::string t(topic);
   auto it = this->_topicMap.find(t);
-    if ( it ==  this->_topicMap.end()) { // Need to advertise topic?
-        std::string clientName =
-                std::string(ros_bridge::com_private::_topicAdvertiserKey)
-                + t;
+  if (it == this->_topicMap.end()) { // Need to advertise topic?
+    std::string clientName = std::string(topicAdvertiserKey) + t;
     this->_topicMap.insert(std::make_pair(t, clientName));
     lk.unlock();
     this->_rbc.addClient(clientName);
@@ -139,9 +126,9 @@ bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic, const
   } else {
     lk.unlock();
 #if ROS_BRIDGE_DEBUG
-        std::cerr << "TopicPublisher: topic " << topic << " already advertised" << std::endl;
+    std::cerr << "TopicPublisher: topic " << topic << " already advertised"
+              << std::endl;
 #endif
     return false;
   }
 }
-

src/comm/ros_bridge/include/topic_subscriber.cpp

@@ -2,29 +2,23 @@
 
 #include <thread>
 
-ros_bridge::com_private::TopicSubscriber::TopicSubscriber(RosbridgeWsClient &rbc) :
-    _rbc(rbc)
-  , _stopped(std::make_shared<std::atomic_bool>(true))
-{
+static const char *topicSubscriberKey = "topic_subscriber";
 
-}
+ros_bridge::com_private::TopicSubscriber::TopicSubscriber(
+    RosbridgeWsClient &rbc)
+    : _rbc(rbc), _stopped(std::make_shared<std::atomic_bool>(true)) {}
 
-ros_bridge::com_private::TopicSubscriber::~TopicSubscriber()
-{
-    this->reset();
-}
+ros_bridge::com_private::TopicSubscriber::~TopicSubscriber() { this->reset(); }
 
-void ros_bridge::com_private::TopicSubscriber::start()
-{
+void ros_bridge::com_private::TopicSubscriber::start() {
   _stopped->store(false);
 }
 
-void ros_bridge::com_private::TopicSubscriber::reset()
-{
-    if ( !_stopped->load() ){
+void ros_bridge::com_private::TopicSubscriber::reset() {
+  if (!_stopped->load()) {
     _stopped->store(true);
     {
-            for (auto &item : _topicMap){
+      for (auto &item : _topicMap) {
         _rbc.unsubscribe(item.second);
         _rbc.removeClient(item.first);
       }
@@ -34,37 +28,31 @@ void ros_bridge::com_private::TopicSubscriber::reset()
 }
 
 void ros_bridge::com_private::TopicSubscriber::subscribe(
-        const char *topic,
-        const std::function<void(JsonDocUPtr)> &callback)
-{
-    if ( _stopped->load() )
+    const char *topic, const std::function<void(JsonDocUPtr)> &callback) {
+  if (_stopped->load())
     return;
 
-    std::string clientName = ros_bridge::com_private::_topicSubscriberKey
-                           + std::string(topic);
+  std::string clientName = topicSubscriberKey + std::string(topic);
   auto it = _topicMap.find(clientName);
-    if ( it != _topicMap.end() ){ // Topic already subscribed?
+  if (it != _topicMap.end()) { // Topic already subscribed?
     return;
   }
   _topicMap.insert(std::make_pair(clientName, std::string(topic)));
 
   // Wrap callback.
-    auto callbackWrapper = [callback](
-            std::shared_ptr<WsClient::Connection>,
-            std::shared_ptr<WsClient::InMessage> in_message)
-    {
+  auto callbackWrapper =
+      [callback](std::shared_ptr<WsClient::Connection>,
+                 std::shared_ptr<WsClient::InMessage> in_message) {
         // Parse document.
         JsonDoc docFull;
         docFull.Parse(in_message->string().c_str());
-        if ( docFull.HasParseError() ) {
-            std::cout << "Json document has parse error: "
-                      << in_message->string()
+        if (docFull.HasParseError()) {
+          std::cout << "Json document has parse error: " << in_message->string()
                     << std::endl;
           return;
         } else if (!docFull.HasMember("msg")) {
           std::cout << "Json document does not contain a message (\"msg\"): "
-                      << in_message->string()
-                      << std::endl;
+                    << in_message->string() << std::endl;
           return;
         }
 
@@ -74,26 +62,26 @@ void ros_bridge::com_private::TopicSubscriber::subscribe(
         callback(std::move(pDoc));
       };
 
-    if ( !_rbc.topicAvailable(topic) ){
+  if (!_rbc.topicAvailable(topic)) {
     // Wait until topic is available.
-        std::cout << "TopicSubscriber: Starting wait thread, " << clientName << std::endl;
-        std::thread t([this, clientName, topic, callbackWrapper]{
-            this->_rbc.waitForTopic(topic, [this]{
-                return this->_stopped->load();
-            });
-            if ( !this->_stopped->load() ){
+    std::cout << "TopicSubscriber: Starting wait thread, " << clientName
+              << std::endl;
+    std::thread t([this, clientName, topic, callbackWrapper] {
+      this->_rbc.waitForTopic(topic, [this] { return this->_stopped->load(); });
+      if (!this->_stopped->load()) {
         this->_rbc.addClient(clientName);
         this->_rbc.subscribe(clientName, topic, callbackWrapper);
-                std::cout << "TopicSubscriber: wait thread subscription successfull: " << clientName << std::endl;
+        std::cout << "TopicSubscriber: wait thread subscription successfull: "
+                  << clientName << std::endl;
       }
-            std::cout << "TopicSubscriber: wait thread end, " << clientName << std::endl;
+      std::cout << "TopicSubscriber: wait thread end, " << clientName
+                << std::endl;
     });
     t.detach();
   } else {
     _rbc.addClient(clientName);
     _rbc.subscribe(clientName, topic, callbackWrapper);
-        std::cout << "TopicSubscriber: subscription successfull: " << clientName << std::endl;
+    std::cout << "TopicSubscriber: subscription successfull: " << clientName
+              << std::endl;
   }
 }
-
-

src/ui/MainWindowInner.qml

@@ -29,6 +29,7 @@ Item {
 
     QGCPalette { id: qgcPal; colorGroupEnabled: true }
 
+    property real   fps: 0.0
     property var    currentPopUp:       null
     property real   currentCenterX:     0
     property var    activeVehicle:      QGroundControl.multiVehicleManager.activeVehicle

src/ui/MainWindowNative.qml

@@ -39,6 +39,7 @@ Window {
     Loader {
         id:             mainWindowInner
         anchors.fill:   parent
+        anchors.margins: 10
         source:         "MainWindowInner.qml"
 
         Connections {