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,27 +805,32 @@ 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) {
     std::stringstream ss;

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.cpp

+#include "NemoInterface.h"
+
+#include "QGCApplication.h"
+#include "QGCToolbox.h"
+#include "SettingsFact.h"
+#include "SettingsManager.h"
+#include "WimaSettings.h"
+
+#include <shared_mutex>
+
+#include <QTimer>
+
+#include "QNemoHeartbeat.h"
+#include "QNemoProgress.h"
+
+#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
+#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
+#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
+#include "ros_bridge/include/messages/nemo_msgs/progress.h"
+#include "ros_bridge/include/ros_bridge.h"
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
+#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
+
+#define EVENT_TIMER_INTERVAL 100 // ms
+auto static timeoutInterval = std::chrono::milliseconds(3000);
+
+using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
+using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
+using UniqueLock = std::unique_lock<std::shared_timed_mutex>;
+using SharedLock = std::shared_lock<std::shared_timed_mutex>;
+using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
+
+class NemoInterface::Impl {
+  using TimePoint = std::chrono::time_point<std::chrono::high_resolution_clock>;
+
+public:
+  Impl(NemoInterface *p);
+
+  void start();
+  void stop();
+  void setTilesENU(const SnakeTilesLocal &tilesENU);
+  void setENUOrigin(const QGeoCoordinate &ENUOrigin);
+  NemoInterface::NemoStatus status();
+  QVector<int> progress();
+
+private:
+  bool doTopicServiceSetup();
+  void loop();
+  //!
+  //! \brief Publishes tilesENU
+  //! \pre this->tilesENUMutex must be locked
+  //!
+  void publishTilesENU();
+  //!
+  //! \brief Publishes ENUOrigin
+  //! \pre this->ENUOriginMutex must be locked
+  //!
+  void publishENUOrigin();
+
+  // Data.
+  SnakeTilesLocal tilesENU;
+  mutable std::shared_timed_mutex tilesENUMutex;
+  QGeoCoordinate ENUOrigin;
+  mutable std::shared_timed_mutex ENUOriginMutex;
+  QNemoProgress progress;
+  mutable std::shared_timed_mutex progressMutex;
+  QNemoHeartbeat heartbeat;
+  TimePoint nextTimeout;
+  mutable std::shared_timed_mutex heartbeatMutex;
+
+  // Internals
+  bool running;
+  std::atomic_bool topicServiceSetupDone;
+  ROSBridgePtr pRosBridge;
+  QTimer loopTimer;
+  NemoInterface *parent;
+};
+
+NemoInterface::Impl::Impl(NemoInterface *p)
+    : nextTimeout(TimePoint::max()), running(false),
+      topicServiceSetupDone(false), parent(p) {
+
+  // ROS Bridge.
+  WimaSettings *wimaSettings =
+      qgcApp()->toolbox()->settingsManager()->wimaSettings();
+  auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
+  auto setConnectionString = [connectionStringFact, this] {
+    auto connectionString = connectionStringFact->rawValue().toString();
+    if (ros_bridge::isValidConnectionString(
+            connectionString.toLocal8Bit().data())) {
+      this->pRosBridge.reset(
+          new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
+    } else {
+      QString defaultString("localhost:9090");
+      qgcApp()->showMessage("ROS Bridge connection string invalid: " +
+                            connectionString);
+      qgcApp()->showMessage("Resetting connection string to: " + defaultString);
+      connectionStringFact->setRawValue(
+          QVariant(defaultString)); // calls this function recursively
+    }
+  };
+  connect(connectionStringFact, &SettingsFact::rawValueChanged,
+          setConnectionString);
+  setConnectionString();
+
+  // Periodic.
+  connect(&this->loopTimer, &QTimer::timeout, this, &NemoInterface::Impl::loop);
+  this->loopTimer.start(EVENT_TIMER_INTERVAL);
+}
+
+void NemoInterface::Impl::start() { this->running = true; }
+
+void NemoInterface::Impl::stop() { this->running = false; }
+
+void NemoInterface::Impl::setTilesENU(const SnakeTilesLocal &tilesENU) {
+  UniqueLock lk(this->tilesENUMutex);
+  this->tilesENU = tilesENU;
+  lk.unlock();
+  if (this->running && this->topicServiceSetupDone) {
+    lk.lock();
+    this->publishTilesENU();
+  }
+}
+
+void NemoInterface::Impl::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
+  UniqueLock lk(this->ENUOriginMutex);
+  this->ENUOrigin = ENUOrigin;
+  lk.unlock();
+  if (this->running && this->topicServiceSetupDone) {
+    lk.lock();
+    this->publishENUOrigin();
+  }
+}
+
+NemoInterface::NemoStatus NemoInterface::Impl::status() {
+  SharedLock lk(this->heartbeatMutex);
+  return NemoInterface::NemoStatus(heartbeat.status());
+}
+
+QVector<int> NemoInterface::Impl::progress() {
+  SharedLock lk(this->progressMutex);
+  return this->progress.progress();
+}
+
+bool NemoInterface::Impl::doTopicServiceSetup() {
+  using namespace ros_bridge::messages;
+
+  // Publish snake tiles.
+  UniqueLock lk(this->mutex);
+  {
+    SharedLock lk(this->tilesENUMutex);
+
+    if (this->tilesENU.polygons().size() == 0)
+      return false;
+    this->pRosBridge->advertiseTopic(
+        "/snake/tiles",
+        jsk_recognition_msgs::polygon_array::messageType().c_str());
+    this->publishTilesENU();
+  }
+
+  // Publish snake origin.
+  {
+    SharedLock lk(this->ENUOriginMutex);
+
+    this->pRosBridge->advertiseTopic(
+        "/snake/origin", geographic_msgs::geo_point::messageType().c_str());
+    this->publishENUOrigin();
+  }
+
+  // Subscribe nemo progress.
+  this->pRosBridge->subscribe(
+      "/nemo/progress",
+      /* callback */ [this](JsonDocUPtr pDoc) {
+        std::lock(this->progressMutex, this->tilesENUMutex,
+                  this->ENUOriginMutex);
+        UniqueLock lk1(this->progressMutex, std::adopt_lock);
+        UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
+        UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock);
+
+        int requiredSize = this->tilesENU.polygons().size();
+        auto &progressMsg = this->progress;
+        if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
+            progressMsg.progress().size() !=
+                requiredSize) { // Some error occured.
+          progressMsg.progress().clear();
+
+          // Publish snake origin.
+          JsonDocUPtr jOrigin(
+              std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+          bool ret = geographic_msgs::geo_point::toJson(
+              this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
+          Q_ASSERT(ret);
+          (void)ret;
+          this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
+          // Publish snake tiles.
+          JsonDocUPtr jSnakeTiles(
+              std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+          ret = jsk_recognition_msgs::polygon_array::toJson(
+              this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
+          Q_ASSERT(ret);
+          (void)ret;
+          this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
+        }
+        lk1.unlock();
+        lk2.unlock();
+        lk3.unlock();
+
+        emit this->parent->nemoProgressChanged();
+      });
+
+  // Subscribe /nemo/heartbeat.
+  this->pRosBridge->subscribe(
+      "/nemo/heartbeat",
+      /* callback */ [this](JsonDocUPtr pDoc) {
+        UniqueLock lk(this->heartbeatMutex);
+
+        auto &heartbeatMsg = this->heartbeat;
+        if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
+          heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
+          this->nextTimeout = TimePoint::max();
+        } else {
+          this->nextTimeout =
+              std::chrono::high_resolution_clock::now() + timeoutInterval;
+        }
+
+        emit this->parent->nemoStatusChanged(heartbeatMsg.status());
+      });
+
+  // Advertise /snake/get_origin.
+  this->pRosBridge->advertiseService(
+      "/snake/get_origin", "snake_msgs/GetOrigin",
+      [this](JsonDocUPtr) -> JsonDocUPtr {
+        using namespace ros_bridge::messages;
+        SharedLock lk(this->mutex);
+
+        JsonDocUPtr pDoc(
+            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        auto &origin = this->ENUOrigin;
+        rapidjson::Value jOrigin(rapidjson::kObjectType);
+        bool ret = geographic_msgs::geo_point::toJson(origin, jOrigin,
+                                                      pDoc->GetAllocator());
+        lk.unlock();
+        Q_ASSERT(ret);
+        (void)ret;
+        pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
+        return pDoc;
+      });
+
+  // Advertise /snake/get_tiles.
+  this->pRosBridge->advertiseService(
+      "/snake/get_tiles", "snake_msgs/GetTiles",
+      [this](JsonDocUPtr) -> JsonDocUPtr {
+        SharedLock lk(this->mutex);
+
+        JsonDocUPtr pDoc(
+            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
+        bool ret = jsk_recognition_msgs::polygon_array::toJson(
+            this->tilesENU, jSnakeTiles, pDoc->GetAllocator());
+        Q_ASSERT(ret);
+        (void)ret;
+        pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
+        return pDoc;
+      });
+
+  return true;
+}
+
+void NemoInterface::Impl::loop() {
+
+  // Check ROS Bridge status and do setup if necessary.
+  if (this->running) {
+    if (!this->pRosBridge->isRunning()) {
+      this->pRosBridge->start();
+    } else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
+               !this->topicServiceSetupDone) {
+      if (this->doTopicServiceSetup())
+        this->topicServiceSetupDone = true;
+    } else if (this->pRosBridge->isRunning() &&
+               !this->pRosBridge->connected() && this->topicServiceSetupDone) {
+      this->pRosBridge->reset();
+      this->pRosBridge->start();
+      this->topicServiceSetupDone = false;
+    }
+  } else if (this->pRosBridge->isRunning()) {
+    this->pRosBridge->reset();
+    this->topicServiceSetupDone = false;
+  }
+
+  // Check if heartbeat timeout occured.
+  if (this->running && this->topicServiceSetupDone &&
+      this->nextTimeout != TimePoint::max()) {
+    if (this->nextTimeout < std::chrono::high_resolution_clock::now()) {
+      UniqueLock lk(this->heartbeatMutex);
+      this->heartbeat.setStatus(NemoStatus::Timeout);
+      emit this->parent->statusChanged();
+    }
+  }
+}
+
+void NemoInterface::Impl::publishTilesENU() {
+  JsonDocUPtr jSnakeTiles(
+      std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+  ret = jsk_recognition_msgs::polygon_array::toJson(
+      this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
+  Q_ASSERT(ret);
+  (void)ret;
+  this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
+}
+
+void NemoInterface::Impl::publishENUOrigin() {
+  JsonDocUPtr jOrigin(
+      std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+  bool ret = geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
+                                                jOrigin->GetAllocator());
+  Q_ASSERT(ret);
+  (void)ret;
+  this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+}
+
+void NemoInterface::start() { this->pImpl->start(); }
+
+void NemoInterface::stop() { this->pImpl->stop(); }
+
+void NemoInterface::setTilesENU(const SnakeTilesLocal &tilesENU) {
+  this->pImpl->setTilesENU(tilesENU);
+}
+
+void NemoInterface::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
+  this->pImpl->setENUOrigin(ENUOrigin);
+}
+
+NemoInterface::NemoStatus NemoInterface::status() {
+  return this->pImpl->status();
+}
+
+QVector<int> NemoInterface::progress() {
+  return this->pImpl->progress.progress();
+}

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.cc

@@ -17,273 +17,75 @@
 #include "Wima/Snake/SnakeTilesLocal.h"
 #include "snake.h"
 
-#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
-#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
-#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
-#include "ros_bridge/include/messages/nemo_msgs/progress.h"
-#include "ros_bridge/include/ros_bridge.h"
-#include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
-#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
-
-#define EVENT_TIMER_INTERVAL 500 // ms
-#define TIMEOUT 3000             // ms
-
 using QVariantList = QList<QVariant>;
-using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
 
 using UniqueLock = std::unique_lock<shared_timed_mutex>;
 using SharedLock = std::shared_lock<shared_timed_mutex>;
-using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
 
 class SnakeDataManager::SnakeImpl {
 public:
-  SnakeImpl(SnakeDataManager *p)
-      : rosBridgeEnabeled(false), topicServiceSetupDone(false),
-        lineDistance(1 * si::meter), minTransectLength(1 * si::meter),
-        calcInProgress(false), parent(p) {
-    // ROS Bridge.
-    WimaSettings *wimaSettings =
-        qgcApp()->toolbox()->settingsManager()->wimaSettings();
-    auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
-    auto setConnectionString = [connectionStringFact, this] {
-      auto connectionString = connectionStringFact->rawValue().toString();
-      if (ros_bridge::isValidConnectionString(
-              connectionString.toLocal8Bit().data())) {
-        this->pRosBridge.reset(
-            new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
-      } else {
-        QString defaultString("localhost:9090");
-        qgcApp()->showMessage("ROS Bridge connection string invalid: " +
-                              connectionString);
-        qgcApp()->showMessage("Resetting connection string to: " +
-                              defaultString);
-        connectionStringFact->setRawValue(
-            QVariant(defaultString)); // calls this function recursively
-      }
-    };
-    connect(connectionStringFact, &SettingsFact::rawValueChanged,
-            setConnectionString);
-    setConnectionString();
-
-    // Periodic.
-    connect(&this->eventTimer, &QTimer::timeout, [this] {
-      if (this->rosBridgeEnabeled) {
-        if (!this->pRosBridge->isRunning()) {
-          this->pRosBridge->start();
-        } else if (this->pRosBridge->isRunning() &&
-                   this->pRosBridge->connected() &&
-                   !this->topicServiceSetupDone) {
-          if (this->doTopicServiceSetup())
-            this->topicServiceSetupDone = true;
-        } else if (this->pRosBridge->isRunning() &&
-                   !this->pRosBridge->connected() &&
-                   this->topicServiceSetupDone) {
-          this->pRosBridge->reset();
-          this->pRosBridge->start();
-          this->topicServiceSetupDone = false;
-        }
-      } else if (this->pRosBridge->isRunning()) {
-        this->pRosBridge->reset();
-        this->topicServiceSetupDone = false;
-      }
-    });
-    this->eventTimer.start(EVENT_TIMER_INTERVAL);
-  }
+  struct Input {
 
-  bool precondition() const;
-  void resetOutput();
-  bool doTopicServiceSetup();
-
-  // Private data.
-  ROSBridgePtr pRosBridge;
-  bool rosBridgeEnabeled;
-  bool topicServiceSetupDone;
-  QTimer eventTimer;
-  QTimer timeout;
-  mutable std::shared_timed_mutex mutex;
+    Input()
+        : tileWidth(5 * si::meter), tileHeight(5 * si::meter),
+          minTileArea(1 * si::meter * si::meter), lineDistance(2 * si::meter),
+          minTransectLength(1 * si::meter), scenarioChanged(true),
+          routeParametersChanged(true) {}
 
-  // Scenario
-  snake::Scenario scenario;
-  Length lineDistance;
-  Length minTransectLength;
     QList<QGeoCoordinate> mArea;
+    QGeoCoordinate ENUOrigin;
     QList<QGeoCoordinate> sArea;
     QList<QGeoCoordinate> corridor;
-  QGeoCoordinate ENUOrigin;
+    Length tileWidth;
+    Length tileHeight;
+    Area minTileArea;
+    std::atomic_bool scenarioChanged;
+
+    Length lineDistance;
+    Length minTransectLength;
+    std::atomic_bool routeParametersChanged;
+
+    mutable std::shared_timed_mutex mutex;
+  };
+  struct Output {
     SnakeTiles tiles;
     QmlObjectListModel tilesQml;
     QVariantList tileCenterPoints;
     SnakeTilesLocal tilesENU;
     QVector<QPointF> tileCenterPointsENU;
 
-  // Waypoints
     QVector<QGeoCoordinate> waypoints;
     QVector<QGeoCoordinate> arrivalPath;
     QVector<QGeoCoordinate> returnPath;
     QVector<QPointF> waypointsENU;
     QVector<QPointF> arrivalPathENU;
     QVector<QPointF> returnPathENU;
+
     QString errorMessage;
 
-  // Other
     std::atomic_bool calcInProgress;
-  QNemoProgress qProgress;
-  std::vector<int> progress;
-  QNemoHeartbeat heartbeat;
-  SnakeDataManager *parent;
-};
 
-bool SnakeDataManager::SnakeImpl::doTopicServiceSetup() {
-  using namespace ros_bridge::messages;
-  UniqueLock lk(this->mutex);
-
-  if (this->tilesENU.polygons().size() == 0)
-    return false;
-
-  // Publish snake origin.
-  this->pRosBridge->advertiseTopic(
-      "/snake/origin", geographic_msgs::geo_point::messageType().c_str());
-  JsonDocUPtr jOrigin(
-      std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-  bool ret = geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
-                                                jOrigin->GetAllocator());
-  Q_ASSERT(ret);
-  (void)ret;
-  this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
-
-  // Publish snake tiles.
-  this->pRosBridge->advertiseTopic(
-      "/snake/tiles",
-      jsk_recognition_msgs::polygon_array::messageType().c_str());
-  JsonDocUPtr jSnakeTiles(
-      std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-  ret = jsk_recognition_msgs::polygon_array::toJson(
-      this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
-  Q_ASSERT(ret);
-  (void)ret;
-  this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-
-  // Subscribe nemo progress.
-  this->pRosBridge->subscribe(
-      "/nemo/progress",
-      /* callback */ [this](JsonDocUPtr pDoc) {
-        UniqueLock lk(this->mutex);
-        int requiredSize = this->tilesENU.polygons().size();
-        auto &progressMsg = this->qProgress;
-        if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
-            progressMsg.progress().size() !=
-                requiredSize) { // Some error occured.
-          // Set progress to default.
-          progressMsg.progress().fill(0, requiredSize);
-
-          // Publish snake origin.
-          JsonDocUPtr jOrigin(
-              std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-          bool ret = geographic_msgs::geo_point::toJson(
-              this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
-          Q_ASSERT(ret);
-          (void)ret;
-          this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
-
-          // Publish snake tiles.
-          JsonDocUPtr jSnakeTiles(
-              std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-          ret = jsk_recognition_msgs::polygon_array::toJson(
-              this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
-          Q_ASSERT(ret);
-          (void)ret;
-          this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-        }
-        this->progress.clear();
-        for (const auto &p : progressMsg.progress()) {
-          this->progress.push_back(p);
-        }
-        lk.unlock();
+    mutable std::shared_timed_mutex mutex;
+  };
 
-        emit this->parent->nemoProgressChanged();
-      });
+  SnakeImpl(SnakeDataManager *p);
 
-  // Subscribe /nemo/heartbeat.
-  this->pRosBridge->subscribe(
-      "/nemo/heartbeat",
-      /* callback */ [this](JsonDocUPtr pDoc) {
-        UniqueLock lk(this->mutex);
-        if (this->timeout.isActive()) {
-          this->timeout.stop();
-        }
-        auto &heartbeatMsg = this->heartbeat;
-        if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
-          heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
-        }
-        this->timeout.singleShot(TIMEOUT, [this] {
-          UniqueLock lk(this->mutex);
-          this->heartbeat.setStatus(integral(NemoStatus::Timeout));
-          emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout));
-        });
-        emit this->parent->nemoStatusChanged(heartbeatMsg.status());
-      });
-
-  // Advertise /snake/get_origin.
-  this->pRosBridge->advertiseService(
-      "/snake/get_origin", "snake_msgs/GetOrigin",
-      [this](JsonDocUPtr) -> JsonDocUPtr {
-        using namespace ros_bridge::messages;
-        SharedLock lk(this->mutex);
-
-        JsonDocUPtr pDoc(
-            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        auto &origin = this->ENUOrigin;
-        rapidjson::Value jOrigin(rapidjson::kObjectType);
-        bool ret = geographic_msgs::geo_point::toJson(origin, jOrigin,
-                                                      pDoc->GetAllocator());
-        lk.unlock();
-        Q_ASSERT(ret);
-        (void)ret;
-        pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
-        return pDoc;
-      });
-
-  // Advertise /snake/get_tiles.
-  this->pRosBridge->advertiseService(
-      "/snake/get_tiles", "snake_msgs/GetTiles",
-      [this](JsonDocUPtr) -> JsonDocUPtr {
-        SharedLock lk(this->mutex);
-
-        JsonDocUPtr pDoc(
-            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
-        bool ret = jsk_recognition_msgs::polygon_array::toJson(
-            this->tilesENU, jSnakeTiles, pDoc->GetAllocator());
-        Q_ASSERT(ret);
-        (void)ret;
-        pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
-        return pDoc;
-      });
-
-  return true;
-}
+  bool precondition() const;
+  bool doTopicServiceSetup();
 
-bool SnakeDataManager::SnakeImpl::precondition() const { return true; }
+  // Internal data.
+  snake::Scenario scenario;
+  SnakeDataManager *parent;
 
-//!
-//! \brief Resets waypoint data.
-//! \pre this->_pImpl->mutex must be locked.
-void SnakeDataManager::SnakeImpl::resetOutput() {
-  this->waypoints.clear();
-  this->arrivalPath.clear();
-  this->returnPath.clear();
-  this->ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
-  this->waypointsENU.clear();
-  this->arrivalPathENU.clear();
-  this->returnPathENU.clear();
-  this->tilesQml.clearAndDeleteContents();
-  this->tiles.polygons().clear();
-  this->tileCenterPoints.clear();
-  this->tilesENU.polygons().clear();
-  this->tileCenterPointsENU.clear();
-  this->errorMessage.clear();
-}
+  Input input;
+  // Output
+  Output output;
+};
+
+SnakeDataManager::SnakeImpl::SnakeImpl(SnakeDataManager *p)
+    : rosBridgeEnabeled(false), topicServiceSetupDone(false), parent(p) {}
+
+bool SnakeDataManager::SnakeImpl::precondition() const { return true; }
 
 template <class Callable> class CommandRAII {
 public:
@@ -300,82 +102,84 @@ SnakeDataManager::SnakeDataManager(QObject *parent)
 
 {}
 
-SnakeDataManager::~SnakeDataManager() {}
+// SnakeDataManager::~SnakeDataManager() {}
 
 void SnakeDataManager::setMeasurementArea(
     const QList<QGeoCoordinate> &measurementArea) {
-  UniqueLock lk(this->pImpl->mutex);
-  this->pImpl->mArea = measurementArea;
-  for (auto &vertex : this->pImpl->mArea) {
+  this.input.scenarioChanged = true;
+  UniqueLock lk(this->pImpl->input.mutex);
+  this->pImpl->inptu.mArea = measurementArea;
+  for (auto &vertex : this->pImpl->input.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) {
+  this.input.scenarioChanged = true;
+  UniqueLock lk(this->pImpl->input.mutex);
+  this->pImpl->input.sArea = serviceArea;
+  for (auto &vertex : this->pImpl->input.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) {
+  this.input.scenarioChanged = true;
+  UniqueLock lk(this->pImpl->input.mutex);
+  this->pImpl->input.corridor = corridor;
+  for (auto &vertex : this->pImpl->input.corridor) {
     vertex.setAltitude(0);
   }
 }
 
 const QmlObjectListModel *SnakeDataManager::tiles() const {
-  SharedLock lk(this->pImpl->mutex);
-  return &this->pImpl->tilesQml;
+  SharedLock lk(this->pImpl->output.mutex);
+  return &this->pImpl->output.tilesQml;
 }
 
 QVariantList SnakeDataManager::tileCenterPoints() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->tileCenterPoints;
+  SharedLock lk(this->pImpl->output.mutex);
+  return this->pImpl->output.tileCenterPoints;
 }
 
 QNemoProgress SnakeDataManager::nemoProgress() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->qProgress;
+  SharedLock lk(this->pImpl->output.mutex);
+  return this->pImpl->output.qProgress;
 }
 
 int SnakeDataManager::nemoStatus() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->heartbeat.status();
+  SharedLock lk(this->pImpl->output.mutex);
+  return this->pImpl->output.heartbeat.status();
 }
 
 bool SnakeDataManager::calcInProgress() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->calcInProgress.load();
+  return this->pImpl->output.calcInProgress.load();
 }
 
 QString SnakeDataManager::errorMessage() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->errorMessage;
+  SharedLock lk(this->pImpl->output.mutex);
+  return this->pImpl->output.errorMessage;
 }
 
 bool SnakeDataManager::success() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->errorMessage.isEmpty() ? true : false;
+  SharedLock lk(this->pImpl->output.mutex);
+  return this->pImpl->output.errorMessage.isEmpty() ? true : false;
 }
 
 QVector<QGeoCoordinate> SnakeDataManager::waypoints() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->waypoints;
+  SharedLock lk(this->pImpl->output.mutex);
+  return this->pImpl->output.waypoints;
 }
 
 QVector<QGeoCoordinate> SnakeDataManager::arrivalPath() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->arrivalPath;
+  SharedLock lk(this->pImpl->output.mutex);
+  return this->pImpl->output.arrivalPath;
 }
 
 QVector<QGeoCoordinate> SnakeDataManager::returnPath() const {
-  SharedLock lk(this->pImpl->mutex);
-  return this->pImpl->returnPath;
+  SharedLock lk(this->pImpl->output.mutex);
+  return this->pImpl->output.returnPath;
 }
 
 Length SnakeDataManager::lineDistance() const {
@@ -385,6 +189,7 @@ Length SnakeDataManager::lineDistance() const {
 
 void SnakeDataManager::setLineDistance(Length lineDistance) {
   UniqueLock lk(this->pImpl->mutex);
+  routeParametersChanged = true;
   this->pImpl->lineDistance = lineDistance;
 }
 
@@ -394,6 +199,7 @@ Area SnakeDataManager::minTileArea() const {
 }
 
 void SnakeDataManager::setMinTileArea(Area minTileArea) {
+  scenarioChanged = true;
   UniqueLock lk(this->pImpl->mutex);
   this->pImpl->scenario.setMinTileArea(minTileArea);
 }
@@ -404,6 +210,7 @@ Length SnakeDataManager::tileHeight() const {
 }
 
 void SnakeDataManager::setTileHeight(Length tileHeight) {
+  scenarioChanged = true;
   UniqueLock lk(this->pImpl->mutex);
   this->pImpl->scenario.setTileHeight(tileHeight);
 }
@@ -415,6 +222,7 @@ Length SnakeDataManager::tileWidth() const {
 
 void SnakeDataManager::setTileWidth(Length tileWidth) {
   UniqueLock lk(this->pImpl->mutex);
+  scenarioChanged = true;
   this->pImpl->scenario.setTileWidth(tileWidth);
 }
 
@@ -429,7 +237,6 @@ 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);
@@ -447,8 +254,8 @@ void SnakeDataManager::run() {
   };
   CommandRAII<decltype(onExit)> onExitRAII(onExit);
 
-  UniqueLock lk(this->pImpl->mutex);
-  this->pImpl->resetOutput();
+  // Check preconditions.
+  SharedLock sLock(this->pImpl->mutex);
 
   if (!this->pImpl->precondition())
     return;
@@ -462,10 +269,12 @@ void SnakeDataManager::run() {
     return;
   }
 
+  // Update Scenario if necessary
+  if (this->pImpl->scenarioChanged) {
+    // Set Origin
     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;
@@ -500,57 +309,55 @@ void SnakeDataManager::run() {
       this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
       return;
     }
+  }
+  sLock.unlock;
 
-  // 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);
+  bool storeProgress = false;
+  bool storeRoute = false;
+  if (this->pImpl->scenarioChanged || this->pImpl->routeParametersChanged) {
+    storeRoute = true;
+    // Sample data
+    SharedLock lk(this->pImpl->mutex);
+    size_t nTiles = this->pImpl->tiles.polygons().size();
+    if (this->pImpl->progress.size() != nTiles) {
     }
-      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);
+    const auto scenario = this->pImpl->scenario;
+    std::vector<int> progress;
+    size_t nTiles = this->pImpl->tiles.polygons().size();
+    if (this->pImpl->progress.size() != nTiles) {
+      progress.reserve(nTiles);
+      for (size_t i = 0; i < nTiles; ++i) {
+        progress.push_back(0);
       }
+      storeProgress = true;
+    } else {
+      progress = this->pImpl->progress;
     }
+    const auto lineDistance = this->pImpl->lineDistance;
+    const auto minTransectLength = this->pImpl->minTransectLength;
+    lk.unlock();
 
     // Create transects.
     std::string errorString;
-  snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle *
-                     degree::degree);
+    snake::Angle alpha(-scenario.mAreaBoundingBox().angle * degree::degree);
     snake::flight_plan::Transects transects;
-  transects.push_back(bg::model::linestring<snake::BoostPoint>{
-      this->pImpl->scenario.homePositon()});
+    transects.push_back(
+        bg::model::linestring<snake::BoostPoint>{scenario.homePositon()});
+
     bool value = snake::flight_plan::transectsFromScenario(
-      this->pImpl->lineDistance, this->pImpl->minTransectLength, alpha,
-      this->pImpl->scenario, this->pImpl->progress, transects, errorString);
+        lineDistance, minTransectLength, alpha, scenario, progress, transects,
+        errorString);
     if (!value) {
+      UniqueLock lk(this->pImpl->mutex);
       this->pImpl->errorMessage = errorString.c_str();
-    return;
+      storeRoute = false;
     }
 
     // Route transects
+    if (transects.size() > 1) {
       snake::flight_plan::Transects transectsRouted;
       snake::flight_plan::Route route;
-  value =
-      snake::flight_plan::route(this->pImpl->scenario.joinedArea(), transects,
+      value = snake::flight_plan::route(scenario.joinedArea(), transects,
                                         transectsRouted, route, errorString);
       if (!value) {
         this->pImpl->errorMessage = errorString.c_str();
@@ -596,4 +403,51 @@ void SnakeDataManager::run() {
         this->pImpl->waypointsENU.push_back(enuVertex);
         this->pImpl->waypoints.push_back(geoVertex);
       }
+    }
+  }
+
+  UniqueLock uLock(this->pImpl->mutex);
+  // Store scenario.
+  if (this->pImpl->scenarioChanged) {
+    // Clear some output data.
+    this->pImpl->tiles.polygons().clear();
+    this->pImpl->tilesQml.clearAndDeleteContents();
+    this->pImpl->tileCenterPoints.clear();
+    this->pImpl->tilesENU.polygons().clear();
+    this->pImpl->tileCenterPointsENU.clear();
+    // Convert and store scenario data.
+    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 = 0; 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->tiles.polygons().push_back(geoTile);
+      auto *geoTileCopy = new SnakeTile(geoTile);
+      geoTileCopy->moveToThread(this->pImpl->tilesQml.thread());
+      this->pImpl->tilesQml.append(geoTileCopy);
+      this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
+      this->pImpl->tilesENU.polygons().push_back(enuTile);
+      this->pImpl->tileCenterPointsENU.push_back(enuVertex);
+    }
+  }
+  // Store route etc.
+  if (this->pImpl->scenarioChanged || this->pImpl->routeParametersChanged) {
+  }
+  uLock.unlock;
+  this->pImpl->scenarioChanged = false;
+  this->pImpl->routeParametersChanged = false;
 }

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/Snake/SnakeDataManager_old.cc

+#include "SnakeDataManager.h"
+
+#include <QGeoCoordinate>
+#include <QMutexLocker>
+
+#include "QGCApplication.h"
+#include "QGCToolbox.h"
+#include "SettingsFact.h"
+#include "SettingsManager.h"
+#include "WimaSettings.h"
+
+#include <memory>
+#include <mutex>
+#include <shared_mutex>
+
+#include "Wima/Snake/SnakeTiles.h"
+#include "Wima/Snake/SnakeTilesLocal.h"
+#include "snake.h"
+
+#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
+#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
+#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
+#include "ros_bridge/include/messages/nemo_msgs/progress.h"
+#include "ros_bridge/include/ros_bridge.h"
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
+#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
+
+#define EVENT_TIMER_INTERVAL 500 // ms
+#define TIMEOUT 3000             // ms
+
+using QVariantList = QList<QVariant>;
+using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
+
+using UniqueLock = std::unique_lock<shared_timed_mutex>;
+using SharedLock = std::shared_lock<shared_timed_mutex>;
+using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
+
+class SnakeDataManager::SnakeImpl {
+public:
+  SnakeImpl(SnakeDataManager *p)
+      : rosBridgeEnabeled(false), topicServiceSetupDone(false),
+        lineDistance(1 * si::meter), minTransectLength(1 * si::meter),
+        calcInProgress(false), parent(p) {
+
+  }
+
+  bool precondition() const;
+  void resetOutput();
+  bool doTopicServiceSetup();
+
+  // Private data.
+  ROSBridgePtr pRosBridge;
+  bool rosBridgeEnabeled;
+  bool topicServiceSetupDone;
+  QTimer eventTimer;
+  QTimer timeout;
+  mutable std::shared_timed_mutex mutex;
+
+  // Scenario
+  snake::Scenario scenario;
+  Length lineDistance;
+  Length minTransectLength;
+  QList<QGeoCoordinate> mArea;
+  QList<QGeoCoordinate> sArea;
+  QList<QGeoCoordinate> corridor;
+  QGeoCoordinate ENUOrigin;
+  SnakeTiles tiles;
+  QmlObjectListModel tilesQml;
+  QVariantList tileCenterPoints;
+  SnakeTilesLocal tilesENU;
+  QVector<QPointF> tileCenterPointsENU;
+
+  // Waypoints
+  QVector<QGeoCoordinate> waypoints;
+  QVector<QGeoCoordinate> arrivalPath;
+  QVector<QGeoCoordinate> returnPath;
+  QVector<QPointF> waypointsENU;
+  QVector<QPointF> arrivalPathENU;
+  QVector<QPointF> returnPathENU;
+  QString errorMessage;
+
+  // Other
+  std::atomic_bool calcInProgress;
+  QNemoProgress qProgress;
+  std::vector<int> progress;
+  QNemoHeartbeat heartbeat;
+  SnakeDataManager *parent;
+};
+
+
+
+bool SnakeDataManager::SnakeImpl::precondition() const { return true; }
+
+//!
+//! \brief Resets waypoint data.
+//! \pre this->_pImpl->mutex must be locked.
+void SnakeDataManager::SnakeImpl::resetOutput() {
+  this->waypoints.clear();
+  this->arrivalPath.clear();
+  this->returnPath.clear();
+  this->ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
+  this->waypointsENU.clear();
+  this->arrivalPathENU.clear();
+  this->returnPathENU.clear();
+  this->tilesQml.clearAndDeleteContents();
+  this->tiles.polygons().clear();
+  this->tileCenterPoints.clear();
+  this->tilesENU.polygons().clear();
+  this->tileCenterPointsENU.clear();
+  this->errorMessage.clear();
+}
+
+template <class Callable> class CommandRAII {
+public:
+  CommandRAII(Callable &fun) : _fun(fun) {}
+
+  ~CommandRAII() { _fun(); }
+
+private:
+  Callable _fun;
+};
+
+SnakeDataManager::SnakeDataManager(QObject *parent)
+    : QThread(parent), pImpl(std::make_unique<SnakeImpl>(this))
+
+{}
+
+SnakeDataManager::~SnakeDataManager() {}
+
+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 {
+  SharedLock lk(this->pImpl->mutex);
+  return &this->pImpl->tilesQml;
+}
+
+QVariantList SnakeDataManager::tileCenterPoints() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->tileCenterPoints;
+}
+
+QNemoProgress SnakeDataManager::nemoProgress() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->qProgress;
+}
+
+int SnakeDataManager::nemoStatus() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->heartbeat.status();
+}
+
+bool SnakeDataManager::calcInProgress() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->calcInProgress.load();
+}
+
+QString SnakeDataManager::errorMessage() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->errorMessage;
+}
+
+bool SnakeDataManager::success() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->errorMessage.isEmpty() ? true : false;
+}
+
+QVector<QGeoCoordinate> SnakeDataManager::waypoints() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->waypoints;
+}
+
+QVector<QGeoCoordinate> SnakeDataManager::arrivalPath() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->arrivalPath;
+}
+
+QVector<QGeoCoordinate> SnakeDataManager::returnPath() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->returnPath;
+}
+
+Length SnakeDataManager::lineDistance() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->lineDistance;
+}
+
+void SnakeDataManager::setLineDistance(Length lineDistance) {
+  UniqueLock lk(this->pImpl->mutex);
+  this->pImpl->lineDistance = lineDistance;
+}
+
+Area SnakeDataManager::minTileArea() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->scenario.minTileArea();
+}
+
+void SnakeDataManager::setMinTileArea(Area minTileArea) {
+  UniqueLock lk(this->pImpl->mutex);
+  this->pImpl->scenario.setMinTileArea(minTileArea);
+}
+
+Length SnakeDataManager::tileHeight() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->scenario.tileHeight();
+}
+
+void SnakeDataManager::setTileHeight(Length tileHeight) {
+  UniqueLock lk(this->pImpl->mutex);
+  this->pImpl->scenario.setTileHeight(tileHeight);
+}
+
+Length SnakeDataManager::tileWidth() const {
+  SharedLock lk(this->pImpl->mutex);
+  return this->pImpl->scenario.tileWidth();
+}
+
+void SnakeDataManager::setTileWidth(Length tileWidth) {
+  UniqueLock lk(this->pImpl->mutex);
+  this->pImpl->scenario.setTileWidth(tileWidth);
+}
+
+void SnakeDataManager::enableRosBridge() {
+  this->pImpl->rosBridgeEnabeled = true;
+}
+
+void SnakeDataManager::disableRosBride() {
+  this->pImpl->rosBridgeEnabeled = false;
+}
+
+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, &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());
+  };
+  CommandRAII<decltype(onExit)> onExitRAII(onExit);
+
+  UniqueLock lk(this->pImpl->mutex);
+  this->pImpl->resetOutput();
+
+  if (!this->pImpl->precondition())
+    return;
+
+  if (this->pImpl->mArea.size() < 3) {
+    this->pImpl->errorMessage = "Measurement area invalid: size < 3.";
+    return;
+  }
+  if (this->pImpl->sArea.size() < 3) {
+    this->pImpl->errorMessage = "Service area invalid: size < 3.";
+    return;
+  }
+
+  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;
+  mAreaEnu.clear();
+  for (auto geoVertex : mArea) {
+    snake::BoostPoint p;
+    snake::toENU(origin, geoVertex, p);
+    mAreaEnu.outer().push_back(p);
+  }
+
+  // Update service area.
+  auto &sAreaEnu = this->pImpl->scenario.serviceArea();
+  auto &sArea = this->pImpl->sArea;
+  sAreaEnu.clear();
+  for (auto geoVertex : sArea) {
+    snake::BoostPoint p;
+    snake::toENU(origin, geoVertex, p);
+    sAreaEnu.outer().push_back(p);
+  }
+
+  // Update corridor.
+  auto &corridorEnu = this->pImpl->scenario.corridor();
+  auto &corridor = this->pImpl->corridor;
+  corridor.clear();
+  for (auto geoVertex : corridor) {
+    snake::BoostPoint p;
+    snake::toENU(origin, geoVertex, p);
+    corridorEnu.outer().push_back(p);
+  }
+
+  if (!this->pImpl->scenario.update()) {
+    this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
+    return;
+  }
+
+  // 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;
+  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();
+    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

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