diff --git a/src/PlanView/CircularSurveyItemEditor.qml b/src/PlanView/CircularSurveyItemEditor.qml index 3f238c1d70e4c52bc82a9a673bb71a4f2329a552..2092a5d2af2f0883662473e6bc6e5e7535f6f91d 100644 --- a/src/PlanView/CircularSurveyItemEditor.qml +++ b/src/PlanView/CircularSurveyItemEditor.qml @@ -64,6 +64,7 @@ Rectangle { GridLayout { + id: generalGrid anchors.left: parent.left anchors.right: parent.right columnSpacing: _margin @@ -75,7 +76,48 @@ Rectangle { FactTextField { fact: missionItem.cameraCalc.distanceToSurface Layout.fillWidth: true - //onUpdated: rSlider.value = missionItem.deltaR.value + } + + QGCCheckBox { + id: relAlt + text: qsTr("Relative altitude") + checked: missionItem.cameraCalc.distanceToSurfaceRelative + enabled: missionItem.cameraCalc.isManualCamera && !missionItem.followTerrain + visible: QGroundControl.corePlugin.options.showMissionAbsoluteAltitude || (!missionItem.cameraCalc.distanceToSurfaceRelative && !missionItem.followTerrain) + onClicked: missionItem.cameraCalc.distanceToSurfaceRelative = checked + Layout.fillWidth: true + Layout.columnSpan: 2 + + Connections { + target: missionItem.cameraCalc + onDistanceToSurfaceRelativeChanged: relAlt.checked = missionItem.cameraCalc.distanceToSurfaceRelative + } + } + + QGCLabel { + text: qsTr("Type") + Layout.columnSpan: 2 + } + + property var typeFact: missionItem.type + property int type: typeFact.value + property var names: ["Circular", "Linear"] + + ExclusiveGroup{id: typeGroup} + + Repeater{ + model: missionItem.typeCount + QGCRadioButton { + checked: index === generalGrid.type + text: qsTr(generalGrid.names[index]) + + onCheckedChanged: { + if (checked){ + missionItem.type.value = index + } + checked = Qt.binding(function(){ return index === generalGrid.type}) + } + } } } @@ -92,11 +134,10 @@ Rectangle { columns: 2 visible: transectsHeader.checked - QGCLabel { text: qsTr("Delta R") } + QGCLabel { text: qsTr("Distance") } FactTextField { - fact: missionItem.deltaR + fact: missionItem.transectDistance Layout.fillWidth: true - //onUpdated: rSlider.value = missionItem.deltaR.value } /*QGCSlider { @@ -113,18 +154,16 @@ Rectangle { updateValueWhileDragging: true }*/ - QGCLabel { text: qsTr("Delta Alpha") } + QGCLabel { text: qsTr("Alpha") } FactTextField { - fact: missionItem.deltaAlpha + fact: missionItem.alpha Layout.fillWidth: true - //onUpdated: angleSlider.value = missionItem.deltaAlpha.value } QGCLabel { text: qsTr("Min. Length") } FactTextField { - fact: missionItem.transectMinLength + fact: missionItem.minLength Layout.fillWidth: true - //onUpdated: angleSlider.value = missionItem.deltaAlpha.value } } @@ -148,21 +187,6 @@ Rectangle { Layout.fillWidth: true } - QGCCheckBox { - id: relAlt - text: qsTr("Relative altitude") - checked: missionItem.cameraCalc.distanceToSurfaceRelative - enabled: missionItem.cameraCalc.isManualCamera && !missionItem.followTerrain - visible: QGroundControl.corePlugin.options.showMissionAbsoluteAltitude || (!missionItem.cameraCalc.distanceToSurfaceRelative && !missionItem.followTerrain) - onClicked: missionItem.cameraCalc.distanceToSurfaceRelative = checked - Layout.fillWidth: true - Layout.columnSpan: 2 - - Connections { - target: missionItem.cameraCalc - onDistanceToSurfaceRelativeChanged: relAlt.checked = missionItem.cameraCalc.distanceToSurfaceRelative - } - } } diff --git a/src/Wima/CSWorker.cpp b/src/Wima/CSWorker.cpp index aa40ec0059c6931e7365995f20b4db2e925eac1f..932b949cdf492827d6b505e5e624eced5c7737da 100644 --- a/src/Wima/CSWorker.cpp +++ b/src/Wima/CSWorker.cpp @@ -73,7 +73,7 @@ void RoutingWorker::run() { auto &transectsInfo = pRouteData->transectsInfo; auto &route = pRouteData->route; const auto routingStart = std::chrono::high_resolution_clock::now(); - const auto maxRoutingTime = std::chrono::minutes(1); + const auto maxRoutingTime = std::chrono::minutes(10); const auto routingEnd = routingStart + maxRoutingTime; const auto &restart = this->_restart; auto stopLambda = [&restart, routingEnd] { diff --git a/src/Wima/CS_old.cc b/src/Wima/CS_old.cc deleted file mode 100644 index 35a3be11acebd4cda5c02238c488319227bc594b..0000000000000000000000000000000000000000 --- a/src/Wima/CS_old.cc +++ /dev/null @@ -1,737 +0,0 @@ -#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 -*/ - - diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index 7e4c4bef620e7be64f0dab4bc164df4885241c78..78f49f5dd5ac4e25c34cdb3196b5ae25d121db26 100644 --- a/src/Wima/CircularSurvey.cc +++ b/src/Wima/CircularSurvey.cc @@ -25,15 +25,26 @@ private: Functor fun; }; +template +constexpr typename std::underlying_type::type integral(T value) { + return static_cast::type>(value); +} + bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, - const QList &polygon, + bool useDepot, const QList &polygon, snake::Length deltaR, snake::Angle deltaAlpha, snake::Length minLength, snake::Transects &transects); +bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, + bool useDepot, const QList &polygon, + snake::Length distance, snake::Angle angle, + snake::Length minLength, snake::Transects &transects); + const char *CircularSurvey::settingsGroup = "CircularSurvey"; -const char *CircularSurvey::deltaRName = "DeltaR"; -const char *CircularSurvey::deltaAlphaName = "DeltaAlpha"; -const char *CircularSurvey::transectMinLengthName = "TransectMinLength"; +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"; @@ -45,18 +56,19 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, _referencePoint(QGeoCoordinate(0, 0, 0)), _metaDataMap(FactMetaData::createMapFromJsonFile( QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)), - _deltaR(settingsGroup, _metaDataMap[deltaRName]), - _deltaAlpha(settingsGroup, _metaDataMap[deltaAlphaName]), - _minLength(settingsGroup, _metaDataMap[transectMinLengthName]), + _transectDistance(settingsGroup, _metaDataMap[transectDistanceName]), + _alpha(settingsGroup, _metaDataMap[alphaName]), + _minLength(settingsGroup, _metaDataMap[minLengthName]), + _type(settingsGroup, _metaDataMap[typeName]), _pWorker(std::make_unique()), _needsStoring(false), _needsReversal(false), _hidePolygon(false) { Q_UNUSED(kmlOrShpFile) _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml"; // Connect facts. - connect(&_deltaR, &Fact::valueChanged, this, + connect(&_transectDistance, &Fact::valueChanged, this, &CircularSurvey::_rebuildTransects); - connect(&_deltaAlpha, &Fact::valueChanged, this, + connect(&_alpha, &Fact::valueChanged, this, &CircularSurvey::_rebuildTransects); connect(&_minLength, &Fact::valueChanged, this, &CircularSurvey::_rebuildTransects); @@ -66,6 +78,8 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, &CircularSurvey::_rebuildTransects); connect(this, &CircularSurvey::safeAreaChanged, this, &CircularSurvey::_rebuildTransects); + connect(&this->_type, &Fact::rawValueChanged, this, + &CircularSurvey::_rebuildTransects); // Connect worker. connect(this->_pWorker.get(), &RoutingWorker::result, this, &CircularSurvey::_setTransects); @@ -88,6 +102,7 @@ void CircularSurvey::reverse() { void CircularSurvey::setRefPoint(const QGeoCoordinate &refPt) { if (refPt != _referencePoint) { _referencePoint = refPt; + _referencePoint.setAltitude(0); emit refPointChanged(); } @@ -95,9 +110,9 @@ void CircularSurvey::setRefPoint(const QGeoCoordinate &refPt) { QGeoCoordinate CircularSurvey::refPoint() const { return _referencePoint; } -Fact *CircularSurvey::deltaR() { return &_deltaR; } +Fact *CircularSurvey::transectDistance() { return &_transectDistance; } -Fact *CircularSurvey::deltaAlpha() { return &_deltaAlpha; } +Fact *CircularSurvey::alpha() { return &_alpha; } bool CircularSurvey::hidePolygon() const { return _hidePolygon; } @@ -107,6 +122,10 @@ QList CircularSurvey::safeArea() const { return this->_safeArea; } +const QList> &CircularSurvey::rawTransects() const { + return this->_rawTransects; +} + void CircularSurvey::setHidePolygon(bool hide) { if (this->_hidePolygon != hide) { this->_hidePolygon = hide; @@ -117,6 +136,7 @@ void CircularSurvey::setHidePolygon(bool hide) { void CircularSurvey::setDepot(const QGeoCoordinate &depot) { if (this->_depot != depot) { this->_depot = depot; + this->_depot.setAltitude(0); emit depotChanged(); } } @@ -149,9 +169,10 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, QList keyInfoList = { {VisualMissionItem::jsonTypeKey, QJsonValue::String, true}, {ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true}, - {deltaRName, QJsonValue::Double, true}, - {deltaAlphaName, QJsonValue::Double, true}, - {transectMinLengthName, QJsonValue::Double, true}, + {transectDistanceName, QJsonValue::Double, true}, + {alphaName, QJsonValue::Double, true}, + {minLengthName, QJsonValue::Double, true}, + {typeName, QJsonValue::Double, true}, {refPointLatitudeName, QJsonValue::Double, true}, {refPointLongitudeName, QJsonValue::Double, true}, {refPointAltitudeName, QJsonValue::Double, true}, @@ -189,9 +210,10 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, return false; } - _deltaR.setRawValue(complexObject[deltaRName].toDouble()); - _deltaAlpha.setRawValue(complexObject[deltaAlphaName].toDouble()); - _minLength.setRawValue(complexObject[transectMinLengthName].toDouble()); + _transectDistance.setRawValue(complexObject[transectDistanceName].toDouble()); + _alpha.setRawValue(complexObject[alphaName].toDouble()); + _minLength.setRawValue(complexObject[minLengthName].toDouble()); + _type.setRawValue(complexObject[typeName].toInt()); _referencePoint.setLongitude(complexObject[refPointLongitudeName].toDouble()); _referencePoint.setLatitude(complexObject[refPointLatitudeName].toDouble()); _referencePoint.setAltitude(complexObject[refPointAltitudeName].toDouble()); @@ -221,9 +243,10 @@ void CircularSurvey::save(QJsonArray &planItems) { VisualMissionItem::jsonTypeComplexItemValue; saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = CircularSurveyName; - saveObject[deltaRName] = _deltaR.rawValue().toDouble(); - saveObject[deltaAlphaName] = _deltaAlpha.rawValue().toDouble(); - saveObject[transectMinLengthName] = _minLength.rawValue().toDouble(); + saveObject[transectDistanceName] = _transectDistance.rawValue().toDouble(); + saveObject[alphaName] = _alpha.rawValue().toDouble(); + saveObject[minLengthName] = _minLength.rawValue().toDouble(); + saveObject[typeName] = double(_type.rawValue().toUInt()); saveObject[refPointLongitudeName] = _referencePoint.longitude(); saveObject[refPointLatitudeName] = _referencePoint.latitude(); saveObject[refPointAltitudeName] = _referencePoint.altitude(); @@ -334,7 +357,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { _loadedMissionItemsParent = nullptr; } // Store raw transects. - this->_rawTransects.clear(); const auto &transectsENU = this->_workerOutput->transects; const auto &ori = this->_referencePoint; for (auto &t : transectsENU) { @@ -379,7 +401,7 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { // Convert to geo coordinates. QList list; for (std::size_t i = idxFirst; i <= idxLast; ++i) { - auto vertex = route[i]; + auto &vertex = route[i]; QGeoCoordinate c; snake::fromENU(ori, vertex, c); list.append(CoordInfo_t{c, CoordTypeInterior}); @@ -416,9 +438,9 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { auto start = std::chrono::high_resolution_clock::now(); #endif this->_transects.clear(); + this->_rawTransects.clear(); // Prepare data. auto ref = this->_referencePoint; - ref.setAltitude(0); auto polygon = this->_surveyAreaPolygon.coordinateList(); for (auto &v : polygon) { v.setAltitude(0); @@ -427,27 +449,52 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { for (auto &v : safeArea) { v.setAltitude(0); } - QGeoCoordinate depot; + auto depot = this->_depot; snake::BoostPolygon safeAreaENU; + bool useDepot = false; if (this->_depot.isValid() && this->_safeArea.size() >= 3) { - depot = this->_depot; + useDepot = true; snake::areaToEnu(ref, safeArea, safeAreaENU); } else { snake::areaToEnu(ref, polygon, safeAreaENU); } - auto deltaR = - snake::Length(this->_deltaR.rawValue().toDouble() * bu::si::meter); + auto distance = snake::Length( + this->_transectDistance.rawValue().toDouble() * bu::si::meter); auto minLength = snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter); - auto deltaAlpha = snake::Angle(this->_deltaAlpha.rawValue().toDouble() * - bu::degree::degree); - auto generator = [ref, depot, polygon, deltaR, deltaAlpha, - minLength](snake::Transects &transects) -> bool { - return circularTransects(ref, depot, polygon, deltaR, deltaAlpha, + auto alpha = + snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree); + // Select survey type. + if (this->_type.rawValue().toUInt() == integral(Type::Circular)) { + if (alpha >= snake::Angle(0.3 * bu::degree::degree) && + alpha <= snake::Angle(45 * bu::degree::degree)) { + auto generator = [ref, depot, useDepot, polygon, distance, alpha, + minLength](snake::Transects &transects) -> bool { + return circularTransects(ref, depot, useDepot, polygon, distance, + alpha, minLength, transects); + }; + // Start routing worker. + this->_pWorker->route(safeAreaENU, generator); + } else { + if (alpha < snake::Angle(0.3 * bu::degree::degree)) { + this->_alpha.setCookedValue(QVariant(0.3)); + } else { + this->_alpha.setCookedValue(QVariant(45)); + } + } + } else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) { + auto generator = [ref, depot, useDepot, polygon, distance, alpha, + minLength](snake::Transects &transects) -> bool { + return linearTransects(ref, depot, useDepot, polygon, distance, alpha, minLength, transects); - }; - // Start routing worker. - this->_pWorker->route(safeAreaENU, generator); + }; + // Start routing worker. + this->_pWorker->route(safeAreaENU, generator); + } else { + qWarning() + << "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:" + << this->_type.rawValue().toUInt(); + } // Mark transects as dirty. this->_transectsDirty = true; #ifdef SHOW_CIRCULAR_SURVEY_TIME @@ -481,14 +528,18 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) { this->_rebuildTransects(); } -Fact *CircularSurvey::transectMinLength() { return &_minLength; } +Fact *CircularSurvey::minLength() { return &_minLength; } + +Fact *CircularSurvey::type() { return &_type; } + +int CircularSurvey::typeCount() const { return int(integral(Type::Count)); } bool CircularSurvey::calculating() const { return this->_pWorker->calculating(); } bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, - const QList &polygon, + bool useDepot, const QList &polygon, snake::Length deltaR, snake::Angle deltaAlpha, snake::Length minLength, snake::Transects &transects) { #ifdef SHOW_CIRCULAR_SURVEY_TIME @@ -503,9 +554,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, snake::BoostPoint depotENU{0, 0}; snake::areaToEnu(ref, polygon, polygonENU); snake::toENU(ref, ref, originENU); - bool depotValid = depot.isValid(); - if (depotValid) - snake::toENU(ref, depot, depotENU); + snake::toENU(ref, depot, depotENU); std::string error; // Check validity. if (!bg::is_valid(polygonENU, error)) { @@ -588,7 +637,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, snake::BoostPolygon shrinked; snake::offsetPolygon(polygonENU, shrinked, -0.3); auto &outer = shrinked.outer(); - polygonClipper.reserve(outer.size() - 1); + polygonClipper.reserve(outer.size()); for (auto it = outer.begin(); it < outer.end() - 1; ++it) { auto x = ClipperLib::cInt(std::round(it->get<0>() * CLIPPER_SCALE)); auto y = ClipperLib::cInt(std::round(it->get<1>() * CLIPPER_SCALE)); @@ -603,7 +652,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, // Extract transects from PolyTree and convert them to // BoostLineString - if (depotValid) { + if (useDepot) { transects.push_back(snake::BoostLineString{depotENU}); } for (const auto &child : transectsClipper.Childs) { @@ -663,7 +712,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } } // Remove short transects - auto begin = depotValid ? transects.begin() + 1 : transects.begin(); + auto begin = useDepot ? transects.begin() + 1 : transects.begin(); for (auto it = begin; it < transects.end();) { if (bg::length(*it) < minLength.value()) { it = transects.erase(it); @@ -672,7 +721,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } } - if (!depotValid) { + if (!useDepot) { // Move transect with min. distance to the front. auto minDist = std::numeric_limits::max(); auto minIt = transects.begin(); @@ -718,6 +767,141 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } return false; } + +bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, + bool useDepot, const QList &polygon, + snake::Length distance, snake::Angle angle, + snake::Length minLength, snake::Transects &transects) { + namespace tr = bg::strategy::transform; +#ifdef SHOW_CIRCULAR_SURVEY_TIME + auto s1 = std::chrono::high_resolution_clock::now(); +#endif + // Check preconitions + if (polygon.size() >= 3) { + // Convert to ENU system. + snake::BoostPolygon polygonENU; + snake::areaToEnu(origin, polygon, polygonENU); + std::string error; + // Check validity. + if (!bg::is_valid(polygonENU, error)) { +#ifdef DEBUG_CIRCULAR_SURVEY + qWarning() << "CS::circularTransects(): " + "invalid polygon."; + qWarning() << error.c_str(); + std::stringstream ss; + ss << bg::wkt(polygonENU); + qWarning() << ss.str().c_str(); +#endif + } else { + snake::BoostPoint depotENU; + snake::toENU(origin, depot, depotENU); + tr::rotate_transformer rotate(angle.value() * + 180 / M_PI); + // Rotate polygon by angle and calculate bounding box. + snake::BoostPolygon polygonENURotated; + bg::transform(polygonENU, polygonENURotated, rotate); + snake::BoostBox box; + boost::geometry::envelope(polygonENURotated, box); + double x0 = box.min_corner().get<0>(); + double y0 = box.min_corner().get<1>(); + double x1 = box.max_corner().get<0>(); + double y1 = box.max_corner().get<1>(); + + // Generate transects and convert them to clipper path. + size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects + vector transectsClipper; + transectsClipper.reserve(num_t); + for (size_t i = 0; i < num_t; ++i) { + // calculate transect + snake::BoostPoint v1{x0, y0 + i * distance.value()}; + snake::BoostPoint v2{x1, y0 + i * distance.value()}; + snake::BoostLineString transect; + transect.push_back(v1); + transect.push_back(v2); + // transform back + snake::BoostLineString temp_transect; + tr::rotate_transformer rotate_back( + -angle.value() * 180 / M_PI); + bg::transform(transect, temp_transect, rotate_back); + // to clipper + ClipperLib::IntPoint c1{static_cast( + temp_transect[0].get<0>() * CLIPPER_SCALE), + static_cast( + temp_transect[0].get<1>() * CLIPPER_SCALE)}; + ClipperLib::IntPoint c2{static_cast( + temp_transect[1].get<0>() * CLIPPER_SCALE), + static_cast( + temp_transect[1].get<1>() * CLIPPER_SCALE)}; + ClipperLib::Path path{c1, c2}; + transectsClipper.push_back(path); + } + + if (transectsClipper.size() == 0) { + std::stringstream ss; + ss << "Not able to generate transects. Parameter: distance = " + << distance << std::endl; + qWarning() << "CircularSurvey::linearTransects(): " << ss.str().c_str(); + return false; + } + + // Convert measurement area to clipper path. + snake::BoostPolygon shrinked; + snake::offsetPolygon(polygonENU, shrinked, -0.2); + auto &outer = shrinked.outer(); + ClipperLib::Path polygonClipper; + for (auto vertex : outer) { + polygonClipper.push_back(ClipperLib::IntPoint{ + static_cast(vertex.get<0>() * CLIPPER_SCALE), + static_cast(vertex.get<1>() * CLIPPER_SCALE)}); + } + + // Perform clipping. + // Clip transects to measurement area. + ClipperLib::Clipper clipper; + clipper.AddPath(polygonClipper, ClipperLib::ptClip, true); + clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false); + ClipperLib::PolyTree clippedTransecs; + clipper.Execute(ClipperLib::ctIntersection, clippedTransecs, + ClipperLib::pftNonZero, ClipperLib::pftNonZero); + + // Extract transects from PolyTree and convert them to BoostLineString + if (useDepot) { + transects.push_back(snake::BoostLineString{depotENU}); + } + for (const auto &child : clippedTransecs.Childs) { + const auto &clipperTransect = child->Contour; + snake::BoostPoint v1{ + static_cast(clipperTransect[0].X) / CLIPPER_SCALE, + static_cast(clipperTransect[0].Y) / CLIPPER_SCALE}; + snake::BoostPoint v2{ + static_cast(clipperTransect[1].X) / CLIPPER_SCALE, + static_cast(clipperTransect[1].Y) / CLIPPER_SCALE}; + + snake::BoostLineString transect{v1, v2}; + if (bg::length(transect) >= minLength.value()) { + transects.push_back(transect); + } + } + + if (transects.size() == 0) { + std::stringstream ss; + ss << "Not able to generate transects. Parameter: minLength = " + << minLength << std::endl; + qWarning() << "CircularSurvey::linearTransects(): " << ss.str().c_str(); + return false; + } +#ifdef SHOW_CIRCULAR_SURVEY_TIME + qWarning() << "CS::circularTransects(): transect gen. time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - s1) + .count() + << " ms"; +#endif + return true; + } + } + return false; +} /*! \class CircularSurveyComplexItem \inmodule Wima diff --git a/src/Wima/CircularSurvey.h b/src/Wima/CircularSurvey.h index 4305536c78bd68603442a01d2935741594d22c99..5aecd7152c1b5843ddc6151d712a987a5bf4ab71 100644 --- a/src/Wima/CircularSurvey.h +++ b/src/Wima/CircularSurvey.h @@ -13,6 +13,11 @@ 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 + }; /// @param vehicle Vehicle which this is being contructed for /// @param flyView true: Created for use in the Fly View, false: Created for @@ -25,9 +30,11 @@ public: Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY refPointChanged) - Q_PROPERTY(Fact *deltaR READ deltaR CONSTANT) - Q_PROPERTY(Fact *deltaAlpha READ deltaAlpha CONSTANT) - Q_PROPERTY(Fact *transectMinLength READ transectMinLength CONSTANT) + 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(int typeCount READ typeCount CONSTANT) Q_PROPERTY(bool calculating READ calculating NOTIFY calculatingChanged) Q_PROPERTY(bool hidePolygon READ hidePolygon NOTIFY hidePolygonChanged) @@ -42,13 +49,16 @@ public: // Property getters QGeoCoordinate refPoint() const; - Fact *deltaR(); - Fact *deltaAlpha(); - Fact *transectMinLength(); + Fact *transectDistance(); + Fact *alpha(); + Fact *minLength(); + Fact *type(); + int typeCount() const; bool calculating() const; bool hidePolygon() const; QGeoCoordinate depot() const; QList safeArea() const; + const QList> &rawTransects() const; // Overrides bool load(const QJsonObject &complexObject, int sequenceNumber, @@ -67,9 +77,10 @@ public: double additionalTimeDelay(void) const override final; static const char *settingsGroup; - static const char *deltaRName; - static const char *deltaAlphaName; - static const char *transectMinLengthName; + static const char *transectDistanceName; + static const char *alphaName; + static const char *minLengthName; + static const char *typeName; static const char *CircularSurveyName; static const char *refPointLongitudeName; static const char *refPointLatitudeName; @@ -97,16 +108,17 @@ private: // center of the circular lanes, e.g. base station QGeoCoordinate _referencePoint; - QMap _metaDataMap; // distance between two neighbour circles - SettingsFact _deltaR; + SettingsFact _transectDistance; // angle discretisation of the circles - SettingsFact _deltaAlpha; + SettingsFact _alpha; // minimal transect lenght, transects are rejected if they are shorter than // this value SettingsFact _minLength; + SettingsFact _type; + // Worker using PtrWorker = std::shared_ptr; PtrWorker _pWorker; PtrRoutingData _workerOutput; diff --git a/src/Wima/Geometry/WimaJoinedAreaData.cc b/src/Wima/Geometry/WimaJoinedAreaData.cc index 1a7b7f0b2d8d041167bb06737b5aa1499d2f9d60..41114207626dd14aa9fc1d27e01784515d095bb9 100644 --- a/src/Wima/Geometry/WimaJoinedAreaData.cc +++ b/src/Wima/Geometry/WimaJoinedAreaData.cc @@ -3,21 +3,18 @@ const char *WimaJoinedAreaData::typeString = "WimaJoinedAreaData"; WimaJoinedAreaData::WimaJoinedAreaData(QObject *parent) - :WimaAreaData (parent) -{ + : WimaAreaData(parent) {} +WimaJoinedAreaData::WimaJoinedAreaData(const WimaJoinedAreaData &other, + QObject *parent) + : WimaAreaData(parent) { + *this = other; } -WimaJoinedAreaData::WimaJoinedAreaData(const WimaJoinedAreaData &other, QObject *parent) - : WimaAreaData (parent) -{ - *this = other; -} - -WimaJoinedAreaData::WimaJoinedAreaData(const WimaJoinedArea &other, QObject *parent) - : WimaAreaData (parent) -{ - *this = other; +WimaJoinedAreaData::WimaJoinedAreaData(const WimaJoinedArea &other, + QObject *parent) + : WimaAreaData(parent) { + *this = other; } /*! @@ -25,11 +22,11 @@ WimaJoinedAreaData::WimaJoinedAreaData(const WimaJoinedArea &other, QObject *par * * Assigns \a other to the invoking object. */ -WimaJoinedAreaData &WimaJoinedAreaData::operator=(const WimaJoinedAreaData &other) -{ - assign(other); +WimaJoinedAreaData &WimaJoinedAreaData:: +operator=(const WimaJoinedAreaData &other) { + assign(other); - return *this; + return *this; } /*! @@ -37,36 +34,29 @@ WimaJoinedAreaData &WimaJoinedAreaData::operator=(const WimaJoinedAreaData &othe * * Assigns \a other to the invoking object. */ -WimaJoinedAreaData &WimaJoinedAreaData::operator=(const WimaJoinedArea &other) -{ - assign(other); - return *this; +WimaJoinedAreaData &WimaJoinedAreaData::operator=(const WimaJoinedArea &other) { + assign(other); + return *this; } -QString WimaJoinedAreaData::type() const -{ - return this->typeString; -} +QString WimaJoinedAreaData::type() const { return this->typeString; } -void WimaJoinedAreaData::assign(const WimaJoinedAreaData &other) -{ - WimaAreaData::assign(other); +void WimaJoinedAreaData::assign(const WimaJoinedAreaData &other) { + WimaAreaData::assign(other); } -void WimaJoinedAreaData::assign(const WimaJoinedArea &other) -{ - WimaAreaData::assign(other); +void WimaJoinedAreaData::assign(const WimaJoinedArea &other) { + WimaAreaData::assign(other); } - - /*! * \class WimaAreaData::WimaJoinedAreaData * \brief Class to store and exchange data of a \c WimaJoinedAreaData Object. - * Class to store and exchange data of a \c WimaJoinedArea Object. In contrast to \c WimaJoinedArea this class - * does not provied any interface to a grafical user interface, neiter it uses the QGC Fact System. - * It is designed to exchange data between the \c WimaPlaner and the \c WimaController class. And it - * is the derived from WimaAreaData. + * Class to store and exchange data of a \c WimaJoinedArea Object. In contrast + * to \c WimaJoinedArea this class does not provied any interface to a grafical + * user interface, neiter it uses the QGC Fact System. It is designed to + * exchange data between the \c WimaPlaner and the \c WimaController class. And + * it is the derived from WimaAreaData. * * \sa WimaJoinedArea, WimaAreaData */ diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.cc b/src/Wima/Geometry/WimaMeasurementAreaData.cc index a9de85cfbf1e74711b6066a4f2ab8528ddf92692..6f5c36ca3e31d6d23111482d0aeea50db653fd70 100644 --- a/src/Wima/Geometry/WimaMeasurementAreaData.cc +++ b/src/Wima/Geometry/WimaMeasurementAreaData.cc @@ -1,23 +1,21 @@ #include "WimaMeasurementAreaData.h" +#include "SnakeTile.h" const char *WimaMeasurementAreaData::typeString = "WimaMeasurementAreaData"; WimaMeasurementAreaData::WimaMeasurementAreaData(QObject *parent) - : WimaAreaData(parent) -{ + : WimaAreaData(parent) {} +WimaMeasurementAreaData::WimaMeasurementAreaData( + const WimaMeasurementAreaData &other, QObject *parent) + : WimaAreaData(parent) { + *this = other; } -WimaMeasurementAreaData::WimaMeasurementAreaData(const WimaMeasurementAreaData &other, QObject *parent) - : WimaAreaData (parent) -{ - *this = other; -} - -WimaMeasurementAreaData::WimaMeasurementAreaData(const WimaMeasurementArea &other, QObject *parent) - : WimaAreaData (parent) -{ - *this = other; +WimaMeasurementAreaData::WimaMeasurementAreaData( + const WimaMeasurementArea &other, QObject *parent) + : WimaAreaData(parent) { + *this = other; } /*! @@ -25,11 +23,11 @@ WimaMeasurementAreaData::WimaMeasurementAreaData(const WimaMeasurementArea &othe * * Assigns \a other to the invoking object. */ -WimaMeasurementAreaData &WimaMeasurementAreaData::operator=(const WimaMeasurementAreaData &other) -{ - assign(other); +WimaMeasurementAreaData &WimaMeasurementAreaData:: +operator=(const WimaMeasurementAreaData &other) { + assign(other); - return *this; + return *this; } /*! @@ -37,28 +35,44 @@ WimaMeasurementAreaData &WimaMeasurementAreaData::operator=(const WimaMeasuremen * * Assigns \a other to the invoking object. */ -WimaMeasurementAreaData &WimaMeasurementAreaData::operator=(const WimaMeasurementArea &other) -{ - assign(other); - - return *this; -} +WimaMeasurementAreaData &WimaMeasurementAreaData:: +operator=(const WimaMeasurementArea &other) { + assign(other); -QString WimaMeasurementAreaData::type() const -{ - return this->typeString; + return *this; } -void WimaMeasurementAreaData::assign(const WimaMeasurementAreaData &other) -{ - WimaAreaData::assign(other); +QString WimaMeasurementAreaData::type() const { return this->typeString; } + +void WimaMeasurementAreaData::assign(const WimaMeasurementAreaData &other) { + WimaAreaData::assign(other); + this->tiles.clearAndDeleteContents(); + for (std::size_t i = 0; i < other.tiles.count(); ++i) { + auto *obj = other.tiles.get(i); + auto *tile = qobject_cast(obj); + if (tile != nullptr) { + this->tiles.append(new SnakeTile(*tile, this)); + } else { + qWarning() << "WimaMeasurementAreaData::assign(): type cast failed."; + } + } } -void WimaMeasurementAreaData::assign(const WimaMeasurementArea &other) -{ - WimaAreaData::assign(other); +void WimaMeasurementAreaData::assign(const WimaMeasurementArea &other) { + WimaAreaData::assign(other); + this->tiles.clearAndDeleteContents(); + if (other.ready()) { + for (std::size_t i = 0; i < other.tiles()->count(); ++i) { + auto *obj = other.tiles()->get(i); + auto *tile = qobject_cast(obj); + if (tile != nullptr) { + this->tiles.append(new SnakeTile(*tile, this)); + } else { + qWarning() << "WimaMeasurementAreaData::assign(): type cast failed."; + } + } + } else { + qWarning() + << "WimaMeasurementAreaData::assign(): WimaMeasurementArea not ready."; + } } - - - - diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.h b/src/Wima/Geometry/WimaMeasurementAreaData.h index b1592a6918888fb937d91293ed2331761197807b..6bcc3247e8ea12623fa47a8ab22e8f30f4feefa6 100644 --- a/src/Wima/Geometry/WimaMeasurementAreaData.h +++ b/src/Wima/Geometry/WimaMeasurementAreaData.h @@ -1,38 +1,39 @@ #pragma once -#include #include - +#include #include "WimaAreaData.h" #include "WimaMeasurementArea.h" - - -class WimaMeasurementAreaData : public WimaAreaData -{ - Q_OBJECT +class WimaMeasurementAreaData : public WimaAreaData { + Q_OBJECT public: - WimaMeasurementAreaData(QObject *parent = nullptr); - WimaMeasurementAreaData(const WimaMeasurementAreaData &other, QObject *parent = nullptr); - WimaMeasurementAreaData(const WimaMeasurementArea &other, QObject *parent = nullptr); - WimaMeasurementAreaData& operator=(const WimaMeasurementAreaData &other); - WimaMeasurementAreaData& operator=(const WimaMeasurementArea &other); + WimaMeasurementAreaData(QObject *parent = nullptr); + WimaMeasurementAreaData(const WimaMeasurementAreaData &other, + QObject *parent = nullptr); + WimaMeasurementAreaData(const WimaMeasurementArea &other, + QObject *parent = nullptr); + WimaMeasurementAreaData &operator=(const WimaMeasurementAreaData &other); + WimaMeasurementAreaData &operator=(const WimaMeasurementArea &other); - QString type() const; - WimaMeasurementAreaData *Clone() const {return new WimaMeasurementAreaData(*this);} + QString type() const; + WimaMeasurementAreaData *Clone() const { + return new WimaMeasurementAreaData(*this); + } - static const char* typeString; + static const char *typeString; signals: public slots: protected: - void assign(const WimaMeasurementAreaData &other); - void assign(const WimaMeasurementArea &other); + void assign(const WimaMeasurementAreaData &other); + void assign(const WimaMeasurementArea &other); private: - // see WimaMeasurementArea.h for explanation + // see WimaMeasurementArea.h for explanation + QmlObjectListModel tiles; }; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index f8441f52a78c6098a2343a97975d04a0d3e85e0b..73824901d4d1a61bc40c636191a630a0d79d5742 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -423,38 +423,42 @@ bool WimaController::setWimaPlanData(QSharedPointer planData) { emit visualItemsChanged(); - // extract mission items - auto tempMissionItems = planData->missionItems(); - if (tempMissionItems.size() < 1) { - qWarning("WimaController: Mission items from WimaPlaner empty!"); - return false; - } - - qWarning() << "WimaController:"; - for (auto *item : tempMissionItems) { - qWarning() << item->coordinate(); - _defaultWM.push_back(item->coordinate()); - } - - _WMSettings.setHomePosition(QGeoCoordinate( - _serviceArea.depot().latitude(), _serviceArea.depot().longitude(), 0)); - qWarning() << "service area depot: " << _serviceArea.depot(); - - if (!_defaultWM.reset()) { - qWarning() << "_defaultWM.reset() failed"; - return false; - } - - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit waypointPathChanged(); - emit currentWaypointPathChanged(); - - // Update Snake Data Manager - _snakeThread.setMeasurementArea(_measurementArea.coordinateList()); - _snakeThread.setServiceArea(_serviceArea.coordinateList()); - _snakeThread.setCorridor(_corridor.coordinateList()); - _currentThread->start(); + // Copy transects. + this->_rawTransects = planData->transects(); + + // // extract mission items + // auto tempMissionItems = planData->missionItems(); + // if (tempMissionItems.size() < 1) { + // qWarning("WimaController: Mission items from WimaPlaner empty!"); + // return false; + // } + + // qWarning() << "WimaController:"; + // for (auto *item : tempMissionItems) { + // qWarning() << item->coordinate(); + // _defaultWM.push_back(item->coordinate()); + // } + + // _WMSettings.setHomePosition(QGeoCoordinate( + // _serviceArea.depot().latitude(), _serviceArea.depot().longitude(), + // 0)); + // qWarning() << "service area depot: " << _serviceArea.depot(); + + // if (!_defaultWM.reset()) { + // qWarning() << "_defaultWM.reset() failed"; + // return false; + // } + + // emit missionItemsChanged(); + // emit currentMissionItemsChanged(); + // emit waypointPathChanged(); + // emit currentWaypointPathChanged(); + + // // Update Snake Data Manager + // _snakeThread.setMeasurementArea(_measurementArea.coordinateList()); + // _snakeThread.setServiceArea(_serviceArea.coordinateList()); + // _snakeThread.setCorridor(_corridor.coordinateList()); + // _currentThread->start(); _localPlanDataValid = true; return true; diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 0d71f70284633959139796fd087fe253030c4975..7c7d48001cdbb85456b1d5246f75f2b9bd55d778 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -254,8 +254,8 @@ private: // Wima Data. QmlObjectListModel _areas; // contains all visible areas - WimaJoinedAreaData - _joinedArea; // joined area fromed by opArea, serArea, _corridor + // joined area fromed by opArea, serArea, _corridor + WimaJoinedAreaData _joinedArea; WimaMeasurementAreaData _measurementArea; // measurement area WimaServiceAreaData _serviceArea; // area for supplying WimaCorridorData _corridor; // corridor connecting opArea and serArea @@ -274,11 +274,11 @@ private: // Settings Facts. QMap _metaDataMap; SettingsFact _enableWimaController; // enables or disables the wimaControler - SettingsFact - _overlapWaypoints; // determines the number of overlapping waypoints - // between two consecutive mission phases - SettingsFact _maxWaypointsPerPhase; // determines the maximum number waypoints - // per phase + // determines the number of overlapping waypoints between two consecutive + // mission phases + SettingsFact _overlapWaypoints; + // determines the maximum number waypoints per phase + SettingsFact _maxWaypointsPerPhase; SettingsFact _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get // index of item in _missionItems) of the @@ -294,11 +294,6 @@ private: SettingsFact _arrivalReturnSpeed; // arrival and return path speed SettingsFact _altitude; // mission altitude SettingsFact _enableSnake; // Enable Snake (see snake.h) - SettingsFact _snakeTileWidth; - SettingsFact _snakeTileHeight; - SettingsFact _snakeMinTileArea; - SettingsFact _snakeLineDistance; - SettingsFact _snakeMinTransectLength; // Smart RTL. QTimer _smartRTLTimer; @@ -308,7 +303,7 @@ private: double _measurementPathLength; // the lenght of the phase in meters // Snake - QmlObjectListModel tiles; + QList> _rawTransects; SnakeThread _snakeThread; // Snake Data Manager SnakeThread _emptyThread; SnakeThread *_currentThread; diff --git a/src/Wima/WimaPlanData.cc b/src/Wima/WimaPlanData.cc index e1ea35911b9b77c657d368f7d441191125500fcd..9e3b7097f6e71ae75256727f62befa32c9f791cb 100644 --- a/src/Wima/WimaPlanData.cc +++ b/src/Wima/WimaPlanData.cc @@ -76,6 +76,10 @@ void WimaPlanData::append(const WimaCorridorData &areaData) { } } +void WimaPlanData::setTransects(const QList> &transects) { + this->_transects = transects; +} + /*! * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData) * @@ -110,6 +114,10 @@ const QList &WimaPlanData::areaList() const { return _areaList; } +const QList> &WimaPlanData::transects() const { + return _transects; +} + const QList &WimaPlanData::missionItems() const { return _missionItems; } diff --git a/src/Wima/WimaPlanData.h b/src/Wima/WimaPlanData.h index e073af0030fa0c9c64209d6136fe565e13be251b..a3d74fc2b12aae9774bc24fe456ff0771168b7dc 100644 --- a/src/Wima/WimaPlanData.h +++ b/src/Wima/WimaPlanData.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include "Geometry/WimaAreaData.h" @@ -7,7 +8,6 @@ #include "Geometry/WimaJoinedAreaData.h" #include "Geometry/WimaMeasurementAreaData.h" #include "Geometry/WimaServiceAreaData.h" -#include "MissionItem.h" class WimaPlanData : public QObject { Q_OBJECT @@ -20,7 +20,8 @@ public: void append(const WimaJoinedAreaData &areaData); void append(const WimaServiceAreaData &areaData); void append(const WimaCorridorData &areaData); - void append(const WimaMeasurementAreaData &areaData); + + void setTransects(const QList> &transects); //! //! \brief append //! \param missionItems @@ -29,14 +30,11 @@ public: void clear(); const QList &areaList() const; - const QList &missionItems() const; + const QList> &transects() const; signals: void areaListChanged(); -private: - void _clearAndDeleteMissionItems(); - private: WimaJoinedAreaData _joinedArea; WimaServiceAreaData _serviceArea; @@ -44,5 +42,5 @@ private: WimaMeasurementAreaData _measurementArea; QList _areaList; - QList _missionItems; + QList> _transects; }; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 192b7a42b8d38ded72ffb441aee29b948848a77c..0a551d8912722e3cdb7d731dca13471909e33367 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -666,17 +666,21 @@ void WimaPlaner::updatePolygonInteractivity(int index) { void WimaPlaner::synchronize() { if (_wimaBridge != nullptr) { - if (_needsUpdate) - return; - auto planData = toPlanData(); - (void)_wimaBridge->setWimaPlanData(planData); - this->_synchronized = true; - emit synchronizedChanged(); + if (readyForSynchronization()) { + auto planData = toPlanData(); + (void)_wimaBridge->setWimaPlanData(planData); + this->_synchronized = true; + emit synchronizedChanged(); + } } else { qWarning("WimaPlaner::uploadToContainer(): no container assigned."); } } +bool WimaPlaner::readyForSynchronization() { + return !_needsUpdate && _measurementArea.ready(); +} + bool WimaPlaner::shortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) { @@ -737,14 +741,7 @@ QSharedPointer WimaPlaner::toPlanData() { planData->append(WimaJoinedAreaData(_joinedArea)); // convert mission items to mavlink commands - QList missionItems; - _TSComplexItem->appendMissionItems(missionItems, nullptr); - // store mavlink commands - qWarning() << "WimaPlaner"; - for (auto *item : missionItems) { - qWarning() << item->coordinate(); - } - planData->append(missionItems); + planData->setTransects(this->_TSComplexItem->rawTransects()); return planData; } diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 7a1f2226e99aebcbc8ec43870df097708e9c7d4b..cb50623b8beb0c7a76771e1c2cdc03f38ac913a9 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -84,6 +84,7 @@ public: Q_INVOKABLE bool update(); /// Pushes the generated mission data to the wimaController. Q_INVOKABLE void synchronize(); + Q_INVOKABLE bool readyForSynchronization(); Q_INVOKABLE void saveToCurrent(); Q_INVOKABLE void saveToFile(const QString &filename); Q_INVOKABLE bool loadFromCurrent(); diff --git a/src/Wima/json/CircularSurvey.SettingsGroup.json b/src/Wima/json/CircularSurvey.SettingsGroup.json index 86781cef248e2b81aff5952f187e925f4e1a8bbd..8e217b571edbd1cc0e7795403484c081cd1ec629 100644 --- a/src/Wima/json/CircularSurvey.SettingsGroup.json +++ b/src/Wima/json/CircularSurvey.SettingsGroup.json @@ -1,7 +1,7 @@ [ { - "name": "DeltaR", - "shortDescription": "The distance between two consecutive circles.", + "name": "TransectDistance", + "shortDescription": "The distance between transects.", "type": "double", "units": "m", "min": 0.3, @@ -9,18 +9,18 @@ "defaultValue": 20.0 }, { - "name": "DeltaAlpha", - "shortDescription": "Angle discretisation of the circles.", + "name": "Alpha", + "shortDescription": "Angle discretisation or transect angle (depending on type).", "type": "double", "units": "Deg", - "min": 0.3, - "max": 90, + "min": 0, + "max": 180, "decimalPlaces": 1, "defaultValue": 5.0 }, { - "name": "TransectMinLength", - "shortDescription": "The minimal length transects must have to be accepted.", + "name": "MinLength", + "shortDescription": "The minimal transect length.", "type": "double", "units": "m", "min": 0.3, @@ -28,23 +28,11 @@ "defaultValue": 5.0 }, { - "name": "FixedDirection", - "shortDescription": "Determines whether all transects have the same direction or not.", - "type": "bool", - "defaultValue": 1 -}, -{ - "name": "Reverse", - "shortDescription": "Reverses the transect path.", - "type": "bool", + "name": "Type", + "shortDescription": "Survey Type.", + "type": "uint64", + "min": 0, + "max": 1, "defaultValue": 0 -}, -{ - "name": "MaxWaypoints", - "shortDescription": "The maximum number of waypoints the circular survey can containt. To many waypoints cause a performance hit.", - "type": "uint32", - "defaultValue": 2000, - "min": 1, - "max": 20000 } ] diff --git a/src/WimaView/CircularSurveyMapVisual.qml b/src/WimaView/CircularSurveyMapVisual.qml index b80ffd551219c3cdeceab747961c8632b72746d6..22bd9d63f05969de03d20038021d6eda7b4e969b 100644 --- a/src/WimaView/CircularSurveyMapVisual.qml +++ b/src/WimaView/CircularSurveyMapVisual.qml @@ -27,29 +27,68 @@ Item { property var _missionItem: object property var _mapPolygon: object.surveyAreaPolygon - property var _visualTransectsComponent + property var _transectsComponent property var _entryCoordinate property var _exitCoordinate property var _refPoint + property bool showRefPoint: _missionItem.type.value === 0 // type == Circular signal clicked(int sequenceNumber) - function _addVisualElements() { - _visualTransectsComponent = visualTransectsComponent.createObject(map) - _entryCoordinate = entryPointComponent.createObject(map) - _exitCoordinate = exitPointComponent.createObject(map) - _refPoint = refPointComponent.createObject(map) - map.addMapItem(_visualTransectsComponent) - map.addMapItem(_entryCoordinate) - map.addMapItem(_exitCoordinate) - map.addMapItem(_refPoint) + function _addTransectsComponent(){ + if (!_transectsComponent){ + _transectsComponent = visualTransectsComponent.createObject(map) + map.addMapItem(_transectsComponent) + } + } + + function _addExitCoordinate(){ + if (!_exitCoordinate){ + _exitCoordinate = exitPointComponent.createObject(map) + map.addMapItem(_exitCoordinate) + } + } + + function _addEntryCoordinate(){ + if (!_entryCoordinate){ + _entryCoordinate = entryPointComponent.createObject(map) + map.addMapItem(_entryCoordinate) + } + } + + function _addRefPoint(){ + if (!_refPoint){ + _refPoint = refPointComponent.createObject(map) + map.addMapItem(_refPoint) + } + } + + function _destroyEntryCoordinate(){ + if (_entryCoordinate){ + _entryCoordinate.destroy() + _entryCoordinate = undefined + } + } + + function _destroyExitCoordinate(){ + if (_exitCoordinate){ + _exitCoordinate.destroy() + _exitCoordinate = undefined + } } - function _destroyVisualElements() { - _visualTransectsComponent.destroy() - _entryCoordinate.destroy() - _exitCoordinate.destroy() - _refPoint.destroy() + function _destroyRefPoint(){ + if (_refPoint){ + _refPoint.destroy() + _refPoint = undefined + } + } + + function _destroyTransectsComponent(){ + if (_transectsComponent){ + _transectsComponent.destroy() + _transectsComponent = undefined + } } /// Add an initial 4 sided polygon if there is none @@ -83,20 +122,33 @@ Item { } } - function _setRefPoint() { - _missionItem.resetReference(); - } Component.onCompleted: { - if ( _missionItem.visualTransectPoints.length === 0 ) { + if ( _mapPolygon.count === 0 ) { _addInitialPolygon() - _setRefPoint() + _missionItem.resetReference(); } - _addVisualElements() + _addEntryCoordinate() + _addExitCoordinate() + if (showRefPoint){ + _addRefPoint() + } + _addTransectsComponent() } Component.onDestruction: { - _destroyVisualElements() + _destroyEntryCoordinate() + _destroyExitCoordinate() + _destroyRefPoint() + _destroyTransectsComponent() + } + + onShowRefPointChanged: { + if (showRefPoint){ + _addRefPoint() + } else { + _destroyRefPoint() + } } WimaMapPolygonVisuals { @@ -117,9 +169,10 @@ Item { id: visualTransectsComponent MapPolyline { + property var transects: _missionItem.visualTransectPoints line.color: "white" line.width: 2 - path: _missionItem.visualTransectPoints + path: transects.length > 0 ? transects : [] } } @@ -174,20 +227,13 @@ Item { checked: _missionItem.isCurrentItem coordinate: _missionItem.refPoint - property var refPoint: _missionItem.refPoint - - onRefPointChanged: { - if (refPoint !== coordinate) { - coordinate = refPoint - } - } - onClicked: { _root.clicked(_missionItem.sequenceNumber) } onDragReleased: { _missionItem.refPoint = coordinate + coordinate = Qt.binding(function (){return _missionItem.refPoint}) } } } diff --git a/src/WimaView/WimaServiceAreaMapVisual.qml b/src/WimaView/WimaServiceAreaMapVisual.qml index d0a3f275622a035ae80dfc4ad9b67e183cd3305f..69f006a505c9891523720b2aa30d6b71d217e1b3 100644 --- a/src/WimaView/WimaServiceAreaMapVisual.qml +++ b/src/WimaView/WimaServiceAreaMapVisual.qml @@ -104,7 +104,7 @@ Item { Component.onCompleted: { _addInitialPolygon() - if (interactive){ + if (showDepot){ _addDepot() } }