#include "CircularSurveyComplexItem.h" #include "JsonHelper.h" #include "QGCApplication.h" #include const char* CircularSurveyComplexItem::settingsGroup = "CircularSurvey"; const char* CircularSurveyComplexItem::deltaRName = "DeltaR"; const char* CircularSurveyComplexItem::deltaAlphaName = "DeltaAlpha"; const char* CircularSurveyComplexItem::transectMinLengthName = "TransectMinLength"; const char* CircularSurveyComplexItem::fixedDirectionName = "FixedDirection"; const char* CircularSurveyComplexItem::reverseName = "Reverse"; const char* CircularSurveyComplexItem::maxWaypointsName = "MaxWaypoints"; const char* CircularSurveyComplexItem::jsonComplexItemTypeValue = "circularSurvey"; const char* CircularSurveyComplexItem::jsonDeltaRKey = "deltaR"; const char* CircularSurveyComplexItem::jsonDeltaAlphaKey = "deltaAlpha"; const char* CircularSurveyComplexItem::jsonTransectMinLengthKey = "transectMinLength"; const char* CircularSurveyComplexItem::jsonfixedDirectionKey = "fixedDirection"; const char* CircularSurveyComplexItem::jsonReverseKey = "reverse"; const char* CircularSurveyComplexItem::jsonReferencePointLatKey = "referencePointLat"; const char* CircularSurveyComplexItem::jsonReferencePointLongKey = "referencePointLong"; const char* CircularSurveyComplexItem::jsonReferencePointAltKey = "referencePointAlt"; CircularSurveyComplexItem::CircularSurveyComplexItem(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile, QObject *parent) : TransectStyleComplexItem (vehicle, flyView, settingsGroup, parent) , _referencePoint (QGeoCoordinate(0, 0,0)) , _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)) , _deltaR (settingsGroup, _metaDataMap[deltaRName]) , _deltaAlpha (settingsGroup, _metaDataMap[deltaAlphaName]) , _transectMinLength (settingsGroup, _metaDataMap[transectMinLengthName]) , _fixedDirection (settingsGroup, _metaDataMap[fixedDirectionName]) , _reverse (settingsGroup, _metaDataMap[reverseName]) , _maxWaypoints (settingsGroup, _metaDataMap[maxWaypointsName]) , _isInitialized (false) , _reverseOnly (false) { Q_UNUSED(kmlOrShpFile) _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml"; connect(&_deltaR, &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc); connect(&_deltaAlpha, &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc); connect(&_transectMinLength, &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc); connect(&_fixedDirection, &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc); connect(&_maxWaypoints, &Fact::valueChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc); connect(&_reverse, &Fact::valueChanged, this, &CircularSurveyComplexItem::_reverseTransects); connect(this, &CircularSurveyComplexItem::refPointChanged, this, &CircularSurveyComplexItem::_triggerSlowRecalc); //connect(&_cameraCalc.distanceToSurface(), &Fact::rawValueChanged, this->) } void CircularSurveyComplexItem::resetReference() { setRefPoint(_surveyAreaPolygon.center()); } void CircularSurveyComplexItem::comprehensiveUpdate() { _triggerSlowRecalc(); } void CircularSurveyComplexItem::setRefPoint(const QGeoCoordinate &refPt) { if (refPt != _referencePoint){ _referencePoint = refPt; emit refPointChanged(); } } void CircularSurveyComplexItem::setIsInitialized(bool isInitialized) { if (isInitialized != _isInitialized) { _isInitialized = isInitialized; emit isInitializedChanged(); } } QGeoCoordinate CircularSurveyComplexItem::refPoint() const { return _referencePoint; } Fact *CircularSurveyComplexItem::deltaR() { return &_deltaR; } Fact *CircularSurveyComplexItem::deltaAlpha() { return &_deltaAlpha; } bool CircularSurveyComplexItem::isInitialized() { return _isInitialized; } bool CircularSurveyComplexItem::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 }, { jsonDeltaRKey, QJsonValue::Double, true }, { jsonDeltaAlphaKey, QJsonValue::Double, true }, { jsonTransectMinLengthKey, QJsonValue::Double, true }, { jsonfixedDirectionKey, QJsonValue::Bool, true }, { jsonReverseKey, QJsonValue::Bool, true }, { jsonReferencePointLatKey, QJsonValue::Double, true }, { jsonReferencePointLongKey, QJsonValue::Double, true }, { jsonReferencePointAltKey, 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, errorString)) { _ignoreRecalc = false; return false; } _deltaR.setRawValue (complexObject[jsonDeltaRKey].toDouble()); _deltaAlpha.setRawValue (complexObject[jsonDeltaAlphaKey].toDouble()); _transectMinLength.setRawValue (complexObject[jsonTransectMinLengthKey].toDouble()); _referencePoint.setLongitude (complexObject[jsonReferencePointLongKey].toDouble()); _referencePoint.setLatitude (complexObject[jsonReferencePointLatKey].toDouble()); _referencePoint.setAltitude (complexObject[jsonReferencePointAltKey].toDouble()); _fixedDirection.setRawValue (complexObject[jsonfixedDirectionKey].toBool()); _reverse.setRawValue (complexObject[jsonReverseKey].toBool()); setIsInitialized(true); _ignoreRecalc = false; _recalcComplexDistance(); if (_cameraShots == 0) { // Shot count was possibly not available from plan file _recalcCameraShots(); } return true; } void CircularSurveyComplexItem::save(QJsonArray &planItems) { QJsonObject saveObject; _save(saveObject); saveObject[JsonHelper::jsonVersionKey] = 1; saveObject[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = jsonComplexItemTypeValue; saveObject[jsonDeltaRKey] = _deltaR.rawValue().toDouble(); saveObject[jsonDeltaAlphaKey] = _deltaAlpha.rawValue().toDouble(); saveObject[jsonTransectMinLengthKey] = _transectMinLength.rawValue().toDouble(); saveObject[jsonfixedDirectionKey] = _fixedDirection.rawValue().toBool(); saveObject[jsonReverseKey] = _reverse.rawValue().toBool(); saveObject[jsonReferencePointLongKey] = _referencePoint.longitude(); saveObject[jsonReferencePointLatKey] = _referencePoint.latitude(); saveObject[jsonReferencePointAltKey] = _referencePoint.altitude(); // Polygon shape _surveyAreaPolygon.saveToJson(saveObject); planItems.append(saveObject); } void CircularSurveyComplexItem::appendMissionItems(QList &items, QObject *missionItemParent) { if (_transectsDirty) return; if (_loadedMissionItems.count()) { // We have mission items from the loaded plan, use those _appendLoadedMissionItems(items, missionItemParent); } else { // Build the mission items on the fly _buildAndAppendMissionItems(items, missionItemParent); } } void CircularSurveyComplexItem::_appendLoadedMissionItems(QList& items, QObject* missionItemParent) { //qCDebug(SurveyComplexItemLog) << "_appendLoadedMissionItems"; if (_transectsDirty) return; int seqNum = _sequenceNumber; for (const MissionItem* loadedMissionItem: _loadedMissionItems) { MissionItem* item = new MissionItem(*loadedMissionItem, missionItemParent); item->setSequenceNumber(seqNum++); items.append(item); } } void CircularSurveyComplexItem::_buildAndAppendMissionItems(QList& items, QObject* missionItemParent) { // original code: SurveyComplexItem::_buildAndAppendMissionItems() //qCDebug(SurveyComplexItemLog) << "_buildAndAppendMissionItems"; // Now build the mission items from the transect points if (_transectsDirty) return; MissionItem* item; int seqNum = _sequenceNumber; MAV_FRAME mavFrame = followTerrain() || !_cameraCalc.distanceToSurfaceRelative() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT; for (const QList& transect : _transects) { //bool transectEntry = true; for (const CoordInfo_t& transectCoordInfo: transect) { item = new MissionItem(seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame, 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) 0.0, // No acceptance radius specified 0.0, // Pass through waypoint std::numeric_limits::quiet_NaN(), // Yaw unchanged transectCoordInfo.coord.latitude(), transectCoordInfo.coord.longitude(), transectCoordInfo.coord.altitude(), true, // autoContinue false, // isCurrentItem missionItemParent); items.append(item); } } } void CircularSurveyComplexItem::applyNewAltitude(double newAltitude) { _cameraCalc.valueSetIsDistance()->setRawValue(true); _cameraCalc.distanceToSurface()->setRawValue(newAltitude); _cameraCalc.setDistanceToSurfaceRelative(true); } double CircularSurveyComplexItem::timeBetweenShots() { return 1; } bool CircularSurveyComplexItem::readyForSave() const { return TransectStyleComplexItem::readyForSave(); } double CircularSurveyComplexItem::additionalTimeDelay() const { return 0; } void CircularSurveyComplexItem::_rebuildTransectsPhase1(void){ if (_doFastRecalc){ _rebuildTransectsFast(); } else { _rebuildTransectsSlow(); _doFastRecalc = true; } } void CircularSurveyComplexItem::_rebuildTransectsFast() { using namespace GeoUtilities; using namespace PolygonCalculus; using namespace PlanimetryCalculus; _transects.clear(); QPolygonF surveyPolygon; toCartesianList(_surveyAreaPolygon.coordinateList(), _referencePoint, surveyPolygon); if (!_rebuildTransectsInputCheck(surveyPolygon)) return; // If the transects are getting rebuilt then any previously loaded mission items are now invalid if (_loadedMissionItemsParent) { _loadedMissionItems.clear(); _loadedMissionItemsParent->deleteLater(); _loadedMissionItemsParent = nullptr; } QVector> transectPath; if(!_generateTransectPath(transectPath, surveyPolygon)) return; /// optimize path to snake or zig-zag pattern bool fixedDirectionBool = _fixedDirection.rawValue().toBool(); QVector currentSection = transectPath.takeFirst(); if ( currentSection.isEmpty() ) return; QVector optiPath; // optimized path while( !transectPath.empty() ) { optiPath.append(currentSection); QPointF endVertex = currentSection.last(); double minDist = std::numeric_limits::infinity(); int index = 0; bool reversePath = false; // iterate over all paths in fullPath and assign the one with the shortest distance to endVertex to currentSection for (int i = 0; i < transectPath.size(); i++) { auto iteratorPath = transectPath[i]; double dist = PlanimetryCalculus::distance(endVertex, iteratorPath.first()); if ( dist < minDist ) { minDist = dist; index = i; reversePath = false; } dist = PlanimetryCalculus::distance(endVertex, iteratorPath.last()); if (dist < minDist) { minDist = dist; index = i; reversePath = true; } } currentSection = transectPath.takeAt(index); if (reversePath && !fixedDirectionBool) { PolygonCalculus::reversePath(currentSection); } } optiPath.append(currentSection); // append last section if (optiPath.size() > _maxWaypoints.rawValue().toInt()) return; _rebuildTransectsToGeo(optiPath, _referencePoint); _transectsDirty = true; } void CircularSurveyComplexItem::_rebuildTransectsSlow() { using namespace GeoUtilities; using namespace PolygonCalculus; using namespace PlanimetryCalculus; if (_reverseOnly) { // reverse transects and return _reverseOnly = false; QList> transectsReverse; transectsReverse.reserve(_transects.size()); for (auto list : _transects) { QList listReverse; for (auto coordinate : list) listReverse.prepend(coordinate); transectsReverse.prepend(listReverse); } _transects = transectsReverse; return; } _transects.clear(); QPolygonF surveyPolygon; toCartesianList(_surveyAreaPolygon.coordinateList(), _referencePoint, surveyPolygon); if (!_rebuildTransectsInputCheck(surveyPolygon)) return; // If the transects are getting rebuilt then any previously loaded mission items are now invalid. if (_loadedMissionItemsParent) { _loadedMissionItems.clear(); _loadedMissionItemsParent->deleteLater(); _loadedMissionItemsParent = nullptr; } QVector> transectPath; if(!_generateTransectPath(transectPath, surveyPolygon)) return; // optimize path to snake or zig-zag pattern const bool fixedDirectionBool = _fixedDirection.rawValue().toBool(); QVector currentSection = transectPath.takeFirst(); if ( currentSection.isEmpty() ) return; QVector optimizedPath(currentSection); bool reversePath = true; // controlls if currentSection gets reversed, has nothing todo with _reverseOnly while( !transectPath.empty() ) { QPointF endVertex = currentSection.last(); double minDist = std::numeric_limits::infinity(); int index = 0; // iterate over all paths in fullPath and assign the one with the shortest distance to endVertex to currentSection QVector connectorPath; for (int i = 0; i < transectPath.size(); i++) { QVector iteratorPath = transectPath[i]; QVector tempConnectorPath; bool retVal; if (reversePath && !fixedDirectionBool) { retVal = PolygonCalculus::shortestPath(surveyPolygon, endVertex, iteratorPath.last(), tempConnectorPath); } else { retVal = PolygonCalculus::shortestPath(surveyPolygon, endVertex, iteratorPath.first(), tempConnectorPath); } if (!retVal) qWarning("CircularSurveyComplexItem::_rebuildTransectsPhase1: internal error; false shortestPath"); double dist = 0; for (int i = 0; i < tempConnectorPath.size()-1; ++i) dist += PlanimetryCalculus::distance(tempConnectorPath[i], tempConnectorPath[i+1]); if (dist < minDist) { minDist = dist; index = i; connectorPath = tempConnectorPath; } } currentSection = transectPath.takeAt(index); if (reversePath && !fixedDirectionBool) { PolygonCalculus::reversePath(currentSection); } reversePath ^= true; // toggle connectorPath.pop_front(); connectorPath.pop_back(); if (connectorPath.size() > 0) optimizedPath.append(connectorPath); optimizedPath.append(currentSection); } if (optimizedPath.size() > _maxWaypoints.rawValue().toInt()) return; _rebuildTransectsToGeo(optimizedPath, _referencePoint); _transectsDirty = false; //_triggerSlowRecalcTimer.stop(); // stop any pending slow recalculations return; } void CircularSurveyComplexItem::_triggerSlowRecalc() { _doFastRecalc = false; _rebuildTransects(); } void CircularSurveyComplexItem::_recalcComplexDistance() { _complexDistance = 0; if (_transectsDirty) return; for (int i=0; i<_visualTransectPoints.count() - 1; i++) { _complexDistance += _visualTransectPoints[i].value().distanceTo(_visualTransectPoints[i+1].value()); } emit complexDistanceChanged(); } // no cameraShots in Circular Survey, add if desired void CircularSurveyComplexItem::_recalcCameraShots() { _cameraShots = 0; } void CircularSurveyComplexItem::_reverseTransects() { _reverseOnly = true; _triggerSlowRecalc(); } bool CircularSurveyComplexItem::_shortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &shortestPath) { using namespace GeoUtilities; using namespace PolygonCalculus; QPolygonF polygon2D; toCartesianList(this->surveyAreaPolygon()->coordinateList(), /*origin*/ start, polygon2D); QPointF start2D(0,0); QPointF end2D; toCartesian(destination, start, end2D); QVector path2D; bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); if (retVal) toGeoList(path2D, /*origin*/ start, shortestPath); return retVal; } bool CircularSurveyComplexItem::_generateTransectPath(QVector > &transectPath, const QPolygonF &surveyPolygon) { using namespace PlanimetryCalculus; QVector distances; for (const QPointF &p : surveyPolygon) distances.append(norm(p)); // fetch input data double dalpha = _deltaAlpha.rawValue().toDouble()/180.0*M_PI; // angle discretisation of circles double dr = _deltaR.rawValue().toDouble(); // distance between circles double lmin = _transectMinLength.rawValue().toDouble(); // minimal transect length double r_min = dr; // minimal circle radius double r_max = (*std::max_element(distances.begin(), distances.end())); // maximal circle radius unsigned int maxWaypoints = _maxWaypoints.rawValue().toUInt(); QPointF origin(0, 0); IntersectType type; bool originInside = true; if (!contains(surveyPolygon, origin, type)) { QVector angles; for (const QPointF &p : surveyPolygon) angles.append(truncateAngle(angle(p))); // determine r_min by successive approximation double r = r_min; while ( r < r_max) { Circle circle(r, origin); if (intersects(circle, surveyPolygon)) { r_min = r; break; } r += dr; } originInside = false; } double r = r_min; unsigned int waypointCounter = 0; while (r < r_max) { Circle circle(r, origin); QVector intersectPoints; QVector typeList; QVector> neighbourList; if (intersects(circle, surveyPolygon, intersectPoints, neighbourList, typeList)) { // intersection Points between circle and polygon, entering polygon // when walking in counterclockwise direction along circle QVector entryPoints; // intersection Points between circle and polygon, leaving polygon // when walking in counterclockwise direction along circle QVector exitPoints; // determine entryPoints and exit Points for (int j = 0; j < intersectPoints.size(); j++) { QVector intersects = intersectPoints[j]; // one pt = tangent, two pt = sekant QPointF p1 = surveyPolygon[neighbourList[j].first]; QPointF p2 = surveyPolygon[neighbourList[j].second]; QLineF intersetLine(p1, p2); double lineAngle = angle(intersetLine); for (QPointF ipt : intersects) { double circleTangentAngle = angle(ipt)+M_PI_2; if ( !qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle)) && !qFuzzyIsNull(truncateAngle(lineAngle - circleTangentAngle - M_PI) )) { if (truncateAngle(circleTangentAngle - lineAngle) > M_PI) { entryPoints.append(ipt); } else { exitPoints.append(ipt); } } } } // sort std::sort(entryPoints.begin(), entryPoints.end(), [](QPointF p1, QPointF p2) { return angle(p1) < angle(p2); }); std::sort(exitPoints.begin(), exitPoints.end(), [](QPointF p1, QPointF p2) { return angle(p1) < angle(p2); }); // match entry and exit points int offset = 0; double minAngle = std::numeric_limits::infinity(); for (int k = 0; k < exitPoints.size(); k++) { QPointF pt = exitPoints[k]; double alpha = truncateAngle(angle(pt) - angle(entryPoints[0])); if (minAngle > alpha) { minAngle = alpha; offset = k; } } // generate circle sectors for (int k = 0; k < entryPoints.size(); k++) { double alpha1 = angle(entryPoints[k]); double alpha2 = angle(exitPoints[(k+offset) % entryPoints.size()]); double dAlpha = truncateAngle(alpha2-alpha1); int numNodes = int(ceil(dAlpha/dalpha)) + 1; QVector sectorPath = circle.approximateSektor(numNodes, alpha1, alpha2); // use shortestPath() here if necessary, could be a problem if dr >> if (sectorPath.size() > 0) { waypointCounter += uint(sectorPath.size()); if (waypointCounter > maxWaypoints ) return false; transectPath.append(sectorPath); } } } else if (originInside) { // circle fully inside polygon int numNodes = int(ceil(2*M_PI/dalpha)) + 1; QVector sectorPath = circle.approximateSektor(numNodes, 0, 2*M_PI); // use shortestPath() here if necessary, could be a problem if dr >> waypointCounter += uint(sectorPath.size()); if (waypointCounter > maxWaypoints ) return false; transectPath.append(sectorPath); } r += dr; } if (transectPath.size() == 0) return false;; // remove short transects for (int i = 0; i < transectPath.size(); i++) { auto transect = transectPath[i]; double len = 0; for (int j = 0; j < transect.size()-1; ++j) { len += PlanimetryCalculus::distance(transect[j], transect[j+1]); } if (len < lmin) transectPath.removeAt(i--); } if (transectPath.size() == 0) return false; return true; } bool CircularSurveyComplexItem::_rebuildTransectsInputCheck(QPolygonF &poly) { // rebuild not necessary? if (!_isInitialized) return false; // check if input is valid if ( _surveyAreaPolygon.count() < 3) return false; // some more checks if (!PolygonCalculus::isSimplePolygon(poly)) return false; // even more checks if (!PolygonCalculus::hasClockwiseWinding(poly)) PolygonCalculus::reversePath(poly); // check if input is valid if ( _deltaAlpha.rawValue() > _deltaAlpha.rawMax() && _deltaAlpha.rawValue() < _deltaAlpha.rawMin()) return false; if ( _deltaR.rawValue() > _deltaR.rawMax() && _deltaR.rawValue() < _deltaR.rawMin()) return false; return true; } void CircularSurveyComplexItem::_rebuildTransectsToGeo(const QVector &path, const QGeoCoordinate &reference) { using namespace GeoUtilities; QVector geoPath; toGeoList(path, reference, geoPath); QList transectList; transectList.reserve(path.size()); for ( const QGeoCoordinate &coordinate : geoPath) { CoordInfo_t coordinfo = {coordinate, CoordTypeInterior}; transectList.append(coordinfo); } _transects.append(transectList); } Fact *CircularSurveyComplexItem::transectMinLength() { return &_transectMinLength; } Fact *CircularSurveyComplexItem::fixedDirection() { return &_fixedDirection; } Fact *CircularSurveyComplexItem::reverse() { return &_reverse; } Fact *CircularSurveyComplexItem::maxWaypoints() { return &_maxWaypoints; } /*! \class CircularSurveyComplexItem \inmodule Wima \brief The \c CircularSurveyComplexItem class provides a survey mission item with circular transects around a point of interest. CircularSurveyComplexItem class provides a survey mission item with circular transects around a point of interest. Within the \c Wima module it's used to scan a defined area with constant angle (circular transects) to the base station (point of interest). \sa WimaArea */