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/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

@@ -90,22 +90,7 @@ WimaController::WimaController(QObject *parent)
     , _snakeTicker              (EVENT_TIMER_INTERVAL, 200 /*ms*/)
     , _nemoTimeoutTicker        (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
     , _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);
@@ -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;