#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, parent), _masterController(masterController), _sequenceNumber(0), _followTerrain(false), _state(STATE::IDLE), _metaDataMap(FactMetaData::createMapFromJsonFile( QStringLiteral(":/json/MeasurementComplexItem.SettingsGroup.json"), this)), _altitude(settingsGroup, _metaDataMap[altitudeName]), _variant(settingsGroup, _metaDataMap[variantName]), _pAreaData(new AreaData(this)), _pEditorData(new AreaData(this)), _pCurrentData(_pAreaData), _pGenerator(nullptr), _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::_storeRoutingData); // Connect coordinate and exitCoordinate. connect(this, &MeasurementComplexItem::routeChanged, [this] { emit this->coordinateChanged(this->coordinate()); }); connect(this, &MeasurementComplexItem::routeChanged, [this] { emit this->exitCoordinateChanged(this->exitCoordinate()); }); connect(this, &MeasurementComplexItem::routeChanged, [this] { emit this->exitCoordinateSameAsEntryChanged( this->exitCoordinateSameAsEntry()); }); // Connect complexDistance. connect(this, &MeasurementComplexItem::routeChanged, [this] { emit this->complexDistanceChanged(); }); // Register Generators. auto lg = new routing::LinearGenerator(this->_pAreaData, this); registerGenerator(lg->name(), lg); auto cg = new routing::CircularGenerator(this->_pAreaData, this); registerGenerator(cg->name(), cg); qCritical() << "ToDo: _altitude connections missing."; qCritical() << "ToDo: add generator saveing."; } MeasurementComplexItem::~MeasurementComplexItem() {} void MeasurementComplexItem::reverseRoute() { _reverseRoute(); } const AreaData *MeasurementComplexItem::areaData() const { return this->_pCurrentData; } AreaData *MeasurementComplexItem::areaData() { return this->_pCurrentData; } QVariantList MeasurementComplexItem::route() { return _route; } QStringList MeasurementComplexItem::variantNames() const { return _variantNames; } bool MeasurementComplexItem::load(const QJsonObject &complexObject, int sequenceNumber, QString &errorString) { qWarning() << "MeasurementComplexItem::load(): area data load missing."; qWarning() << "MeasurementComplexItem::load(): mission item load missing."; qWarning() << "MeasurementComplexItem::load(): add editingStart/Stop."; // 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; } setSequenceNumber(sequenceNumber); if (!load(complexObject, sequenceNumber, errorString)) { return false; } _variant.setRawValue(complexObject[variantName].toInt()); _altitude.setRawValue(complexObject[altitudeName].toDouble()); return true; } double MeasurementComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const { double d = -1 * std::numeric_limits::infinity(); if (other.isValid()) { if (this->_route.size() > 0) { std::for_each(this->_route.cbegin(), this->_route.cend(), [&d, &other](const QVariant &variant) { auto vertex = variant.value(); d = std::max(d, vertex.distanceTo(other)); }); } } else { qCDebug(MeasurementComplexItemLog) << "greatestDistanceTo(): invalid QGeoCoordinate: " << other; } return d; } 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) { qWarning() << "MeasurementComplexItem::save(): area data save missing."; qWarning() << "MeasurementComplexItem::save(): mission item save missing."; if (ready()) { QJsonObject 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()); 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(); } QString MeasurementComplexItem::commandDescription() const { return QStringLiteral("Measurement"); } QString MeasurementComplexItem::commandName() const { return QStringLiteral("Measurement"); } QString MeasurementComplexItem::abbreviation() const { return QStringLiteral("M"); } 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().value() : QGeoCoordinate(); } QGeoCoordinate MeasurementComplexItem::exitCoordinate() const { return this->_route.size() > 0 ? _route.last().value() : 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) { if (ready()) { qCDebug(MeasurementComplexItemLog) << "appendMissionItems()"; int seqNum = this->_sequenceNumber; MAV_FRAME mavFrame = followTerrain() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; for (const auto &variant : this->_route) { auto vertex = variant.value(); 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); } } else { qCDebug(MeasurementComplexItemLog) << "appendMissionItems(): called while not ready()."; } } void MeasurementComplexItem::setMissionFlightStatus( const MissionController::MissionFlightStatus_t &missionFlightStatus) { ComplexMissionItem::setMissionFlightStatus(missionFlightStatus); } void MeasurementComplexItem::applyNewAltitude(double newAltitude) { this->_altitude.setRawValue(newAltitude); qWarning() << "applyNewAltitude(): impl. missing."; } 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::_updateRoute); } this->_pGenerator = newG; if (this->_pGenerator != nullptr) { connect(this->_pGenerator, &routing::GeneratorBase::generatorChanged, this, &MeasurementComplexItem::_updateRoute); } emit generatorChanged(); if (!editing()) { this->_setState(STATE::IDLE); _updateRoute(); } 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(); } if (_ready(oldState) != _ready(state)) { emit readyChanged(); } } } bool MeasurementComplexItem::_calculating(MeasurementComplexItem::STATE state) { return state == STATE::ROUTING; } bool MeasurementComplexItem::_editing(MeasurementComplexItem::STATE state) { return state == STATE::EDITING; } bool MeasurementComplexItem::_ready(MeasurementComplexItem::STATE state) { return state == STATE::IDLE; } void MeasurementComplexItem::_setAreaData( MeasurementComplexItem::PtrAreaData data) { if (_pCurrentData != data) { _pCurrentData = data; emit areaDataChanged(); } } bool MeasurementComplexItem::_updateRoute() { // Reset data. this->_route.clear(); this->_variantVector.clear(); this->_variantNames.clear(); emit variantNamesChanged(); if (this->_pAreaData->isValid()) { // Prepare data. auto origin = this->_pAreaData->origin(); origin.setAltitude(0); if (!origin.isValid()) { qCDebug(MeasurementComplexItemLog) << "_updateWorker(): origin invalid." << origin; return false; } // Convert safe area. auto serviceArea = getGeoArea(*this->_pAreaData->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 != nullptr) { 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::_changeVariant() { if (ready()) { 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->_route containes a route, swap it back to // this->_solutionVector auto &oldVariantCoordinates = this->_variantVector[old_variant]; oldVariantCoordinates.swap(this->_route); } auto &newVariantCoordinates = this->_variantVector[variant]; this->_route.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->_changeVariant(); } } } } } void MeasurementComplexItem::_reverseRoute() { if (ready()) { if (this->_route.size() > 0) { auto &t = this->_route; std::reverse(t.begin(), t.end()); } } } ComplexMissionItem::ReadyForSaveState MeasurementComplexItem::readyForSaveState() const { if (ready()) { return ReadyForSaveState::ReadyForSave; } else { return ReadyForSaveState::NotReadyForSaveData; } } 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; } double MeasurementComplexItem::complexDistance() const { double d = 0; if (this->_route.size() > 1) { auto vertex = _route.first().value(); std::for_each(this->_route.cbegin() + 1, this->_route.cend(), [&vertex, &d](const QVariant &variant) { auto otherVertex = variant.value(); d += vertex.distanceTo(otherVertex); vertex = otherVertex; }); } return d; } int MeasurementComplexItem::lastSequenceNumber() const { return _sequenceNumber + std::max(0, this->_route.size() - 1); } 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) << "switchToGenerator(): 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)) { *_pEditorData = *_pAreaData; _setAreaData(_pEditorData); _setState(STATE::EDITING); } } void MeasurementComplexItem::editingStop() { if (_editing(this->_state)) { if (_pEditorData->isValid()) { *_pAreaData = *_pEditorData; } _setAreaData(_pAreaData); _setState(STATE::IDLE); if (_pEditorData->isValid() && *_pEditorData != *_pAreaData) { _updateRoute(); } } } void MeasurementComplexItem::_storeRoutingData( MeasurementComplexItem::PtrRoutingData pRoute) { if (this->_state == STATE::ROUTING) { // Store solutions. auto ori = this->_pAreaData->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; 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. for (std::size_t i = idxFirst; i <= idxLast; ++i) { auto &vertex = path[i]; QGeoCoordinate c; snake::fromENU(ori, vertex, c); var.append(QVariant::fromValue(c)); } } 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) { variantVector.push_back(std::move(var)); } } // Assign routes if no error occured. if (variantVector.size() > 0) { // Swap first route to _route. this->_variantVector.swap(variantVector); // 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->_changeVariant(); this->_setState(STATE::IDLE); emit routeChanged(); } 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::ready() const { return _ready(this->_state); } bool MeasurementComplexItem::followTerrain() const { return _followTerrain; }