temp

qgroundcontrol.pro

@@ -408,6 +408,7 @@ FORMS += \
 
 HEADERS += \
     src/Snake/clipper/clipper.hpp \
+    src/Snake/flight_plan.h \
     src/Snake/mapbox/feature.hpp \
     src/Snake/mapbox/geometry.hpp \
     src/Snake/mapbox/geometry/box.hpp \
@@ -430,22 +431,22 @@ HEADERS += \
     src/Snake/mapbox/variant_io.hpp \
     src/Snake/snake.h \
     src/Snake/snake_geometry.h \
-    src/Snake/snake_global.h \
+    src/Snake/snake_typedefs.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/QtROSJsonFactory.h \
     src/Wima/Snake/QtROSTypeFactory.h \
-    src/Wima/Snake/SnakeTiles.h \
-    src/Wima/Snake/SnakeTilesLocal.h \
     src/Wima/Snake/SnakeWorker.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 \
@@ -497,10 +498,12 @@ HEADERS += \
     src/comm/utilities.h
 SOURCES += \
     src/Snake/clipper/clipper.cpp \
+    src/Snake/flight_plan.cpp \
     src/Snake/snake.cpp \
     src/Snake/snake_geometry.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/WaypointManager/AreaInterface.cpp \
@@ -516,7 +519,6 @@ SOURCES += \
     src/comm/ros_bridge/include/TopicPublisher.cpp \
     src/comm/ros_bridge/include/TopicSubscriber.cpp \
     src/comm/ros_bridge/src/CasePacker.cpp \
-    src/Wima/Snake/snaketile.cpp \
     src/api/QGCCorePlugin.cc \
     src/api/QGCOptions.cc \
     src/api/QGCSettings.cc \

src/Snake/flight_plan.h

+#pragma once
+
+namespace flight_plan {
+
+
+
+//========================================================================================
+// FlightPlan
+//========================================================================================
+
+class FlightPlan{
+public:
+    FlightPlan();
+
+    void setScenario(Scenario &scenario)          {_scenario = scenario;}
+    void setProgress(vector<int> &progress)       {_progress = progress;}
+
+    const Scenario          &getScenario(void)          const {return _scenario;}
+    const BoostLineString   &getWaypointsENU(void)      const {return _waypointsENU;}
+    const GeoPoint2DList    &getWaypoints(void)         const {return _waypoints;}
+    const vector<uint64_t>  &getArrivalPathIdx(void)    const {return _arrivalPathIdx;}
+    const vector<uint64_t>  &getReturnPathIdx(void)     const {return _returnPathIdx;}
+#ifndef NDEBUG
+    const BoostLineString   &getPathVertices(void)      const {return _PathVertices;}
+#endif
+
+    bool                            generate(double lineDistance, double minTransectLength);
+    const vector<BoostLineString>  &getTransects() const {return _transects;}
+
+    string errorString;
+
+private:
+    // Search Filter to speed up routing.SolveWithParameters(...);
+    // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
+    class SearchFilter;
+
+    struct RoutingDataModel;
+
+    bool _generateTransects(double lineDistance, double minTransectLength);
+    void _generateRoutingModel(const BoostLineString &vertices,
+                               const BoostPolygon &polygonOffset,
+                               size_t n0,
+                               RoutingDataModel &dataModel,
+                               Matrix<double> &graph);
+
+    Scenario                                _scenario;
+    BoostLineString                         _waypointsENU;
+    GeoPoint2DList                          _waypoints;
+    vector<BoostLineString>                 _transects;
+    vector<int>                             _progress;
+    vector<uint64_t>                        _arrivalPathIdx;
+    vector<uint64_t>                        _returnPathIdx;
+
+#ifndef NDEBUG
+    BoostLineString                         _PathVertices;
+#endif
+};
+
+}

src/Snake/snake_geometry.h

-#pragma once
-#include <vector>
-#include <array>
-#include <boost/geometry.hpp>
-#include <bits/stl_set.h>
-
-namespace bg = boost::geometry;
-
-namespace snake_geometry {
-
-typedef std::array<double, 3>   Point2D;
-typedef std::array<double, 3>   Point3D;
-typedef std::array<double, 3>   GeoPoint3D;
-typedef std::array<double, 2>   GeoPoint2D;
-typedef std::vector<Point2D>    Point2DList;
-typedef std::vector<Point3D>    Point3DList;
-typedef std::vector<GeoPoint2D> GeoPoint2DList;
-typedef std::vector<GeoPoint3D> GeoPoint3DList;
-
-typedef bg::model::point<double, 2, bg::cs::cartesian>  BoostPoint;
-//typedef std::vector<BoostPoint>                        BoostPointList;
-typedef bg::model::polygon<BoostPoint>                  BoostPolygon;
-typedef bg::model::linestring<BoostPoint>               BoostLineString;
-typedef std::vector<std::vector<int64_t>>               Int64Matrix;
-
-
-template<class T>
-class Matrix{
-public:
-    Matrix() :
-        _elements(0),
-        _m(0),
-        _n(0),
-        _isInitialized(false)
-    {
-    }
-    Matrix(size_t m, size_t n) : Matrix(m, n, T{0}){}
-    Matrix(size_t m, size_t n, T value) :
-        _elements(m*n),
-        _m(m),
-        _n(n),
-        _isInitialized(true)
-    {
-        assert((m > 0) || (n > 0));
-        _matrix.resize(_elements, value);
-    }
-
-    double get(size_t i, size_t j) const
-    {
-        assert(_isInitialized);
-        assert(i < _m);
-        assert(j < _n);
-        return _matrix[i*_m+j];
-    }
-
-    size_t getM() const { return _m;}
-    size_t getN() const { return _n;}
-
-    void set(size_t i, size_t j, const T &value)
-    {
-        assert(_isInitialized);
-        assert(i < _m);
-        assert(j < _n);
-        _matrix[i*_m+j] = value;
-    }
-
-    void setDimension(size_t m, size_t n, const T &value)
-    {
-        assert((m > 0) || (n > 0));
-        assert(!_isInitialized);
-        _m = m;
-        _n = n;
-        _elements = n*m;
-        _matrix.resize(_elements, value);
-        _isInitialized = true;
-    }
-
-    void setDimension(size_t m, size_t n)
-    {
-        setDimension(m, n, T{0});
-    }
-
-private:
-    size_t _elements;
-    size_t _m;
-    size_t _n;
-    bool   _isInitialized;
-    std::vector<T> _matrix;
-};
-
-typedef struct {
-    double width;
-    double height;
-    double angle;
-    BoostPolygon corners;
-}min_bbox_rt;
-
-void toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position, Point3D &ENUPosition);
-void fromENU(const Point3D &WGS84Reference, const Point3D &ENUPosition, GeoPoint3D &WGS84Position);
-
-void polygonCenter(const BoostPolygon &polygon, BoostPoint &center);
-void minimalBoundingBox(const BoostPolygon &polygon, min_bbox_rt &minBBox);
-void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset);
-
-void graphFromPolygon(const BoostPolygon &polygon, const BoostLineString &vertices, Matrix<double> &graph);
-void toDistanceMatrix(Matrix<double> &graph);
-bool dijkstraAlgorithm(const size_t numElements,
-                       size_t startIndex,
-                       size_t endIndex,
-                       std::vector<size_t> &elementPath,
-                       std::function<double (const size_t, const size_t)> distanceDij);
-void shortestPathFromGraph(const Matrix<double> &graph,
-                           size_t startIndex,
-                           size_t endIndex,
-                           std::vector<size_t> &pathIdx);
-
-
-void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree);
-void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad);
-
-bool isClockwise(const Point2DList &point_list);
-
-void toBoost(const Point2D &point, BoostPoint &boost_point);
-void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon);
-
-void fromBoost(const BoostPoint &boost_point, Point2D &point);
-void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list);
-}

src/Snake/snake_global.h

-#ifndef SNAKE_GLOBAL_H
-#define SNAKE_GLOBAL_H
-
-#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
-#  define Q_DECL_EXPORT __declspec(dllexport)
-#  define Q_DECL_IMPORT __declspec(dllimport)
-#else
-#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
-#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
-#endif
-
-#if defined(SNAKE_LIBRARY)
-#  define SNAKE_EXPORT Q_DECL_EXPORT
-#else
-#  define SNAKE_EXPORT Q_DECL_IMPORT
-#endif
-
-#endif // SNAKE_GLOBAL_H

src/Snake/snake_typedefs.h

+#pragma once
+
+#include <vector>
+#include <array>
+#include <boost/geometry.hpp>
+
+namespace bg = boost::geometry;
+namespace snake {
+
+typedef bg::model::point<double, 2, bg::cs::cartesian>  BoostPoint;
+//typedef std::vector<BoostPoint>                        BoostPointList;
+typedef bg::model::polygon<BoostPoint>                  BoostPolygon;
+typedef bg::model::linestring<BoostPoint>               BoostLineString;
+typedef std::vector<std::vector<int64_t>>               Int64Matrix;
+
+}

src/Wima/Geometry/WimaPolygonArray.h

@@ -41,6 +41,12 @@ public:
         return _polygons;
     }
 
+    WimaPolygonArray &operator =(const WimaPolygonArray &other){
+        this->_polygons = other._polygons;
+        this->_dirty = true;
+        return *this;
+    }
+
 
 
 private:

src/Wima/Snake/SnakeTiles.h → src/Wima/Snake/GeoPolygonArray.h

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

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

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

src/Wima/Snake/GeoTile.h

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

src/Wima/Snake/SnakeTilesLocal.h → src/Wima/Snake/PolygonArray.h

@@ -5,4 +5,4 @@
 #include "Wima/Geometry/WimaPolygonArray.h"
 
 namespace  MsgGroups = ROSBridge::MessageGroups;
-typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> SnakeTilesLocal;
+typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> PolygonArray;

src/Wima/Snake/SnakeWorker.cc

@@ -118,28 +118,28 @@ void SnakeWorker::run()
         const auto &cps = scenario.getTileCenterPoints();
         for ( unsigned int i=0; i < tiles.size(); ++i ) {
             const auto &tile = tiles[i];
-            SnakeTile Tile;
+            GeoTile geoTile;
             for ( const auto &vertex : tile) {
                 QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
-                Tile.append(QVertex);
+                geoTile.append(QVertex);
             }
             const auto &centerPoint = cps[i];
-            QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
-            Tile.setCenter(QCenterPoint);
-            _result.tiles.polygons().append(Tile);
-            _result.tileCenterPoints.append(QVariant::fromValue(QCenterPoint));
+            QGeoCoordinate c(centerPoint[0], centerPoint[1], centerPoint[2]);
+            geoTile.setCenter(c);
+            _result.tiles.polygons().append(geoTile);
+            _result.tileCenterPoints.append(QVariant::fromValue(c));
          }
 
         // Get local tiles.
         const auto &tilesENU = scenario.getTilesENU();
         for ( unsigned int i=0; i < tilesENU.size(); ++i ) {
             const auto &tile = tilesENU[i];
-            Polygon2D Tile;
+            Polygon2D localTile;
             for ( const auto &vertex : tile.outer()) {
                 QPointF QVertex(vertex.get<0>(), vertex.get<1>());
-                Tile.path().append(QVertex);
+                localTile.path().append(QVertex);
             }
-            _result.tilesLocal.polygons().append(Tile);
+            _result.tilesLocal.polygons().append(localTile);
         }
 
     }

src/Wima/Snake/SnakeWorker.h

@@ -7,22 +7,22 @@
 #include <QGeoCoordinate>
 #include <vector>
 
-#include "SnakeTiles.h"
-#include "SnakeTilesLocal.h"
+#include "GeoPolygonArray.h"
+#include "PolygonArray.h"
 
 
 typedef QList<QVariant> QVariantList;
 
 typedef struct Result{
     QVector<QGeoCoordinate> waypoints;
-    SnakeTiles              tiles;
-    SnakeTilesLocal         tilesLocal;
+    GeoPolygonArray         tiles;
+    PolygonArray            tilesLocal;
     QVariantList            tileCenterPoints;
     QGeoCoordinate          origin;
     QVector<unsigned long>  arrivalPathIdx;
     QVector<unsigned long>  returnPathIdx;
     bool                    success;
-    QString                 errorMessage;    
+    QString                 errorMessage;
 
     void clear();
 }WorkerResult_t;

src/Wima/Snake/snaketile.h

-#pragma once
-
-#include "Wima/Geometry/WimaAreaData.h"
-
-class SnakeTile : public WimaAreaData
-{
-public:
-    SnakeTile();
-    SnakeTile(const SnakeTile&other);
-
-    QString type() const {return "SnakeTile";}
-    SnakeTile* Clone() const {return new SnakeTile(*this);}
-
-    SnakeTile& operator=(const SnakeTile &other);
-
-protected:
-    void assign(const SnakeTile &other);
-};
-

src/Wima/WimaController.cc

@@ -112,7 +112,7 @@ WimaController::WimaController(QObject *parent)
     _eventTimer.start(EVENT_TIMER_INTERVAL);
 
     // Snake Worker Thread.
-    connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults);
+    connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_updateSnakeData);
     connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
     connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
 
@@ -818,13 +818,12 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress)
     }
 }
 
-void WimaController::_snakeStoreWorkerResults()
-{
+void WimaController::_updateSnakeData()
+{    
+    _setSnakeCalcInProgress(false);
     auto start = std::chrono::high_resolution_clock::now();
     _snakeManager.clear();
-
-    const WorkerResult_t &r = _snakeWorker.result();
-    _setSnakeCalcInProgress(false);
+    const auto& r = _snakeWorker.result();
     if (!r.success) {
         //qgcApp()->showMessage(r.errorMessage);
         return;
@@ -841,18 +840,43 @@ void WimaController::_snakeStoreWorkerResults()
     unsigned  long endIdx = r.returnPathIdx.first();
     for (unsigned long i = startIdx; i <= endIdx; ++i) {
         _snakeManager.push_back(r.waypoints[int(i)]);
-    }
+    }    
+    auto end = std::chrono::high_resolution_clock::now();
+    double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: push_back waypoints execution time: " << duration << " ms.";
 
+
+    start = std::chrono::high_resolution_clock::now();
     _snakeManager.update();
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: _snakeManager.update(); execution time: " << duration << " ms.";
+
 
+    start = std::chrono::high_resolution_clock::now();
     emit missionItemsChanged();
     emit currentMissionItemsChanged();
     emit currentWaypointPathChanged();
     emit waypointPathChanged();
-
-    auto end = std::chrono::high_resolution_clock::now();
-    double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
-    qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: gui update execution time: " << duration << " ms.";
+
+    start = std::chrono::high_resolution_clock::now();
+    _snakeOrigin            = r.origin;
+    _snakeTileCenterPoints  = r.tileCenterPoints;
+    _snakeTiles             = r.tiles;
+    _snakeTilesLocal        = r.tilesLocal;
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: tiles copy execution time: " << duration << " ms.";
+
+    start = std::chrono::high_resolution_clock::now();
+    emit snakeTilesChanged();
+    emit snakeTileCenterPointsChanged();
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: gui update 2 execution time: " << duration << " ms.";
 }
 
 void WimaController::_initStartSnakeWorker()

src/Wima/WimaController.h

@@ -29,8 +29,8 @@
 
 #include "snake.h"
 #include "Snake/SnakeWorker.h"
-#include "Snake/SnakeTiles.h"
-#include "Snake/SnakeTilesLocal.h"
+#include "Snake/GeoPolygonArray.h"
+#include "Snake/PolygonArray.h"
 #include "Geometry/GeoPoint3D.h"
 #include "Snake/QNemoProgress.h"
 #include "Snake/QNemoHeartbeat.h"
@@ -338,7 +338,7 @@ private slots:
     void    _smartRTLCleanUp                    (bool flying);
     // Snake.
     void _setSnakeCalcInProgress             (bool inProgress);
-    void _snakeStoreWorkerResults            ();
+    void _updateSnakeData                    ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
     // Periodic tasks.
@@ -402,9 +402,9 @@ private:
     // Snake
     bool                    _snakeCalcInProgress;
     SnakeWorker             _snakeWorker;
-    GeoPoint              _snakeOrigin;
-    SnakeTiles              _snakeTiles; // tiles
-    SnakeTilesLocal         _snakeTilesLocal; // tiles local coordinate system
+    GeoPoint                _snakeOrigin;
+    GeoPolygonArray         _snakeTiles; // tiles
+    PolygonArray            _snakeTilesLocal; // tiles local coordinate system
     QVariantList            _snakeTileCenterPoints;
     QNemoProgress           _nemoProgress; // measurement progress
     QNemoHeartbeat          _nemoHeartbeat; // measurement progress

src/comm/ros_bridge/include/RosBridgeClient.h

@@ -5,10 +5,6 @@
 *      Author: Poom Pianpak
 */
 
-
-#ifndef ROSBRIDGECPP_ROSBRIDGE_WS_CLIENT_HPP_
-#define ROSBRIDGECPP_ROSBRIDGE_WS_CLIENT_HPP_
-
 #include "Simple-WebSocket-Server/client_ws.hpp"
 
 #include "rapidjson/include/rapidjson/document.h"