#include "MeasurementComplexItem.h" #include "CircularGenerator.h" #include "LinearGenerator.h" #include "RoutingThread.h" #include "geometry/GenericCircle.h" #include "geometry/MeasurementArea.h" #include "geometry/SafeArea.h" #include "geometry/clipper/clipper.hpp" #include "geometry/snake.h" #include "nemo_interface/SnakeTile.h" // QGC #include "JsonHelper.h" #include "PlanMasterController.h" #include "QGCApplication.h" #include "QGCLoggingCategory.h" // boost #include #include #define CLIPPER_SCALE 1000000 QGC_LOGGING_CATEGORY(MeasurementComplexItemLog, "MeasurementComplexItemLog") template constexpr typename std::underlying_type::type integral(T value) { return static_cast::type>(value); } const char *MeasurementComplexItem::settingsGroup = "MeasurementComplexItem"; const char *MeasurementComplexItem::jsonComplexItemTypeValue = "MeasurementComplexItem"; const char *MeasurementComplexItem::variantName = "Variant"; const char *MeasurementComplexItem::altitudeName = "Altitude"; const QString MeasurementComplexItem::name(tr("Measurement")); MeasurementComplexItem::MeasurementComplexItem( PlanMasterController *masterController, bool flyView, const QString &kmlOrShpFile, QObject *parent) : ComplexMissionItem(masterController, flyView, settingsGroup, parent), _masterController(masterController), _sequenceNumber(0), _followTerrain(false), _state(STATE::IDLE), _metaDataMap(FactMetaData::createMapFromJsonFile( QStringLiteral(":/json/MeasurementComplexItem.SettingsGroup.json"), this)), _variant(settingsGroup, _metaDataMap[variantName]), _variant(settingsGroup, _metaDataMap[altitudeName]), _areaData(new AreaData(this)), _editorData(new AreaData(this)), _currentData(_areaData), _pWorker(new RoutingThread(this)) { Q_UNUSED(kmlOrShpFile) _editorQml = "qrc:/qml/MeasurementItemEditor.qml"; // Connect facts. connect(&this->_variant, &Fact::rawValueChanged, this, &MeasurementComplexItem::_changeVariant); // Connect worker. connect(this->_pWorker, &RoutingThread::result, this, &MeasurementComplexItem::_setTransects); connect(this->_pWorker, &RoutingThread::calculatingChanged, this, &MeasurementComplexItem::calculatingChanged); // Register Generators. auto lg = new routing::LinearGenerator(this->_areaData, this); registerGenerator(lg->name(), lg); auto cg = new routing::CircularGenerator(this->_areaData, this); registerGenerator(cg->name(), cg); qCritical() << "ToDo: _altitude connections missing."; qCritical() << "ToDo: add generator saveing."; } MeasurementComplexItem::~MeasurementComplexItem() {} void MeasurementComplexItem::revertPath() { this->_setState(STATE::REVERT_PATH); this->_update(); } const AreaData *MeasurementComplexItem::areaData() const { return this->_currentData; } AreaData *MeasurementComplexItem::areaData() { return this->_currentData; } QStringList MeasurementComplexItem::variantNames() const { return _variantNames; } bool MeasurementComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString) { // We need to pull version first to determine what validation/conversion // needs to be performed QList versionKeyInfoList = { {JsonHelper::jsonVersionKey, QJsonValue::Double, true}, }; if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList, errorString)) { return false; } int version = complexObject[JsonHelper::jsonVersionKey].toInt(); if (version != 1) { errorString = tr("Survey items do not support version %1").arg(version); return false; } QList keyInfoList = { {VisualMissionItem::jsonTypeKey, QJsonValue::String, true}, {ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true}, {variantName, QJsonValue::Double, false}, {altitudeName, QJsonValue::Double, true}, }; if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) { return false; } QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString(); QString complexType = complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString(); if (itemType != VisualMissionItem::jsonTypeComplexItemValue || complexType != jsonComplexItemTypeValue) { errorString = tr("%1 does not support loading this complex mission item " "type: %2:%3") .arg(qgcApp()->applicationName()) .arg(itemType) .arg(complexType); return false; } _ignoreRecalc = true; setSequenceNumber(sequenceNumber); if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */, errorString)) { _surveyAreaPolygon.clear(); return false; } if (!load(complexObject, sequenceNumber, errorString)) { _ignoreRecalc = false; return false; } _variant.setRawValue(complexObject[variantName].toInt()); _altitude.setRawValue(complexObject[altitudeName].toDouble()); _ignoreRecalc = false; if (_cameraShots == 0) { // Shot count was possibly not available from plan file _recalcCameraShots(); } return true; } bool MeasurementComplexItem::dirty() const { return _dirty; } bool MeasurementComplexItem::isSimpleItem() const { return false; } bool MeasurementComplexItem::isStandaloneCoordinate() const { return false; } QString MeasurementComplexItem::mapVisualQML() const { return QStringLiteral("MeasurementItemMapVisual.qml"); } void MeasurementComplexItem::save(QJsonArray &planItems) { QJsonObject saveObject; _save(saveObject); saveObject[JsonHelper::jsonVersionKey] = 1; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; saveObject[variantName] = double(_variant.rawValue().toUInt()); saveObject[altitudeName] = double(_altitude.rawValue().toUInt()); // Polygon shape _surveyAreaPolygon.saveToJson(saveObject); planItems.append(saveObject); } double MeasurementComplexItem::amslEntryAlt() const { return _altitude.rawValue().toDouble() + this->_masterController->missionController() ->plannedHomePosition() .altitude(); } double MeasurementComplexItem::amslExitAlt() const { return amslEntryAlt(); } double MeasurementComplexItem::minAMSLAltitude() const { return amslEntryAlt(); } double MeasurementComplexItem::maxAMSLAltitude() const { return amslEntryAlt(); } bool MeasurementComplexItem::specifiesCoordinate() const { return _route.count() > 0; } bool MeasurementComplexItem::specifiesAltitudeOnly() const { return false; } QGeoCoordinate MeasurementComplexItem::coordinate() const { return this->_route.size() > 0 ? _route.first() : QGeoCoordinate(); } QGeoCoordinate MeasurementComplexItem::exitCoordinate() const { return this->_route.size() > 0 ? _route.last() : QGeoCoordinate(); } int MeasurementComplexItem::sequenceNumber() const { return _sequenceNumber; } double MeasurementComplexItem::specifiedFlightSpeed() { return std::numeric_limits::quiet_NaN(); } double MeasurementComplexItem::specifiedGimbalYaw() { return std::numeric_limits::quiet_NaN(); } double MeasurementComplexItem::specifiedGimbalPitch() { return std::numeric_limits::quiet_NaN(); } void MeasurementComplexItem::appendMissionItems(QList &items, QObject *missionItemParent) { qCDebug(MeasurementComplexItemLog) << "appendMissionItems()"; int seqNum = this->_sequenceNumber; MAV_FRAME mavFrame = followTerrain() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; int transectIndex = 0; for (const auto &vertex : this->_route) { MissionItem *item = new MissionItem( seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame, 0, // hold time 0.0, // No acceptance radius specified 0.0, // Pass through waypoint std::numeric_limits::quiet_NaN(), // Yaw unchanged vertex.latitude(), vertex.longitude(), vertex.altitude(), true, // autoContinue false, // isCurrentItem missionItemParent); items.append(item); } } void MeasurementComplexItem::setMissionFlightStatus( const MissionController::MissionFlightStatus_t &missionFlightStatus) { ComplexMissionItem::setMissionFlightStatus(missionFlightStatus); } void MeasurementComplexItem::applyNewAltitude(double newAltitude) { this->_altitude->setRawValue(newAltitude); } double MeasurementComplexItem::additionalTimeDelay() const { return 0; } bool MeasurementComplexItem::_setGenerator(PtrGenerator newG) { if (this->_pGenerator != newG) { if (this->_pGenerator != nullptr) { disconnect(this->_pGenerator, &routing::GeneratorBase::generatorChanged, this, &MeasurementComplexItem::_update); } this->_pGenerator = newG; connect(this->_pGenerator, &routing::GeneratorBase::generatorChanged, this, &MeasurementComplexItem::_update); emit generatorChanged(); this->_setState(STATE::IDLE); _update(); return true; } else { return false; } } void MeasurementComplexItem::_setState(MeasurementComplexItem::STATE state) { if (this->_state != state) { auto oldState = this->_state; this->_state = state; if (_calculating(oldState) != _calculating(state)) { emit calculatingChanged(); } if (_editing(oldState) != _editing(state)) { emit editingChanged(); } } } bool MeasurementComplexItem::_calculating( MeasurementComplexItem::STATE state) const { return state == STATE::ROUTING; } void MeasurementComplexItem::_setAreaData( MeasurementComplexItem::PtrAreaData data) { if (_currentData != data) { _currentData = data; emit areaDataChanged(); } } void MeasurementComplexItem::_changeVariant() { this->_setState(STATE::CHANGE_VARIANT); this->_update(); } bool MeasurementComplexItem::_updateRouteWorker() { // Reset data. this->_transects.clear(); this->_variantVector.clear(); this->_variantNames.clear(); emit variantNamesChanged(); if (this->_areaData->isValid()) { // Prepare data. auto origin = this->_areaData->origin(); origin.setAltitude(0); if (!origin.isValid()) { qCDebug(MeasurementComplexItemLog) << "_updateWorker(): origin invalid." << origin; return false; } // Convert safe area. auto serviceArea = getGeoArea(*this->_areaData->areaList()); auto geoSafeArea = serviceArea->coordinateList(); if (!(geoSafeArea.size() >= 3)) { qCDebug(MeasurementComplexItemLog) << "_updateWorker(): safe area invalid." << geoSafeArea; return false; } for (auto &v : geoSafeArea) { if (v.isValid()) { v.setAltitude(0); } else { qCDebug(MeasurementComplexItemLog) << "_updateWorker(): safe area contains invalid coordinate." << geoSafeArea; return false; } } // Routing par. RoutingParameter par; par.numSolutions = 5; auto &safeAreaENU = par.safeArea; snake::areaToEnu(origin, geoSafeArea, safeAreaENU); // Create generator. if (this->_pGenerator) { routing::GeneratorBase::Generator g; // Transect generator. if (this->_pGenerator->get(g)) { // Start/Restart routing worker. this->_pWorker->route(par, g); return true; } else { qCDebug(MeasurementComplexItemLog) << "_updateWorker(): generator creation failed."; return false; } } else { qCDebug(MeasurementComplexItemLog) << "_updateWorker(): pGenerator == nullptr, number of registered " "generators: " << this->_generatorList.size(); return false; } } else { qCDebug(MeasurementComplexItemLog) << "_updateWorker(): plan data invalid."; return false; } } void MeasurementComplexItem::_changeVariantWorker() { auto variant = this->_variant.rawValue().toUInt(); // Find old variant and run. Old run corresponts with empty list. std::size_t old_variant = std::numeric_limits::max(); for (std::size_t i = 0; i < std::size_t(this->_variantVector.size()); ++i) { const auto &variantCoordinates = this->_variantVector.at(i); if (variantCoordinates.isEmpty()) { old_variant = i; break; } } // Swap route. if (variant != old_variant) { // Swap in new variant. if (variant < std::size_t(this->_variantVector.size())) { if (old_variant != std::numeric_limits::max()) { // this->_transects containes a route, swap it back to // this->_solutionVector auto &oldVariantCoordinates = this->_variantVector[old_variant]; oldVariantCoordinates.swap(this->_transects); } auto &newVariantCoordinates = this->_variantVector[variant]; this->_transects.swap(newVariantCoordinates); } else { // error qCDebug(MeasurementComplexItemLog) << "Variant out of bounds (variant =" << variant << ")."; qCDebug(MeasurementComplexItemLog) << "Resetting variant to zero."; disconnect(&this->_variant, &Fact::rawValueChanged, this, &MeasurementComplexItem::_changeVariant); this->_variant.setCookedValue(QVariant(0)); connect(&this->_variant, &Fact::rawValueChanged, this, &MeasurementComplexItem::_changeVariant); if (this->_variantVector.size() > 0) { this->_changeVariantWorker(); } } } } void MeasurementComplexItem::_reverseWorker() { if (this->_transects.size() > 0) { auto &t = this->_transects.front(); std::reverse(t.begin(), t.end()); } } double MeasurementComplexItem::timeBetweenShots() { return 0; } QString MeasurementComplexItem::commandDescription() const { return tr("Route"); } QString MeasurementComplexItem::commandName() const { return tr("Route"); } QString MeasurementComplexItem::abbreviation() const { return tr("R"); } TransectStyleComplexItem::ReadyForSaveState MeasurementComplexItem::readyForSaveState() const { if (TransectStyleComplexItem::readyForSaveState() == TransectStyleComplexItem::ReadyForSaveState::ReadyForSave) { if (this->_state == STATE::IDLE) { return ReadyForSaveState::ReadyForSave; } else { return ReadyForSaveState::NotReadyForSaveData; } } else { return TransectStyleComplexItem::readyForSaveState(); } } bool MeasurementComplexItem::exitCoordinateSameAsEntry() const { return this->_route.size() > 0 ? this->_route.first() == this->_route.last() : false; } void MeasurementComplexItem::setDirty(bool dirty) { if (this->_dirty != dirty) { this->_dirty = dirty; emit dirtyChanged(this->_dirty); } } void MeasurementComplexItem::setCoordinate(const QGeoCoordinate &coordinate) { Q_UNUSED(coordinate); } void MeasurementComplexItem::setSequenceNumber(int sequenceNumber) { if (this->_sequenceNumber != sequenceNumber) { this->_sequenceNumber = sequenceNumber; emit sequenceNumberChanged(this->_sequenceNumber); } } QString MeasurementComplexItem::patternName() const { return name; } bool MeasurementComplexItem::registerGenerator(const QString &name, routing::GeneratorBase *g) { if (name.isEmpty()) { qCDebug(MeasurementComplexItemLog) << "registerGenerator(): empty name string."; return false; } if (!g) { qCDebug(MeasurementComplexItemLog) << "registerGenerator(): empty generator."; return false; } if (this->_generatorNameList.contains(name)) { qCDebug(MeasurementComplexItemLog) << "registerGenerator(): generator " "already registered."; return false; } else { this->_generatorNameList.push_back(name); this->_generatorList.push_back(g); if (this->_generatorList.size() == 1) { _setGenerator(g); } emit generatorNameListChanged(); return true; } } bool MeasurementComplexItem::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 == this->_pGenerator) { if (index > 0) { _setGenerator(this->_generatorList.at(index - 1)); } else { _setGenerator(nullptr); qCDebug(MeasurementComplexItemLog) << "unregisterGenerator(): last generator unregistered."; } } this->_generatorNameList.removeAt(index); auto gen = this->_generatorList.takeAt(index); gen->deleteLater(); emit generatorNameListChanged(); return true; } else { qCDebug(MeasurementComplexItemLog) << "unregisterGenerator(): generator " << name << " not registered."; return false; } } bool MeasurementComplexItem::unregisterGenerator(int index) { if (index > 0 && index < this->_generatorNameList.size()) { return unregisterGenerator(this->_generatorNameList.at(index)); } else { qCDebug(MeasurementComplexItemLog) << "unregisterGenerator(): index (" << index << ") out" "of bounds ( " << this->_generatorList.size() << " )."; return false; } } bool MeasurementComplexItem::switchToGenerator(const QString &name) { auto index = this->_generatorNameList.indexOf(name); if (index >= 0) { _setGenerator(this->_generatorList.at(index)); return true; } else { qCDebug(MeasurementComplexItemLog) << "switchToGenerator(): generator " << name << " not registered."; return false; } } bool MeasurementComplexItem::switchToGenerator(int index) { if (index >= 0) { _setGenerator(this->_generatorList.at(index)); return true; } else { qCDebug(MeasurementComplexItemLog) << "unregisterGenerator(): index (" << index << ") out" "of bounds ( " << this->_generatorNameList.size() << " )."; return false; } } QStringList MeasurementComplexItem::generatorNameList() { return this->_generatorNameList; } routing::GeneratorBase *MeasurementComplexItem::generator() { return _pGenerator; } int MeasurementComplexItem::generatorIndex() { return this->_generatorList.indexOf(this->_pGenerator); } void MeasurementComplexItem::editingStart() { if (!_editing(this->_state)) { *_editorData = *_areaData; _setAreaData(_editorData); _setState(STATE::EDITING); } } void MeasurementComplexItem::editingStop() { if (_editing(this->_state)) { if (_editorData->isValid()) { *_areaData = *_editorData; } _setAreaData(_areaData); _setState(STATE::IDLE); } } void MeasurementComplexItem::_update(void) { auto start = std::chrono::high_resolution_clock::now(); switch (this->_state) { case STATE::SKIPP: qCDebug(MeasurementComplexItemLog) << "rebuildTransectsPhase1: skipp."; this->_setState(STATE::IDLE); break; case STATE::CHANGE_VARIANT: qCDebug(MeasurementComplexItemLog) << "rebuildTransectsPhase1: variant change."; this->_changeVariantWorker(); this->_setState(STATE::IDLE); break; case STATE::REVERT_PATH: qCDebug(MeasurementComplexItemLog) << "rebuildTransectsPhase1: reverse."; this->_reverseWorker(); this->_setState(STATE::IDLE); break; case STATE::IDLE: case STATE::ROUTING: this->_setState(STATE::ROUTING); qCDebug(MeasurementComplexItemLog) << "rebuildTransectsPhase1: update."; if (!this->_updateRouteWorker()) { this->_setState(STATE::IDLE); } break; } qCDebug(MeasurementComplexItemLog) << "rebuildTransectsPhase1(): " << std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start) .count() << " ms"; } void MeasurementComplexItem::_recalcCameraShots() { _cameraShots = 0; } void MeasurementComplexItem::_setTransects( MeasurementComplexItem::PtrRoutingData pRoute) { // Store solutions. auto ori = this->_areaData->origin(); ori.setAltitude(0); const auto &transectsENU = pRoute->transects; QVector variantVector; const auto nSolutions = pRoute->solutionVector.size(); for (std::size_t j = 0; j < nSolutions; ++j) { Variant var{QList()}; const auto &solution = pRoute->solutionVector.at(j); if (solution.size() > 0) { const auto &route = solution.at(0); const auto &path = route.path; const auto &info = route.info; if (info.size() > 1) { // Find index of first waypoint. std::size_t idxFirst = 0; const auto &infoFirst = info.at(1); const auto &firstTransect = transectsENU[infoFirst.index]; if (firstTransect.size() > 0) { const auto &firstWaypoint = infoFirst.reversed ? firstTransect.back() : firstTransect.front(); double th = 0.01; for (std::size_t i = 0; i < path.size(); ++i) { auto dist = bg::distance(path[i], firstWaypoint); if (dist < th) { idxFirst = i; break; } } // Find index of last waypoint. std::size_t idxLast = path.size() - 1; const auto &infoLast = info.at(info.size() - 2); const auto &lastTransect = transectsENU[infoLast.index]; if (lastTransect.size() > 0) { const auto &lastWaypoint = infoLast.reversed ? lastTransect.front() : lastTransect.back(); for (long i = path.size() - 1; i >= 0; --i) { auto dist = bg::distance(path[i], lastWaypoint); if (dist < th) { idxLast = i; break; } } // Convert to geo coordinates. auto &list = var.front(); for (std::size_t i = idxFirst; i <= idxLast; ++i) { auto &vertex = path[i]; QGeoCoordinate c; snake::fromENU(ori, vertex, c); list.append(CoordInfo_t{c, CoordTypeInterior}); } } else { qCDebug(MeasurementComplexItemLog) << "_setTransects(): lastTransect.size() == 0"; } } else { qCDebug(MeasurementComplexItemLog) << "_setTransects(): firstTransect.size() == 0"; } } else { qCDebug(MeasurementComplexItemLog) << "_setTransects(): transectsInfo.size() <= 1"; } } else { qCDebug(MeasurementComplexItemLog) << "_setTransects(): solution.size() == 0"; } if (var.size() > 0 && var.front().size() > 0) { variantVector.push_back(std::move(var)); } } // Assign routes if no error occured. if (variantVector.size() > 0) { // Swap first route to _transects. this->_variantVector.swap(variantVector); // If the transects are getting rebuilt then any previously loaded // mission items are now invalid. if (_loadedMissionItemsParent) { _loadedMissionItems.clear(); _loadedMissionItemsParent->deleteLater(); _loadedMissionItemsParent = nullptr; } // Add route variant names. this->_variantNames.clear(); for (std::size_t i = 1; i <= std::size_t(this->_variantVector.size()); ++i) { this->_variantNames.append(QString::number(i)); } emit variantNamesChanged(); disconnect(&this->_variant, &Fact::rawValueChanged, this, &MeasurementComplexItem::_changeVariant); this->_variant.setCookedValue(QVariant(0)); connect(&this->_variant, &Fact::rawValueChanged, this, &MeasurementComplexItem::_changeVariant); this->_changeVariantWorker(); this->_setState(STATE::SKIPP); this->_update(); } else { qCDebug(MeasurementComplexItemLog) << "_setTransects(): failed, variantVector empty."; this->_setState(STATE::IDLE); } } Fact *MeasurementComplexItem::variant() { return &_variant; } Fact *MeasurementComplexItem::altitude() { return &this->_altitude; } bool MeasurementComplexItem::calculating() const { return this->_calculating(this->_state); } bool MeasurementComplexItem::editing() const { return _editing(this->_state); } bool MeasurementComplexItem::followTerrain() const { return _followTerrain; }