Merge pull request #8846 from airmap/terrain-test

Add a follow terrain test

src/MissionManager/TransectStyleComplexItem.cc

@@ -51,7 +51,7 @@ TransectStyleComplexItem::TransectStyleComplexItem(PlanMasterController* masterC
     , _terrainAdjustMaxClimbRateFact    (settingsGroup, _metaDataMap[terrainAdjustMaxClimbRateName])
     , _terrainAdjustMaxDescentRateFact  (settingsGroup, _metaDataMap[terrainAdjustMaxDescentRateName])
 {
-    _terrainQueryTimer.setInterval(_terrainQueryTimeoutMsecs);
+    _terrainQueryTimer.setInterval(qgcApp()->runningUnitTests() ? 10 : _terrainQueryTimeoutMsecs);
     _terrainQueryTimer.setSingleShot(true);
     connect(&_terrainQueryTimer, &QTimer::timeout, this, &TransectStyleComplexItem::_reallyQueryTransectsPathHeightInfo);
 

src/MissionManager/TransectStyleComplexItemTest.cc

@@ -12,22 +12,17 @@
 
 TransectStyleComplexItemTest::TransectStyleComplexItemTest(void)
 {
-    _polygonVertices << QGeoCoordinate(47.633550640000003, -122.08982199)
-                     << QGeoCoordinate(47.634129020000003, -122.08887249)
-                     << QGeoCoordinate(47.633619320000001, -122.08811074)
-                     << QGeoCoordinate(47.633189139999999, -122.08900124);
 }
 
 void TransectStyleComplexItemTest::init(void)
 {
     TransectStyleComplexItemTestBase::init();
 
-    _transectStyleItem = new TransectStyleItem(_masterController, this);
+    _transectStyleItem = new TestTransectStyleItem(_masterController, this);
     _transectStyleItem->cameraTriggerInTurnAround()->setRawValue(false);
     _transectStyleItem->cameraCalc()->cameraName()->setRawValue(_transectStyleItem->cameraCalc()->customCameraName());
     _transectStyleItem->cameraCalc()->valueSetIsDistance()->setRawValue(true);
     _transectStyleItem->cameraCalc()->distanceToSurface()->setRawValue(100);
-    _setSurveyAreaPolygon();
     _transectStyleItem->setDirty(false);
 
     _rgSignals[cameraShotsChangedIndex] =               SIGNAL(cameraShotsChanged());
@@ -85,7 +80,7 @@ void TransectStyleComplexItemTest::_testDirty(void)
     }
     rgFacts.clear();
 
-    _adjustSurveAreaPolygon();
+    _transectStyleItem->_adjustSurveAreaPolygon();
     QVERIFY(_transectStyleItem->dirty());
     _transectStyleItem->setDirty(false);
     QVERIFY(!_transectStyleItem->surveyAreaPolygon()->dirty());
@@ -98,20 +93,13 @@ void TransectStyleComplexItemTest::_testDirty(void)
     _multiSpy->clearAllSignals();
 }
 
-void TransectStyleComplexItemTest::_setSurveyAreaPolygon(void)
-{
-    for (const QGeoCoordinate vertex: _polygonVertices) {
-        _transectStyleItem->surveyAreaPolygon()->appendVertex(vertex);
-    }
-}
-
 void TransectStyleComplexItemTest::_testRebuildTransects(void)
 {
     // Changing the survey polygon should trigger:
     //  _rebuildTransects calls
     //  coveredAreaChanged signal
     //  lastSequenceNumberChanged signal
-    _adjustSurveAreaPolygon();
+    _transectStyleItem->_adjustSurveAreaPolygon();
     QVERIFY(_transectStyleItem->rebuildTransectsPhase1Called);
     QVERIFY(_transectStyleItem->recalcCameraShotsCalled);
     // FIXME: Temproarily not possible
@@ -176,7 +164,7 @@ void TransectStyleComplexItemTest::_testRebuildTransects(void)
 
 void TransectStyleComplexItemTest::_testDistanceSignalling(void)
 {
-    _adjustSurveAreaPolygon();
+    _transectStyleItem->_adjustSurveAreaPolygon();
     QVERIFY(_multiSpy->checkSignalsByMask(complexDistanceChangedMask | greatestDistanceToChangedMask));
     _transectStyleItem->setDirty(false);
     _multiSpy->clearAllSignals();
@@ -195,12 +183,7 @@ void TransectStyleComplexItemTest::_testDistanceSignalling(void)
     rgFacts.clear();
 }
 
-void TransectStyleComplexItemTest::_adjustSurveAreaPolygon(void)
-{
-    QGeoCoordinate vertex = _transectStyleItem->surveyAreaPolygon()->vertexCoordinate(0);
-    vertex.setLatitude(vertex.latitude() + 1);
-    _transectStyleItem->surveyAreaPolygon()->adjustVertex(0, vertex);
-}
+
 
 void TransectStyleComplexItemTest::_testAltMode(void)
 {
@@ -226,21 +209,57 @@ void TransectStyleComplexItemTest::_testAltMode(void)
     QVERIFY(!_transectStyleItem->followTerrain());
 }
 
-TransectStyleItem::TransectStyleItem(PlanMasterController* masterController, QObject* parent)
+void TransectStyleComplexItemTest::_testFollowTerrain(void) {
+    _multiSpy->clearAllSignals();
+    _transectStyleItem->cameraCalc()->distanceToSurface()->setRawValue(50);
+    _transectStyleItem->setFollowTerrain(true);
+    _multiSpy->clearAllSignals();
+    while(_transectStyleItem->readyForSaveState() != TransectStyleComplexItem::ReadyForSave) {
+        QVERIFY(_multiSpy->waitForSignalByIndex(lastSequenceNumberChangedIndex, 50));
+    }
+    QList<double> expectedTerrainValues{497,509,512,512};
+    QCOMPARE(_transectStyleItem->transects().size(), 1);
+    for (const auto& transect : _transectStyleItem->transects()) {
+        QCOMPARE(transect.size(), 4);
+        for (const auto pt : transect) {
+            QCOMPARE(pt.coord.altitude(), expectedTerrainValues.front());
+            expectedTerrainValues.pop_front();
+        }
+    }
+}
+
+TestTransectStyleItem::TestTransectStyleItem(PlanMasterController* masterController, QObject* parent)
     : TransectStyleComplexItem      (masterController, false /* flyView */, QStringLiteral("UnitTestTransect"), parent)
     , rebuildTransectsPhase1Called  (false)
     , recalcComplexDistanceCalled   (false)
     , recalcCameraShotsCalled       (false)
 {
-
+    // We use a 100m by 100m square test polygon
+    const double edgeDistance = 100;
+    surveyAreaPolygon()->appendVertex(UnitTestTerrainQuery::linearSlopeRegion.center());
+    surveyAreaPolygon()->appendVertex(surveyAreaPolygon()->vertexCoordinate(0).atDistanceAndAzimuth(edgeDistance, 90));
+    surveyAreaPolygon()->appendVertex(surveyAreaPolygon()->vertexCoordinate(1).atDistanceAndAzimuth(edgeDistance, 180));
+    surveyAreaPolygon()->appendVertex(surveyAreaPolygon()->vertexCoordinate(2).atDistanceAndAzimuth(edgeDistance, -90.0));
+    _transects.append(QList<TransectStyleComplexItem::CoordInfo_t>{
+        {surveyAreaPolygon()->vertexCoordinate(0), CoordTypeSurveyEntry},
+        {surveyAreaPolygon()->vertexCoordinate(2), CoordTypeSurveyExit}}
+    );
 }
 
-void TransectStyleItem::_rebuildTransectsPhase1(void)
+void TestTransectStyleItem::_rebuildTransectsPhase1(void)
 {
     rebuildTransectsPhase1Called = true;
 }
 
-void TransectStyleItem::_recalcCameraShots(void)
+void TestTransectStyleItem::_recalcCameraShots(void)
 {
     recalcCameraShotsCalled = true;
 }
+
+void TestTransectStyleItem::_adjustSurveAreaPolygon(void)
+{
+    QGeoCoordinate vertex = surveyAreaPolygon()->vertexCoordinate(0);
+    vertex.setLatitude(vertex.latitude() + 1);
+    surveyAreaPolygon()->adjustVertex(0, vertex);
+}
+

src/MissionManager/TransectStyleComplexItemTest.h

@@ -16,7 +16,7 @@
 
 #include <QGeoCoordinate>
 
-class TransectStyleItem;
+class TestTransectStyleItem;
 
 class TransectStyleComplexItemTest : public TransectStyleComplexItemTestBase
 {
@@ -34,11 +34,9 @@ private slots:
     void _testRebuildTransects  (void);
     void _testDistanceSignalling(void);
     void _testAltMode           (void);
+    void _testFollowTerrain     (void);
 
 private:
-    void _setSurveyAreaPolygon  (void);
-    void _adjustSurveAreaPolygon(void);
-
     enum {
         // These signals are from TransectStyleComplexItem
         cameraShotsChangedIndex = 0,
@@ -74,16 +72,15 @@ private:
     const char*         _rgSignals[_cSignals];
 
     MultiSignalSpy*         _multiSpy =             nullptr;
-    QList<QGeoCoordinate>   _polygonVertices;
-    TransectStyleItem*      _transectStyleItem =    nullptr;
+    TestTransectStyleItem*  _transectStyleItem =    nullptr;
 };
 
-class TransectStyleItem : public TransectStyleComplexItem
+class TestTransectStyleItem : public TransectStyleComplexItem
 {
     Q_OBJECT
 
 public:
-    TransectStyleItem(PlanMasterController* masterController, QObject* parent = nullptr);
+    TestTransectStyleItem(PlanMasterController* masterController, QObject* parent = nullptr);
 
     // Overrides from ComplexMissionItem
     QString patternName         (void) const final { return QString(); }
@@ -98,6 +95,10 @@ public:
     bool rebuildTransectsPhase1Called;
     bool recalcComplexDistanceCalled;
     bool recalcCameraShotsCalled;
+    void _adjustSurveAreaPolygon(void);
+    QList<QList<CoordInfo_t>> transects() const {
+        return _transects;
+    }
 
 private slots:
     // Overrides from TransectStyleComplexItem

src/Terrain/TerrainQuery.cc

@@ -41,7 +41,7 @@ TerrainAirMapQuery::TerrainAirMapQuery(QObject* parent)
 void TerrainAirMapQuery::requestCoordinateHeights(const QList<QGeoCoordinate>& coordinates)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit coordinateHeightsReceived(false, QList<double>());
+        UnitTestTerrainQuery(this).requestCoordinateHeights(coordinates);
         return;
     }
 
@@ -62,7 +62,7 @@ void TerrainAirMapQuery::requestCoordinateHeights(const QList<QGeoCoordinate>& c
 void TerrainAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit pathHeightsReceived(false, qQNaN(), qQNaN(), QList<double>());
+        UnitTestTerrainQuery(this).requestPathHeights(fromCoord, toCoord);
         return;
     }
 
@@ -82,7 +82,7 @@ void TerrainAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, con
 void TerrainAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit carpetHeightsReceived(false, qQNaN(), qQNaN(), QList<QList<double>>());
+        UnitTestTerrainQuery(this).requestCarpetHeights(swCoord, neCoord, statsOnly);
         return;
     }
 
@@ -283,7 +283,7 @@ TerrainOfflineAirMapQuery::TerrainOfflineAirMapQuery(QObject* parent)
 void TerrainOfflineAirMapQuery::requestCoordinateHeights(const QList<QGeoCoordinate>& coordinates)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit coordinateHeightsReceived(false, QList<double>());
+        UnitTestTerrainQuery(this).requestCoordinateHeights(coordinates);
         return;
     }
 
@@ -297,7 +297,7 @@ void TerrainOfflineAirMapQuery::requestCoordinateHeights(const QList<QGeoCoordin
 void TerrainOfflineAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit pathHeightsReceived(false, qQNaN(), qQNaN(), QList<double>());
+        UnitTestTerrainQuery(this).requestPathHeights(fromCoord, toCoord);
         return;
     }
 
@@ -307,7 +307,7 @@ void TerrainOfflineAirMapQuery::requestPathHeights(const QGeoCoordinate& fromCoo
 void TerrainOfflineAirMapQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly)
 {
     if (qgcApp()->runningUnitTests()) {
-        emit carpetHeightsReceived(false, qQNaN(), qQNaN(), QList<QList<double>>());
+        UnitTestTerrainQuery(this).requestCarpetHeights(swCoord, neCoord, statsOnly);
         return;
     }
 
@@ -802,3 +802,117 @@ void TerrainPolyPathQuery::_terrainDataReceived(bool success, const TerrainPathQ
         _pathQuery.requestData(_rgCoords[_curIndex], _rgCoords[_curIndex+1]);
     }
 }
+
+
+
+const QGeoCoordinate UnitTestTerrainQuery::pointNemo{-48.875556, -123.392500};
+const UnitTestTerrainQuery::Flat10Region UnitTestTerrainQuery::flat10Region{{
+      pointNemo,
+      QGeoCoordinate{
+          pointNemo.latitude() - UnitTestTerrainQuery::regionExtentDeg,
+          pointNemo.longitude() + UnitTestTerrainQuery::regionExtentDeg
+      }
+}};
+const double UnitTestTerrainQuery::Flat10Region::elevationMts = 10;
+
+const UnitTestTerrainQuery::LinearSlopeRegion UnitTestTerrainQuery::linearSlopeRegion{{
+    flat10Region.topRight(),
+    QGeoCoordinate{
+        flat10Region.topRight().latitude() - UnitTestTerrainQuery::regionExtentDeg,
+        flat10Region.topRight().longitude() + UnitTestTerrainQuery::regionExtentDeg
+    }
+}};
+const double UnitTestTerrainQuery::LinearSlopeRegion::minElevationMts = -100;
+const double UnitTestTerrainQuery::LinearSlopeRegion::maxElevationMts = 1000;
+const double UnitTestTerrainQuery::LinearSlopeRegion::dElevationMts   = maxElevationMts-minElevationMts;
+
+UnitTestTerrainQuery::UnitTestTerrainQuery(TerrainQueryInterface* parent)
+:TerrainQueryInterface(parent)
+{}
+
+void UnitTestTerrainQuery::requestCoordinateHeights(const QList<QGeoCoordinate>& coordinates) {
+    QList<double> result = requestCoordinateHeightsSync(coordinates);
+    emit qobject_cast<TerrainQueryInterface*>(parent())->coordinateHeightsReceived(result.size() == coordinates.size(), result);
+}
+
+void UnitTestTerrainQuery::requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) {
+    QPair<QList<QGeoCoordinate>, QList<double>> result = requestPathHeightsSync(fromCoord, toCoord);
+    emit qobject_cast<TerrainQueryInterface*>(parent())->pathHeightsReceived(
+        result.second.size() > 0,
+        result.first[0].distanceTo(result.first[1]),
+        result.first[result.first.size()-2].distanceTo(result.first.back()),
+        result.second
+    );
+}
+
+void UnitTestTerrainQuery::requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool) {
+    assert(swCoord.longitude() < neCoord.longitude());
+    assert(swCoord.latitude() < neCoord.latitude());
+    double min = std::numeric_limits<double>::max();
+    double max = std::numeric_limits<double>::min();
+    QList<QList<double>> carpet;
+    for (double lat = swCoord.latitude(); lat < neCoord.latitude(); lat++) {
+        QList<double> row = requestPathHeightsSync({lat,swCoord.longitude()}, {lat,neCoord.longitude()}).second;
+        if (row.size() == 0) {
+            emit carpetHeightsReceived(false, qQNaN(), qQNaN(), QList<QList<double>>());
+            return;
+        }
+        for (const auto val : row) {
+            min = std::min(val,min);
+            max = std::max(val,max);
+        }
+        carpet.push_back(row);
+    }
+    emit qobject_cast<TerrainQueryInterface*>(parent())->carpetHeightsReceived(true, min, max, carpet);
+}
+
+QPair<QList<QGeoCoordinate>, QList<double>> UnitTestTerrainQuery::requestPathHeightsSync(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) {
+    QList<QGeoCoordinate> coordinates;
+    coordinates.push_back(fromCoord);
+
+    //cast to pixels
+    long x0 = std::floor(fromCoord.longitude()/one_second_deg);
+    long x1 = std::floor(toCoord.longitude()/one_second_deg);
+    long y0 = std::floor(fromCoord.latitude()/one_second_deg);
+    long y1 = std::floor(toCoord.latitude()/one_second_deg);
+
+    //bresenham line algo
+    long dx = abs(x1-x0), sx = x0<x1 ? 1 : -1;
+    long dy = abs(y1-y0), sy = y0<y1 ? 1 : -1;
+    long err = (dx>dy ? dx : -dy)/2, e2;
+    while(true) {
+        e2 = err;
+        if (e2 >-dx) { err -= dy; x0 += sx; }
+        if (e2 < dy) { err += dx; y0 += sy; }
+        if ((x0==x1 && y0==y1)) {
+            break;
+        }
+        coordinates.push_back({y0*one_second_deg, x0*one_second_deg});
+    }
+    coordinates.push_back(toCoord);
+    return QPair<QList<QGeoCoordinate>, QList<double>>(coordinates, requestCoordinateHeightsSync(coordinates));
+}
+
+QList<double>  UnitTestTerrainQuery::requestCoordinateHeightsSync(const QList<QGeoCoordinate>& coordinates) {
+    QList<double> result;
+    for (const auto& coordinate : coordinates) {
+        if (flat10Region.contains(coordinate)) {
+            result.push_back(UnitTestTerrainQuery::Flat10Region::elevationMts);
+        } else if (linearSlopeRegion.contains(coordinate)) {
+            //cast to one_second_deg grid and round to int to emulate SRTM1 even better
+            long x = (coordinate.longitude() - linearSlopeRegion.topLeft().longitude())/one_second_deg;
+            long dx = regionExtentDeg/one_second_deg;
+            double fraction = 1.0 * x / dx;
+            result.push_back(
+                std::round(
+                    UnitTestTerrainQuery::LinearSlopeRegion::minElevationMts
+                    + (fraction * UnitTestTerrainQuery::LinearSlopeRegion::dElevationMts)
+                )
+            );
+        } else {
+            result.clear();
+            break;
+        }
+    }
+    return result;
+}

src/Terrain/TerrainQuery.h

@@ -15,6 +15,7 @@
 
 #include <QObject>
 #include <QGeoCoordinate>
+#include <QGeoRectangle>
 #include <QNetworkAccessManager>
 #include <QNetworkReply>
 #include <QTimer>
@@ -294,3 +295,54 @@ private:
     QList<TerrainPathQuery::PathHeightInfo_t>   _rgPathHeightInfo;
     TerrainPathQuery                            _pathQuery;
 };
+
+///
+/// @brief The MockTerrainQuery class provides unit test terrain query responses for the disconnected environment.
+/// @details It provides preset, emulated, 1 arc-second (SRMT1) resultion regions that are either
+/// flat, sloped or rugged in a fashion that aids testing terrain-sensitive functionality. All emulated
+/// regions are positioned around Point Nemo - should real terrain became useful and checked in one day.
+///
+class UnitTestTerrainQuery : public TerrainQueryInterface {
+public:
+
+    static constexpr double regionExtentDeg = 0.1; //every region 0.1deg x 0.1deg across (around 11km north to south)
+    static constexpr double one_second_deg  = 1.0/3600;
+
+    /// @brief Point Nemo is a point on Earth furthest from land
+    static const QGeoCoordinate pointNemo;
+
+    ///
+    /// @brief flat10Region is a region with constant 10m terrain elevation
+    ///
+    struct Flat10Region : public QGeoRectangle {
+        Flat10Region(const QGeoRectangle& region)
+        :QGeoRectangle(region)
+        {}
+
+        static const double elevationMts;
+    };
+    static const Flat10Region flat10Region;
+
+    ///
+    /// @brief linearSlopeRegion is a region with a linear west to east slope raising from -100m to 1000m
+    ///
+    struct LinearSlopeRegion : public QGeoRectangle {
+        LinearSlopeRegion(const QGeoRectangle& region)
+        :QGeoRectangle(region)
+        {}
+
+        static const double minElevationMts;
+        static const double maxElevationMts;
+        static const double dElevationMts;
+    };
+    static const LinearSlopeRegion linearSlopeRegion;
+
+    UnitTestTerrainQuery(TerrainQueryInterface* parent = nullptr);
+
+    void requestCoordinateHeights(const QList<QGeoCoordinate>& coordinates) Q_DECL_OVERRIDE;
+    void requestPathHeights(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord) Q_DECL_OVERRIDE;
+    void requestCarpetHeights(const QGeoCoordinate& swCoord, const QGeoCoordinate& neCoord, bool statsOnly) Q_DECL_OVERRIDE;
+    QList<double> requestCoordinateHeightsSync(const QList<QGeoCoordinate>& coordinates);
+    QPair<QList<QGeoCoordinate>, QList<double>> requestPathHeightsSync(const QGeoCoordinate& fromCoord, const QGeoCoordinate& toCoord);
+};
+