From 60b604d9739fac6978008231af5fc8bffe2b9b2f Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Mon, 19 Oct 2020 21:12:28 +0200 Subject: [PATCH] temp, with errors --- src/Wima/CircularSurvey.cc | 423 +++++++++++---------------- src/Wima/CircularSurvey.h | 139 ++++----- src/Wima/RoutingThread.h | 2 +- src/Wima/Snake/CircularGenerator.cpp | 3 + src/Wima/Snake/CircularGenerator.h | 3 + src/comm/LinkConfiguration.h | 2 +- src/comm/LinkInterface.h | 2 +- src/qgcunittest/UnitTest.h | 2 +- 8 files changed, 236 insertions(+), 340 deletions(-) diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index 828d7ce42..391029f28 100644 --- a/src/Wima/CircularSurvey.cc +++ b/src/Wima/CircularSurvey.cc @@ -9,10 +9,6 @@ #define CLIPPER_SCALE 1000000 #include "clipper/clipper.hpp" -using namespace ClipperLib; -template <> inline auto get<0>(const IntPoint &p) { return p.X; } -template <> inline auto get<1>(const IntPoint &p) { return p.Y; } - #include "Geometry/GenericCircle.h" #include "Snake/SnakeTile.h" @@ -20,6 +16,9 @@ template <> inline auto get<1>(const IntPoint &p) { return p.Y; } #include #include +#include "CircularGenerator.h" +#include "LinearGenerator.h" + QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog") template @@ -27,72 +26,30 @@ constexpr typename std::underlying_type::type integral(T value) { return static_cast::type>(value); } -bool circularTransects(const snake::FPolygon &polygon, - const std::vector &tiles, - snake::Length deltaR, snake::Angle deltaAlpha, - snake::Length minLength, snake::Transects &transects); - -bool linearTransects(const snake::FPolygon &polygon, - const std::vector &tiles, - snake::Length distance, snake::Angle angle, - snake::Length minLength, snake::Transects &transects); - const char *CircularSurvey::settingsGroup = "CircularSurvey"; -const char *CircularSurvey::transectDistanceName = "TransectDistance"; -const char *CircularSurvey::alphaName = "Alpha"; -const char *CircularSurvey::minLengthName = "MinLength"; const char *CircularSurvey::typeName = "Type"; const char *CircularSurvey::CircularSurveyName = "CircularSurvey"; -const char *CircularSurvey::refPointLatitudeName = "ReferencePointLat"; -const char *CircularSurvey::refPointLongitudeName = "ReferencePointLong"; -const char *CircularSurvey::refPointAltitudeName = "ReferencePointAlt"; const char *CircularSurvey::variantName = "Variant"; -const char *CircularSurvey::numRunsName = "NumRuns"; -const char *CircularSurvey::runName = "Run"; CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile, QObject *parent) : TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent), - _referencePoint(QGeoCoordinate(0, 0, 0)), + _state(STATE::IDLE), _metaDataMap(FactMetaData::createMapFromJsonFile( QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)), - _transectDistance(settingsGroup, _metaDataMap[transectDistanceName]), - _alpha(settingsGroup, _metaDataMap[alphaName]), - _minLength(settingsGroup, _metaDataMap[minLengthName]), _type(settingsGroup, _metaDataMap[typeName]), _variant(settingsGroup, _metaDataMap[variantName]), - _numRuns(settingsGroup, _metaDataMap[numRunsName]), - _run(settingsGroup, _metaDataMap[runName]), - _pWorker(std::make_unique()), _state(STATE::DEFAULT), - _hidePolygon(false) { + _areaData(std::make_shared()), + _pWorker(std::make_unique()) { + Q_UNUSED(kmlOrShpFile) _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml"; // Connect facts. - connect(&_transectDistance, &Fact::valueChanged, this, - &CircularSurvey::_rebuildTransects); - connect(&_alpha, &Fact::valueChanged, this, - &CircularSurvey::_rebuildTransects); - connect(&_minLength, &Fact::valueChanged, this, - &CircularSurvey::_rebuildTransects); - connect(this, &CircularSurvey::refPointChanged, this, - &CircularSurvey::_rebuildTransects); - connect(this, &CircularSurvey::depotChanged, this, - &CircularSurvey::_rebuildTransects); connect(&this->_type, &Fact::rawValueChanged, this, &CircularSurvey::_rebuildTransects); connect(&this->_variant, &Fact::rawValueChanged, this, &CircularSurvey::_changeVariant); - connect(&this->_run, &Fact::rawValueChanged, this, - &CircularSurvey::_changeRun); - connect(&this->_numRuns, &Fact::rawValueChanged, this, - &CircularSurvey::_rebuildTransects); - - // Areas. - connect(this, &CircularSurvey::measurementAreaChanged, this, - &CircularSurvey::_rebuildTransects); - connect(this, &CircularSurvey::joinedAreaChanged, this, - &CircularSurvey::_rebuildTransects); // Connect worker. connect(this->_pWorker.get(), &RoutingThread::result, this, @@ -106,88 +63,27 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, &CircularSurvey::coordinateHasRelativeAltitudeChanged); connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, &CircularSurvey::exitCoordinateHasRelativeAltitudeChanged); + + // Register Generators. + auto cg = std::make_shared(this->_areaData); + registerGenerator(cg->name(), cg); + auto lg = std::make_shared(this->_areaData); + registerGenerator(lg->name(), lg); } CircularSurvey::~CircularSurvey() {} -void CircularSurvey::resetReference() { setRefPoint(_mArea.center()); } - void CircularSurvey::reverse() { this->_state = STATE::REVERSE; this->_rebuildTransects(); } -void CircularSurvey::setRefPoint(const QGeoCoordinate &refPt) { - if (refPt != _referencePoint) { - _referencePoint = refPt; - _referencePoint.setAltitude(0); - - emit refPointChanged(); - } +void CircularSurvey::setPlanData(const WimaPlanData &d) { + *this->_areaData = d; } -QGeoCoordinate CircularSurvey::refPoint() const { return _referencePoint; } - -Fact *CircularSurvey::transectDistance() { return &_transectDistance; } - -Fact *CircularSurvey::alpha() { return &_alpha; } - -bool CircularSurvey::hidePolygon() const { return _hidePolygon; } - QList CircularSurvey::variantNames() const { return _variantNames; } -QList CircularSurvey::runNames() const { return _runNames; } - -QGeoCoordinate CircularSurvey::depot() const { return this->_depot; } - -const QList> &CircularSurvey::rawTransects() const { - return this->_rawTransects; -} - -void CircularSurvey::setHidePolygon(bool hide) { - if (this->_hidePolygon != hide) { - this->_hidePolygon = hide; - emit hidePolygonChanged(); - } -} - -void CircularSurvey::setMeasurementArea(const WimaMeasurementAreaData &mArea) { - if (this->_mArea != mArea) { - this->_mArea = mArea; - emit measurementAreaChanged(); - } -} - -void CircularSurvey::setJoinedArea(const WimaJoinedAreaData &jArea) { - if (this->_jArea != jArea) { - this->_jArea = jArea; - emit joinedAreaChanged(); - } -} - -void CircularSurvey::setMeasurementArea(const WimaMeasurementArea &mArea) { - if (this->_mArea != mArea) { - this->_mArea = mArea; - emit measurementAreaChanged(); - } -} - -void CircularSurvey::setJoinedArea(const WimaJoinedArea &jArea) { - if (this->_jArea != jArea) { - this->_jArea = jArea; - emit joinedAreaChanged(); - } -} - -void CircularSurvey::setDepot(const QGeoCoordinate &depot) { - if (this->_depot.latitude() != depot.latitude() || - this->_depot.longitude() != depot.longitude()) { - this->_depot = depot; - this->_depot.setAltitude(0); - emit depotChanged(); - } -} - bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString) { // We need to pull version first to determine what validation/conversion @@ -253,9 +149,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, return false; } - _transectDistance.setRawValue(complexObject[transectDistanceName].toDouble()); - _alpha.setRawValue(complexObject[alphaName].toDouble()); - _minLength.setRawValue(complexObject[minLengthName].toDouble()); _type.setRawValue(complexObject[typeName].toInt()); _variant.setRawValue(complexObject[variantName].toInt()); _numRuns.setRawValue(complexObject[numRunsName].toInt()); @@ -289,9 +182,6 @@ void CircularSurvey::save(QJsonArray &planItems) { VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = CircularSurveyName; - saveObject[transectDistanceName] = _transectDistance.rawValue().toDouble(); - saveObject[alphaName] = _alpha.rawValue().toDouble(); - saveObject[minLengthName] = _minLength.rawValue().toDouble(); saveObject[typeName] = double(_type.rawValue().toUInt()); saveObject[variantName] = double(_variant.rawValue().toUInt()); saveObject[numRunsName] = double(_numRuns.rawValue().toUInt()); @@ -370,6 +260,27 @@ void CircularSurvey::_buildAndAppendMissionItems(QList &items, } } +bool CircularSurvey::_switchToGenerator( + const CircularSurvey::PtrGenerator &newG) { + if (this->_pGenerator != newG) { + if (this->_pGenerator != nullptr) { + disconnect(this->_pGenerator.get(), &GeneratorBase::generatorChanged, + this, &CircularSurvey::_rebuildTransects); + } + + this->_pGenerator = newG; + connect(this->_pGenerator.get(), &GeneratorBase::generatorChanged, this, + &CircularSurvey::_rebuildTransects); + + this->_state = STATE::IDLE; + _rebuildTransects(); + + return true; + } else { + return false; + } +} + void CircularSurvey::_changeVariant() { this->_state = STATE::VARIANT_CHANGE; this->_rebuildTransects(); @@ -385,130 +296,53 @@ void CircularSurvey::_updateWorker() { this->_transectsDirty = true; // Reset data. this->_transects.clear(); - this->_rawTransects.clear(); this->_variantVector.clear(); this->_variantNames.clear(); - this->_runNames.clear(); emit variantNamesChanged(); - emit runNamesChanged(); - // Prepare data. - auto ref = this->_referencePoint; - auto geoPolygon = this->_mArea.coordinateList(); - for (auto &v : geoPolygon) { - v.setAltitude(0); - } - auto pPolygon = std::make_shared(); - snake::areaToEnu(ref, geoPolygon, *pPolygon); - - // Progress and tiles. - const auto &progress = this->_mArea.progress(); - const auto *tiles = this->_mArea.tiles(); - auto pTiles = std::make_shared>(); - if (progress.size() == tiles->count()) { - for (int i = 0; i < tiles->count(); ++i) { - if (progress[i] == 100) { - const auto *tile = tiles->value(i); - if (tile != nullptr) { - snake::FPolygon tileENU; - snake::areaToEnu(ref, tile->coordinateList(), tileENU); - pTiles->push_back(std::move(tileENU)); - } else { - qCWarning(CircularSurveyLog) - << "_updateWorker(): progress.size() != tiles->count()."; - return; - } + if (this->_areaData->isValid()) { + + // Prepare data. + auto origin = this->_areaData->origin(); + // Convert safe area. + auto geoSafeArea = this->_jArea.coordinateList(); + if (!(geoSafeArea.size() >= 3)) { + qCWarning(CircularSurveyLog) + << "_updateWorker(): safe area invalid." << geoSafeArea; + return; + } + for (auto &v : geoSafeArea) { + if (v.isValid()) { + v.setAltitude(0); + } else { + qCWarning(CircularSurveyLog) + << "_updateWorker(): safe area contains invalid coordinate." + << geoSafeArea; + return; } } - } else { - qCWarning(CircularSurveyLog) - << "_updateWorker(): progress.size() != tiles->count()."; - return; - } - // Convert safe area. - auto geoDepot = this->_depot; - auto geoSafeArea = this->_jArea.coordinateList(); - if (!geoDepot.isValid()) { - qCWarning(CircularSurveyLog) - << "_updateWorker(): depot invalid." << geoDepot; - return; - } - if (!(geoSafeArea.size() >= 3)) { - qCWarning(CircularSurveyLog) - << "_updateWorker(): safe area invalid." << geoSafeArea; - return; - } - for (auto &v : geoSafeArea) { - v.setAltitude(0); - } - snake::FPoint depot; - snake::toENU(ref, geoDepot, depot); - - // Routing par. - RoutingParameter par; - par.numSolutions = 5; - if (this->_numRuns.rawValue().toUInt() < 1) { - disconnect(&this->_numRuns, &Fact::rawValueChanged, this, - &CircularSurvey::_rebuildTransects); - - this->_numRuns.setCookedValue(QVariant(1)); - - connect(&this->_numRuns, &Fact::rawValueChanged, this, - &CircularSurvey::_rebuildTransects); - } - par.numRuns = this->_numRuns.rawValue().toUInt(); - - auto &safeAreaENU = par.safeArea; - snake::areaToEnu(ref, geoSafeArea, safeAreaENU); - - // Fetch transect parameter. - auto distance = snake::Length(this->_transectDistance.rawValue().toDouble() * - bu::si::meter); - auto minLength = - snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter); - auto alpha = - snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree); - - // Select survey type. - if (this->_type.rawValue().toUInt() == integral(Type::Circular)) { - // Clip angle. - if (alpha >= snake::Angle(0.3 * bu::degree::degree) && - alpha <= snake::Angle(45 * bu::degree::degree)) { - auto generator = [depot, pPolygon, pTiles, distance, alpha, - minLength](snake::Transects &transects) -> bool { - bool value = circularTransects(*pPolygon, *pTiles, distance, alpha, - minLength, transects); - transects.insert(transects.begin(), snake::FLineString{depot}); - return value; - }; - // Start routing worker. - this->_pWorker->route(par, generator); + // Routing par. + RoutingParameter par; + par.numSolutions = 5; + auto &safeAreaENU = par.safeArea; + snake::areaToEnu(origin, geoSafeArea, safeAreaENU); + + // Create generator. + GeneratorBase::Generator g; // Transect generator. + if (_pGenerator->get(g)) { + // Start/Restart routing worker. + this->_pWorker->route(par, g); } else { - if (alpha < snake::Angle(0.3 * bu::degree::degree)) { - this->_alpha.setCookedValue(QVariant(0.3)); - } else { - this->_alpha.setCookedValue(QVariant(45)); - } + qCWarning(CircularSurveyLog) + << "_updateWorker(): generator creation failed."; } - } else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) { - auto generator = [depot, pPolygon, pTiles, distance, alpha, - minLength](snake::Transects &transects) -> bool { - bool value = linearTransects(*pPolygon, *pTiles, distance, alpha, - minLength, transects); - transects.insert(transects.begin(), snake::FLineString{depot}); - return value; - }; - // Start routing worker. - this->_pWorker->route(par, generator); } else { - qCWarning(CircularSurveyLog) - << "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:" - << this->_type.rawValue().toUInt(); + qCWarning(CircularSurveyLog) << "_updateWorker(): plan data invalid."; } } -void CircularSurvey::_changeVariantRunWorker() { +void CircularSurvey::_changeVariantWorker() { auto variant = this->_variant.rawValue().toUInt(); auto run = this->_run.rawValue().toUInt(); @@ -582,7 +416,7 @@ void CircularSurvey::_changeVariantRunWorker() { &CircularSurvey::_changeRun); if (this->_variantVector.size() > 0 && this->_variantVector.front().size() > 0) { - this->_changeVariantRunWorker(); + this->_changeVariantWorker(); } } } @@ -621,13 +455,13 @@ void CircularSurvey::_storeWorker() { } // Store solutions. - QVector solutionVector; + QVector solutionVector; const auto nSolutions = pRoutingData->solutionVector.size(); for (std::size_t j = 0; j < nSolutions; ++j) { const auto &solution = pRoutingData->solutionVector.at(j); const auto nRuns = solution.size(); // Store runs. - Runs runs(nRuns, Transects{QList()}); + Variant runs(nRuns, Transects{QList()}); for (std::size_t k = 0; k < nRuns; ++k) { const auto &route = solution.at(k); const auto &path = route.path; @@ -737,7 +571,7 @@ void CircularSurvey::_storeWorker() { &CircularSurvey::_changeVariant); connect(&this->_run, &Fact::rawValueChanged, this, &CircularSurvey::_changeRun); - this->_changeVariantRunWorker(); + this->_changeVariantWorker(); // Mark transect as stored and ready. this->_transectsDirty = false; } @@ -760,11 +594,108 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); } QString CircularSurvey::abbreviation() const { return tr("C.S."); } bool CircularSurvey::readyForSave() const { - return TransectStyleComplexItem::readyForSave() && !_transectsDirty; + return TransectStyleComplexItem::readyForSave() && !_transectsDirty && + this->_state == STATE::IDLE; } double CircularSurvey::additionalTimeDelay() const { return 0; } +bool CircularSurvey::registerGenerator(const QString &name, + std::shared_ptr g) { + if (name.isEmpty()) { + qCWarning(CircularSurveyLog) << "registerGenerator(): empty name string."; + return false; + } + + if (g) { + qCWarning(CircularSurveyLog) << "registerGenerator(): empty generator."; + return false; + } + + if (this->_generatorNameList.contains(name)) { + qCWarning(CircularSurveyLog) << "registerGenerator(): generator " + "already registered."; + return false; + } else { + this->_generatorNameList.push_back(name); + this->_generatorList.push_back(g); + if (this->_generatorList.size() == 1) { + this->_pGenerator = g; + } + + emit generatorNameListChanged(); + return true; + } +} + +bool CircularSurvey::unregisterGenerator(const QString &name) { + auto index = this->_generatorNameList.indexOf(name); + if (index >= 0) { + // Is this the current generator? + const auto &g = this->_generatorList.at(index); + if (g.get() == this->_pGenerator.get()) { + if (index > 0) { + _switchToGenerator(this->_generatorList.at(index - 1)); + } else { + _switchToGenerator(nullptr); + qCWarning(CircularSurveyLog) + << "unregisterGenerator(): last generator unregistered."; + } + } + + this->_generatorNameList.removeAt(index); + this->_generatorList.removeAt(index); + + emit generatorNameListChanged(); + return true; + } else { + qCWarning(CircularSurveyLog) + << "unregisterGenerator(): generator " << name << " not registered."; + return false; + } +} + +bool CircularSurvey::unregisterGenerator(int index) { + if (index > 0 && index < this->_generatorNameList.size()) { + return unregisterGenerator(this->_generatorNameList.at(index)); + } else { + qCWarning(CircularSurveyLog) << "unregisterGenerator(): index (" << index + << ") out" + "of bounds ( " + << this->_generatorList.size() << " )."; + return false; + } +} + +bool CircularSurvey::switchToGenerator(const QString &name) { + auto index = this->_generatorNameList.indexOf(name); + if (index >= 0) { + _switchToGenerator(this->_generatorList.at(index)); + return true; + } else { + qCWarning(CircularSurveyLog) + << "switchToGenerator(): generator " << name << " not registered."; + return false; + } +} + +bool CircularSurvey::switchToGenerator(int index) { + if (index >= 0) { + _switchToGenerator(this->_generatorList.at(index)); + return true; + } else { + qCWarning(CircularSurveyLog) << "unregisterGenerator(): index (" << index + << ") out" + "of bounds ( " + << this->_generatorNameList.size() << " )."; + return false; + } +} + +QList CircularSurvey::generatorNameList() { + return this->_generatorNameList; +} + void CircularSurvey::_rebuildTransectsPhase1(void) { auto start = std::chrono::high_resolution_clock::now(); @@ -775,22 +706,22 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { break; case STATE::VARIANT_CHANGE: qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: variant change."; - this->_changeVariantRunWorker(); + this->_changeVariantWorker(); break; case STATE::RUN_CHANGE: qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: run change."; - this->_changeVariantRunWorker(); + this->_changeVariantWorker(); break; case STATE::REVERSE: qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: reverse."; this->_reverseWorker(); break; - case STATE::DEFAULT: + case STATE::IDLE: qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: update."; this->_updateWorker(); break; } - this->_state = STATE::DEFAULT; + this->_state = STATE::IDLE; qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1(): " @@ -821,8 +752,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) { this->_rebuildTransects(); } -Fact *CircularSurvey::minLength() { return &_minLength; } - Fact *CircularSurvey::type() { return &_type; } Fact *CircularSurvey::variant() { return &_variant; } diff --git a/src/Wima/CircularSurvey.h b/src/Wima/CircularSurvey.h index 4d68a40a0..2b6c9820a 100644 --- a/src/Wima/CircularSurvey.h +++ b/src/Wima/CircularSurvey.h @@ -2,26 +2,26 @@ #include #include +#include #include "SettingsFact.h" #include "TransectStyleComplexItem.h" #include "Geometry/WimaJoinedAreaData.h" #include "Geometry/WimaMeasurementAreaData.h" +#include "WimaPlanData.h" class RoutingThread; class RoutingData; +class GeneratorBase; class CircularSurvey : public TransectStyleComplexItem { Q_OBJECT -public: - using PtrRoutingData = QSharedPointer; - enum class Type { - Circular = 0, - Linear = 1, - Count = 2 // Must me last, onyl for counting - }; + using PtrGenerator = std::shared_ptr; + using PtrRoutingData = std::shared_ptr; + +public: /// @param vehicle Vehicle which this is being contructed for /// @param flyView true: Created for use in the Fly View, false: Created for /// use in the Plan View @@ -31,50 +31,25 @@ public: QObject *parent); ~CircularSurvey(); - Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY - refPointChanged) - Q_PROPERTY(Fact *transectDistance READ transectDistance CONSTANT) - Q_PROPERTY(Fact *alpha READ alpha CONSTANT) - Q_PROPERTY(Fact *minLength READ minLength CONSTANT) Q_PROPERTY(Fact *type READ type CONSTANT) - Q_PROPERTY(Fact *variant READ variant CONSTANT) - Q_PROPERTY(Fact *numRuns READ numRuns CONSTANT) - Q_PROPERTY(Fact *run READ run CONSTANT) Q_PROPERTY(int typeCount READ typeCount CONSTANT) - Q_PROPERTY(bool calculating READ calculating NOTIFY calculatingChanged) - Q_PROPERTY(bool hidePolygon READ hidePolygon NOTIFY hidePolygonChanged) + Q_PROPERTY(Fact *variant READ variant CONSTANT) Q_PROPERTY( QList variantNames READ variantNames NOTIFY variantNamesChanged) - Q_PROPERTY(QList runNames READ runNames NOTIFY runNamesChanged) + Q_PROPERTY(QList generatorNameList READ generatorNameList NOTIFY + generatorNameListChanged) + Q_PROPERTY(bool calculating READ calculating NOTIFY calculatingChanged) - Q_INVOKABLE void resetReference(void); Q_INVOKABLE void reverse(void); // Property setters - void setRefPoint(const QGeoCoordinate &refPt); - void setHidePolygon(bool hide); - void setMeasurementArea(const WimaMeasurementAreaData &mArea); - void setJoinedArea(const WimaJoinedAreaData &jArea); - void setMeasurementArea(const WimaMeasurementArea &mArea); - void setJoinedArea(const WimaJoinedArea &jArea); - void setDepot(const QGeoCoordinate &depot); + void setPlanData(const WimaPlanData &d); // Property getters - QGeoCoordinate refPoint() const; - Fact *transectDistance(); - Fact *alpha(); - Fact *minLength(); Fact *type(); - Fact *variant(); - Fact *numRuns(); - Fact *run(); int typeCount() const; - bool calculating() const; - bool hidePolygon() const; + Fact *variant(); QList variantNames() const; - QList runNames() const; - QGeoCoordinate depot() const; - const QList> &rawTransects() const; // Overrides bool load(const QJsonObject &complexObject, int sequenceNumber, @@ -92,91 +67,77 @@ public: bool readyForSave(void) const override final; double additionalTimeDelay(void) const override final; + // Generator + bool registerGenerator(const QString &name, std::shared_ptr g); + bool unregisterGenerator(const QString &name); + bool unregisterGenerator(int index); + Q_INVOKABLE bool switchToGenerator(const QString &name); + Q_INVOKABLE bool switchToGenerator(int index); + QList generatorNameList(); + static const char *settingsGroup; - static const char *transectDistanceName; - static const char *alphaName; - static const char *minLengthName; static const char *typeName; static const char *variantName; - static const char *numRunsName; - static const char *runName; static const char *CircularSurveyName; - static const char *refPointLongitudeName; - static const char *refPointLatitudeName; - static const char *refPointAltitudeName; signals: - void refPointChanged(); void calculatingChanged(); - void hidePolygonChanged(); - void depotChanged(); void variantNamesChanged(); - void runNamesChanged(); - void measurementAreaChanged(); - void joinedAreaChanged(); + void generatorNameListChanged(); + void generatorChanged(); private slots: // Overrides from TransectStyleComplexItem void _rebuildTransectsPhase1(void) final; void _recalcComplexDistance(void) final; void _recalcCameraShots(void) final; + + // Worker functions. void _setTransects(PtrRoutingData pRoute); + void _changeVariant(); + void _updateWorker(); + void _changeVariantWorker(); + void _reverseWorker(); + void _storeWorker(); private: void _appendLoadedMissionItems(QList &items, QObject *missionItemParent); void _buildAndAppendMissionItems(QList &items, QObject *missionItemParent); - void _changeVariant(); - void _changeRun(); - void _updateWorker(); - void _changeVariantRunWorker(); - void _reverseWorker(); - void _storeWorker(); - void _changeRunWorker(); + bool _switchToGenerator(const PtrGenerator &newG); + + // State. + enum class STATE { + IDLE, + STORE, + REVERSE, + VARIANT_CHANGE, + RUN_CHANGE, + }; + STATE _state; // center of the circular lanes, e.g. base station - QGeoCoordinate _referencePoint; QMap _metaDataMap; - // distance between two neighbour circles - SettingsFact _transectDistance; - // angle discretisation of the circles - SettingsFact _alpha; - // minimal transect lenght, transects are rejected if they are shorter than - // this value - SettingsFact _minLength; SettingsFact _type; SettingsFact _variant; QList _variantNames; - SettingsFact _numRuns; - SettingsFact _run; - QList _runNames; // Area data - WimaMeasurementAreaData _mArea; - WimaJoinedAreaData _jArea; - QGeoCoordinate _depot; + std::shared_ptr _areaData; + + // Generators + QList _generatorList; + QList _generatorNameList; + PtrGenerator _pGenerator; // Worker using PtrWorker = std::shared_ptr; PtrWorker _pWorker; - PtrRoutingData _pRoutingData; + PtrRoutingData _pRoutingData; // remove this, not necessary. // Routing data. - QList> _rawTransects; - using Runs = QVector; - QVector _variantVector; - - // State. - enum class STATE { - DEFAULT, - STORE, - REVERSE, - VARIANT_CHANGE, - RUN_CHANGE, - }; - STATE _state; - - bool _hidePolygon; + using Variant = Transects; + QVector _variantVector; }; diff --git a/src/Wima/RoutingThread.h b/src/Wima/RoutingThread.h index ee8444ab7..bc5a3f113 100644 --- a/src/Wima/RoutingThread.h +++ b/src/Wima/RoutingThread.h @@ -31,7 +31,7 @@ class RoutingThread : public QThread { using Lock = std::unique_lock; public: - using PtrRoutingData = QSharedPointer; + using PtrRoutingData = shared_ptr; using Generator = std::function; using Consumer = std::function; diff --git a/src/Wima/Snake/CircularGenerator.cpp b/src/Wima/Snake/CircularGenerator.cpp index 5e10112f9..3b7167a27 100644 --- a/src/Wima/Snake/CircularGenerator.cpp +++ b/src/Wima/Snake/CircularGenerator.cpp @@ -26,6 +26,9 @@ const char *CircularGenerator::settingsGroup = "CircularGenerator"; const char *CircularGenerator::distanceName = "TransectDistance"; const char *CircularGenerator::deltaAlphaName = "DeltaAlpha"; const char *CircularGenerator::minLengthName = "MinLength"; +const char *CircularGenerator::refPointLatitudeName = "ReferencePointLat"; +const char *CircularGenerator::refPointLongitudeName = "ReferencePointLong"; +const char *CircularGenerator::refPointAltitudeName = "ReferencePointAlt"; CircularGenerator::CircularGenerator(QObject *parent) : CircularGenerator(nullptr, parent) {} diff --git a/src/Wima/Snake/CircularGenerator.h b/src/Wima/Snake/CircularGenerator.h index 57b7c5f65..37c4bcaba 100644 --- a/src/Wima/Snake/CircularGenerator.h +++ b/src/Wima/Snake/CircularGenerator.h @@ -36,6 +36,9 @@ public: static const char *distanceName; static const char *deltaAlphaName; static const char *minLengthName; + static const char *refPointLongitudeName; + static const char *refPointLatitudeName; + static const char *refPointAltitudeName; signals: void referenceChanged(); diff --git a/src/comm/LinkConfiguration.h b/src/comm/LinkConfiguration.h index a32e65f53..c8805f185 100644 --- a/src/comm/LinkConfiguration.h +++ b/src/comm/LinkConfiguration.h @@ -213,5 +213,5 @@ private: bool _highLatency; }; -typedef QSharedPointer SharedLinkConfigurationPointer; +typedef shared_ptr SharedLinkConfigurationPointer; diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 36c01fb72..78b2db66a 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -305,5 +305,5 @@ private: QMap _mavlinkMessagesTimers; }; -typedef QSharedPointer SharedLinkInterfacePointer; +typedef shared_ptr SharedLinkInterfacePointer; diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h index f9d555dc0..74cfb5441 100644 --- a/src/qgcunittest/UnitTest.h +++ b/src/qgcunittest/UnitTest.h @@ -215,7 +215,7 @@ public: } private: - QSharedPointer _unitTest; + shared_ptr _unitTest; }; #endif -- 2.22.0