editing SnakeDataManager

qgroundcontrol.pro

@@ -431,23 +431,20 @@ HEADERS += \
     src/Snake/mapbox/variant_io.hpp \
     src/Snake/snake.h \
     src/Snake/snake_typedefs.h \
+    src/Wima/Geometry/GenericPolygon.h \
+    src/Wima/Geometry/GenericPolygonArray.h \
     src/Wima/Geometry/GeoPoint3D.h \
-    src/Wima/Geometry/Polygon2D.h \
-    src/Wima/Geometry/PolygonArray.h \
-    src/Wima/Snake/GeoPolygonArray.h \
-    src/Wima/Snake/GeoTile.h \
-    src/Wima/Snake/PolygonArray.h \
     src/Wima/Snake/QNemoHeartbeat.h \
     src/Wima/Snake/QNemoProgress.h \
     src/Wima/Snake/QNemoProgress.h \
+    src/Wima/Snake/SnakeTile.h \
+    src/Wima/Snake/SnakeTileLocal.h \
     src/Wima/Snake/SnakeTiles.h \
     src/Wima/Snake/SnakeTilesLocal.h \
-    src/Wima/Snake/SnakeWorker.h \
+    src/Wima/Snake/SnakeW.h \
     src/Wima/WaypointManager/AreaInterface.h \
     src/Wima/WaypointManager/DefaultManager.h \
     src/Wima/WaypointManager/GenericWaypointManager.h \
-    src/Wima/Geometry/WimaPolygonArray.h \
-    src/Wima/Snake/snaketile.h \
     src/Wima/WaypointManager/RTLManager.h \
     src/Wima/WaypointManager/Settings.h \
     src/Wima/WaypointManager/Slicer.h \
@@ -473,7 +470,6 @@ HEADERS += \
     src/Wima/WimaPlanData.h \
     src/Wima/Geometry/WimaJoinedArea.h \
     src/Wima/Geometry/WimaJoinedAreaData.h \
-    src/Wima/Geometry/SphereCalculus.h \
     src/Wima/CircularSurveyComplexItem.h \
     src/Wima/Geometry/PlanimetryCalculus.h \
     src/Wima/Geometry/Circle.h \
@@ -504,10 +500,9 @@ SOURCES += \
     src/Snake/clipper/clipper.cpp \
     src/Snake/snake.cpp \
     src/Wima/Geometry/GeoPoint3D.cpp \
-    src/Wima/Geometry/PolygonArray.cc \
-    src/Wima/Snake/GeoTile.cpp \
     src/Wima/Snake/QNemoProgress.cc \
-    src/Wima/Snake/SnakeWorker.cc \
+    src/Wima/Snake/SnakeDataManager.cc \
+    src/Wima/Snake/SnakeTile.cpp \
     src/Wima/WaypointManager/AreaInterface.cpp \
     src/Wima/WaypointManager/DefaultManager.cpp \
     src/Wima/WaypointManager/GenericWaypointManager.cpp \
@@ -530,7 +525,6 @@ SOURCES += \
     src/comm/ros_bridge/include/server.cpp \
     src/comm/ros_bridge/include/topic_publisher.cpp \
     src/comm/ros_bridge/include/topic_subscriber.cpp \
-    src/Wima/Snake/snaketile.cpp \
     src/api/QGCCorePlugin.cc \
     src/api/QGCOptions.cc \
     src/api/QGCSettings.cc \
@@ -550,7 +544,6 @@ SOURCES += \
     src/Wima/Geometry/WimaMeasurementAreaData.cc \
     src/Wima/Geometry/WimaJoinedArea.cc \
     src/Wima/Geometry/WimaJoinedAreaData.cc \
-    src/Wima/Geometry/SphereCalculus.cc \
     src/Wima/CircularSurveyComplexItem.cc \
     src/Wima/Geometry/PlanimetryCalculus.cc \
     src/Wima/Geometry/Circle.cc \

src/Snake/snake.cpp

@@ -446,6 +446,21 @@ const BoostPolygon &Scenario::corridor() const
     return _corridor;
 }
 
+BoostPolygon &Scenario::measurementArea()
+{
+    return _mArea;
+}
+
+BoostPolygon &Scenario::serviceArea()
+{
+    return _sArea;
+}
+
+BoostPolygon &Scenario::corridor()
+{
+    return _corridor;
+}
+
 const BoostPolygon &Scenario::joinedArea() const {
     return _jArea;
 }

src/Snake/snake.h

@@ -168,14 +168,18 @@ class Scenario{
         const BoostPolygon &serviceArea()const;
         const BoostPolygon &corridor()const;
 
-        double tileWidth() const;
-        void setTileWidth(double tileWidth);
+        BoostPolygon &measurementArea();
+        BoostPolygon &serviceArea();
+        BoostPolygon &corridor();
+
+        Length tileWidth() const;
+        void setTileWidth(Length tileWidth);
 
-        double tileHeight() const;
-        void setTileHeight(double tileHeight);
+        Length tileHeight() const;
+        void setTileHeight(Length tileHeight);
 
-        double minTileArea() const;
-        void setMinTileArea(double minTileArea);
+        Area minTileArea() const;
+        void setMinTileArea(Area minTileArea);
 
         const BoostPolygon         &joinedArea()            const;
         const vector<BoostPolygon> &tiles()                 const;
@@ -192,9 +196,9 @@ private:
         bool _calculateTiles();
         bool _calculateJoinedArea();
 
-        double _tileWidth;
-        double _tileHeight;
-        double _minTileArea;
+        Length  _tileWidth;
+        Length  _tileHeight;
+        Area    _minTileArea;
 
         mutable bool _needsUpdate;
 

src/Snake/snake_typedefs.h

@@ -3,8 +3,10 @@
 #include <vector>
 #include <array>
 #include <boost/geometry.hpp>
+#include <boost/units/systems/si.hpp>
 
 namespace bg = boost::geometry;
+namespace bu = boost::units;
 namespace snake {
 
 typedef bg::model::point<double, 2, bg::cs::cartesian>  BoostPoint;
@@ -13,4 +15,7 @@ typedef bg::model::polygon<BoostPoint>                  BoostPolygon;
 typedef bg::model::linestring<BoostPoint>               BoostLineString;
 typedef std::vector<std::vector<int64_t>>               Int64Matrix;
 
+typedef bu::quantity<bu::si::length>    Length;
+typedef bu::quantity<bu::si::area>      Area;
+
 }

src/Wima/Geometry/Polygon2D.h → src/Wima/Geometry/GenericPolygon.h

@@ -5,16 +5,14 @@
 
 #include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
 
-namespace polygon_stamped = ros_bridge::messages::geometry_msgs::polygon_stamped;
-
 template <class PointType = QPointF, template <class, class...> class ContainerType = QVector>
-class Polygon2DTemplate{ //
+class GenericPolygon{ //
     typedef ros_bridge::messages::geometry_msgs::polygon::GenericPolygon<PointType, ContainerType> Polygon;
 public:
-    Polygon2DTemplate(){}
-    Polygon2DTemplate(const Polygon2DTemplate &other) : _polygon(other._polygon){}
+    GenericPolygon(){}
+    GenericPolygon(const GenericPolygon &other) : _polygon(other._polygon){}
 
-    Polygon2DTemplate& operator=(const Polygon2DTemplate& other) {
+    GenericPolygon& operator=(const GenericPolygon& other) {
         this->_polygon = other._polygon;
         return *this;
     }
@@ -29,5 +27,3 @@ private:
     Polygon _polygon;
 
 };
-
-typedef Polygon2DTemplate<> Polygon2D;

src/Wima/Geometry/WimaPolygonArray.h → src/Wima/Geometry/GenericPolygonArray.h

@@ -6,14 +6,14 @@
 #include <QString>
 
 template <class PolygonType, template <class, class...> class ContainerType = QVector>
-class WimaPolygonArray
+class GenericPolygonArray
 {
 public:
-    WimaPolygonArray() {}
-    WimaPolygonArray(const WimaPolygonArray &other) :
+    GenericPolygonArray() {}
+    GenericPolygonArray(const GenericPolygonArray &other) :
         _polygons(other._polygons), _dirty(true)
     {}
-    ~WimaPolygonArray(){
+    ~GenericPolygonArray(){
         _objs.clearAndDeleteContents();
     }
 
@@ -33,7 +33,7 @@ public:
         return _polygons;
     }
 
-    WimaPolygonArray &operator =(const WimaPolygonArray &other){
+    GenericPolygonArray &operator =(const GenericPolygonArray &other){
         this->_polygons = other._polygons;
         this->_dirty = true;
         return *this;

src/Wima/Geometry/PolygonArray.cc

-#include "PolygonArray.h"

src/Wima/Geometry/PolygonArray.h

-#pragma once
-
-#include <QString>
-
-template <class PolygonType, template <class,class...> class ContainerType >
-class PolygonArray : public ContainerType<PolygonType> {
-public:
-    explicit PolygonArray() : ContainerType<PolygonType>() {}
-    PolygonArray(const PolygonArray &other) : ContainerType<PolygonType>(other) {}
-
-    QString type() const override {return "PolygonArray";}
-};

src/Wima/Geometry/SphereCalculus.cc

-#include "SphereCalculus.h"
-
-
-
-
-
-/*!
-    \class SphereCalculus
-    \inmodule Wima
-
-    \brief The \c WimaArea class provides the a base class for
-    all areas used within the Wima extension.
-
-    \c WimaArea uses a \l {Simple Polygon} derived from \c {QGCMapPolygon}
-    to define areas inside which certain taskts are performed. The polygon (often refered to as the path) can
-    be displayed visually on a map.
-*/
-
-
-
-/*!
-    \externalpage https://en.wikipedia.org/wiki/Simple_polygon
-    \title Simple Polygon
-*/
-
-/*!
-    \externalpage https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
-    \title Dijkstra Algorithm
-*/
-
-
-
-
-
-
-

src/Wima/Geometry/SphereCalculus.h

-#ifndef SphereCalculus_H
-#define SphereCalculus_H
-#endif // SphereCalculus_H
-
-#include <QObject>
-#include <QGeoCoordinate>
-#include <QPointF>
-#include <QLineF>
-
-#include "QGCGeo.h"
-
-#include "PlanimetryCalculus.h"
-
-namespace  SphereCalculus {
-
-
-}
-
-

src/Wima/Geometry/WimaAreaData.cc

@@ -55,6 +55,11 @@ void WimaAreaData::append(const QGeoCoordinate &c) {
     _path.push_back(QVariant::fromValue(c));
 }
 
+void WimaAreaData::push_back(const QGeoCoordinate &c)
+{
+    append(c);
+}
+
 void WimaAreaData::clear() {
     _list.clear();
     _path.clear();

src/Wima/Geometry/WimaAreaData.h

@@ -26,6 +26,7 @@ public:
     virtual QString         type() const = 0;
 
     void append(const QGeoCoordinate &c);
+    void push_back(const QGeoCoordinate &c);
     void clear();
 
 signals:

src/Wima/Snake/GeoPolygonArray.h

-#pragma once
-#include "GeoTile.h"
-#include "Wima/Geometry/WimaPolygonArray.h"
-
-using GeoPolygonArray =  WimaPolygonArray<GeoTile, QVector>;

src/Wima/Snake/PolygonArray.h

-#pragma once
-
-#include "Wima/Geometry/Polygon2D.h"
-#include "Wima/Geometry/WimaPolygonArray.h"
-
-typedef WimaPolygonArray<Polygon2D, QVector> SnakeTilesLocal;

src/Wima/Snake/SnakeWorker.cc → src/Wima/Snake/SnakeDataManager.cc

-#include "SnakeWorker.h"
+#include "SnakeDataManager.h"
+
 #include <QGeoCoordinate>
 #include <QMutexLocker>
 
+#include "QGCApplication.h"
+#include "SettingsManager.h"
+#include "QGCToolbox.h"
+#include "WimaSettings.h"
+#include "SettingsFact.h"
+
+#include <memory>
+#include <shared_mutex>
+#include <mutex>
 
 #include "snake.h"
+#include "Wima/Snake/SnakeTiles.h"
+#include "Wima/Snake/SnakeTilesLocal.h"
+
+#include "comm/ros_bridge/include/ros_bridge.h"
+
+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>;
+
+class SnakeImpl : public QObject{
+    Q_OBJECT
+public:
+    SnakeImpl() :
+        lineDistance(1*si::meter)
+      , minTransectLength(1*si::meter)
+      , calcInProgress(false)
+      , topicServiceSetupDone(false)
+    {
+        // 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 {
+                qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
+                this->pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090"));
+            }
+        };
+        setConnectionString();
+        connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString);
+    }
+
+    // Private data.
+    ROSBridgePtr                pRosBridge;
+    bool                        topicServiceSetupDone;
+    mutable std::shared_timed_mutex   mutex;
+
+    // Input
+    snake::Scenario scenario;
+    Length lineDistance;
+    Length minTransectLength;
+    QList<QGeoCoordinate> mArea;
+    QList<QGeoCoordinate> sArea;
+    QList<QGeoCoordinate> corridor;
+
+
+    // Output
+    std::atomic_bool            calcInProgress;
+    QNemoProgress               progress;
+    QNemoHeartbeat              heartbeat;
+
+    QVector<QGeoCoordinate>     waypoints;
+    QVector<QGeoCoordinate>     arrivalPath;
+    QVector<QGeoCoordinate>     returnPath;
+    QGeoCoordinate              ENUorigin;
+    QVector<QPointF>            waypointsENU;
+    QVector<QPointF>            arrivalPathENU;
+    QVector<QPointF>            returnPathENU;
+    SnakeTiles                  tiles;
+    QVariantList                tileCenterPoints;
+    SnakeTilesLocal             tilesENU;
+    QVector<QPointF>            tileCenterPointsENU;
+    QString                     errorMessage;
+};
+
+template<class AtomicType>
+class ToggleRAII{
+public:
+    ToggleRAII(AtomicType &t)
+        : _t(t)
+    {}
+
+    ~ToggleRAII()
+    {
+        if ( _t.load() ){
+            _t.store(false);
+        } else {
+            _t.store(true);
+        }
+    }
+private:
+    AtomicType &_t;
+};
+
 
 
 SnakeDataManager::SnakeDataManager(QObject *parent)
     : QThread(parent)
-  , _pScenario(std::make_shared<snake::Scenario>())
-  , _pProgress(std::make_shared<std::vector<int>>())
-  , _pFlightplan(std::make_shared<snake::Flightplan>(_pScenario, _pProgress))
-  , _pData(std::make_shared<SnakeData>())
+    , _pImpl(std::make_unique<SnakeImpl>())
 
 {
 }
@@ -21,48 +116,124 @@ SnakeDataManager::~SnakeDataManager()
 
 }
 
-SnakeDataManager::SnakeDataPtr SnakeDataManager::snakeData()
+void SnakeDataManager::setMeasurementArea(const QList<QGeoCoordinate> &measurementArea)
 {
-    return _pData;
+    UniqueLock lk(this->_pImpl->m);
+    this->_pImpl->mArea = measurementArea;
 }
 
-bool SnakeDataManager::precondition() const
+void SnakeDataManager::setServiceArea(const QList<QGeoCoordinate> &serviceArea)
 {
+    UniqueLock lk(this->_pImpl->m);
+    this->_pImpl->sArea = serviceArea;
+}
 
-    return _mArea.size() > 0
-            && _sArea.size() > 0
-            && _corridor.size() > 0
-            && _pProgress->size() > 0;
+void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
+{
+    UniqueLock lk(this->_pImpl->m);
+    this->_pImpl->corridor = corridor;
 }
 
-double SnakeDataManager::lineDistance() const
+QNemoProgress SnakeDataManager::progress()
 {
-    _pFlightplan->lineDistance();
+    SharedLock lk(this->_pImpl->m);
+    return this->_pImpl->progress;
+}
+
+QNemoHeartbeat SnakeDataManager::heartbeat()
+{
+    SharedLock lk(this->_pImpl->m);
+    return this->_pImpl->heartbeat;
+}
+
+bool SnakeDataManager::calcInProgress()
+{
+    return this->_pImpl->calcInProgress.load();
+}
+
+Length SnakeDataManager::lineDistance() const
+{
+    SharedLock lk(this->_pImpl->m);
+    return this->_pImpl->lineDistance;
+}
+
+void SnakeDataManager::setLineDistance(Length lineDistance)
+{
+    UniqueLock lk(this->_pImpl->m);
+    this->_pImpl->lineDistance = lineDistance;
+}
+
+Area SnakeDataManager::minTileArea() const
+{
+    SharedLock lk(this->_pImpl->m);
+    return this->_pImpl->scenario.minTileArea();
+}
+
+void SnakeDataManager::setMinTileArea(Area minTileArea)
+{
+    UniqueLock lk(this->_pImpl->m);
+    this->_pImpl->scenario.setMinTileArea(minTileArea);
+}
+
+Length SnakeDataManager::tileHeight() const
+{
+    SharedLock lk(this->_pImpl->m);
+    return this->_pImpl->scenario.tileHeight();
+}
+
+void SnakeDataManager::setTileHeight(Length tileHeight)
+{
+    UniqueLock lk(this->_pImpl->m);
+    this->_pImpl->scenario.setTileHeight(tileHeight);
+}
+
+Length SnakeDataManager::tileWidth() const
+{
+    SharedLock lk(this->_pImpl->m);
+    return this->_pImpl->scenario.tileWidth();
+}
+
+void SnakeDataManager::setTileWidth(Length tileWidth)
+{
+    UniqueLock lk(this->_pImpl->m);
+    this->_pImpl->scenario.setTileWidth(tileWidth);
+}
+
+bool SnakeDataManager::precondition() const
+{
+    return true;
 }
 
 void SnakeDataManager::run()
 {
-    {
-        QMutexLocker lk(&_pData->m);
-        _pData->clear();
-    }
+    this->_pImpl->calcInProgress.store(true);
+    ToggleRAII<std::atomic_bool> tr(this->_pImpl->calcInProgress);
+
+    UniqueLock lk(&_pImpl->m);
+    _pImpl->clear();
 
     if ( !precondition() )
         return;
 
-    if ( _mArea.size() <= 0
+    if ( this->_pImpl->mArea.size() < 3) {
+        _pImpl->errorMessage = "Measurement area invalid: size < 3.";
+        return;
+    }
+    if ( this->_pImpl->sArea.size() < 3) {
+        _pImpl->errorMessage = "Service area invalid: size < 3.";
+        return;
+    }
+
+
          || _sArea.size() <= 0
          || _corridor.size() <= 0 ){
-        QMutexLocker lk(&_pData->m);
-        _pData->errorMessage = "At least one area invald. Size < 1.";
-    }
 
     auto origin = _mArea.front();
     // Measurement area update necessary.
     if ( _mArea.size() != _pScenario->measurementArea().outer().size() ){
         {
-            QMutexLocker lk(&_pData->m);
-            _pData->ENUorigin = origin;
+            QMutexLocker lk(&_pImpl->m);
+            _pImpl->ENUorigin = origin;
         }
         for (auto geoVertex : _mArea){
             snake::BoostPoint p;
@@ -91,9 +262,9 @@ void SnakeDataManager::run()
 
     if ( !_pScenario->update() ){
         {
-            QMutexLocker lk(&_pData->m);
+            QMutexLocker lk(&_pImpl->m);
             for (auto c : _pScenario->errorString){
-                _pData->errorMessage.push_back(QChar(c));
+                _pImpl->errorMessage.push_back(QChar(c));
             }
         }
         return;
@@ -104,31 +275,31 @@ void SnakeDataManager::run()
 
     // Continue with storing scenario data in the mean time.
     {
-        QMutexLocker lk(&_pData->m);
+        QMutexLocker lk(&_pImpl->m);
         // Get tiles.
         const auto &tiles = _pScenario->tiles();
         const auto &centerPoints = _pScenario->tileCenterPoints();
         for ( unsigned int i=0; i < tiles.size(); ++i ) {
             const auto &tile = tiles[i];
-            GeoTile     geoTile;
-            Polygon2D   enuTile;
+            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>(), 0.0);
+                QPointF enuVertex(p.get<0>(), p.get<1>());
                 QGeoCoordinate geoVertex;
-                snake::fromENU(origin, *it, geoVertex);
-                enuTile.polygon().push_back(enuVertex);
-                geoTile.push_back(QVertex);
+                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>(), 0.0);
+            QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>());
             QGeoCoordinate geoVertex;
-            snake::fromENU(origin, enuVertex, geoVertex);
+            snake::fromENU(origin, boostPoint, geoVertex);
             geoTile.setCenter(geoVertex);
-            _pData->tiles.polygons().push_back(geoTile);
-            _pData->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
-            _pData->tilesENU.polygons().push_back(enuTile);
-            _pData->tileCenterPointsENU.push_back(enuVertex);
+            _pImpl->tiles.polygons().push_back(geoTile);
+            _pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
+            _pImpl->tilesENU.polygons().push_back(enuTile);
+            _pImpl->tileCenterPointsENU.push_back(enuVertex);
          }
     }
 
@@ -136,138 +307,38 @@ void SnakeDataManager::run()
     // Trying to generate flight plan.
     if ( !future.get() ){
         // error
-        QMutexLocker lk(&_pData->m);
+        QMutexLocker lk(&_pImpl->m);
         for (auto c : _pFlightplan->errorString){
-            _pData->errorMessage.push_back(QChar(c));
+            _pImpl->errorMessage.push_back(QChar(c));
         }
     } else {
         //success!!!
-        QMutexLocker lk(&_pData->m);
+        QMutexLocker lk(&_pImpl->m);
         // Store waypoints.
         for (auto boostVertex : _pFlightplan->waypoints()){
-            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
+            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
             QGeoCoordinate geoVertex;
-            snake::fromENU(origin, enuVertex, geoVertex);
-            _pData->waypointsENU.push_back(enuVertex);
-            _pData->waypoints.push_back(geoVertex);
+            snake::fromENU(origin, boostVertex, geoVertex);
+            _pImpl->waypointsENU.push_back(enuVertex);
+            _pImpl->waypoints.push_back(geoVertex);
         }
         // Store arrival path.
         for (auto boostVertex : _pFlightplan->arrivalPath()){
-            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
+            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
             QGeoCoordinate geoVertex;
-            snake::fromENU(origin, enuVertex, geoVertex);
-            _pData->arrivalPathENU.push_back(enuVertex);
-            _pData->arrivalPath.push_back(geoVertex);
+            snake::fromENU(origin, boostVertex, geoVertex);
+            _pImpl->arrivalPathENU.push_back(enuVertex);
+            _pImpl->arrivalPath.push_back(geoVertex);
         }
         // Store return path.
         for (auto boostVertex : _pFlightplan->returnPath()){
-            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
+            QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
             QGeoCoordinate geoVertex;
-            snake::fromENU(origin, enuVertex, geoVertex);
-            _pData->returnPathENU.push_back(enuVertex);
-            _pData->returnPath.push_back(geoVertex);
-        }
-    }
-}
-
-double SnakeDataManager::minTransectLength() const
-{
-    return _pFlightplan->minTransectLength();
-}
-
-void SnakeDataManager::setMinTransectLength(double minTransectLength)
-{
-    _pFlightplan->setMinTransectLength(minTransectLength);
-}
-
-double SnakeDataManager::minTileArea() const
-{
-    return _pScenario->minTileArea();
-}
-
-void SnakeDataManager::setLineDistance(double lineDistance)
-{
-    _pFlightplan->setLineDistance(lineDistance);
-}
-
-double SnakeDataManager::tileWidth() const
-{
-    return _pScenario->tileWidth();
-}
-
-void SnakeDataManager::setTileWidth(double tileWidth)
-{
-    _pScenario->setTileWidth(tileWidth);
-}
-
-double SnakeDataManager::tileHeight() const
-{
-    return _pScenario->_tileHeight();
-}
-
-void SnakeDataManager::setTileHeight(double tileHeight)
-{
-    _pScenario->setTileHeight(tileHeight);
-}
-
-void SnakeDataManager::setMinTileArea(double minTileArea)
-{
-    _pScenario->setMinTileArea(minTileArea);
-}
-
-SnakeData::SnakeData()
-{
-
-}
-
-void SnakeData::clear()
-{
-    this->waypoints.clear();
-    this->arrivalPath.clear();
-    this->returnPath.clear();
-    this->tiles.polygons().clear();
-    this->tilesLocal.polygons().clear();
-    this->tileCenterPoints.clear();
-    this->waypointsENU.clear();
-    this->arrivalPathENU.clear();
-    this->returnPathENU.clear();
-    this->tilesENU.polygons().clear();
-    this->tileCenterPoints.clear();
-    this->errorMessage = "";
-}
-
-void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
-{
-    _corridor.clear();
-    _pScenario->corridor().clear();
-    for (auto vertex : corridor) {
-        _corridor.push_back(vertex);
+            snake::fromENU(origin, boostVertex, geoVertex);
+            _pImpl->returnPathENU.push_back(enuVertex);
+            _pImpl->returnPath.push_back(geoVertex);
         }
-}
-
-void SnakeDataManager::setProgress(const QList<IntType> &progress)
-{
-    _pProgress->clear();
-    for (auto p : progress) {
-        assert(p >= -1 && p <= 100);
-        _pProgress->push_back(p);
     }
 }
 
-void SnakeDataManager::setServiceArea(const QList<QGeoCoordinate> &serviceArea)
-{
-    _sArea.clear();
-    _pScenario->serviceArea().clear();
-    for (auto vertex : serviceArea) {
-        _sArea.push_back(vertex);
-    }
-}
 
-void SnakeDataManager::setMeasurementArea(const QList<QGeoCoordinate> &measurementArea)
-{
-    _mArea.clear();
-    _pScenario->measurementArea().clear();
-    for (auto vertex : measurementArea) {
-        _mArea.push_back(vertex);
-    }
-}

src/Wima/Snake/SnakeDataManager.h

+#pragma once
+
+#include <QObject>
+#include <QThread>
+#include <QList>
+#include <QGeoCoordinate>
+
+#include "QNemoProgress.h"
+#include "QNemoHeartbeat.h"
+
+#include <boost/units/systems/si.hpp>>
+
+
+using namespace boost::units;
+using Length = quantity<si::length>;
+using Area = quantity<si::area>;
+
+class SnakeImpl;
+
+class SnakeDataManager : public QThread{
+    Q_OBJECT
+
+public:
+    using SnakeImplPtr  = std::unique_ptr<SnakeImpl>;
+
+    SnakeDataManager(QObject *parent = nullptr);
+    ~SnakeDataManager() override;
+
+
+    void setMeasurementArea     (const QList<QGeoCoordinate>  &measurementArea);
+    void setServiceArea         (const QList<QGeoCoordinate>  &serviceArea);
+    void setCorridor            (const QList<QGeoCoordinate>  &corridor);
+
+    QNemoProgress progress();
+    QNemoHeartbeat heartbeat();
+    bool calcInProgress();
+
+    Length lineDistance() const;
+    void setLineDistance(Length lineDistance);
+
+    Length minTransectLength() const;
+    void setMinTransectLength(Length minTransectLength);
+
+    Area minTileArea() const;
+    void setMinTileArea(Area minTileArea);
+
+    Length tileHeight() const;
+    void setTileHeight(Length tileHeight);
+
+    Length tileWidth() const;
+    void setTileWidth(Length tileWidth);
+
+
+
+protected:
+    void run() override;
+
+private:
+    bool precondition() const;
+    SnakeImplPtr _pImpl;
+};
+
+
+
+
+
+
+
+
+

src/Wima/Snake/GeoTile.cpp → src/Wima/Snake/SnakeTile.cpp

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

src/Wima/Snake/GeoTile.h → src/Wima/Snake/SnakeTile.h

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

src/Wima/Snake/SnakeTileLocal.h

+#pragma once
+#include "Wima/Geometry/GenericPolygon.h"
+
+using SnakeTileLocal = GenericPolygon<>;

src/Wima/Snake/SnakeTiles.h

+#pragma once
+#include "SnakeTile.h"
+#include "Wima/Geometry/GenericPolygonArray.h"
+
+using SnakeTiles =  GenericPolygonArray<SnakeTile, QVector>;

src/Wima/Snake/SnakeTilesLocal.h

+#pragma once
+
+#include "Wima/Snake/SnakeTileLocal.h"
+#include "Wima/Geometry/GenericPolygonArray.h"
+
+typedef GenericPolygonArray<SnakeTileLocal, QVector> SnakeTilesLocal;

src/Wima/Snake/SnakeWorker.h

-#pragma once
-
-#include <QObject>
-#include <QVariant>
-#include <QThread>
-#include <QVector>
-#include <QGeoCoordinate>
-#include <QMutex>
-
-#include <vector>
-#include <memory>
-
-#include "GeoPolygonArray.h"
-#include "PolygonArray.h"
-#include "SnakeWorker.h"
-#include "QNemoProgress.h"
-
-
-typedef QList<QVariant> QVariantList;
-
-namespace snake {
-    class Scenario;
-    class Flightplan;
-}
-
-struct SnakeData{
-    SnakeData();
-
-    QVector<QGeoCoordinate> waypoints;
-    QVector<QGeoCoordinate> arrivalPath;
-    QVector<QGeoCoordinate> returnPath;
-    QGeoCoordinate          ENUorigin;
-    QVector<QGeoCoordinate> waypointsENU;
-    QVector<QGeoCoordinate> arrivalPathENU;
-    QVector<QGeoCoordinate> returnPathENU;
-    GeoPolygonArray         tiles;
-    QVariantList            tileCenterPoints;
-    PolygonArray            tilesENU;
-    QVector<QGeoCoordinate> tileCenterPointsENU;
-    QString                 errorMessage;
-    mutable QMutex          m;
-
-    void clear();
-};
-
-class SnakeDataManager : public QThread{
-    Q_OBJECT
-
-public:
-
-    using ProgressPtr   = std::shared_ptr<std::vector<int>>;
-    using ScenarioPtr   = std::shared_ptr<snake::Scenario>;
-    using FlightPlanPtr = std::shared_ptr<snake::Flightplan>;
-    using SnakeDataPtr  = std::shared_ptr<SnakeData>;
-
-    SnakeDataManager(QObject *parent = nullptr);
-    ~SnakeDataManager() override;
-
-
-    void setMeasurementArea     (const QList<QGeoCoordinate>  &measurementArea);
-    void setServiceArea         (const QList<QGeoCoordinate>  &serviceArea);
-    void setCorridor            (const QList<QGeoCoordinate>  &corridor);
-
-    SnakeDataPtr snakeData();
-
-    double lineDistance() const;
-    void setLineDistance(double lineDistance);
-
-    double minTransectLength() const;
-    void setMinTransectLength(double minTransectLength);
-
-    double minTileArea() const;
-    void setMinTileArea(double minTileArea);
-
-    double tileHeight() const;
-    void setTileHeight(double tileHeight);
-
-    double tileWidth() const;
-    void setTileWidth(double tileWidth);
-
-protected:
-    void run() override;
-
-private:
-    bool precondition() const;
-
-    std::vector<QGeoCoordinate> _mArea;
-    std::vector<QGeoCoordinate> _sArea;
-    std::vector<QGeoCoordinate> _corridor;
-    ScenarioPtr                 _pScenario;
-    ProgressPtr                 _pProgress;
-    FlightPlanPtr               _pFlightplan;
-    SnakeDataPtr                _pData;
-};
-
-
-
-
-
-
-
-
-

src/Wima/WimaController.cc

@@ -92,21 +92,6 @@ WimaController::WimaController(QObject *parent)
     , _topicServiceSetupDone    (false)
 {
 
-    // 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 {
-            qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
-            this->_pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090"));
-        }
-    };
-    setConnectionString();
-    connect(wimaSettings->rosbridgeConnectionString(), &SettingsFact::rawValueChanged, setConnectionString);
-
     // Set up facts.
     _showAllMissionItems.setRawValue(true);
     _showCurrentMissionItems.setRawValue(true);
@@ -138,9 +123,9 @@ WimaController::WimaController(QObject *parent)
     _eventTimer.start(EVENT_TIMER_INTERVAL);
 
     // Snake Worker Thread.
-    connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults);
+    connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults);
     connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
-    connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
+    connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeWorker::quit);
 
     // Snake.
     connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
@@ -899,7 +884,7 @@ void WimaController::_snakeStoreWorkerResults()
     auto start = std::chrono::high_resolution_clock::now();
     _snakeManager.clear();
 
-    const auto &r = _snakeWorker.getResult();
+    const auto &r = _snakeDataManager.getResult();
     if (!r.success) {
         //qgcApp()->showMessage(r.errorMessage);
         return;
@@ -936,27 +921,27 @@ void WimaController::_initStartSnakeWorker()
         return;
 
     // Stop worker thread if running.
-    if ( _snakeWorker.isRunning() ) {
-        _snakeWorker.quit();
+    if ( _snakeDataManager.isRunning() ) {
+        _snakeDataManager.quit();
     }
 
     // Initialize _snakeWorker.
-    _snakeWorker.setProgress(
+    _snakeDataManager.setProgress(
                 _nemoProgress.progress());
-    _snakeWorker.setLineDistance(
+    _snakeDataManager.setLineDistance(
                 _snakeLineDistance.rawValue().toDouble());
-    _snakeWorker.setMinTransectLength(
+    _snakeDataManager.setMinTransectLength(
                 _snakeMinTransectLength.rawValue().toDouble());
-    _snakeWorker.setTileHeight(
+    _snakeDataManager.setTileHeight(
                 _snakeTileHeight.rawValue().toDouble());
-    _snakeWorker.setTileWidth(
+    _snakeDataManager.setTileWidth(
                 _snakeTileWidth.rawValue().toDouble());
-    _snakeWorker.setMinTileArea(
+    _snakeDataManager.setMinTileArea(
                 _snakeMinTileArea.rawValue().toDouble());
     _setSnakeCalcInProgress(true);
 
     // Start worker thread.
-    _snakeWorker.start();
+    _snakeDataManager.start();
 }
 
 void WimaController::_switchSnakeManager(QVariant variant)

src/Wima/WimaController.h

@@ -24,16 +24,13 @@
 #include "JsonHelper.h"
 #include "QGCApplication.h"
 #include "SettingsFact.h"
-#include "WimaSettings.h"
 #include "SettingsManager.h"
 
 #include "snake.h"
-#include "Snake/SnakeWorker.h"
-#include "Snake/GeoPolygonArray.h"
-#include "Snake/PolygonArray.h"
+#include "Snake/SnakeDataManager.h"
+#include "Snake/SnakeTiles.h"
+#include "Snake/SnakeTilesLocal.h"
 #include "Geometry/GeoPoint3D.h"
-#include "Snake/QNemoProgress.h"
-#include "Snake/QNemoHeartbeat.h"
 
 #include "ros_bridge/include/ros_bridge.h"
 
@@ -52,11 +49,7 @@ typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;
 class WimaController : public QObject
 {
     Q_OBJECT
-
     enum FileType {WimaFile, PlanFile};
-
-    typedef QScopedPointer<ros_bridge::ROSBridge> ROSBridgePtr;
-
 public:
 
 
@@ -395,18 +388,9 @@ private:
     double                       _measurementPathLength; // the lenght of the phase in meters
 
     // Snake
-    bool                    _snakeCalcInProgress;
-    SnakeDataManager             _snakeWorker;
-    GeoPoint                _snakeOrigin;
-    GeoPolygonArray         _snakeTiles; // tiles
-    PolygonArray            _snakeTilesLocal; // tiles local coordinate system
-    QVariantList            _snakeTileCenterPoints;
-    QNemoProgress           _nemoProgress; // measurement progress
-    QNemoHeartbeat          _nemoHeartbeat; // measurement progress
+    SnakeDataManager        _snakeDataManager;
     int                     _fallbackStatus;
-    ROSBridgePtr            _pRosBridge;
     static StatusMap        _nemoStatusMap;
-    bool                    _topicServiceSetupDone;
 
     // Periodic tasks.
     QTimer      _eventTimer;