From 887e51f1aa4c77bb4a27d9150e5a24c20cb4b0e8 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Fri, 30 Aug 2019 16:24:53 +0200 Subject: [PATCH] hello --- ToDo.txt | 3 +- qgroundcontrol.qrc | 1 + src/PlanView/CircularSurveyItemEditor.qml | 8 +- .../QGroundControl.Controls.qmldir | 1 + src/Wima/CircularSurvey.SettingsGroup.json | 4 +- src/Wima/CircularSurveyComplexItem.cc | 71 ++++++------ src/Wima/OptimisationTools.h | 9 +- src/Wima/PolygonCalculus.cc | 18 ++-- src/Wima/PolygonCalculus.h | 2 +- src/Wima/WimaPlaner.cc | 9 +- src/WimaView/WimaJoinedAreaMapVisual.qml | 101 ++++++++++++++++++ 11 files changed, 168 insertions(+), 59 deletions(-) create mode 100644 src/WimaView/WimaJoinedAreaMapVisual.qml diff --git a/ToDo.txt b/ToDo.txt index 7fae9ae45..c6970d121 100644 --- a/ToDo.txt +++ b/ToDo.txt @@ -1,3 +1,4 @@ optimize circular survey remove Reference artefacts -solve Dijkstra issue (no path found, for some geometries) +solve Dijkstra issue (path not found, or not minimal) + diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index fc92fbc32..adea3edae 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -225,6 +225,7 @@ src/WimaView/DragCoordinate.qml src/WimaView/CoordinateIndicatorDrag.qml src/WimaView/CoordinateIndicator.qml + src/WimaView/WimaJoinedAreaMapVisual.qml src/Settings/APMMavlinkStreamRate.SettingsGroup.json diff --git a/src/PlanView/CircularSurveyItemEditor.qml b/src/PlanView/CircularSurveyItemEditor.qml index 0929168fe..0caff7599 100644 --- a/src/PlanView/CircularSurveyItemEditor.qml +++ b/src/PlanView/CircularSurveyItemEditor.qml @@ -81,15 +81,15 @@ Rectangle { /*QGCSlider { id: rSlider - minimumValue: 0.1 - maximumValue: 5 + minimumValue: 0.3 + maximumValue: 20 stepSize: 0.1 tickmarksEnabled: false Layout.fillWidth: true Layout.columnSpan: 2 Layout.preferredHeight: ScreenTools.defaultFontPixelHeight * 1.5 onValueChanged: missionItem.deltaR.value = value - Component.onCompleted: value = missionItem.deltaR.value + Component.onCompleted: value = missionItem.deltaR.defaultValue updateValueWhileDragging: true }*/ @@ -110,7 +110,7 @@ Rectangle { Layout.columnSpan: 2 Layout.preferredHeight: ScreenTools.defaultFontPixelHeight * 1.5 onValueChanged: missionItem.deltaAlpha.value = value - Component.onCompleted: value = missionItem.deltaAlpha.value + Component.onCompleted: value = missionItem.deltaAlpha.defaultValue updateValueWhileDragging: true } } diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index a9a8ef2b2..e78fa1814 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -87,6 +87,7 @@ FlyAreaItemEditor 1.0 FlyAreaItemEditor.qml WimaMapVisual 1.0 WimaMapVisual.qml WimaMeasurementAreaMapVisual 1.0 WimaMeasurementAreaMapVisual.qml +WimaJoinedAreaMapVisual 1.0 WimaJoinedAreaMapVisual.qml WimaMeasurementAreaEditor 1.0 WimaMeasurementAreaEditor.qml WimaServiceAreaMapVisual 1.0 WimaServiceAreaMapVisual.qml WimaAreaMapVisual 1.0 WimaAreaMapVisual.qml diff --git a/src/Wima/CircularSurvey.SettingsGroup.json b/src/Wima/CircularSurvey.SettingsGroup.json index 88eeccfb4..77d0e5039 100644 --- a/src/Wima/CircularSurvey.SettingsGroup.json +++ b/src/Wima/CircularSurvey.SettingsGroup.json @@ -6,7 +6,7 @@ "units": "m", "min": 0.3, "decimalPlaces": 1, - "defaultValue": 3 + "defaultValue": 20 }, { "name": "DeltaAlpha", @@ -15,6 +15,6 @@ "units": "Deg", "min": 0.3, "decimalPlaces": 1, - "defaultValue": 1 + "defaultValue": 5 } ] diff --git a/src/Wima/CircularSurveyComplexItem.cc b/src/Wima/CircularSurveyComplexItem.cc index fcbd2b45d..3e9fd9195 100644 --- a/src/Wima/CircularSurveyComplexItem.cc +++ b/src/Wima/CircularSurveyComplexItem.cc @@ -111,8 +111,10 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() return; - double dalpha = _deltaAlpha.rawValue().toDouble()/180*M_PI; // radiants + double dalpha = _deltaAlpha.rawValue().toDouble()/180.0*M_PI; // radiants double dr = _deltaR.rawValue().toDouble(); // meter +// double dalpha = 1.0/180.0*M_PI; // radiants +// double dr = 10.0; // meter double r_min = dr; // meter double r_max = (*std::max_element(distances.begin(), distances.end())); // meter @@ -230,42 +232,43 @@ void CircularSurveyComplexItem::_rebuildTransectsPhase1() } } - // optimize path to lawn pattern - if (fullPath.size() == 0) - return; - QList currentSection = fullPath.takeFirst(); - QList> optiPath; // optimized path - while( !fullPath.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 < fullPath.size(); i++) { - auto iteratorPath = fullPath[i]; - double dist = PlanimetryCalculus::distance(endVertex, iteratorPath.first()); - if ( dist < minDist ) { - minDist = dist; - index = i; - } - dist = PlanimetryCalculus::distance(endVertex, iteratorPath.last()); - if (dist < minDist) { - minDist = dist; - index = i; - reversePath = true; - } - } - currentSection = fullPath.takeAt(index); - if (reversePath) { - PolygonCalculus::reversePath(currentSection); - } - } +// // optimize path to lawn pattern +// if (fullPath.size() == 0) +// return; +// QList currentSection = fullPath.takeFirst(); +// QList> optiPath; // optimized path +// while( !fullPath.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 < fullPath.size(); i++) { +// auto iteratorPath = fullPath[i]; +// double dist = PlanimetryCalculus::distance(endVertex, iteratorPath.first()); +// if ( dist < minDist ) { +// minDist = dist; +// index = i; +// } +// dist = PlanimetryCalculus::distance(endVertex, iteratorPath.last()); +// if (dist < minDist) { +// minDist = dist; +// index = i; +// reversePath = true; +// } +// } +// currentSection = fullPath.takeAt(index); +// if (reversePath) { +// PolygonCalculus::reversePath(currentSection); +// } +// } // convert to CoordInfo_t - for ( const QList &transect : optiPath) { +// for ( const QList &transect : optiPath) { + for ( const QList &transect : fullPath) { QList geoPath = toGeo(transect, _referencePoint); QList transectList; for ( const QGeoCoordinate &coordinate : geoPath) { diff --git a/src/Wima/OptimisationTools.h b/src/Wima/OptimisationTools.h index e1c94bbda..1dd535926 100644 --- a/src/Wima/OptimisationTools.h +++ b/src/Wima/OptimisationTools.h @@ -16,7 +16,7 @@ namespace OptimisationTools { * \sa QList */ template - bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function distance) + bool dijkstraAlgorithm(const QList &elements, int startIndex, int endIndex, QList &elementPath, std::function distanceDij) { if ( elements.isEmpty() || startIndex < 0 || startIndex >= elements.size() || endIndex < 0 @@ -53,7 +53,7 @@ namespace OptimisationTools { while (workingSet.size() > 0) { // serach Node with minimal distance double minDist = std::numeric_limits::infinity(); - int minDistIndex = 0; + int minDistIndex = -1; for (int i = 0; i < workingSet.size(); i++) { Node* node = workingSet.value(i); double dist = node->distance; @@ -62,12 +62,15 @@ namespace OptimisationTools { minDistIndex = i; } } + if (minDistIndex == -1) + return false; + Node* u = workingSet.takeAt(minDistIndex); //update distance for (int i = 0; i < workingSet.size(); i++) { Node* v = workingSet[i]; - double dist = distance(u->element, v->element); + double dist = distanceDij(u->element, v->element); // is ther a alternative path which is shorter? double alternative = u->distance + dist; if (alternative < v->distance) { diff --git a/src/Wima/PolygonCalculus.cc b/src/Wima/PolygonCalculus.cc index 871165a74..adc5f9309 100644 --- a/src/Wima/PolygonCalculus.cc +++ b/src/Wima/PolygonCalculus.cc @@ -167,7 +167,7 @@ namespace PolygonCalculus { int startIndex = 0; bool crossContainsWalker = true; for (int i = 0; i < walkerPoly->size(); i++) { - if ( !crossPoly->contains(walkerPoly->value(i)) ) { + if ( !contains(*crossPoly, walkerPoly->at(i)) ) { crossContainsWalker = false; startIndex = i; break; @@ -178,11 +178,10 @@ namespace PolygonCalculus { joinedPolygon.append(*crossPoly); return JoinPolygonError::PolygonJoined; } - QPointF lastVertex = walkerPoly->last(); - QPointF currentVertex = walkerPoly->value(startIndex); + QPointF currentVertex = walkerPoly->at(startIndex); QPointF startVertex = currentVertex; // possible nextVertex (if no intersection between currentVertex and protoVertex with crossPoly) - int nextVertexNumber = nextVertexIndex(walkerPoly->size(), startIndex); + int nextVertexNumber = nextVertexIndex(walkerPoly->size(), startIndex); QPointF protoNextVertex = walkerPoly->value(nextVertexNumber); while (1) { //qDebug("nextVertexNumber: %i", nextVertexNumber); @@ -206,14 +205,13 @@ namespace PolygonCalculus { for (int i = 0; i < intersectionList.size(); i++) { double currentDist = PlanimetryCalculus::distance(currentVertex, intersectionList[i]); - if ( minDist > currentDist && currentVertex != intersectionList[i]) { + if ( minDist > currentDist && !qFuzzyIsNull(distance(currentVertex, intersectionList[i])) ) { minDist = currentDist; minDistIndex = i; } } if (minDistIndex != -1){ - lastVertex = currentVertex; currentVertex = intersectionList.value(minDistIndex); QPair neighbours = neighbourList.value(minDistIndex); protoNextVertex = crossPoly->value(neighbours.second); @@ -224,14 +222,12 @@ namespace PolygonCalculus { walkerPoly = crossPoly; crossPoly = temp; } else { - lastVertex = currentVertex; currentVertex = walkerPoly->value(nextVertexNumber); nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber); protoNextVertex = walkerPoly->value(nextVertexNumber); } } else { - lastVertex = currentVertex; currentVertex = walkerPoly->value(nextVertexNumber); nextVertexNumber = nextVertexIndex(walkerPoly->size(), nextVertexNumber); protoNextVertex = walkerPoly->value(nextVertexNumber); @@ -479,7 +475,7 @@ namespace PolygonCalculus { return; } - bool shortestPath(QPolygonF polygon, QPointF startVertex, const QPointF &endVertex, QList &shortestPath) + bool shortestPath(QPolygonF polygon,const QPointF &startVertex, const QPointF &endVertex, QList &shortestPath) { using namespace PlanimetryCalculus; offsetPolygon(polygon, 0.01); // solves numerical errors @@ -488,7 +484,7 @@ namespace PolygonCalculus { // lambda QPolygonF polygon2 = polygon; offsetPolygon(polygon2, 0.01); // solves numerical errors - std::function distance = [polygon2](const QPointF &p1, const QPointF &p2) -> double { + std::function distanceDij = [polygon2](const QPointF &p1, const QPointF &p2) -> double { if (containsPath(polygon2, p1, p2)){ double dx = p1.x()-p2.x(); double dy = p1.y()-p2.y(); @@ -503,7 +499,7 @@ namespace PolygonCalculus { for (int i = 0; i < polygon.size(); i++) { elementList.append(polygon[i]); } - return OptimisationTools::dijkstraAlgorithm(elementList, 0, 1, shortestPath, distance); + return OptimisationTools::dijkstraAlgorithm(elementList, 0, 1, shortestPath, distanceDij); } else { return false; } diff --git a/src/Wima/PolygonCalculus.h b/src/Wima/PolygonCalculus.h index 96f57c1ae..81658edbd 100644 --- a/src/Wima/PolygonCalculus.h +++ b/src/Wima/PolygonCalculus.h @@ -26,7 +26,7 @@ namespace PolygonCalculus { void offsetPolygon (QPolygonF &polygon, double offset); bool containsPath (QPolygonF polygon, const QPointF &c1, const QPointF &c2); void decomposeToConvex (const QPolygonF &polygon, QList &convexPolygons); - bool shortestPath (QPolygonF polygon, QPointF startVertex, const QPointF &endVertex, QList &shortestPath); + bool shortestPath (QPolygonF polygon, const QPointF &startVertex, const QPointF &endVertex, QList &shortestPath); QPolygonF toQPolygonF(const QVector3DList &polygon); QPolygonF toQPolygonF(const QPointFList &polygon); diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 76570612f..a0ab837bf 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -263,6 +263,9 @@ bool WimaPlaner::updateMission() } QGeoCoordinate start = _serviceArea.center(); QGeoCoordinate end = survey->visualTransectPoints().first().value(); + if (!_visualItems.contains(&_joinedArea)) + _visualItems.append(&_joinedArea); + QList path; if ( !calcShortestPath(start, end, path)) { qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data()); @@ -576,10 +579,10 @@ bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordin using namespace PolygonCalculus; QList path2D; bool retVal = PolygonCalculus::shortestPath( - toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/start)), + toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)), /*start point*/ QPointF(0,0), - /*destination*/toCartesian2D(destination, start), - /*shortest path*/path2D); + /*destination*/ toCartesian2D(destination, start), + /*shortest path*/ path2D); path.append(toGeo(path2D, /*origin*/start)); return retVal; diff --git a/src/WimaView/WimaJoinedAreaMapVisual.qml b/src/WimaView/WimaJoinedAreaMapVisual.qml new file mode 100644 index 000000000..fac7abe93 --- /dev/null +++ b/src/WimaView/WimaJoinedAreaMapVisual.qml @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtLocation 5.3 +import QtPositioning 5.3 + +import QGroundControl 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FlightMap 1.0 + +/// Wima Global Measurement Area visuals +Item { + id: _root + + property var map ///< Map control to place item in + property var qgcView ///< QGCView to use for popping dialogs + + property var areaItem: object + property var _polygon: areaItem + //property var _polyline: areaItem.polyline + + signal clicked(int sequenceNumber) + + /// Add an initial 4 sided polygon if there is none + function _addInitialPolygon() { + if (_polygon.count < 3) { + // Initial polygon is inset to take 2/3rds space + var rect = Qt.rect(map.centerViewport.x, map.centerViewport.y, map.centerViewport.width, map.centerViewport.height) + rect.x += (rect.width * 0.25) / 2 + rect.y += (rect.height * 0.25) / 2 + rect.width *= 0.25 + rect.height *= 0.25 + + var centerCoord = map.toCoordinate(Qt.point(rect.x + (rect.width / 2), rect.y + (rect.height / 2)), false /* clipToViewPort */) + var topLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y), false /* clipToViewPort */) + var topRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y), false /* clipToViewPort */) + var bottomLeftCoord = map.toCoordinate(Qt.point(rect.x, rect.y + rect.height), false /* clipToViewPort */) + var bottomRightCoord = map.toCoordinate(Qt.point(rect.x + rect.width, rect.y + rect.height), false /* clipToViewPort */) + + // Adjust polygon to max size + var maxSize = 100 + var halfWidthMeters = Math.min(topLeftCoord.distanceTo(topRightCoord), maxSize) / 2 + var halfHeightMeters = Math.min(topLeftCoord.distanceTo(bottomLeftCoord), maxSize) / 2 + topLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 0) + topRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 0) + bottomLeftCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, -90).atDistanceAndAzimuth(halfHeightMeters, 180) + bottomRightCoord = centerCoord.atDistanceAndAzimuth(halfWidthMeters, 90).atDistanceAndAzimuth(halfHeightMeters, 180) + + _polygon.appendVertex(topLeftCoord) + _polygon.appendVertex(topRightCoord) + _polygon.appendVertex(bottomRightCoord) + _polygon.appendVertex(bottomLeftCoord) + } + } + + /*function _addInitialPolyline(){ + _polyline.setStartVertexIndex(0); + _polyline.setEndVertexIndex(1); + }*/ + + + + Component.onCompleted: { + //_addInitialPolygon() + //_addInitialPolyline() + } + + Component.onDestruction: { + } + + WimaMapPolygonVisuals { + qgcView: _root.qgcView + mapControl: map + mapPolygon: _polygon + borderWidth: 1 + borderColor: "black" + interiorColor: "blue" + interiorOpacity: 0.25 + } + + /*WimaMapPolylineVisuals { + qgcView: _root.qgcView + mapControl: map + mapPolyline: _polyline + lineWidth: 4 + lineColor: interactive ? "white" : "yellow" + enableSplitHandels: false + enableDragHandels: true + edgeHandelsOnly: true + }*/ +} -- 2.22.0