diff --git a/Paths/KlingenbachTest2.wima b/Paths/KlingenbachTest2.wima
new file mode 100644
index 0000000000000000000000000000000000000000..e6d74be02383fe69ba58871b412ec25ff8cbf608
--- /dev/null
+++ b/Paths/KlingenbachTest2.wima
@@ -0,0 +1,785 @@
+{
+ "AreaItems": [
+ {
+ "AreaType": "Service Area",
+ "BorderPolygonOffset": 6,
+ "DepotAltitude": 0,
+ "DepotLatitude": 47.76779586216649,
+ "DepotLongitude": 16.530510396830728,
+ "ShowBorderPolygon": 0,
+ "maxAltitude": 30,
+ "polygon": [
+ [
+ 47.76780094967389,
+ 16.530343715825353
+ ],
+ [
+ 47.76788238112663,
+ 16.530625096151965
+ ],
+ [
+ 47.767783696604035,
+ 16.530655731490725
+ ],
+ [
+ 47.76771642095403,
+ 16.53041704413795
+ ]
+ ]
+ },
+ {
+ "AreaType": "Measurement Area",
+ "BorderPolygonOffset": 6,
+ "MinTileAreaPercent": 30,
+ "ShowBorderPolygon": 0,
+ "ShowTiles": true,
+ "TileHeight": 5,
+ "TileWidth": 5,
+ "maxAltitude": 30,
+ "polygon": [
+ [
+ 47.76809580679245,
+ 16.530246122817612
+ ],
+ [
+ 47.76823933601322,
+ 16.53060087654427
+ ],
+ [
+ 47.7683711160486,
+ 16.530967006195464
+ ],
+ [
+ 47.7680076482754,
+ 16.531153949077463
+ ],
+ [
+ 47.7677855557718,
+ 16.530403347547246
+ ],
+ [
+ 47.76812093862815,
+ 16.53088714314225
+ ]
+ ]
+ }
+ ],
+ "MissionItems": {
+ "fileType": "Plan",
+ "geoFence": {
+ "circles": [
+ ],
+ "polygons": [
+ ],
+ "version": 2
+ },
+ "groundStation": "QGroundControl",
+ "mission": {
+ "cruiseSpeed": 15,
+ "firmwareType": 3,
+ "hoverSpeed": 1,
+ "items": [
+ {
+ "autoContinue": true,
+ "command": 22,
+ "doJumpId": 1,
+ "frame": 3,
+ "params": [
+ 15,
+ 0,
+ 0,
+ null,
+ 47.76779586216649,
+ 16.530510396830728,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "Alpha": 23,
+ "MinLength": 5,
+ "NumRuns": 1,
+ "ReferencePointAlt": 0,
+ "ReferencePointLat": 47.76810433066452,
+ "ReferencePointLong": 16.530712738301304,
+ "Run": 1,
+ "TransectDistance": 3,
+ "TransectStyleComplexItem": {
+ "CameraCalc": {
+ "AdjustedFootprintFrontal": 25,
+ "AdjustedFootprintSide": 25,
+ "CameraName": "Manual (no camera specs)",
+ "DistanceToSurface": 10,
+ "DistanceToSurfaceRelative": true,
+ "version": 1
+ },
+ "CameraShots": 0,
+ "CameraTriggerInTurnAround": true,
+ "FollowTerrain": false,
+ "HoverAndCapture": false,
+ "Items": [
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 2,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76783728898401,
+ 16.530481694784847,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 3,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76802613957926,
+ 16.53114161450939,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 4,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.768179446847086,
+ 16.53106276402324,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 5,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76812171672615,
+ 16.530861029906276,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 6,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76831125432985,
+ 16.530806354689858,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 7,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.768358305251056,
+ 16.530970771201435,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 8,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76833275405663,
+ 16.53098391307365,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 9,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76818906969824,
+ 16.530481820523075,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 10,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76810775728066,
+ 16.530504966391423,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 11,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76825610046365,
+ 16.53102333861304,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 12,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.768281651653936,
+ 16.531010196779448,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 13,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76810310388368,
+ 16.530386278613275,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 14,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76809845036399,
+ 16.530267590829624,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 15,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76830720286059,
+ 16.53099705493299,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 16,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76823054925376,
+ 16.531036480433762,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 17,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.768112410554885,
+ 16.530623654217433,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 18,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76811706370638,
+ 16.530742342037925,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 19,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76820499805124,
+ 16.531049622228267,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 20,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76815389564133,
+ 16.531075905805334,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 21,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.768086884966515,
+ 16.53084174152664,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 22,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76803696584865,
+ 16.530769731894658,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 23,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76812834442496,
+ 16.531089047574557,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 24,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76810279321597,
+ 16.53110218933091,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 25,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76798704669417,
+ 16.530697722387153,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 26,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.767937127512056,
+ 16.530625713070812,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 27,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76807724201434,
+ 16.531115331061052,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 28,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.768051690793115,
+ 16.531128472791657,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 29,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76788720826634,
+ 16.530553703838915,
+ 10
+ ],
+ "type": "SimpleItem"
+ }
+ ],
+ "Refly90Degrees": false,
+ "TurnAroundDistance": 10,
+ "VisualTransectPoints": [
+ [
+ 47.76783728898401,
+ 16.530481694784847
+ ],
+ [
+ 47.76802613957926,
+ 16.53114161450939
+ ],
+ [
+ 47.768179446847086,
+ 16.53106276402324
+ ],
+ [
+ 47.76812171672615,
+ 16.530861029906276
+ ],
+ [
+ 47.76831125432985,
+ 16.530806354689858
+ ],
+ [
+ 47.768358305251056,
+ 16.530970771201435
+ ],
+ [
+ 47.76833275405663,
+ 16.53098391307365
+ ],
+ [
+ 47.76818906969824,
+ 16.530481820523075
+ ],
+ [
+ 47.76810775728066,
+ 16.530504966391423
+ ],
+ [
+ 47.76825610046365,
+ 16.53102333861304
+ ],
+ [
+ 47.768281651653936,
+ 16.531010196779448
+ ],
+ [
+ 47.76810310388368,
+ 16.530386278613275
+ ],
+ [
+ 47.76809845036399,
+ 16.530267590829624
+ ],
+ [
+ 47.76830720286059,
+ 16.53099705493299
+ ],
+ [
+ 47.76823054925376,
+ 16.531036480433762
+ ],
+ [
+ 47.768112410554885,
+ 16.530623654217433
+ ],
+ [
+ 47.76811706370638,
+ 16.530742342037925
+ ],
+ [
+ 47.76820499805124,
+ 16.531049622228267
+ ],
+ [
+ 47.76815389564133,
+ 16.531075905805334
+ ],
+ [
+ 47.768086884966515,
+ 16.53084174152664
+ ],
+ [
+ 47.76803696584865,
+ 16.530769731894658
+ ],
+ [
+ 47.76812834442496,
+ 16.531089047574557
+ ],
+ [
+ 47.76810279321597,
+ 16.53110218933091
+ ],
+ [
+ 47.76798704669417,
+ 16.530697722387153
+ ],
+ [
+ 47.767937127512056,
+ 16.530625713070812
+ ],
+ [
+ 47.76807724201434,
+ 16.531115331061052
+ ],
+ [
+ 47.768051690793115,
+ 16.531128472791657
+ ],
+ [
+ 47.76788720826634,
+ 16.530553703838915
+ ]
+ ],
+ "version": 1
+ },
+ "Type": 1,
+ "Variant": 0,
+ "complexItemType": "CircularSurvey",
+ "polygon": [
+ [
+ 50.54939570164431,
+ 7.196328001507449
+ ],
+ [
+ 50.54939570164431,
+ 7.238775845997314
+ ],
+ [
+ 50.522416083956976,
+ 7.238775845997314
+ ],
+ [
+ 50.522416083956976,
+ 7.196328001507449
+ ]
+ ],
+ "type": "ComplexItem",
+ "version": 1
+ },
+ {
+ "AMSLAltAboveTerrain": null,
+ "Altitude": 10,
+ "AltitudeMode": 1,
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 33,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76807639917751,
+ 16.530893624112736,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "AMSLAltAboveTerrain": null,
+ "Altitude": 10,
+ "AltitudeMode": 1,
+ "autoContinue": true,
+ "command": 16,
+ "doJumpId": 34,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76779586216649,
+ 16.530510396830728,
+ 10
+ ],
+ "type": "SimpleItem"
+ },
+ {
+ "AMSLAltAboveTerrain": null,
+ "Altitude": 0,
+ "AltitudeMode": 1,
+ "autoContinue": true,
+ "command": 21,
+ "doJumpId": 35,
+ "frame": 3,
+ "params": [
+ 0,
+ 0,
+ 0,
+ null,
+ 47.76779586216649,
+ 16.530510396830728,
+ 0
+ ],
+ "type": "SimpleItem"
+ }
+ ],
+ "plannedHomePosition": [
+ 47.76779586216649,
+ 16.530510396830728,
+ 178
+ ],
+ "vehicleType": 2,
+ "version": 2
+ },
+ "rallyPoints": {
+ "points": [
+ ],
+ "version": 2
+ },
+ "version": 1
+ }
+}
diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 8a40bde801ebdae22a414daa5372528287561796..808f020b8392e730eff2e6d3c2e29e931cbd425d 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -418,6 +418,9 @@ HEADERS += \
src/Wima/GenericSingelton.h \
src/Wima/Geometry/GenericCircle.h \
src/Wima/RoutingThread.h \
+ src/Wima/Snake/CircularGenerator.h \
+ src/Wima/Snake/GeneratorBase.h \
+ src/Wima/Snake/LinearGenerator.h \
src/Wima/Snake/clipper/clipper.hpp \
src/Wima/Snake/mapbox/feature.hpp \
src/Wima/Snake/mapbox/geometry.hpp \
@@ -510,6 +513,9 @@ SOURCES += \
src/Wima/CircularSurvey.cc \
src/Wima/GenericSingelton.cpp \
src/Wima/RoutingThread.cpp \
+ src/Wima/Snake/CircularGenerator.cpp \
+ src/Wima/Snake/GeneratorBase.cc \
+ src/Wima/Snake/LinearGenerator.cpp \
src/Wima/Snake/clipper/clipper.cpp \
src/Wima/Snake/snake.cpp \
src/Wima/Geometry/GeoPoint3D.cpp \
@@ -1449,5 +1455,8 @@ contains (CONFIG, QGC_DISABLE_INSTALLER_SETUP) {
}
DISTFILES += \
+ src/Wima/Routing/json/CircularGenerator.SettingsGroup.json \
+ src/Wima/Snake/json/LinearGenerator.SettingsGroup.json \
+ src/Wima/json/CircularSurvey.SettingsGroup.json \
src/WimaView/WimaMeasurementAreaEditor.qml \
src/Settings/Wima.SettingsGroup.json
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 82b70a88178ee45b1f121acf5b9bf7aa1c81a27c..0c50bb2111b92f00d6af7227e5ae8abd3df3b819 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -7,93 +7,146 @@
src/ui/toolbar/BatteryIndicator.qml
src/ui/toolbar/GPSIndicator.qml
src/ui/toolbar/GPSRTKIndicator.qml
+ src/ui/toolbar/JoystickIndicator.qml
+ src/ui/toolbar/LinkIndicator.qml
src/ui/toolbar/MessageIndicator.qml
src/ui/toolbar/ModeIndicator.qml
- src/ui/toolbar/VTOLModeIndicator.qml
src/ui/toolbar/RCRSSIIndicator.qml
src/ui/toolbar/TelemetryRSSIIndicator.qml
- src/ui/toolbar/JoystickIndicator.qml
- src/ui/toolbar/LinkIndicator.qml
+ src/ui/toolbar/VTOLModeIndicator.qml
- src/PlanView/CorridorScanEditor.qml
- src/PlanView/CameraCalc.qml
- src/PlanView/CorridorScanMapVisual.qml
- src/QmlControls/EditPositionDialog.qml
- src/QmlControls/FileButton.qml
- src/MissionManager/QGCMapCircleVisuals.qml
- src/MissionManager/QGCMapPolylineVisuals.qml
- src/PlanView/StructureScanMapVisual.qml
+ src/AnalyzeView/AnalyzePage.qml
src/AnalyzeView/AnalyzeView.qml
- src/ui/AppSettings.qml
- src/ui/preferences/BluetoothSettings.qml
- src/FlightMap/Widgets/CameraPageWidget.qml
- src/ViewWidgets/CustomCommandWidget.qml
- src/ui/preferences/DebugWindow.qml
+ src/AnalyzeView/GeoTagPage.qml
+ src/AnalyzeView/LogDownloadPage.qml
+ src/AnalyzeView/MavlinkConsolePage.qml
src/AutoPilotPlugins/Common/ESP8266Component.qml
src/AutoPilotPlugins/Common/ESP8266ComponentSummary.qml
- src/VehicleSetup/FirmwareUpgrade.qml
+ src/AutoPilotPlugins/Common/MotorComponent.qml
+ src/AutoPilotPlugins/Common/RadioComponent.qml
+ src/AutoPilotPlugins/Common/SetupPage.qml
+ src/AutoPilotPlugins/Common/SyslinkComponent.qml
+ src/FactSystem/FactControls/AltitudeFactTextField.qml
+ src/FactSystem/FactControls/FactBitmask.qml
+ src/FactSystem/FactControls/FactCheckBox.qml
+ src/FactSystem/FactControls/FactComboBox.qml
+ src/FactSystem/FactControls/FactLabel.qml
+ src/FactSystem/FactControls/FactPanel.qml
+ src/FactSystem/FactControls/FactTextField.qml
+ src/FactSystem/FactControls/FactTextFieldGrid.qml
+ src/FactSystem/FactControls/FactTextFieldRow.qml
+ src/FactSystem/FactControls/FactTextFieldSlider.qml
+ src/FactSystem/FactControls/FactValueSlider.qml
+ src/FactSystem/FactControls/qmldir
+ src/FlightDisplay/BuiltInPreFlightCheckModel.qml
+ src/FlightDisplay/FlightDisplayView.qml
src/FlightDisplay/FlightDisplayViewDummy.qml
+ src/FlightDisplay/FlightDisplayViewMap.qml
src/FlightDisplay/FlightDisplayViewUVC.qml
- src/PlanView/FWLandingPatternEditor.qml
- src/ui/preferences/GeneralSettings.qml
- src/AnalyzeView/GeoTagPage.qml
+ src/FlightDisplay/FlightDisplayViewVideo.qml
+ src/FlightDisplay/FlightDisplayViewWidgets.qml
+ src/FlightDisplay/FlightDisplayWimaMenu.qml
+ src/FlightDisplay/GuidedActionConfirm.qml
+ src/FlightDisplay/GuidedActionList.qml
+ src/FlightDisplay/GuidedActionsController.qml
+ src/FlightDisplay/GuidedAltitudeSlider.qml
+ src/FlightDisplay/MultiVehicleList.qml
+ src/FlightDisplay/PreFlightBatteryCheck.qml
+ src/FlightDisplay/PreFlightGPSCheck.qml
+ src/FlightDisplay/PreFlightRCCheck.qml
+ src/FlightDisplay/PreFlightSensorsHealthCheck.qml
+ src/FlightDisplay/PreFlightSoundCheck.qml
+ src/FlightDisplay/SmallValue.qml
+ src/FlightDisplay/VirtualJoystick.qml
+ src/FlightDisplay/qmldir
+ src/FlightMap/FlightMap.qml
+ src/FlightMap/MapItems/CameraTriggerIndicator.qml
+ src/FlightMap/MapItems/CustomMapItems.qml
+ src/FlightMap/MapItems/MissionItemIndicator.qml
+ src/FlightMap/MapItems/MissionItemIndicatorDrag.qml
+ src/FlightMap/MapItems/MissionItemView.qml
+ src/FlightMap/MapItems/MissionLineView.qml
+ src/FlightMap/MapItems/PlanMapItems.qml
+ src/FlightMap/MapItems/PolygonEditor.qml
+ src/FlightMap/MapItems/VehicleMapItem.qml
+ src/FlightMap/MapItems/WimaPlanMapItems.qml
+ src/FlightMap/MapScale.qml
+ src/FlightMap/QGCVideoBackground.qml
+ src/FlightMap/Widgets/CameraPageWidget.qml
+ src/FlightMap/Widgets/CenterMapDropButton.qml
+ src/FlightMap/Widgets/CenterMapDropPanel.qml
+ src/FlightMap/Widgets/CompassRing.qml
src/FlightMap/Widgets/HealthPageWidget.qml
- src/ui/preferences/HelpSettings.qml
- src/VehicleSetup/JoystickConfig.qml
- src/ui/preferences/LinkSettings.qml
- src/AnalyzeView/LogDownloadPage.qml
- src/ui/preferences/LogReplaySettings.qml
- src/ui/MainWindowHybrid.qml
- src/ui/MainWindowInner.qml
- src/ui/MainWindowNative.qml
- src/AnalyzeView/MavlinkConsolePage.qml
- src/ui/preferences/MavlinkSettings.qml
- src/PlanView/MissionSettingsEditor.qml
- src/ui/preferences/MockLink.qml
- src/ui/preferences/MockLinkSettings.qml
- src/AutoPilotPlugins/Common/MotorComponent.qml
- src/QtLocationPlugin/QMLControl/OfflineMap.qml
- src/PlanView/PlanView.qml
- src/VehicleSetup/PX4FlowSensor.qml
+ src/FlightMap/Widgets/InstrumentSwipeView.qml
+ src/FlightMap/Widgets/MapFitFunctions.qml
+ src/FlightMap/Widgets/QGCArtificialHorizon.qml
+ src/FlightMap/Widgets/QGCAttitudeHUD.qml
+ src/FlightMap/Widgets/QGCAttitudeWidget.qml
+ src/FlightMap/Widgets/QGCCompassWidget.qml
src/FlightMap/Widgets/QGCInstrumentWidget.qml
src/FlightMap/Widgets/QGCInstrumentWidgetAlternate.qml
- src/AnalyzeView/AnalyzePage.qml
- src/QmlControls/AppMessages.qml
+ src/FlightMap/Widgets/QGCPitchIndicator.qml
+ src/FlightMap/Widgets/ValuePageWidget.qml
+ src/FlightMap/Widgets/VibrationPageWidget.qml
+ src/FlightMap/Widgets/VideoPageWidget.qml
+ src/FlightMap/qmldir
+ src/MissionManager/QGCMapCircleVisuals.qml
+ src/MissionManager/QGCMapPolygonVisuals.qml
+ src/MissionManager/QGCMapPolylineVisuals.qml
+ src/PlanView/CameraCalc.qml
src/PlanView/CameraSection.qml
+ src/PlanView/CorridorScanEditor.qml
+ src/PlanView/CorridorScanMapVisual.qml
+ src/PlanView/FWLandingPatternEditor.qml
+ src/PlanView/FWLandingPatternMapVisual.qml
+ src/PlanView/GeoFenceEditor.qml
+ src/PlanView/GeoFenceMapVisuals.qml
+ src/PlanView/MissionItemEditor.qml
+ src/PlanView/MissionItemMapVisual.qml
+ src/PlanView/MissionItemStatus.qml
+ src/PlanView/MissionSettingsEditor.qml
+ src/PlanView/PlanToolBar.qml
+ src/PlanView/PlanView.qml
+ src/PlanView/RallyPointEditorHeader.qml
+ src/PlanView/RallyPointItemEditor.qml
+ src/PlanView/RallyPointMapVisuals.qml
+ src/PlanView/SimpleItemEditor.qml
+ src/PlanView/SimpleItemMapVisual.qml
+ src/PlanView/StructureScanEditor.qml
+ src/PlanView/StructureScanMapVisual.qml
+ src/PlanView/SurveyItemEditor.qml
+ src/PlanView/SurveyMapVisual.qml
+ src/PlanView/TransectStyleComplexItemStats.qml
+ src/PlanView/WimaMissionItemMapVisual.qml
+ src/QmlControls/AppMessages.qml
src/QmlControls/ClickableColor.qml
src/QmlControls/DeadMouseArea.qml
src/QmlControls/DropButton.qml
src/QmlControls/DropPanel.qml
+ src/QmlControls/EditPositionDialog.qml
src/QmlControls/ExclusiveGroupItem.qml
src/QmlControls/FactSliderPanel.qml
+ src/QmlControls/FileButton.qml
src/QmlControls/FlightModeDropdown.qml
src/QmlControls/FlightModeMenu.qml
- src/PlanView/FWLandingPatternMapVisual.qml
- src/PlanView/GeoFenceEditor.qml
- src/PlanView/GeoFenceMapVisuals.qml
src/QmlControls/IndicatorButton.qml
src/QmlControls/JoystickThumbPad.qml
- src/ui/toolbar/MainToolBar.qml
- src/ui/toolbar/MainToolBarIndicators.qml
src/QmlControls/MissionCommandDialog.qml
- src/PlanView/MissionItemEditor.qml
src/QmlControls/MissionItemIndexLabel.qml
- src/PlanView/MissionItemMapVisual.qml
- src/PlanView/MissionItemStatus.qml
src/QmlControls/ModeSwitchDisplay.qml
src/QmlControls/MultiRotorMotorDisplay.qml
src/QmlControls/OfflineMapButton.qml
+ src/QmlControls/PIDTuning.qml
src/QmlControls/PageView.qml
src/QmlControls/ParameterEditor.qml
src/QmlControls/ParameterEditorDialog.qml
- src/QmlControls/PIDTuning.qml
- src/PlanView/PlanToolBar.qml
src/QmlControls/PreFlightCheckButton.qml
src/QmlControls/PreFlightCheckGroup.qml
src/QmlControls/PreFlightCheckList.qml
src/QmlControls/PreFlightCheckModel.qml
src/QmlControls/QGCButton.qml
+ src/QmlControls/QGCButtonColumn.qml
src/QmlControls/QGCCheckBox.qml
src/QmlControls/QGCColoredImage.qml
src/QmlControls/QGCComboBox.qml
@@ -105,7 +158,6 @@
src/QmlControls/QGCLabel.qml
src/QmlControls/QGCListView.qml
src/QmlControls/QGCMapLabel.qml
- src/MissionManager/QGCMapPolygonVisuals.qml
src/QmlControls/QGCMouseArea.qml
src/QmlControls/QGCMovableItem.qml
src/QmlControls/QGCPipable.qml
@@ -120,142 +172,83 @@
src/QmlControls/QGCViewMessage.qml
src/QmlControls/QGCViewPanel.qml
src/QmlControls/QGroundControl.Controls.qmldir
- src/PlanView/RallyPointEditorHeader.qml
- src/PlanView/RallyPointItemEditor.qml
- src/PlanView/RallyPointMapVisuals.qml
+ src/QmlControls/QGroundControl.ScreenTools.qmldir
+ src/QmlControls/QmlTest.qml
src/QmlControls/RCChannelMonitor.qml
src/QmlControls/RoundButton.qml
+ src/QmlControls/ScreenTools.qml
src/QmlControls/SectionHeader.qml
- src/AutoPilotPlugins/Common/SetupPage.qml
- src/ui/toolbar/SignalStrength.qml
- src/PlanView/SimpleItemMapVisual.qml
src/QmlControls/SliderSwitch.qml
src/QmlControls/SubMenuButton.qml
- src/PlanView/SurveyMapVisual.qml
- src/PlanView/TransectStyleComplexItemStats.qml
src/QmlControls/ToolStrip.qml
src/QmlControls/VehicleRotationCal.qml
src/QmlControls/VehicleSummaryRow.qml
- src/ViewWidgets/ViewWidget.qml
- src/FactSystem/FactControls/AltitudeFactTextField.qml
- src/FactSystem/FactControls/FactBitmask.qml
- src/FactSystem/FactControls/FactCheckBox.qml
- src/FactSystem/FactControls/FactComboBox.qml
- src/FactSystem/FactControls/FactLabel.qml
- src/FactSystem/FactControls/FactPanel.qml
- src/FactSystem/FactControls/FactTextField.qml
- src/FactSystem/FactControls/FactTextFieldGrid.qml
- src/FactSystem/FactControls/FactTextFieldRow.qml
- src/FactSystem/FactControls/FactValueSlider.qml
- src/FactSystem/FactControls/FactTextFieldSlider.qml
- src/FactSystem/FactControls/qmldir
- src/FlightDisplay/FlightDisplayView.qml
- src/FlightDisplay/FlightDisplayViewMap.qml
- src/FlightDisplay/FlightDisplayViewVideo.qml
- src/FlightDisplay/FlightDisplayViewWidgets.qml
- src/FlightDisplay/GuidedActionConfirm.qml
- src/FlightDisplay/GuidedActionList.qml
- src/FlightDisplay/GuidedActionsController.qml
- src/FlightDisplay/GuidedAltitudeSlider.qml
- src/FlightDisplay/MultiVehicleList.qml
- src/FlightDisplay/PreFlightBatteryCheck.qml
- src/FlightDisplay/BuiltInPreFlightCheckModel.qml
- src/FlightDisplay/PreFlightGPSCheck.qml
- src/FlightDisplay/PreFlightRCCheck.qml
- src/FlightDisplay/PreFlightSensorsHealthCheck.qml
- src/FlightDisplay/PreFlightSoundCheck.qml
- src/FlightDisplay/qmldir
- src/FlightMap/MapItems/CameraTriggerIndicator.qml
- src/FlightMap/Widgets/CenterMapDropButton.qml
- src/FlightMap/Widgets/CenterMapDropPanel.qml
- src/FlightMap/Widgets/CompassRing.qml
- src/FlightMap/MapItems/CustomMapItems.qml
- src/FlightMap/FlightMap.qml
- src/FlightMap/Widgets/InstrumentSwipeView.qml
- src/FlightMap/Widgets/MapFitFunctions.qml
- src/FlightMap/MapScale.qml
- src/FlightMap/MapItems/MissionItemIndicator.qml
- src/FlightMap/MapItems/MissionItemIndicatorDrag.qml
- src/FlightMap/MapItems/MissionItemView.qml
- src/FlightMap/MapItems/MissionLineView.qml
- src/FlightMap/MapItems/PlanMapItems.qml
- src/FlightMap/MapItems/PolygonEditor.qml
- src/FlightMap/Widgets/QGCArtificialHorizon.qml
- src/FlightMap/Widgets/QGCAttitudeHUD.qml
- src/FlightMap/Widgets/QGCAttitudeWidget.qml
- src/FlightMap/Widgets/QGCCompassWidget.qml
- src/FlightMap/Widgets/QGCPitchIndicator.qml
- src/FlightMap/QGCVideoBackground.qml
- src/FlightMap/qmldir
- src/FlightMap/MapItems/VehicleMapItem.qml
- src/QmlControls/QGroundControl.ScreenTools.qmldir
- src/QmlControls/ScreenTools.qml
- src/QmlControls/QmlTest.qml
- src/AutoPilotPlugins/Common/RadioComponent.qml
- src/ui/preferences/SerialSettings.qml
+ src/QtLocationPlugin/QMLControl/OfflineMap.qml
+ src/Taisync/TaisyncSettings.qml
+ src/VehicleSetup/FirmwareUpgrade.qml
+ src/VehicleSetup/JoystickConfig.qml
+ src/VehicleSetup/PX4FlowSensor.qml
src/VehicleSetup/SetupParameterEditor.qml
src/VehicleSetup/SetupView.qml
- src/PlanView/SimpleItemEditor.qml
- src/PlanView/StructureScanEditor.qml
- src/PlanView/SurveyItemEditor.qml
- src/AutoPilotPlugins/Common/SyslinkComponent.qml
- src/ui/preferences/TcpSettings.qml
- src/Taisync/TaisyncSettings.qml
- src/test.qml
- src/ui/preferences/UdpSettings.qml
- src/FlightMap/Widgets/ValuePageWidget.qml
src/VehicleSetup/VehicleSummary.qml
- src/FlightMap/Widgets/VibrationPageWidget.qml
- src/FlightMap/Widgets/VideoPageWidget.qml
- src/FlightDisplay/VirtualJoystick.qml
- src/WimaView/WimaToolBar.qml
- src/WimaView/WimaView.qml
- src/WimaView/WimaMapVisual.qml
- src/WimaView/WimaItemEditor.qml
- src/WimaView/WimaMapPolylineVisuals.qml
- src/WimaView/WimaMapPolygonVisuals.qml
- src/WimaView/WimaServiceAreaMapVisual.qml
- src/WimaView/WimaServiceAreaEditor.qml
- src/WimaView/WimaAreaMapVisual.qml
- src/WimaView/WimaMeasurementAreaMapVisual.qml
- src/WimaView/WimaCorridorMapVisual.qml
- src/WimaView/WimaMeasurementAreaEditor.qml
- src/PlanView/CircularSurveyItemEditor.qml
- src/WimaView/DragCoordinate.qml
- src/WimaView/CoordinateIndicatorDrag.qml
- src/WimaView/CoordinateIndicator.qml
- src/WimaView/WimaJoinedAreaMapVisual.qml
- src/WimaView/WimaCorridorEditor.qml
- src/FlightMap/MapItems/WimaPlanMapItems.qml
- src/PlanView/WimaMissionItemMapVisual.qml
- src/FlightDisplay/FlightDisplayWimaMenu.qml
+ src/ViewWidgets/CustomCommandWidget.qml
+ src/ViewWidgets/ViewWidget.qml
+ src/Wima/Snake/WimaAreaNoVisual.qml
+ src/WimaView/CircularGeneratorEditor.qml
+ src/WimaView/CircularGeneratorMapVisual.qml
+ src/WimaView/CircularSurveyItemEditor.qml
src/WimaView/CircularSurveyMapVisual.qml
- src/FlightDisplay/SmallValue.qml
- src/WimaView/ProgressIndicator.qml
- src/WimaView/WimaServiceAreaDataVisual.qml
- src/WimaView/WimaCorridorDataVisual.qml
- src/WimaView/WimaJoinedAreaDataVisual.qml
- src/Wima/Snake/WimaAreaNoVisual.qml
- src/WimaView/WimaMeasurementAreaDataVisual.qml
- src/QmlControls/QGCButtonColumn.qml
+ src/WimaView/CoordinateIndicator.qml
+ src/WimaView/CoordinateIndicatorDrag.qml
+ src/WimaView/DragCoordinate.qml
+ src/WimaView/LinearGeneratorEditor.qml
+ src/WimaView/ProgressIndicator.qml
+ src/WimaView/Wima.qmldir
+ src/WimaView/WimaAreaMapVisual.qml
+ src/WimaView/WimaCorridorDataVisual.qml
+ src/WimaView/WimaCorridorEditor.qml
+ src/WimaView/WimaCorridorMapVisual.qml
+ src/WimaView/WimaItemEditor.qml
+ src/WimaView/WimaJoinedAreaDataVisual.qml
+ src/WimaView/WimaJoinedAreaMapVisual.qml
+ src/WimaView/WimaMapPolygonVisuals.qml
+ src/WimaView/WimaMapPolylineVisuals.qml
+ src/WimaView/WimaMapVisual.qml
+ src/WimaView/WimaMeasurementAreaDataVisual.qml
+ src/WimaView/WimaMeasurementAreaEditor.qml
+ src/WimaView/WimaMeasurementAreaMapVisual.qml
+ src/WimaView/WimaServiceAreaDataVisual.qml
+ src/WimaView/WimaServiceAreaEditor.qml
+ src/WimaView/WimaServiceAreaMapVisual.qml
+ src/WimaView/WimaToolBar.qml
+ src/WimaView/WimaView.qml
+ src/test.qml
+ src/ui/AppSettings.qml
+ src/ui/MainWindowHybrid.qml
+ src/ui/MainWindowInner.qml
+ src/ui/MainWindowNative.qml
+ src/ui/preferences/BluetoothSettings.qml
+ src/ui/preferences/DebugWindow.qml
+ src/ui/preferences/GeneralSettings.qml
+ src/ui/preferences/HelpSettings.qml
+ src/ui/preferences/LinkSettings.qml
+ src/ui/preferences/LogReplaySettings.qml
+ src/ui/preferences/MavlinkSettings.qml
+ src/ui/preferences/MockLink.qml
+ src/ui/preferences/MockLinkSettings.qml
+ src/ui/preferences/SerialSettings.qml
+ src/ui/preferences/TcpSettings.qml
+ src/ui/preferences/UdpSettings.qml
+ src/ui/toolbar/MainToolBar.qml
+ src/ui/toolbar/MainToolBarIndicators.qml
+ src/ui/toolbar/SignalStrength.qml
- src/Settings/APMMavlinkStreamRate.SettingsGroup.json
src/MissionManager/CameraCalc.FactMetaData.json
+ src/MissionManager/CameraSection.FactMetaData.json
src/MissionManager/CameraSpec.FactMetaData.json
src/MissionManager/CorridorScan.SettingsGroup.json
- src/QmlControls/EditPositionDialog.FactMetaData.json
- src/MissionManager/QGCMapCircle.Facts.json
- src/MissionManager/StructureScan.SettingsGroup.json
- src/MissionManager/TransectStyle.SettingsGroup.json
- src/Settings/App.SettingsGroup.json
- src/Settings/AutoConnect.SettingsGroup.json
- src/Settings/BrandImage.SettingsGroup.json
- src/MissionManager/CameraSection.FactMetaData.json
- src/Settings/FlightMap.SettingsGroup.json
src/MissionManager/FWLandingPattern.FactMetaData.json
- src/Settings/FlyView.SettingsGroup.json
- src/Settings/PlanView.SettingsGroup.json
src/MissionManager/MavCmdInfoCommon.json
src/MissionManager/MavCmdInfoFixedWing.json
src/MissionManager/MavCmdInfoMultiRotor.json
@@ -263,12 +256,24 @@
src/MissionManager/MavCmdInfoSub.json
src/MissionManager/MavCmdInfoVTOL.json
src/MissionManager/MissionSettings.FactMetaData.json
+ src/MissionManager/QGCMapCircle.Facts.json
src/MissionManager/RallyPoint.FactMetaData.json
- src/Settings/RTK.SettingsGroup.json
src/MissionManager/SpeedSection.FactMetaData.json
+ src/MissionManager/StructureScan.SettingsGroup.json
src/MissionManager/Survey.SettingsGroup.json
+ src/MissionManager/TransectStyle.SettingsGroup.json
+ src/QmlControls/EditPositionDialog.FactMetaData.json
+ src/Settings/APMMavlinkStreamRate.SettingsGroup.json
+ src/Settings/App.SettingsGroup.json
+ src/Settings/AutoConnect.SettingsGroup.json
+ src/Settings/BrandImage.SettingsGroup.json
+ src/Settings/FlightMap.SettingsGroup.json
+ src/Settings/FlyView.SettingsGroup.json
+ src/Settings/PlanView.SettingsGroup.json
+ src/Settings/RTK.SettingsGroup.json
src/Settings/Units.SettingsGroup.json
- src/comm/USBBoardInfo.json
+ src/Settings/Video.SettingsGroup.json
+ src/Settings/Wima.SettingsGroup.json
src/Vehicle/BatteryFact.json
src/Vehicle/ClockFact.json
src/Vehicle/DistanceSensorFact.json
@@ -281,12 +286,13 @@
src/Vehicle/VehicleFact.json
src/Vehicle/VibrationFact.json
src/Vehicle/WindFact.json
- src/Settings/Video.SettingsGroup.json
+ src/Wima/Geometry/json/WimaArea.SettingsGroup.json
src/Wima/Geometry/json/WimaMeasurementArea.SettingsGroup.json
+ src/Wima/Snake/json/CircularGenerator.SettingsGroup.json
+ src/Wima/Snake/json/LinearGenerator.SettingsGroup.json
src/Wima/json/CircularSurvey.SettingsGroup.json
- src/Wima/Geometry/json/WimaArea.SettingsGroup.json
src/Wima/json/WimaController.SettingsGroup.json
- src/Settings/Wima.SettingsGroup.json
+ src/comm/USBBoardInfo.json
src/comm/APMArduCopterMockLink.params
diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml
index b3faab35d9543d45d74fa92b3f4e4738a1f5c73b..12ce0a1e7709ab015dc7f03af4bba9d1517482b3 100644
--- a/src/FlightDisplay/FlightDisplayViewMap.qml
+++ b/src/FlightDisplay/FlightDisplayViewMap.qml
@@ -24,6 +24,8 @@ import QGroundControl.Palette 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Vehicle 1.0
+import Wima 1.0
+
FlightMap {
id: flightMap
anchors.fill: parent
diff --git a/src/FlightMap/MapItems/MissionLineView.qml b/src/FlightMap/MapItems/MissionLineView.qml
index 39382391f3f2d2c5569351083fa3086716f21f93..3d0adac28314740044b34457383cb52d084778af 100644
--- a/src/FlightMap/MapItems/MissionLineView.qml
+++ b/src/FlightMap/MapItems/MissionLineView.qml
@@ -30,5 +30,9 @@ MapItemView {
object.coordinate1,
object.coordinate2,
] : []
+
+// onParentChanged: {
+// console.log("MapItemView, path:" + path)
+// }
}
}
diff --git a/src/PlanView/MissionItemMapVisual.qml b/src/PlanView/MissionItemMapVisual.qml
index 96d07ff67e432bbb20d331ed5c32b23c03dcc1c1..49eb8c601e3544121ff30a36fdffcd08c3c2e228 100644
--- a/src/PlanView/MissionItemMapVisual.qml
+++ b/src/PlanView/MissionItemMapVisual.qml
@@ -15,7 +15,7 @@ import QtPositioning 5.3
import QGroundControl.ScreenTools 1.0
import QGroundControl.Palette 1.0
import QGroundControl.Controls 1.0
-
+import Wima 1.0
/// Mission item map visual
Item {
diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc
index b6adf4980c33914108681a933f59a1a8adcdbf55..9241b9b6212a5becb7597d48fb1d0682f1b491ce 100644
--- a/src/QGCApplication.cc
+++ b/src/QGCApplication.cc
@@ -89,6 +89,8 @@
#include "VideoSurface.h"
#include "ViewWidgetController.h"
#include "VisualMissionItem.h"
+#include "Wima/Snake/CircularGenerator.h"
+#include "Wima/Snake/LinearGenerator.h"
#include "Wima/Snake/NemoInterface.h"
#include "Wima/WimaController.h"
#include "Wima/WimaPlaner.h"
@@ -520,6 +522,10 @@ void QGCApplication::_initCommon(void) {
qmlRegisterType("Wima", 1, 0, "WimaController");
qmlRegisterType("Wima", 1, 0, "WimaPlaner");
qmlRegisterType("Wima", 1, 0, "NemoInterface");
+ qmlRegisterInterface("GeneratorBase");
+ qmlRegisterType("Wima", 1, 0,
+ "CircularGenerator");
+ qmlRegisterType("Wima", 1, 0, "LinearGenerator");
// Register Qml Singletons
qmlRegisterSingletonType(
diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir
index 83953b980534590a92c5a85957d9716638473ac3..14d85db16be3fc6a4268129ef7dc05e3ca954f5a 100644
--- a/src/QmlControls/QGroundControl.Controls.qmldir
+++ b/src/QmlControls/QGroundControl.Controls.qmldir
@@ -65,7 +65,6 @@ QGCViewDialog 1.0 QGCViewDialog.qml
QGCViewDialogContainer 1.0 QGCViewDialogContainer.qml
QGCViewMessage 1.0 QGCViewMessage.qml
QGCViewPanel 1.0 QGCViewPanel.qml
-WimaToolBar 1.0 WimaToolBar.qml
RallyPointEditorHeader 1.0 RallyPointEditorHeader.qml
RallyPointItemEditor 1.0 RallyPointItemEditor.qml
RallyPointMapVisuals 1.0 RallyPointMapVisuals.qml
@@ -85,22 +84,6 @@ VehicleSummaryRow 1.0 VehicleSummaryRow.qml
ViewWidget 1.0 ViewWidget.qml
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
-WimaServiceAreaEditor 1.0 WimaServiceAreaEditor.qml
-WimaCorridorMapVisual 1.0 WimaCorridorMapVisual.qml
-WimaItemEditor 1.0 WimaItemEditor.qml
-WimaMapPolygonVisuals 1.0 WimaMapPolygonVisuals.qml
-WimaMapPolylineVisuals 1.0 WimaMapPolylineVisuals.qml
-CircularSurveyMapVisual 1.0 CircularSurveyMapVisual.qml
-DragCoordinate 1.0 DragCoordinate.qml
-CoordinateIndicator 1.0 CoordinateIndicator.qml
-CoordinateIndicatorDrag 1.0 CoordinateIndicatorDrag.qml
-ProgressIndicator 1.0 ProgressIndicator.qml
QGCButtonColumn 1.0 QGCButtonColumn.qml
diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc
index 460abb69e707bbe3b8dc8bf4f5dd909e20f3839a..19182eb41a9947d5b3bcfc6331822c18eeb4d87e 100644
--- a/src/Wima/CircularSurvey.cc
+++ b/src/Wima/CircularSurvey.cc
@@ -16,92 +16,36 @@
#include
#include
-QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog")
-
-using namespace ClipperLib;
-template <> auto get<0>(const IntPoint &p) { return p.X; }
-template <> auto get<1>(const IntPoint &p) { return p.Y; }
-
-template class CommandRAII {
-public:
- CommandRAII(Functor f) : fun(f) {}
- ~CommandRAII() { fun(); }
+#include "CircularGenerator.h"
+#include "LinearGenerator.h"
-private:
- Functor fun;
-};
+QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog")
template
constexpr typename std::underlying_type::type integral(T value) {
return static_cast::type>(value);
}
-bool circularTransects(const snake::FPolygon &polygon,
- const std::vector &tiles,
- snake::Length deltaR, snake::Angle deltaAlpha,
- snake::Length minLength, snake::Transects &transects);
-
-bool linearTransects(const snake::FPolygon &polygon,
- const std::vector &tiles,
- snake::Length distance, snake::Angle angle,
- snake::Length minLength, snake::Transects &transects);
-
const char *CircularSurvey::settingsGroup = "CircularSurvey";
-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";
-const char *CircularSurvey::refPointAltitudeName = "ReferencePointAlt";
const char *CircularSurvey::variantName = "Variant";
-const char *CircularSurvey::numRunsName = "NumRuns";
-const char *CircularSurvey::runName = "Run";
CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
const QString &kmlOrShpFile, QObject *parent)
: TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent),
- _referencePoint(QGeoCoordinate(0, 0, 0)),
+ _state(STATE::IDLE),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)),
- _transectDistance(settingsGroup, _metaDataMap[transectDistanceName]),
- _alpha(settingsGroup, _metaDataMap[alphaName]),
- _minLength(settingsGroup, _metaDataMap[minLengthName]),
- _type(settingsGroup, _metaDataMap[typeName]),
_variant(settingsGroup, _metaDataMap[variantName]),
- _numRuns(settingsGroup, _metaDataMap[numRunsName]),
- _run(settingsGroup, _metaDataMap[runName]),
- _pWorker(std::make_unique()), _state(STATE::DEFAULT),
- _hidePolygon(false) {
+ _pAreaData(std::make_shared()),
+ _pWorker(std::make_unique()) {
+
Q_UNUSED(kmlOrShpFile)
_editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";
// Connect facts.
- connect(&_transectDistance, &Fact::valueChanged, this,
- &CircularSurvey::_rebuildTransects);
- connect(&_alpha, &Fact::valueChanged, this,
- &CircularSurvey::_rebuildTransects);
- connect(&_minLength, &Fact::valueChanged, this,
- &CircularSurvey::_rebuildTransects);
- connect(this, &CircularSurvey::refPointChanged, this,
- &CircularSurvey::_rebuildTransects);
- connect(this, &CircularSurvey::depotChanged, this,
- &CircularSurvey::_rebuildTransects);
- connect(&this->_type, &Fact::rawValueChanged, this,
- &CircularSurvey::_rebuildTransects);
connect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
- connect(&this->_run, &Fact::rawValueChanged, this,
- &CircularSurvey::_changeRun);
- connect(&this->_numRuns, &Fact::rawValueChanged, this,
- &CircularSurvey::_rebuildTransects);
-
- // Areas.
- connect(this, &CircularSurvey::measurementAreaChanged, this,
- &CircularSurvey::_rebuildTransects);
- connect(this, &CircularSurvey::joinedAreaChanged, this,
- &CircularSurvey::_rebuildTransects);
// Connect worker.
connect(this->_pWorker.get(), &RoutingThread::result, this,
@@ -115,87 +59,32 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
&CircularSurvey::coordinateHasRelativeAltitudeChanged);
connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this,
&CircularSurvey::exitCoordinateHasRelativeAltitudeChanged);
+
+ // Register Generators.
+ auto lg = std::make_shared(this->_pAreaData);
+ registerGenerator(lg->name(), lg);
+ auto cg = std::make_shared(this->_pAreaData);
+ registerGenerator(cg->name(), cg);
}
CircularSurvey::~CircularSurvey() {}
-void CircularSurvey::resetReference() { setRefPoint(_mArea.center()); }
-
void CircularSurvey::reverse() {
this->_state = STATE::REVERSE;
this->_rebuildTransects();
}
-void CircularSurvey::setRefPoint(const QGeoCoordinate &refPt) {
- if (refPt != _referencePoint) {
- _referencePoint = refPt;
- _referencePoint.setAltitude(0);
-
- emit refPointChanged();
- }
+void CircularSurvey::setPlanData(const WimaPlanData &d) {
+ *this->_pAreaData = d;
}
-QGeoCoordinate CircularSurvey::refPoint() const { return _referencePoint; }
-
-Fact *CircularSurvey::transectDistance() { return &_transectDistance; }
-
-Fact *CircularSurvey::alpha() { return &_alpha; }
-
-bool CircularSurvey::hidePolygon() const { return _hidePolygon; }
-
-QList CircularSurvey::variantNames() const { return _variantNames; }
-
-QList CircularSurvey::runNames() const { return _runNames; }
-
-QGeoCoordinate CircularSurvey::depot() const { return this->_depot; }
-
-const QList> &CircularSurvey::rawTransects() const {
- return this->_rawTransects;
+const WimaPlanData &CircularSurvey::planData() const {
+ return *this->_pAreaData;
}
-void CircularSurvey::setHidePolygon(bool hide) {
- if (this->_hidePolygon != hide) {
- this->_hidePolygon = hide;
- emit hidePolygonChanged();
- }
-}
-
-void CircularSurvey::setMeasurementArea(const WimaMeasurementAreaData &mArea) {
- if (this->_mArea != mArea) {
- this->_mArea = mArea;
- emit measurementAreaChanged();
- }
-}
-
-void CircularSurvey::setJoinedArea(const WimaJoinedAreaData &jArea) {
- if (this->_jArea != jArea) {
- this->_jArea = jArea;
- emit joinedAreaChanged();
- }
-}
+WimaPlanData &CircularSurvey::planData() { return *this->_pAreaData; }
-void CircularSurvey::setMeasurementArea(const WimaMeasurementArea &mArea) {
- if (this->_mArea != mArea) {
- this->_mArea = mArea;
- emit measurementAreaChanged();
- }
-}
-
-void CircularSurvey::setJoinedArea(const WimaJoinedArea &jArea) {
- if (this->_jArea != jArea) {
- this->_jArea = jArea;
- emit joinedAreaChanged();
- }
-}
-
-void CircularSurvey::setDepot(const QGeoCoordinate &depot) {
- if (this->_depot.latitude() != depot.latitude() ||
- this->_depot.longitude() != depot.longitude()) {
- this->_depot = depot;
- this->_depot.setAltitude(0);
- emit depotChanged();
- }
-}
+QStringList CircularSurvey::variantNames() const { return _variantNames; }
bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
QString &errorString) {
@@ -218,16 +107,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
QList keyInfoList = {
{VisualMissionItem::jsonTypeKey, QJsonValue::String, true},
{ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true},
- {transectDistanceName, QJsonValue::Double, true},
- {alphaName, QJsonValue::Double, true},
- {minLengthName, QJsonValue::Double, true},
- {typeName, QJsonValue::Double, true},
{variantName, QJsonValue::Double, false},
- {numRunsName, QJsonValue::Double, false},
- {runName, QJsonValue::Double, false},
- {refPointLatitudeName, QJsonValue::Double, true},
- {refPointLongitudeName, QJsonValue::Double, true},
- {refPointAltitudeName, QJsonValue::Double, true},
};
if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
@@ -262,16 +142,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber,
return false;
}
- _transectDistance.setRawValue(complexObject[transectDistanceName].toDouble());
- _alpha.setRawValue(complexObject[alphaName].toDouble());
- _minLength.setRawValue(complexObject[minLengthName].toDouble());
- _type.setRawValue(complexObject[typeName].toInt());
_variant.setRawValue(complexObject[variantName].toInt());
- _numRuns.setRawValue(complexObject[numRunsName].toInt());
- _run.setRawValue(complexObject[runName].toInt());
- _referencePoint.setLongitude(complexObject[refPointLongitudeName].toDouble());
- _referencePoint.setLatitude(complexObject[refPointLatitudeName].toDouble());
- _referencePoint.setAltitude(complexObject[refPointAltitudeName].toDouble());
_ignoreRecalc = false;
@@ -298,16 +169,7 @@ void CircularSurvey::save(QJsonArray &planItems) {
VisualMissionItem::jsonTypeComplexItemValue;
saveObject[ComplexMissionItem::jsonComplexItemTypeKey] = CircularSurveyName;
- saveObject[transectDistanceName] = _transectDistance.rawValue().toDouble();
- saveObject[alphaName] = _alpha.rawValue().toDouble();
- saveObject[minLengthName] = _minLength.rawValue().toDouble();
- saveObject[typeName] = double(_type.rawValue().toUInt());
saveObject[variantName] = double(_variant.rawValue().toUInt());
- saveObject[numRunsName] = double(_numRuns.rawValue().toUInt());
- saveObject[runName] = double(_numRuns.rawValue().toUInt());
- saveObject[refPointLongitudeName] = _referencePoint.longitude();
- saveObject[refPointLatitudeName] = _referencePoint.latitude();
- saveObject[refPointAltitudeName] = _referencePoint.altitude();
// Polygon shape
_surveyAreaPolygon.saveToJson(saveObject);
@@ -379,13 +241,31 @@ void CircularSurvey::_buildAndAppendMissionItems(QList &items,
}
}
-void CircularSurvey::_changeVariant() {
- this->_state = STATE::VARIANT_CHANGE;
- this->_rebuildTransects();
+bool CircularSurvey::_switchToGenerator(
+ const CircularSurvey::PtrGenerator &newG) {
+ if (this->_pGenerator != newG) {
+ if (this->_pGenerator != nullptr) {
+ disconnect(this->_pGenerator.get(),
+ &routing::GeneratorBase::generatorChanged, this,
+ &CircularSurvey::_rebuildTransects);
+ }
+
+ this->_pGenerator = newG;
+ connect(this->_pGenerator.get(), &routing::GeneratorBase::generatorChanged,
+ this, &CircularSurvey::_rebuildTransects);
+ emit generatorChanged();
+
+ this->_state = STATE::IDLE;
+ _rebuildTransects();
+
+ return true;
+ } else {
+ return false;
+ }
}
-void CircularSurvey::_changeRun() {
- this->_state = STATE::RUN_CHANGE;
+void CircularSurvey::_changeVariant() {
+ this->_state = STATE::VARIANT_CHANGE;
this->_rebuildTransects();
}
@@ -394,204 +274,105 @@ void CircularSurvey::_updateWorker() {
this->_transectsDirty = true;
// Reset data.
this->_transects.clear();
- this->_rawTransects.clear();
this->_variantVector.clear();
this->_variantNames.clear();
- this->_runNames.clear();
emit variantNamesChanged();
- emit runNamesChanged();
- // Prepare data.
- auto ref = this->_referencePoint;
- auto geoPolygon = this->_mArea.coordinateList();
- for (auto &v : geoPolygon) {
- v.setAltitude(0);
- }
- auto pPolygon = std::make_shared();
- snake::areaToEnu(ref, geoPolygon, *pPolygon);
-
- // Progress and tiles.
- const auto &progress = this->_mArea.progress();
- const auto *tiles = this->_mArea.tiles();
- auto pTiles = std::make_shared>();
- if (progress.size() == tiles->count()) {
- for (int i = 0; i < tiles->count(); ++i) {
- if (progress[i] == 100) {
- const auto *tile = tiles->value(i);
- if (tile != nullptr) {
- snake::FPolygon tileENU;
- snake::areaToEnu(ref, tile->coordinateList(), tileENU);
- pTiles->push_back(std::move(tileENU));
- } else {
- qCWarning(CircularSurveyLog)
- << "_updateWorker(): progress.size() != tiles->count().";
- return;
- }
- }
- }
- } else {
- qCWarning(CircularSurveyLog)
- << "_updateWorker(): progress.size() != tiles->count().";
- return;
- }
+ if (this->_pAreaData->isValid()) {
- // Convert safe area.
- auto geoDepot = this->_depot;
- auto geoSafeArea = this->_jArea.coordinateList();
- if (!geoDepot.isValid()) {
- qCWarning(CircularSurveyLog)
- << "_updateWorker(): depot invalid." << geoDepot;
- return;
- }
- if (!(geoSafeArea.size() >= 3)) {
- qCWarning(CircularSurveyLog)
- << "_updateWorker(): safe area invalid." << geoSafeArea;
- return;
- }
- for (auto &v : geoSafeArea) {
- v.setAltitude(0);
- }
- snake::FPoint depot;
- snake::toENU(ref, geoDepot, depot);
-
- // Routing par.
- RoutingParameter par;
- par.numSolutionsPerRun = 5;
- if (this->_numRuns.rawValue().toUInt() < 1) {
- disconnect(&this->_numRuns, &Fact::rawValueChanged, this,
- &CircularSurvey::_rebuildTransects);
+ // Prepare data.
+ auto origin = this->_pAreaData->origin();
+ origin.setAltitude(0);
+ if (!origin.isValid()) {
+ qCDebug(CircularSurveyLog)
+ << "_updateWorker(): origin invalid." << origin;
+ return;
+ }
- this->_numRuns.setCookedValue(QVariant(1));
+ // Convert safe area.
+ auto geoSafeArea = this->_pAreaData->joinedArea().coordinateList();
+ if (!(geoSafeArea.size() >= 3)) {
+ qCDebug(CircularSurveyLog)
+ << "_updateWorker(): safe area invalid." << geoSafeArea;
+ return;
+ }
+ for (auto &v : geoSafeArea) {
+ if (v.isValid()) {
+ v.setAltitude(0);
+ } else {
+ qCDebug(CircularSurveyLog)
+ << "_updateWorker(): safe area contains invalid coordinate."
+ << geoSafeArea;
+ return;
+ }
+ }
- connect(&this->_numRuns, &Fact::rawValueChanged, this,
- &CircularSurvey::_rebuildTransects);
- }
- par.numRuns = this->_numRuns.rawValue().toUInt();
-
- auto &safeAreaENU = par.safeArea;
- snake::areaToEnu(ref, geoSafeArea, safeAreaENU);
-
- // Fetch transect parameter.
- auto distance = snake::Length(this->_transectDistance.rawValue().toDouble() *
- bu::si::meter);
- auto minLength =
- snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
- auto alpha =
- snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);
-
- // Select survey type.
- if (this->_type.rawValue().toUInt() == integral(Type::Circular)) {
- // Clip angle.
- if (alpha >= snake::Angle(0.3 * bu::degree::degree) &&
- alpha <= snake::Angle(45 * bu::degree::degree)) {
- auto generator = [depot, pPolygon, pTiles, distance, alpha,
- minLength](snake::Transects &transects) -> bool {
- bool value = circularTransects(*pPolygon, *pTiles, distance, alpha,
- minLength, transects);
- transects.insert(transects.begin(), snake::FLineString{depot});
- return value;
- };
- // Start routing worker.
- this->_pWorker->route(par, generator);
- } else {
- if (alpha < snake::Angle(0.3 * bu::degree::degree)) {
- this->_alpha.setCookedValue(QVariant(0.3));
+ // Routing par.
+ RoutingParameter par;
+ par.numSolutions = 5;
+ auto &safeAreaENU = par.safeArea;
+ snake::areaToEnu(origin, geoSafeArea, safeAreaENU);
+
+ // Create generator.
+ if (this->_pGenerator) {
+ routing::GeneratorBase::Generator g; // Transect generator.
+ if (this->_pGenerator->get(g)) {
+ // Start/Restart routing worker.
+ this->_pWorker->route(par, g);
} else {
- this->_alpha.setCookedValue(QVariant(45));
+ qCDebug(CircularSurveyLog)
+ << "_updateWorker(): generator creation failed.";
}
+ } else {
+ qCDebug(CircularSurveyLog)
+ << "_updateWorker(): pGenerator == nullptr, number of registered "
+ "generators: "
+ << this->_generatorList.size();
}
- } else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) {
- auto generator = [depot, pPolygon, pTiles, distance, alpha,
- minLength](snake::Transects &transects) -> bool {
- bool value = linearTransects(*pPolygon, *pTiles, distance, alpha,
- minLength, transects);
- transects.insert(transects.begin(), snake::FLineString{depot});
- return value;
- };
- // Start routing worker.
- this->_pWorker->route(par, generator);
} else {
- qCWarning(CircularSurveyLog)
- << "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:"
- << this->_type.rawValue().toUInt();
+ qCDebug(CircularSurveyLog) << "_updateWorker(): plan data invalid.";
}
}
-void CircularSurvey::_changeVariantRunWorker() {
+void CircularSurvey::_changeVariantWorker() {
auto variant = this->_variant.rawValue().toUInt();
- auto run = this->_run.rawValue().toUInt();
// Find old variant and run. Old run corresponts with empty list.
std::size_t old_variant = std::numeric_limits::max();
- std::size_t old_run = std::numeric_limits::max();
for (std::size_t i = 0; i < std::size_t(this->_variantVector.size()); ++i) {
- const auto &solution = this->_variantVector.at(i);
- for (std::size_t j = 0; j < std::size_t(solution.size()); ++j) {
- const auto &r = solution[j];
- if (r.isEmpty()) {
- old_variant = i;
- old_run = j;
- // break
- i = std::numeric_limits::max() - 1;
- j = std::numeric_limits::max() - 1;
- }
+ const auto &variantCoordinates = this->_variantVector.at(i);
+ if (variantCoordinates.isEmpty()) {
+ old_variant = i;
+ break;
}
}
// Swap route.
- if (variant != old_variant || run != old_run) {
- // Swap in new variant, if condition.
- if (variant < std::size_t(this->_variantVector.size()) &&
- run < std::size_t(this->_variantVector.at(variant).size())) {
+ if (variant != old_variant) {
+ // Swap in new variant.
+ if (variant < std::size_t(this->_variantVector.size())) {
if (old_variant != std::numeric_limits::max()) {
// this->_transects containes a route, swap it back to
// this->_solutionVector
- auto &old_solution = this->_variantVector[old_variant];
- auto &old_route = old_solution[old_run];
- old_route.swap(this->_transects);
- }
- auto &solution = this->_variantVector[variant];
- auto &route = solution[run];
- this->_transects.swap(route);
-
- if (variant != old_variant) {
- // Add run names.
- this->_runNames.clear();
- for (std::size_t i = 1; i <= std::size_t(solution.size()); ++i) {
- this->_runNames.append(QString::number(i));
- }
- emit runNamesChanged();
+ auto &oldVariantCoordinates = this->_variantVector[old_variant];
+ oldVariantCoordinates.swap(this->_transects);
}
+ auto &newVariantCoordinates = this->_variantVector[variant];
+ this->_transects.swap(newVariantCoordinates);
} else { // error
- qCWarning(CircularSurveyLog)
- << "Variant or run out of bounds (variant = " << variant
- << ", run = " << run << ").";
- qCWarning(CircularSurveyLog) << "Resetting variant and run.";
+ qCDebug(CircularSurveyLog)
+ << "Variant out of bounds (variant =" << variant << ").";
+ qCDebug(CircularSurveyLog) << "Resetting variant to zero.";
disconnect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
- disconnect(&this->_run, &Fact::rawValueChanged, this,
- &CircularSurvey::_changeRun);
- if (old_variant < std::size_t(this->_variantVector.size())) {
- this->_variant.setCookedValue(QVariant::fromValue(old_variant));
- auto &solution = this->_variantVector[old_variant];
- if (old_run < std::size_t(solution.size())) {
- this->_run.setCookedValue(QVariant::fromValue(old_run));
- } else {
- this->_run.setCookedValue(QVariant(0));
- }
- } else {
- this->_variant.setCookedValue(QVariant(0));
- this->_run.setCookedValue(QVariant(0));
- }
+ this->_variant.setCookedValue(QVariant(0));
connect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
- connect(&this->_run, &Fact::rawValueChanged, this,
- &CircularSurvey::_changeRun);
- if (this->_variantVector.size() > 0 &&
- this->_variantVector.front().size() > 0) {
- this->_changeVariantRunWorker();
+
+ if (this->_variantVector.size() > 0) {
+ this->_changeVariantWorker();
}
}
}
@@ -604,41 +385,195 @@ void CircularSurvey::_reverseWorker() {
}
}
-void CircularSurvey::_storeWorker() {
- // If the transects are getting rebuilt then any previously loaded
- // mission items are now invalid.
- if (_loadedMissionItemsParent) {
- _loadedMissionItems.clear();
- _loadedMissionItemsParent->deleteLater();
- _loadedMissionItemsParent = nullptr;
+void CircularSurvey::applyNewAltitude(double newAltitude) {
+ _cameraCalc.valueSetIsDistance()->setRawValue(true);
+ _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
+ _cameraCalc.setDistanceToSurfaceRelative(true);
+}
+
+double CircularSurvey::timeBetweenShots() { return 1; }
+
+QString CircularSurvey::commandDescription() const {
+ return tr("Circular Survey");
+}
+
+QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
+
+QString CircularSurvey::abbreviation() const { return tr("C.S."); }
+
+bool CircularSurvey::readyForSave() const {
+ return TransectStyleComplexItem::readyForSave() && !_transectsDirty &&
+ this->_state == STATE::IDLE;
+}
+
+double CircularSurvey::additionalTimeDelay() const { return 0; }
+
+bool CircularSurvey::registerGenerator(
+ const QString &name, std::shared_ptr g) {
+ if (name.isEmpty()) {
+ qCDebug(CircularSurveyLog) << "registerGenerator(): empty name string.";
+ return false;
}
- // Store raw transects.
- const auto &pRoutingData = this->_pRoutingData;
- const auto &ori = this->_referencePoint;
- const auto &transectsENU = pRoutingData->transects;
- QList> rawTransects;
- for (std::size_t i = 1; i < transectsENU.size(); ++i) {
- const auto &t = transectsENU[i];
- rawTransects.append(QList());
- auto trGeo = rawTransects.back();
- for (auto &v : t) {
- QGeoCoordinate c;
- snake::fromENU(ori, v, c);
- trGeo.append(c);
+ if (!g) {
+ qCDebug(CircularSurveyLog) << "registerGenerator(): empty generator.";
+ return false;
+ }
+
+ if (this->_generatorNameList.contains(name)) {
+ qCDebug(CircularSurveyLog) << "registerGenerator(): generator "
+ "already registered.";
+ return false;
+ } else {
+ this->_generatorNameList.push_back(name);
+ this->_generatorList.push_back(g);
+ if (this->_generatorList.size() == 1) {
+ _switchToGenerator(g);
}
+
+ emit generatorNameListChanged();
+ return true;
}
+}
+bool CircularSurvey::unregisterGenerator(const QString &name) {
+ auto index = this->_generatorNameList.indexOf(name);
+ if (index >= 0) {
+ // Is this the current generator?
+ const auto &g = this->_generatorList.at(index);
+ if (g == this->_pGenerator) {
+ if (index > 0) {
+ _switchToGenerator(this->_generatorList.at(index - 1));
+ } else {
+ _switchToGenerator(nullptr);
+ qCDebug(CircularSurveyLog)
+ << "unregisterGenerator(): last generator unregistered.";
+ }
+ }
+
+ this->_generatorNameList.removeAt(index);
+ this->_generatorList.removeAt(index);
+
+ emit generatorNameListChanged();
+ return true;
+ } else {
+ qCDebug(CircularSurveyLog)
+ << "unregisterGenerator(): generator " << name << " not registered.";
+ return false;
+ }
+}
+
+bool CircularSurvey::unregisterGenerator(int index) {
+ if (index > 0 && index < this->_generatorNameList.size()) {
+ return unregisterGenerator(this->_generatorNameList.at(index));
+ } else {
+ qCDebug(CircularSurveyLog) << "unregisterGenerator(): index (" << index
+ << ") out"
+ "of bounds ( "
+ << this->_generatorList.size() << " ).";
+ return false;
+ }
+}
+
+bool CircularSurvey::switchToGenerator(const QString &name) {
+ auto index = this->_generatorNameList.indexOf(name);
+ if (index >= 0) {
+ _switchToGenerator(this->_generatorList.at(index));
+ return true;
+ } else {
+ qCDebug(CircularSurveyLog)
+ << "switchToGenerator(): generator " << name << " not registered.";
+ return false;
+ }
+}
+
+bool CircularSurvey::switchToGenerator(int index) {
+ if (index >= 0) {
+ _switchToGenerator(this->_generatorList.at(index));
+ return true;
+ } else {
+ qCDebug(CircularSurveyLog) << "unregisterGenerator(): index (" << index
+ << ") out"
+ "of bounds ( "
+ << this->_generatorNameList.size() << " ).";
+ return false;
+ }
+}
+
+QStringList CircularSurvey::generatorNameList() {
+ return this->_generatorNameList;
+}
+
+routing::GeneratorBase *CircularSurvey::generator() {
+ return _pGenerator.get();
+}
+
+int CircularSurvey::generatorIndex() {
+ return this->_generatorList.indexOf(this->_pGenerator);
+}
+
+void CircularSurvey::_rebuildTransectsPhase1(void) {
+ auto start = std::chrono::high_resolution_clock::now();
+
+ switch (this->_state) {
+ case STATE::SKIPP:
+ qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: skipp.";
+ break;
+ case STATE::VARIANT_CHANGE:
+ qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: variant change.";
+ this->_changeVariantWorker();
+ break;
+ case STATE::RUN_CHANGE:
+ qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: run change.";
+ this->_changeVariantWorker();
+ break;
+ case STATE::REVERSE:
+ qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: reverse.";
+ this->_reverseWorker();
+ break;
+ case STATE::IDLE:
+ qCDebug(CircularSurveyLog) << "rebuildTransectsPhase1: update.";
+ this->_updateWorker();
+ break;
+ }
+ this->_state = STATE::IDLE;
+
+ qCDebug(CircularSurveyLog)
+ << "rebuildTransectsPhase1(): "
+ << std::chrono::duration_cast(
+ std::chrono::high_resolution_clock::now() - start)
+ .count()
+ << " ms";
+}
+
+void CircularSurvey::_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 CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
+
+void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
// Store solutions.
- QVector solutionVector;
- const auto nSolutions = pRoutingData->solutionVector.size();
+ auto ori = this->_pAreaData->origin();
+ ori.setAltitude(0);
+ const auto &transectsENU = pRoute->transects;
+ QVector variantVector;
+ const auto nSolutions = pRoute->solutionVector.size();
+
for (std::size_t j = 0; j < nSolutions; ++j) {
- const auto &solution = pRoutingData->solutionVector.at(j);
- const auto nRuns = solution.size();
- // Store runs.
- Runs runs(nRuns, Transects{QList()});
- for (std::size_t k = 0; k < nRuns; ++k) {
- const auto &route = solution.at(k);
+ Variant var{QList()};
+ const auto &solution = pRoute->solutionVector.at(j);
+ if (solution.size() > 0) {
+ const auto &route = solution.at(0);
const auto &path = route.path;
const auto &info = route.info;
if (info.size() > 1) {
@@ -674,56 +609,47 @@ void CircularSurvey::_storeWorker() {
}
// Convert to geo coordinates.
- auto &list = runs[k].front();
+ auto &list = var.front();
for (std::size_t i = idxFirst; i <= idxLast; ++i) {
auto &vertex = path[i];
QGeoCoordinate c;
snake::fromENU(ori, vertex, c);
list.append(CoordInfo_t{c, CoordTypeInterior});
}
+
} else {
- qCWarning(CircularSurveyLog)
- << "_storeWorker(): lastTransect.size() == 0";
+ qCDebug(CircularSurveyLog)
+ << "_setTransects(): lastTransect.size() == 0";
}
} else {
- qCWarning(CircularSurveyLog)
- << "_storeWorker(): firstTransect.size() == 0";
+ qCDebug(CircularSurveyLog)
+ << "_setTransects(): firstTransect.size() == 0";
}
} else {
- qCWarning(CircularSurveyLog)
- << "_storeWorker(): transectsInfo.size() <= 1";
- }
- }
- // Remove empty runs.
- bool error = true;
- for (auto it = runs.begin(); it < runs.end();) {
- if (it->size() > 0 && it->front().size() > 0) {
- error = false;
- ++it;
- } else {
- it = runs.erase(it);
+ qCDebug(CircularSurveyLog)
+ << "_setTransects(): transectsInfo.size() <= 1";
}
+ } else {
+ qCDebug(CircularSurveyLog) << "_setTransects(): solution.size() == 0";
}
- if (!error) {
- solutionVector.push_back(std::move(runs));
- }
- }
- // Remove empty solutions.
- std::size_t nSol = 0;
- for (auto it = solutionVector.begin(); it < solutionVector.end();) {
- if (it->size() > 0 && it->front().size() > 0) {
- ++it;
- ++nSol;
- } else {
- it = solutionVector.erase(it);
+ if (var.size() > 0 && var.front().size() > 0) {
+ variantVector.push_back(std::move(var));
}
}
// Assign routes if no error occured.
- if (nSol > 0) {
+ if (variantVector.size() > 0) {
// Swap first route to _transects.
- this->_variantVector.swap(solutionVector);
+ this->_variantVector.swap(variantVector);
+
+ // If the transects are getting rebuilt then any previously loaded
+ // mission items are now invalid.
+ if (_loadedMissionItemsParent) {
+ _loadedMissionItems.clear();
+ _loadedMissionItemsParent->deleteLater();
+ _loadedMissionItemsParent = nullptr;
+ }
// Add route variant names.
this->_variantNames.clear();
@@ -733,482 +659,31 @@ void CircularSurvey::_storeWorker() {
}
emit variantNamesChanged();
- // Swap in rawTransects.
- this->_rawTransects.swap(rawTransects);
-
disconnect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
- disconnect(&this->_run, &Fact::rawValueChanged, this,
- &CircularSurvey::_changeRun);
this->_variant.setCookedValue(QVariant(0));
- this->_run.setCookedValue(QVariant(0));
connect(&this->_variant, &Fact::rawValueChanged, this,
&CircularSurvey::_changeVariant);
- connect(&this->_run, &Fact::rawValueChanged, this,
- &CircularSurvey::_changeRun);
- this->_changeVariantRunWorker();
- // Mark transect as stored and ready.
- this->_transectsDirty = false;
- }
-}
+ this->_changeVariantWorker();
-void CircularSurvey::applyNewAltitude(double newAltitude) {
- _cameraCalc.valueSetIsDistance()->setRawValue(true);
- _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
- _cameraCalc.setDistanceToSurfaceRelative(true);
-}
-
-double CircularSurvey::timeBetweenShots() { return 1; }
-
-QString CircularSurvey::commandDescription() const {
- return tr("Circular Survey");
-}
-
-QString CircularSurvey::commandName() const { return tr("Circular Survey"); }
-
-QString CircularSurvey::abbreviation() const { return tr("C.S."); }
-
-bool CircularSurvey::readyForSave() const {
- return TransectStyleComplexItem::readyForSave() && !_transectsDirty;
-}
-
-double CircularSurvey::additionalTimeDelay() const { return 0; }
-
-void CircularSurvey::_rebuildTransectsPhase1(void) {
- auto start = std::chrono::high_resolution_clock::now();
-
- switch (this->_state) {
- case STATE::STORE:
- qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: store.";
- this->_storeWorker();
- break;
- case STATE::VARIANT_CHANGE:
- qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: variant change.";
- this->_changeVariantRunWorker();
- break;
- case STATE::RUN_CHANGE:
- qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: run change.";
- this->_changeVariantRunWorker();
- break;
- case STATE::REVERSE:
- qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: reverse.";
- this->_reverseWorker();
- break;
- case STATE::DEFAULT:
- qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: update.";
- this->_updateWorker();
- break;
- }
- this->_state = STATE::DEFAULT;
-
- qCWarning(CircularSurveyLog)
- << "rebuildTransectsPhase1(): "
- << std::chrono::duration_cast(
- std::chrono::high_resolution_clock::now() - start)
- .count()
- << " ms";
-}
+ // Mark transect ready.
+ this->_transectsDirty = false;
-void CircularSurvey::_recalcComplexDistance() {
- _complexDistance = 0;
- if (_transectsDirty)
- return;
- for (int i = 0; i < _visualTransectPoints.count() - 1; i++) {
- _complexDistance +=
- _visualTransectPoints[i].value().distanceTo(
- _visualTransectPoints[i + 1].value());
+ this->_state = STATE::SKIPP;
+ this->_rebuildTransects();
+ } else {
+ qCDebug(CircularSurveyLog)
+ << "_setTransects(): failed, variantVector empty.";
+ this->_state = STATE::IDLE;
}
- emit complexDistanceChanged();
-}
-
-// no cameraShots in Circular Survey, add if desired
-void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
-
-void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
- this->_pRoutingData = pRoute;
- this->_state = STATE::STORE;
- this->_rebuildTransects();
}
-Fact *CircularSurvey::minLength() { return &_minLength; }
-
-Fact *CircularSurvey::type() { return &_type; }
-
Fact *CircularSurvey::variant() { return &_variant; }
-Fact *CircularSurvey::numRuns() { return &_numRuns; }
-
-Fact *CircularSurvey::run() { return &_run; }
-
-int CircularSurvey::typeCount() const { return int(integral(Type::Count)); }
-
bool CircularSurvey::calculating() const {
return this->_pWorker->calculating();
}
-bool circularTransects(const snake::FPolygon &polygon,
- const std::vector &tiles,
- snake::Length deltaR, snake::Angle deltaAlpha,
- snake::Length minLength, snake::Transects &transects) {
- auto s1 = std::chrono::high_resolution_clock::now();
-
- // Check preconitions
- if (polygon.outer().size() >= 3) {
- using namespace boost::units;
- // Convert geo polygon to ENU polygon.
- snake::FPoint origin{0, 0};
- std::string error;
- // Check validity.
- if (!bg::is_valid(polygon, error)) {
- qCWarning(CircularSurveyLog) << "circularTransects(): "
- "invalid polygon.";
- qCWarning(CircularSurveyLog) << error.c_str();
- std::stringstream ss;
- ss << bg::wkt(polygon);
- qCWarning(CircularSurveyLog) << ss.str().c_str();
- } else {
- // Calculate polygon distances and angles.
- std::vector distances;
- distances.reserve(polygon.outer().size());
- std::vector angles;
- angles.reserve(polygon.outer().size());
- //#ifdef DEBUG_CIRCULAR_SURVEY
- // qCWarning(CircularSurveyLog) << "circularTransects():";
- //#endif
- for (const auto &p : polygon.outer()) {
- snake::Length distance = bg::distance(origin, p) * si::meter;
- distances.push_back(distance);
- snake::Angle alpha = (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
- alpha = alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
- angles.push_back(alpha);
- //#ifdef DEBUG_CIRCULAR_SURVEY
- // qCWarning(CircularSurveyLog) << "distances, angles,
- // coordinates:"; qCWarning(CircularSurveyLog) <<
- // to_string(distance).c_str(); qCWarning(CircularSurveyLog) <<
- // to_string(snake::Degree(alpha)).c_str();
- // qCWarning(CircularSurveyLog) << "x = " << p.get<0>() << "y = "
- // << p.get<1>();
- //#endif
- }
-
- auto rMin = deltaR; // minimal circle radius
- snake::Angle alpha1(0 * degree::degree);
- snake::Angle alpha2(360 * degree::degree);
- // Determine r_min by successive approximation
- if (!bg::within(origin, polygon.outer())) {
- rMin = bg::distance(origin, polygon) * si::meter;
- }
-
- auto rMax = (*std::max_element(distances.begin(),
- distances.end())); // maximal circle radius
-
- // Scale parameters and coordinates.
- const auto rMinScaled =
- ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
- const auto deltaRScaled =
- ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
- auto originScaled =
- ClipperLib::IntPoint{ClipperLib::cInt(std::round(origin.get<0>())),
- ClipperLib::cInt(std::round(origin.get<1>()))};
-
- // Generate circle sectors.
- auto rScaled = rMinScaled;
- const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
- vector sectors(nTran, ClipperLib::Path());
- const auto nSectors =
- long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
- //#ifdef DEBUG_CIRCULAR_SURVEY
- // qCWarning(CircularSurveyLog) << "circularTransects(): sector
- // parameres:"; qCWarning(CircularSurveyLog) << "alpha1: " <<
- // to_string(snake::Degree(alpha1)).c_str();
- // qCWarning(CircularSurveyLog) << "alpha2:
- // "
- // << to_string(snake::Degree(alpha2)).c_str();
- // qCWarning(CircularSurveyLog) << "n: "
- // << to_string((alpha2 - alpha1) / deltaAlpha).c_str();
- // qCWarning(CircularSurveyLog)
- // << "nSectors: " << nSectors; qCWarning(CircularSurveyLog) <<
- // "rMin: " << to_string(rMin).c_str(); qCWarning(CircularSurveyLog)
- // << "rMax: " << to_string(rMax).c_str();
- // qCWarning(CircularSurveyLog) << "nTran: " << nTran;
- //#endif
- using ClipperCircle =
- GenericCircle;
- for (auto §or : sectors) {
- ClipperCircle circle(rScaled, originScaled);
- approximate(circle, nSectors, sector);
- rScaled += deltaRScaled;
- }
- // Clip sectors to polygonENU.
- ClipperLib::Path polygonClipper;
- snake::FPolygon shrinked;
- snake::offsetPolygon(polygon, shrinked, -0.3);
- auto &outer = shrinked.outer();
- 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));
- polygonClipper.push_back(ClipperLib::IntPoint{x, y});
- }
- ClipperLib::Clipper clipper;
- clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
- clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
- ClipperLib::PolyTree transectsClipper;
- clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
- ClipperLib::pftNonZero, ClipperLib::pftNonZero);
-
- // Subtract holes.
- if (tiles.size() > 0) {
- vector processedTiles;
- for (const auto &tile : tiles) {
- ClipperLib::Path path;
- for (const auto &v : tile.outer()) {
- path.push_back(ClipperLib::IntPoint{
- static_cast(v.get<0>() * CLIPPER_SCALE),
- static_cast(v.get<1>() * CLIPPER_SCALE)});
- }
- processedTiles.push_back(std::move(path));
- }
-
- clipper.Clear();
- for (const auto &child : transectsClipper.Childs) {
- clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
- }
- clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
- transectsClipper.Clear();
- clipper.Execute(ClipperLib::ctDifference, transectsClipper,
- ClipperLib::pftNonZero, ClipperLib::pftNonZero);
- }
-
- // Extract transects from PolyTree and convert them to
- // BoostLineString
- for (const auto &child : transectsClipper.Childs) {
- snake::FLineString transect;
- transect.reserve(child->Contour.size());
- for (const auto &vertex : child->Contour) {
- auto x = static_cast(vertex.X) / CLIPPER_SCALE;
- auto y = static_cast(vertex.Y) / CLIPPER_SCALE;
- transect.push_back(snake::FPoint(x, y));
- }
- transects.push_back(transect);
- }
-
- // Join sectors which where slit due to clipping.
- const double th = 0.01;
- for (auto ito = transects.begin(); ito < transects.end(); ++ito) {
- for (auto iti = ito + 1; iti < transects.end(); ++iti) {
- auto dist1 = bg::distance(ito->front(), iti->front());
- if (dist1 < th) {
- snake::FLineString temp;
- for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
- temp.push_back(*it);
- }
- temp.insert(temp.end(), ito->begin(), ito->end());
- *ito = temp;
- transects.erase(iti);
- break;
- }
- auto dist2 = bg::distance(ito->front(), iti->back());
- if (dist2 < th) {
- snake::FLineString temp;
- temp.insert(temp.end(), iti->begin(), iti->end());
- temp.insert(temp.end(), ito->begin(), ito->end());
- *ito = temp;
- transects.erase(iti);
- break;
- }
- auto dist3 = bg::distance(ito->back(), iti->front());
- if (dist3 < th) {
- snake::FLineString temp;
- temp.insert(temp.end(), ito->begin(), ito->end());
- temp.insert(temp.end(), iti->begin(), iti->end());
- *ito = temp;
- transects.erase(iti);
- break;
- }
- auto dist4 = bg::distance(ito->back(), iti->back());
- if (dist4 < th) {
- snake::FLineString temp;
- temp.insert(temp.end(), ito->begin(), ito->end());
- for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
- temp.push_back(*it);
- }
- *ito = temp;
- transects.erase(iti);
- break;
- }
- }
- }
-
- // Remove short transects
- for (auto it = transects.begin(); it < transects.end();) {
- if (bg::length(*it) < minLength.value()) {
- it = transects.erase(it);
- } else {
- ++it;
- }
- }
-
- qCWarning(CircularSurveyLog)
- << "circularTransects(): transect gen. time: "
- << std::chrono::duration_cast(
- std::chrono::high_resolution_clock::now() - s1)
- .count()
- << " ms";
- return true;
- }
- }
- return false;
-}
-
-bool linearTransects(const snake::FPolygon &polygon,
- const std::vector &tiles,
- snake::Length distance, snake::Angle angle,
- snake::Length minLength, snake::Transects &transects) {
- namespace tr = bg::strategy::transform;
- auto s1 = std::chrono::high_resolution_clock::now();
-
- // Check preconitions
- if (polygon.outer().size() >= 3) {
- // Convert to ENU system.
- std::string error;
- // Check validity.
- if (!bg::is_valid(polygon, error)) {
- std::stringstream ss;
- ss << bg::wkt(polygon);
-
- qCWarning(CircularSurveyLog) << "linearTransects(): "
- "invalid polygon. "
- << error.c_str() << ss.str().c_str();
- } else {
- tr::rotate_transformer rotate(angle.value() *
- 180 / M_PI);
- // Rotate polygon by angle and calculate bounding box.
- snake::FPolygon polygonENURotated;
- bg::transform(polygon.outer(), polygonENURotated.outer(), rotate);
- snake::FBox 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::FPoint v1{x0, y0 + i * distance.value()};
- snake::FPoint v2{x1, y0 + i * distance.value()};
- snake::FLineString transect;
- transect.push_back(v1);
- transect.push_back(v2);
- // transform back
- snake::FLineString 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;
- qCWarning(CircularSurveyLog)
- << "linearTransects(): " << ss.str().c_str();
- return false;
- }
-
- // Convert measurement area to clipper path.
- snake::FPolygon shrinked;
- snake::offsetPolygon(polygon, 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);
-
- // Subtract holes.
- if (tiles.size() > 0) {
- vector processedTiles;
- for (const auto &tile : tiles) {
- ClipperLib::Path path;
- for (const auto &v : tile.outer()) {
- path.push_back(ClipperLib::IntPoint{
- static_cast(v.get<0>() * CLIPPER_SCALE),
- static_cast(v.get<1>() * CLIPPER_SCALE)});
- }
- processedTiles.push_back(std::move(path));
- }
-
- clipper.Clear();
- for (const auto &child : clippedTransecs.Childs) {
- clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
- }
- clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
- clippedTransecs.Clear();
- clipper.Execute(ClipperLib::ctDifference, clippedTransecs,
- ClipperLib::pftNonZero, ClipperLib::pftNonZero);
- }
-
- // Extract transects from PolyTree and convert them to BoostLineString
- for (const auto &child : clippedTransecs.Childs) {
- const auto &clipperTransect = child->Contour;
- snake::FPoint v1{
- static_cast(clipperTransect[0].X) / CLIPPER_SCALE,
- static_cast(clipperTransect[0].Y) / CLIPPER_SCALE};
- snake::FPoint v2{
- static_cast(clipperTransect[1].X) / CLIPPER_SCALE,
- static_cast(clipperTransect[1].Y) / CLIPPER_SCALE};
-
- snake::FLineString transect{v1, v2};
- if (bg::length(transect) >= minLength.value()) {
- transects.push_back(transect);
- }
- }
-
- if (transects.size() == 0) {
- std::stringstream ss;
- ss << "Not able to generatetransects. Parameter: minLength = "
- << minLength << std::endl;
- qCWarning(CircularSurveyLog)
- << "linearTransects(): " << ss.str().c_str();
- return false;
- }
- qCWarning(CircularSurveyLog)
- << "linearTransects(): transect gen. time: "
- << std::chrono::duration_cast(
- std::chrono::high_resolution_clock::now() - s1)
- .count()
- << " ms";
- return true;
- }
- }
- return false;
-}
/*!
\class CircularSurveyComplexItem
\inmodule Wima
diff --git a/src/Wima/CircularSurvey.h b/src/Wima/CircularSurvey.h
index 4d68a40a0b20a3a258c69db5a38cb655f6c10631..de7253a2b29e1982b281c8d545ae5d751c951ab2 100644
--- a/src/Wima/CircularSurvey.h
+++ b/src/Wima/CircularSurvey.h
@@ -2,26 +2,30 @@
#include
#include
+#include
#include "SettingsFact.h"
#include "TransectStyleComplexItem.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaMeasurementAreaData.h"
+#include "WimaPlanData.h"
class RoutingThread;
class RoutingData;
+namespace routing {
+class GeneratorBase;
+}
+
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
- };
+ using PtrGenerator = std::shared_ptr;
+ using PtrRoutingData = std::shared_ptr;
+ using PtrWorker = std::shared_ptr;
+
+public:
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
/// use in the Plan View
@@ -31,50 +35,27 @@ public:
QObject *parent);
~CircularSurvey();
- Q_PROPERTY(QGeoCoordinate refPoint READ refPoint WRITE setRefPoint NOTIFY
- refPointChanged)
- 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(Fact *variant READ variant CONSTANT)
- Q_PROPERTY(Fact *numRuns READ numRuns CONSTANT)
- Q_PROPERTY(Fact *run READ run CONSTANT)
- Q_PROPERTY(int typeCount READ typeCount CONSTANT)
+ Q_PROPERTY(
+ QStringList variantNames READ variantNames NOTIFY variantNamesChanged)
+ Q_PROPERTY(QStringList generatorNameList READ generatorNameList NOTIFY
+ generatorNameListChanged)
Q_PROPERTY(bool calculating READ calculating NOTIFY calculatingChanged)
- Q_PROPERTY(bool hidePolygon READ hidePolygon NOTIFY hidePolygonChanged)
Q_PROPERTY(
- QList variantNames READ variantNames NOTIFY variantNamesChanged)
- Q_PROPERTY(QList runNames READ runNames NOTIFY runNamesChanged)
+ routing::GeneratorBase *generator READ generator NOTIFY generatorChanged)
+ Q_PROPERTY(int generatorIndex READ generatorIndex NOTIFY generatorChanged)
- Q_INVOKABLE void resetReference(void);
Q_INVOKABLE void reverse(void);
// Property setters
- void setRefPoint(const QGeoCoordinate &refPt);
- void setHidePolygon(bool hide);
- void setMeasurementArea(const WimaMeasurementAreaData &mArea);
- void setJoinedArea(const WimaJoinedAreaData &jArea);
- void setMeasurementArea(const WimaMeasurementArea &mArea);
- void setJoinedArea(const WimaJoinedArea &jArea);
- void setDepot(const QGeoCoordinate &depot);
+ void setPlanData(const WimaPlanData &d);
// Property getters
- QGeoCoordinate refPoint() const;
- Fact *transectDistance();
- Fact *alpha();
- Fact *minLength();
- Fact *type();
+ const WimaPlanData &planData() const;
+ WimaPlanData &planData();
Fact *variant();
- Fact *numRuns();
- Fact *run();
- int typeCount() const;
+ QStringList variantNames() const;
bool calculating() const;
- bool hidePolygon() const;
- QList variantNames() const;
- QList runNames() const;
- QGeoCoordinate depot() const;
- const QList> &rawTransects() const;
// Overrides
bool load(const QJsonObject &complexObject, int sequenceNumber,
@@ -92,91 +73,73 @@ public:
bool readyForSave(void) const override final;
double additionalTimeDelay(void) const override final;
+ // Generator
+ bool registerGenerator(const QString &name,
+ std::shared_ptr g);
+ bool unregisterGenerator(const QString &name);
+ bool unregisterGenerator(int index);
+ Q_INVOKABLE bool switchToGenerator(const QString &name);
+ Q_INVOKABLE bool switchToGenerator(int index);
+ QStringList generatorNameList();
+ routing::GeneratorBase *generator();
+ int generatorIndex();
+
static const char *settingsGroup;
- static const char *transectDistanceName;
- static const char *alphaName;
- static const char *minLengthName;
- static const char *typeName;
static const char *variantName;
- static const char *numRunsName;
- static const char *runName;
static const char *CircularSurveyName;
- static const char *refPointLongitudeName;
- static const char *refPointLatitudeName;
- static const char *refPointAltitudeName;
signals:
- void refPointChanged();
void calculatingChanged();
- void hidePolygonChanged();
- void depotChanged();
void variantNamesChanged();
- void runNamesChanged();
- void measurementAreaChanged();
- void joinedAreaChanged();
+ void generatorNameListChanged();
+ void generatorChanged();
private slots:
// Overrides from TransectStyleComplexItem
void _rebuildTransectsPhase1(void) final;
void _recalcComplexDistance(void) final;
void _recalcCameraShots(void) final;
+
+ // Worker functions.
void _setTransects(PtrRoutingData pRoute);
+ void _changeVariant();
+ void _updateWorker();
+ void _changeVariantWorker();
+ void _reverseWorker();
private:
void _appendLoadedMissionItems(QList &items,
QObject *missionItemParent);
void _buildAndAppendMissionItems(QList &items,
QObject *missionItemParent);
- void _changeVariant();
- void _changeRun();
- void _updateWorker();
- void _changeVariantRunWorker();
- void _reverseWorker();
- void _storeWorker();
- void _changeRunWorker();
-
- // center of the circular lanes, e.g. base station
- QGeoCoordinate _referencePoint;
- QMap _metaDataMap;
- // distance between two neighbour circles
- SettingsFact _transectDistance;
- // angle discretisation of the circles
- SettingsFact _alpha;
- // minimal transect lenght, transects are rejected if they are shorter than
- // this value
- SettingsFact _minLength;
- SettingsFact _type;
- SettingsFact _variant;
- QList _variantNames;
- SettingsFact _numRuns;
- SettingsFact _run;
- QList _runNames;
-
- // Area data
- WimaMeasurementAreaData _mArea;
- WimaJoinedAreaData _jArea;
- QGeoCoordinate _depot;
-
- // Worker
- using PtrWorker = std::shared_ptr;
- PtrWorker _pWorker;
- PtrRoutingData _pRoutingData;
-
- // Routing data.
- QList> _rawTransects;
- using Runs = QVector;
- QVector _variantVector;
+ bool _switchToGenerator(const PtrGenerator &newG);
// State.
enum class STATE {
- DEFAULT,
- STORE,
+ IDLE,
+ SKIPP,
REVERSE,
VARIANT_CHANGE,
RUN_CHANGE,
};
STATE _state;
- bool _hidePolygon;
+ // center of the circular lanes, e.g. base station
+ QMap _metaDataMap;
+ SettingsFact _variant;
+ QStringList _variantNames;
+
+ // Area data
+ std::shared_ptr _pAreaData;
+
+ // Generators
+ QList _generatorList;
+ QStringList _generatorNameList;
+ PtrGenerator _pGenerator;
+
+ // Routing.
+ using Variant = Transects;
+ QVector _variantVector;
+ PtrWorker _pWorker;
};
diff --git a/src/Wima/Geometry/WimaAreaData.cc b/src/Wima/Geometry/WimaAreaData.cc
index 56909032e92b43b0f046de1858681cecd8e7e793..ca977dc226454138182d6dd182a09cb5e4bce920 100644
--- a/src/Wima/Geometry/WimaAreaData.cc
+++ b/src/Wima/Geometry/WimaAreaData.cc
@@ -2,7 +2,14 @@
WimaAreaData::WimaAreaData(QObject *parent) : QObject(parent) {}
-WimaAreaData::~WimaAreaData() {}
+WimaAreaData::WimaAreaData(const WimaAreaData &other, QObject *parent)
+ : QObject(parent), _path(other._path), _list(other._list),
+ _center(other._center) {}
+
+WimaAreaData::WimaAreaData(const WimaArea &otherData, QObject *parent)
+ : QObject(parent), _center(otherData.center()) {
+ _setPathImpl(otherData.path());
+}
bool WimaAreaData::operator==(const WimaAreaData &data) const {
return this->_path == data._path && this->_center == data._center;
@@ -25,7 +32,7 @@ bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const {
using namespace PolygonCalculus;
using namespace GeoUtilities;
- if (_path.size() > 2) {
+ if (this->coordinateList().size() > 2) {
QPolygonF polygon;
toCartesianList(this->coordinateList(), coordinate, polygon);
return PlanimetryCalculus::contains(polygon, QPointF(0, 0));
@@ -36,44 +43,60 @@ bool WimaAreaData::containsCoordinate(const QGeoCoordinate &coordinate) const {
void WimaAreaData::append(const QGeoCoordinate &c) {
_list.append(c);
_path.push_back(QVariant::fromValue(c));
+ emit pathChanged();
}
void WimaAreaData::push_back(const QGeoCoordinate &c) { append(c); }
void WimaAreaData::clear() {
- _list.clear();
- _path.clear();
+ if (_list.size() > 0 || _path.size() > 0) {
+ _list.clear();
+ _path.clear();
+ emit pathChanged();
+ }
}
void WimaAreaData::setPath(const QVariantList &coordinateList) {
- _path = coordinateList;
- _list.clear();
- for (auto variant : coordinateList) {
- _list.push_back(variant.value());
+ if (_path != coordinateList) {
+ _setPathImpl(coordinateList);
+ emit pathChanged();
}
}
void WimaAreaData::setCenter(const QGeoCoordinate ¢er) {
if (_center != center) {
_center = center;
-
emit centerChanged();
}
}
-/*!
- * \fn void WimaAreaData::assign(const WimaAreaData &other)
- *
- * Assigns \a other to the invoking object
- */
-void WimaAreaData::assign(const WimaAreaData &other) {
- setPath(other.path());
- setCenter(other.center());
+WimaAreaData &WimaAreaData::operator=(const WimaAreaData &otherData) {
+ setPath(otherData._list);
+ setCenter(otherData._center);
+ return *this;
}
-void WimaAreaData::assign(const WimaArea &other) {
- setPath(other.path());
- setCenter(other.center());
+WimaAreaData &WimaAreaData::operator=(const WimaArea &otherData) {
+ setPath(otherData.path());
+ setCenter(otherData.center());
+ return *this;
+}
+
+void WimaAreaData::_setPathImpl(const QList &coordinateList) {
+ _list = coordinateList;
+ _path.clear();
+ // copy all coordinates to _path
+ for (const auto &vertex : coordinateList) {
+ _path.append(QVariant::fromValue(vertex));
+ }
+}
+
+void WimaAreaData::_setPathImpl(const QVariantList &coordinateList) {
+ _path = coordinateList;
+ _list.clear();
+ for (const auto &variant : coordinateList) {
+ _list.push_back(variant.value());
+ }
}
/*!
@@ -83,24 +106,26 @@ void WimaAreaData::assign(const WimaArea &other) {
* coordinateList. Emits the \c pathChanged() signal.
*/
void WimaAreaData::setPath(const QList &coordinateList) {
- _list = coordinateList;
-
- _path.clear();
- // copy all coordinates to _path
- for (int i = 0; i < coordinateList.size(); i++) {
- _path.append(QVariant::fromValue(coordinateList.value(i)));
+ if (_list != coordinateList) {
+ _setPathImpl(coordinateList);
+ emit pathChanged();
}
-
- emit pathChanged(_path);
}
bool operator==(const WimaAreaData &m1, const WimaArea &m2) {
return m1.path() == m2.path() && m1.center() == m2.center();
}
+
bool operator!=(const WimaAreaData &m1, const WimaArea &m2) {
return !operator==(m1, m2);
}
+bool operator==(const WimaArea &m1, const WimaAreaData &m2) { return m2 == m1; }
+
+bool operator!=(const WimaArea &m1, const WimaAreaData &m2) {
+ return !operator==(m2, m1);
+}
+
/*!
* \class WimaArea::WimaAreaData
* \brief Class to store and exchange data of a \c WimaArea Object.
diff --git a/src/Wima/Geometry/WimaAreaData.h b/src/Wima/Geometry/WimaAreaData.h
index ffbc4c61287cc27ce85f232730e6971f0f45510f..b9348aa5fcfe9ad63271cc7aea27a023665f7101 100644
--- a/src/Wima/Geometry/WimaAreaData.h
+++ b/src/Wima/Geometry/WimaAreaData.h
@@ -1,4 +1,4 @@
-#pragma once
+#pragma once
#include
@@ -11,16 +11,12 @@ class WimaAreaData
{
Q_OBJECT
public:
+ WimaAreaData(QObject *parent = nullptr);
+
Q_PROPERTY(const QVariantList path READ path NOTIFY pathChanged)
Q_PROPERTY(QString type READ type CONSTANT)
Q_PROPERTY(QString mapVisualQML READ mapVisualQML CONSTANT)
- WimaAreaData(QObject *parent = nullptr);
- ~WimaAreaData();
- WimaAreaData(const WimaAreaData &otherData) = delete; // avoid slicing
- WimaAreaData &
- operator=(const WimaAreaData &otherData) = delete; // avoid slicing
-
bool operator==(const WimaAreaData &data) const;
bool operator!=(const WimaAreaData &data) const;
@@ -37,8 +33,8 @@ public:
void clear();
signals:
- void pathChanged(const QVariantList &coordinateList);
- void centerChanged(void);
+ void pathChanged();
+ void centerChanged();
public slots:
void setPath(const QList &coordinateList);
@@ -46,18 +42,21 @@ public slots:
void setCenter(const QGeoCoordinate ¢er);
protected:
- void assign(const WimaAreaData &other);
- void assign(const WimaArea &other);
+ WimaAreaData(const WimaAreaData &otherData, QObject *parent);
+ WimaAreaData(const WimaArea &otherData, QObject *parent);
+ WimaAreaData &operator=(const WimaAreaData &otherData);
+ WimaAreaData &operator=(const WimaArea &otherData);
private:
- // Member Functions
-
+ void _setPathImpl(const QList &coordinateList);
+ void _setPathImpl(const QVariantList &coordinateList);
// Member Variables
- // see WimaArea.h for explanation
- QVariantList _path;
- mutable QList _list;
+ mutable QVariantList _path;
+ QList _list;
QGeoCoordinate _center;
};
bool operator==(const WimaAreaData &m1, const WimaArea &m2);
bool operator!=(const WimaAreaData &m1, const WimaArea &m2);
+bool operator==(const WimaArea &m1, const WimaAreaData &m2);
+bool operator!=(const WimaArea &m1, const WimaAreaData &m2);
diff --git a/src/Wima/Geometry/WimaCorridorData.cpp b/src/Wima/Geometry/WimaCorridorData.cpp
index 23abae3b71656fe99514d906db26055157b82677..2bd90d6e6013e5c877fe1a9bcb4031de9f538197 100644
--- a/src/Wima/Geometry/WimaCorridorData.cpp
+++ b/src/Wima/Geometry/WimaCorridorData.cpp
@@ -6,14 +6,10 @@ WimaCorridorData::WimaCorridorData(QObject *parent) : WimaAreaData(parent) {}
WimaCorridorData::WimaCorridorData(const WimaCorridorData &other,
QObject *parent)
- : WimaAreaData(parent) {
- *this = other;
-}
+ : WimaAreaData(other, parent) {}
WimaCorridorData::WimaCorridorData(const WimaCorridor &other, QObject *parent)
- : WimaAreaData(parent) {
- *this = other;
-}
+ : WimaAreaData(other, parent) {}
/*!
* \overload operator=()
@@ -21,8 +17,7 @@ WimaCorridorData::WimaCorridorData(const WimaCorridor &other, QObject *parent)
* Assigns \a other to the invoking object.
*/
WimaCorridorData &WimaCorridorData::operator=(const WimaCorridorData &other) {
- this->assign(other);
-
+ WimaAreaData::operator=(other);
return *this;
}
@@ -32,8 +27,7 @@ WimaCorridorData &WimaCorridorData::operator=(const WimaCorridorData &other) {
* Assigns \a other to the invoking object.
*/
WimaCorridorData &WimaCorridorData::operator=(const WimaCorridor &other) {
- this->assign(other);
-
+ WimaAreaData::operator=(other);
return *this;
}
@@ -43,14 +37,6 @@ QString WimaCorridorData::mapVisualQML() const {
QString WimaCorridorData::type() const { return this->typeString; }
-void WimaCorridorData::assign(const WimaCorridorData &corridorData) {
- WimaAreaData::assign(corridorData);
-}
-
-void WimaCorridorData::assign(const WimaCorridor &corridor) {
- WimaAreaData::assign(corridor);
-}
-
/*!
* \class WimaAreaData::WimaCorridorData
* \brief Class to store and exchange data of a \c WimaCorridorData Object.
diff --git a/src/Wima/Geometry/WimaCorridorData.h b/src/Wima/Geometry/WimaCorridorData.h
index b5557a599b28a2d858a67e32e00f0a765f1dbedc..a319b54382969564cf3e9bc1eef37116677ca80a 100644
--- a/src/Wima/Geometry/WimaCorridorData.h
+++ b/src/Wima/Geometry/WimaCorridorData.h
@@ -23,14 +23,4 @@ public:
WimaCorridorData *Clone() const { return new WimaCorridorData(*this); }
static const char *typeString;
-
-signals:
-
-public slots:
-
-protected:
- void assign(const WimaCorridorData &corridorData);
- void assign(const WimaCorridor &corridor);
-
-private:
};
diff --git a/src/Wima/Geometry/WimaJoinedAreaData.cc b/src/Wima/Geometry/WimaJoinedAreaData.cc
index 2929a0d84fa0f97c1ec2f36bf277d55ec83ea5d1..0b7a540ba5b25dfe078d6ba08d51a222531dc673 100644
--- a/src/Wima/Geometry/WimaJoinedAreaData.cc
+++ b/src/Wima/Geometry/WimaJoinedAreaData.cc
@@ -7,15 +7,11 @@ WimaJoinedAreaData::WimaJoinedAreaData(QObject *parent)
WimaJoinedAreaData::WimaJoinedAreaData(const WimaJoinedAreaData &other,
QObject *parent)
- : WimaAreaData(parent) {
- *this = other;
-}
+ : WimaAreaData(other, parent) {}
WimaJoinedAreaData::WimaJoinedAreaData(const WimaJoinedArea &other,
QObject *parent)
- : WimaAreaData(parent) {
- *this = other;
-}
+ : WimaAreaData(other, parent) {}
/*!
* \overload operator=()
@@ -24,8 +20,7 @@ WimaJoinedAreaData::WimaJoinedAreaData(const WimaJoinedArea &other,
*/
WimaJoinedAreaData &WimaJoinedAreaData::
operator=(const WimaJoinedAreaData &other) {
- assign(other);
-
+ WimaAreaData::operator=(other);
return *this;
}
@@ -35,7 +30,7 @@ operator=(const WimaJoinedAreaData &other) {
* Assigns \a other to the invoking object.
*/
WimaJoinedAreaData &WimaJoinedAreaData::operator=(const WimaJoinedArea &other) {
- assign(other);
+ WimaAreaData::operator=(other);
return *this;
}
@@ -45,14 +40,6 @@ QString WimaJoinedAreaData::mapVisualQML() const {
QString WimaJoinedAreaData::type() const { return this->typeString; }
-void WimaJoinedAreaData::assign(const WimaJoinedAreaData &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.
diff --git a/src/Wima/Geometry/WimaJoinedAreaData.h b/src/Wima/Geometry/WimaJoinedAreaData.h
index 1d205dcb1b278fba95698b8e5efd0cec0bb14537..35c903580747f018e1682958c199bd5985193599 100644
--- a/src/Wima/Geometry/WimaJoinedAreaData.h
+++ b/src/Wima/Geometry/WimaJoinedAreaData.h
@@ -24,10 +24,4 @@ public:
WimaJoinedAreaData *Clone() const { return new WimaJoinedAreaData(*this); }
static const char *typeString;
-
-protected:
- void assign(const WimaJoinedAreaData &other);
- void assign(const WimaJoinedArea &other);
-
-private:
};
diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc
index 0aa6b52041667c184512957d6c9936cca4805c2e..e02bef9ff177b74144be8b56eb9e5d07d94aff7f 100644
--- a/src/Wima/Geometry/WimaMeasurementArea.cc
+++ b/src/Wima/Geometry/WimaMeasurementArea.cc
@@ -72,7 +72,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
- this /* QObject parent */)),
+ this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
@@ -91,7 +91,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this /* QObject parent */)),
_minTileAreaPercent(SettingsFact(settingsGroup,
_metaDataMap[minTileAreaName],
- this /* QObject parent */)),
+ this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_state(STATE::IDLE) {
@@ -157,12 +157,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) {
if (ready()) {
- this->WimaArea::saveToJson(json);
- json[tileHeightName] = _tileHeight.rawValue().toDouble();
- json[tileWidthName] = _tileWidth.rawValue().toDouble();
+ this->WimaArea::saveToJson(json);
+ json[tileHeightName] = _tileHeight.rawValue().toDouble();
+ json[tileWidthName] = _tileWidth.rawValue().toDouble();
json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble();
- json[showTilesName] = _showTiles.rawValue().toBool();
- json[areaTypeName] = WimaMeasurementAreaName;
+ json[showTilesName] = _showTiles.rawValue().toBool();
+ json[areaTypeName] = WimaMeasurementAreaName;
} else {
qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing.";
}
@@ -231,70 +231,70 @@ void WimaMeasurementArea::doUpdate() {
using namespace boost::units;
auto start = std::chrono::high_resolution_clock::now();
- // Check state.
+
if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) {
- const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
- const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
- const auto tileArea = width * height;
- const auto totalArea = this->area() * si::meter * si::meter;
- const auto estNumTiles = totalArea / tileArea;
+ const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
+ const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
+ const auto tileArea = width * height;
+ const auto totalArea = this->area() * si::meter * si::meter;
+ const auto estNumTiles = totalArea / tileArea;
// Check some conditions.
if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
- this->count() >= 3 && this->isSimplePolygon()) {
+ this->count() >= 3 && this->isSimplePolygon()) {
setState(STATE::UPDATEING);
- auto polygon = this->coordinateList();
- for (auto &v : polygon) {
- v.setAltitude(0);
- }
- const auto minArea =
+ auto polygon = this->coordinateList();
+ for (auto &v : polygon) {
+ v.setAltitude(0);
+ }
+ const auto minArea =
this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea;
- auto *th = this->thread();
- auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
- auto start = std::chrono::high_resolution_clock::now();
-
- DataPtr pData(new TileData());
- // Convert to ENU system.
- QGeoCoordinate origin = polygon.first();
- FPolygon polygonENU;
- areaToEnu(origin, polygon, polygonENU);
- std::vector tilesENU;
- BoundingBox bbox;
- std::string errorString;
- // Generate tiles.
- if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
- errorString)) {
- // Convert to geo system.
- for (const auto &t : tilesENU) {
- auto geoTile = new SnakeTile(pData.get());
- for (const auto &v : t.outer()) {
- QGeoCoordinate geoVertex;
- fromENU(origin, v, geoVertex);
- geoTile->push_back(geoVertex);
- }
- pData->tiles.append(geoTile);
- // Calculate center.
- snake::FPoint center;
- snake::polygonCenter(t, center);
- QGeoCoordinate geoCenter;
- fromENU(origin, center, geoCenter);
- pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
+ auto *th = this->thread();
+ auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
+ auto start = std::chrono::high_resolution_clock::now();
+
+ DataPtr pData(new TileData());
+ // Convert to ENU system.
+ QGeoCoordinate origin = polygon.first();
+ FPolygon polygonENU;
+ areaToEnu(origin, polygon, polygonENU);
+ std::vector tilesENU;
+ BoundingBox bbox;
+ std::string errorString;
+ // Generate tiles.
+ if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox,
+ errorString)) {
+ // Convert to geo system.
+ for (const auto &t : tilesENU) {
+ auto geoTile = new SnakeTile(pData.get());
+ for (const auto &v : t.outer()) {
+ QGeoCoordinate geoVertex;
+ fromENU(origin, v, geoVertex);
+ geoTile->push_back(geoVertex);
}
+ pData->tiles.append(geoTile);
+ // Calculate center.
+ snake::FPoint center;
+ snake::polygonCenter(t, center);
+ QGeoCoordinate geoCenter;
+ fromENU(origin, center, geoCenter);
+ pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
}
- pData->moveToThread(th);
+ }
+ pData->moveToThread(th);
- qCDebug(WimaMeasurementAreaLog)
- << "doUpdate(): update time: "
- << std::chrono::duration_cast(
- std::chrono::high_resolution_clock::now() - start)
- .count()
- << " ms";
+ qCDebug(WimaMeasurementAreaLog)
+ << "doUpdate(): update time: "
+ << std::chrono::duration_cast(
+ std::chrono::high_resolution_clock::now() - start)
+ .count()
+ << " ms";
- return pData;
- }); // QtConcurrent::run()
+ return pData;
+ }); // QtConcurrent::run()
- this->_watcher.setFuture(future);
- }
+ this->_watcher.setFuture(future);
+ }
}
qCDebug(WimaMeasurementAreaLog)
<< "doUpdate(): execution time: "
diff --git a/src/Wima/Geometry/WimaMeasurementArea.h b/src/Wima/Geometry/WimaMeasurementArea.h
index 65fcd663fefdcb73f319300c5bc74d4703443cf6..07266032584d19d5dfcdf81560e8bc77e4152fe2 100644
--- a/src/Wima/Geometry/WimaMeasurementArea.h
+++ b/src/Wima/Geometry/WimaMeasurementArea.h
@@ -106,7 +106,7 @@ private:
SettingsFact _showTiles;
QVector _progress;
-
+ // Tile stuff.
// Tile stuff.
mutable QTimer _timer;
using DataPtr = std::shared_ptr;
diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.cc b/src/Wima/Geometry/WimaMeasurementAreaData.cc
index 45a66e540ec56746a534d582427d3a41f4afaaf3..5d0b68fcc3b16aaf18b248600ab6404002e08378 100644
--- a/src/Wima/Geometry/WimaMeasurementAreaData.cc
+++ b/src/Wima/Geometry/WimaMeasurementAreaData.cc
@@ -22,6 +22,7 @@ bool WimaMeasurementAreaData::
operator==(const WimaMeasurementAreaData &other) const {
return this->WimaAreaData::operator==(other) &&
this->_tileData == other.tileData() &&
+ this->_progress == other.progress() &&
this->center() == other.center();
}
@@ -33,6 +34,8 @@ operator!=(const WimaMeasurementAreaData &other) const {
void WimaMeasurementAreaData::setTileData(const TileData &d) {
if (this->_tileData != d) {
this->_tileData = d;
+ this->_progress.fill(0, d.size());
+ emit progressChanged();
emit tileDataChanged();
}
}
@@ -51,7 +54,9 @@ void WimaMeasurementAreaData::setProgress(const QVector &d) {
*/
WimaMeasurementAreaData &WimaMeasurementAreaData::
operator=(const WimaMeasurementAreaData &other) {
- assign(other);
+ WimaAreaData::operator=(other);
+ setTileData(other._tileData);
+ setProgress(other._progress);
return *this;
}
@@ -63,7 +68,14 @@ operator=(const WimaMeasurementAreaData &other) {
*/
WimaMeasurementAreaData &WimaMeasurementAreaData::
operator=(const WimaMeasurementArea &other) {
- assign(other);
+ WimaAreaData::operator=(other);
+ if (other.ready()) {
+ setTileData(other.tileData());
+ setProgress(other.progress());
+ } else {
+ qWarning() << "WimaMeasurementAreaData::operator=(): WimaMeasurementArea "
+ "not ready.";
+ }
return *this;
}
@@ -102,23 +114,6 @@ const QVector &WimaMeasurementAreaData::progress() const {
QVector &WimaMeasurementAreaData::progress() { return this->_progress; }
-void WimaMeasurementAreaData::assign(const WimaMeasurementAreaData &other) {
- WimaAreaData::assign(other);
- setTileData(other._tileData);
- setProgress(other._progress);
-}
-
-void WimaMeasurementAreaData::assign(const WimaMeasurementArea &other) {
- WimaAreaData::assign(other);
- if (other.ready()) {
- setTileData(other.tileData());
- setProgress(other.progress());
- } else {
- qWarning()
- << "WimaMeasurementAreaData::assign(): WimaMeasurementArea not ready.";
- }
-}
-
bool operator==(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2) {
return operator==(*static_cast(&m1),
diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.h b/src/Wima/Geometry/WimaMeasurementAreaData.h
index 68481ffb6d243da305158ceff6e584461993ab54..dde208291fdd5c69080d651e9f828dcdd3852bf7 100644
--- a/src/Wima/Geometry/WimaMeasurementAreaData.h
+++ b/src/Wima/Geometry/WimaMeasurementAreaData.h
@@ -50,10 +50,6 @@ signals:
void tileDataChanged();
void progressChanged();
-protected:
- void assign(const WimaMeasurementAreaData &other);
- void assign(const WimaMeasurementArea &other);
-
private:
TileData _tileData;
QVector _progress;
diff --git a/src/Wima/Geometry/WimaServiceAreaData.cc b/src/Wima/Geometry/WimaServiceAreaData.cc
index 7496e90953d1a88a55a21c7760befb06845dd4b9..ef52635dfe56dab66a887408c2f5315a99cc268f 100644
--- a/src/Wima/Geometry/WimaServiceAreaData.cc
+++ b/src/Wima/Geometry/WimaServiceAreaData.cc
@@ -7,26 +7,22 @@ WimaServiceAreaData::WimaServiceAreaData(QObject *parent)
WimaServiceAreaData::WimaServiceAreaData(const WimaServiceAreaData &other,
QObject *parent)
- : WimaAreaData(parent) {
- *this = other;
-}
+ : WimaAreaData(other, parent), _depot(other._depot) {}
WimaServiceAreaData::WimaServiceAreaData(const WimaServiceArea &other,
QObject *parent)
- : WimaAreaData(parent) {
- *this = other;
-}
+ : WimaAreaData(other, parent), _depot(other.depot()) {}
WimaServiceAreaData &WimaServiceAreaData::
operator=(const WimaServiceAreaData &otherData) {
- this->assign(otherData);
+ WimaAreaData::operator=(otherData);
this->setDepot(otherData.depot());
return *this;
}
WimaServiceAreaData &WimaServiceAreaData::
operator=(const WimaServiceArea &otherArea) {
- this->assign(otherArea);
+ WimaAreaData::operator=(otherArea);
this->setDepot(otherArea.depot());
return *this;
}
@@ -57,16 +53,6 @@ void WimaServiceAreaData::setDepot(const QGeoCoordinate &newCoordinate) {
}
}
-void WimaServiceAreaData::assign(const WimaServiceAreaData &other) {
- WimaAreaData::assign(other);
- setDepot(other.depot());
-}
-
-void WimaServiceAreaData::assign(const WimaServiceArea &other) {
- WimaAreaData::assign(other);
- setDepot(other.depot());
-}
-
/*!
* \class WimaAreaData::WimaServiceAreaData
* \brief Class to store and exchange data of a \c WimaServiceArea Object.
diff --git a/src/Wima/Geometry/WimaServiceAreaData.h b/src/Wima/Geometry/WimaServiceAreaData.h
index 08f616e4bbdd14e0558898519fb08cb200a7db6d..2b93a20964008977061f992314174feee299d408 100644
--- a/src/Wima/Geometry/WimaServiceAreaData.h
+++ b/src/Wima/Geometry/WimaServiceAreaData.h
@@ -31,10 +31,6 @@ signals:
public slots:
void setDepot(const QGeoCoordinate &newCoordinate);
-protected:
- void assign(const WimaServiceAreaData &other);
- void assign(const WimaServiceArea &other);
-
private:
// see WimaServieArea.h for explanation
QGeoCoordinate _depot;
diff --git a/src/Wima/RoutingThread.cpp b/src/Wima/RoutingThread.cpp
index 20f05f51619d8c6107fa5a877baa6f6ea4b8a32e..f45bb3c0349231b9e91a4203ef6c38a7ad95a847 100644
--- a/src/Wima/RoutingThread.cpp
+++ b/src/Wima/RoutingThread.cpp
@@ -5,7 +5,7 @@
#include
#include "QGCLoggingCategory.h"
-QGC_LOGGING_CATEGORY(RoutingWorkerLog, "RoutingWorkerLog")
+QGC_LOGGING_CATEGORY(RoutingThreadLog, "RoutingThreadLog")
RoutingThread::RoutingThread(QObject *parent)
: QThread(parent), _calculating(false), _stop(false), _restart(false) {
@@ -44,10 +44,10 @@ void RoutingThread::route(const RoutingParameter &par,
}
void RoutingThread::run() {
- qCWarning(RoutingWorkerLog) << "run(): thread start.";
+ qCDebug(RoutingThreadLog) << "run(): thread start.";
while (!this->_stop) {
- qCWarning(RoutingWorkerLog) << "run(): calculation "
- "started.";
+ qCDebug(RoutingThreadLog) << "run(): calculation "
+ "started.";
// Copy input.
auto start = std::chrono::high_resolution_clock::now();
@@ -59,7 +59,7 @@ void RoutingThread::run() {
lk.unlock();
auto safeAreaENU = par.safeArea;
auto numRuns = par.numRuns;
- auto numSolutionsPerRun = par.numSolutionsPerRun;
+ auto numSolutionsPerRun = par.numSolutions;
PtrRoutingData pRouteData(new RoutingData());
auto &transectsENU = pRouteData->transects;
@@ -67,8 +67,8 @@ void RoutingThread::run() {
if (generator(transectsENU)) {
// Check if generation was successful.
if (transectsENU.size() == 0) {
- qCWarning(RoutingWorkerLog) << "run(): "
- "not able to generate transects.";
+ qCDebug(RoutingThreadLog) << "run(): "
+ "not able to generate transects.";
} else {
// Prepare data for routing.
auto &solutionVector = pRouteData->solutionVector;
@@ -93,24 +93,24 @@ void RoutingThread::run() {
// Check if routing was successful.
if ((!success || solutionVector.size() < 1) && !this->_restart) {
- qCWarning(RoutingWorkerLog) << "run(): "
- "routing failed. "
- << snakePar.errorString.c_str();
+ qCDebug(RoutingThreadLog) << "run(): "
+ "routing failed. "
+ << snakePar.errorString.c_str();
} else if (this->_restart) {
- qCWarning(RoutingWorkerLog) << "run(): "
- "restart requested.";
+ qCDebug(RoutingThreadLog) << "run(): "
+ "restart requested.";
} else {
// Notify main thread.
emit result(pRouteData);
- qCWarning(RoutingWorkerLog) << "run(): "
- "concurrent update success.";
+ qCDebug(RoutingThreadLog) << "run(): "
+ "concurrent update success.";
}
}
} // end calculation
else {
- qCWarning(RoutingWorkerLog) << "run(): generator() failed.";
+ qCDebug(RoutingThreadLog) << "run(): generator() failed.";
}
- qCWarning(RoutingWorkerLog)
+ qCDebug(RoutingThreadLog)
<< "run(): execution time: "
<< std::chrono::duration_cast(
std::chrono::high_resolution_clock::now() - start)
@@ -125,5 +125,5 @@ void RoutingThread::run() {
}
this->_restart = false;
} // main loop
- qCWarning(RoutingWorkerLog) << "run(): thread end.";
+ qCDebug(RoutingThreadLog) << "run(): thread end.";
}
diff --git a/src/Wima/RoutingThread.h b/src/Wima/RoutingThread.h
index 46ba2eb757aff864e10ad89c1bf4452e5cdfb41b..bc5a3f11389c15e847db2438e26abe9c316adc93 100644
--- a/src/Wima/RoutingThread.h
+++ b/src/Wima/RoutingThread.h
@@ -17,9 +17,9 @@ struct RoutingData {
};
struct RoutingParameter {
- RoutingParameter() : numSolutionsPerRun(1), numRuns(1) {}
+ RoutingParameter() : numSolutions(1), numRuns(1) {}
snake::FPolygon safeArea;
- std::size_t numSolutionsPerRun;
+ std::size_t numSolutions;
std::size_t numRuns;
};
//!
@@ -31,7 +31,7 @@ class RoutingThread : public QThread {
using Lock = std::unique_lock;
public:
- using PtrRoutingData = QSharedPointer;
+ using PtrRoutingData = shared_ptr;
using Generator = std::function;
using Consumer = std::function;
diff --git a/src/Wima/Snake/CircularGenerator.cpp b/src/Wima/Snake/CircularGenerator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..129e0e8e681858f1030517dd85cc8008a107f48e
--- /dev/null
+++ b/src/Wima/Snake/CircularGenerator.cpp
@@ -0,0 +1,463 @@
+#include "CircularGenerator.h"
+
+#include "QGCLoggingCategory.h"
+QGC_LOGGING_CATEGORY(CircularGeneratorLog, "CircularGeneratorLog")
+
+#define CLIPPER_SCALE 1000000
+#include "Wima/Geometry/GenericCircle.h"
+#include "clipper/clipper.hpp"
+
+using namespace ClipperLib;
+template <> inline auto get<0>(const IntPoint &p) { return p.X; }
+template <> inline auto get<1>(const IntPoint &p) { return p.Y; }
+
+#include "SnakeTile.h"
+#include "Wima/RoutingThread.h"
+
+namespace routing {
+
+bool circularTransects(const snake::FPoint &reference,
+ const snake::FPolygon &polygon,
+ const std::vector &tiles,
+ snake::Length deltaR, snake::Angle deltaAlpha,
+ snake::Length minLength, snake::Transects &transects);
+
+const char *CircularGenerator::settingsGroup = "CircularGenerator";
+const char *CircularGenerator::distanceName = "TransectDistance";
+const char *CircularGenerator::deltaAlphaName = "DeltaAlpha";
+const char *CircularGenerator::minLengthName = "MinLength";
+const char *CircularGenerator::refPointLatitudeName = "ReferencePointLat";
+const char *CircularGenerator::refPointLongitudeName = "ReferencePointLong";
+const char *CircularGenerator::refPointAltitudeName = "ReferencePointAlt";
+
+CircularGenerator::CircularGenerator(QObject *parent)
+ : CircularGenerator(nullptr, parent) {}
+
+CircularGenerator::CircularGenerator(GeneratorBase::Data d, QObject *parent)
+ : GeneratorBase(d, parent), _connectionsEstablished(false),
+ _metaDataMap(FactMetaData::createMapFromJsonFile(
+ QStringLiteral(":/json/CircularGenerator.SettingsGroup.json"), this)),
+ _distance(settingsGroup, _metaDataMap[distanceName]),
+ _deltaAlpha(settingsGroup, _metaDataMap[deltaAlphaName]),
+ _minLength(settingsGroup, _metaDataMap[minLengthName]) {
+ establishConnections();
+}
+
+QString CircularGenerator::editorQml() {
+ return QStringLiteral("CircularGeneratorEditor.qml");
+}
+
+QString CircularGenerator::mapVisualQml() {
+ return QStringLiteral("CircularGeneratorMapVisual.qml");
+}
+
+QString CircularGenerator::name() {
+ return QStringLiteral("Circular Generator");
+}
+
+QString CircularGenerator::abbreviation() { return QStringLiteral("C. Gen."); }
+
+bool CircularGenerator::get(Generator &generator) {
+ if (this->_d) {
+ if (this->_d->isValid()) {
+ // Prepare data.
+ auto origin = this->_d->origin();
+ origin.setAltitude(0);
+ if (!origin.isValid()) {
+ qCDebug(CircularGeneratorLog) << "get(): origin invalid." << origin;
+ return false;
+ }
+
+ auto ref = this->_reference;
+ ref.setAltitude(0);
+ if (!ref.isValid()) {
+ qCDebug(CircularGeneratorLog) << "get(): reference invalid." << ref;
+ return false;
+ }
+ snake::FPoint reference;
+ snake::toENU(origin, ref, reference);
+
+ auto geoPolygon = this->_d->measurementArea().coordinateList();
+ for (auto &v : geoPolygon) {
+ if (v.isValid()) {
+ v.setAltitude(0);
+ } else {
+ qCDebug(CircularGeneratorLog) << "get(): measurement area invalid.";
+ for (const auto &w : geoPolygon) {
+ qCDebug(CircularGeneratorLog) << w;
+ }
+ return false;
+ }
+ }
+ auto pPolygon = std::make_shared();
+ snake::areaToEnu(origin, geoPolygon, *pPolygon);
+
+ // Progress and tiles.
+ const auto &progress = this->_d->measurementArea().progress();
+ const auto *tiles = this->_d->measurementArea().tiles();
+ auto pTiles = std::make_shared>();
+ if (progress.size() == tiles->count()) {
+ for (int i = 0; i < tiles->count(); ++i) {
+ if (progress[i] == 100) {
+ const auto *tile = tiles->value(i);
+ if (tile != nullptr) {
+ snake::FPolygon tileENU;
+ snake::areaToEnu(origin, tile->coordinateList(), tileENU);
+ pTiles->push_back(std::move(tileENU));
+ } else {
+ qCDebug(CircularGeneratorLog)
+ << "get(): progress.size() != tiles->count().";
+ return false;
+ }
+ }
+ }
+ } else {
+ qCDebug(CircularGeneratorLog)
+ << "get(): progress.size() != tiles->count().";
+ return false;
+ }
+
+ auto geoDepot = this->_d->serviceArea().depot();
+ if (!geoDepot.isValid()) {
+ qCDebug(CircularGeneratorLog) << "get(): depot invalid." << geoDepot;
+ return false;
+ }
+ snake::FPoint depot;
+ snake::toENU(origin, geoDepot, depot);
+
+ // Fetch transect parameter.
+ auto distance =
+ snake::Length(this->_distance.rawValue().toDouble() * bu::si::meter);
+ auto minLength =
+ snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
+ auto alpha = snake::Angle(this->_deltaAlpha.rawValue().toDouble() *
+ bu::degree::degree);
+
+ generator = [reference, depot, pPolygon, pTiles, distance, alpha,
+ minLength](snake::Transects &transects) -> bool {
+ bool value = circularTransects(reference, *pPolygon, *pTiles, distance,
+ alpha, minLength, transects);
+ transects.insert(transects.begin(), snake::FLineString{depot});
+ return value;
+ };
+ return true;
+ } else {
+ qCDebug(CircularGeneratorLog) << "get(): data invalid.";
+ return false;
+ }
+ } else {
+ qCDebug(CircularGeneratorLog) << "get(): data member not set.";
+ return false;
+ }
+}
+
+QGeoCoordinate CircularGenerator::reference() const { return _reference; }
+
+void CircularGenerator::setReference(const QGeoCoordinate &reference) {
+ if (_reference != reference) {
+ _reference = reference;
+ emit referenceChanged();
+ }
+}
+
+void CircularGenerator::resetReferenceIfInvalid() {
+ if (!this->_reference.isValid()) {
+ resetReference();
+ }
+}
+
+void CircularGenerator::resetReference() {
+ if (this->_d->measurementArea().center().isValid()) {
+ setReference(this->_d->measurementArea().center());
+ } else {
+ qCWarning(CircularGeneratorLog)
+ << "measurement area center" << this->_d->measurementArea().center();
+ }
+}
+
+void CircularGenerator::establishConnections() {
+ if (this->_d && !this->_connectionsEstablished) {
+ connect(this->_d.get(), &WimaPlanData::originChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::progressChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::tileDataChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::centerChanged, this,
+ &CircularGenerator::resetReferenceIfInvalid);
+ connect(&this->_d->measurementArea(), &WimaMeasurementAreaData::pathChanged,
+ this, &GeneratorBase::generatorChanged);
+ connect(&this->_d->serviceArea(), &WimaServiceAreaData::depotChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->joinedArea(), &WimaJoinedAreaData::pathChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->distance(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->deltaAlpha(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->minLength(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this, &CircularGenerator::referenceChanged, this,
+ &GeneratorBase::generatorChanged);
+ this->_connectionsEstablished = true;
+ }
+}
+
+void CircularGenerator::deleteConnections() {
+ if (this->_d && this->_connectionsEstablished) {
+ disconnect(this->_d.get(), &WimaPlanData::originChanged, this,
+ &GeneratorBase::generatorChanged);
+ disconnect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::progressChanged, this,
+ &GeneratorBase::generatorChanged);
+ disconnect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::tileDataChanged, this,
+ &GeneratorBase::generatorChanged);
+ disconnect(&this->_d->measurementArea(), &WimaMeasurementAreaData::center,
+ this, &CircularGenerator::resetReferenceIfInvalid);
+ disconnect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::pathChanged, this,
+ &GeneratorBase::generatorChanged);
+ disconnect(&this->_d->serviceArea(), &WimaServiceAreaData::depotChanged,
+ this, &GeneratorBase::generatorChanged);
+ disconnect(&this->_d->joinedArea(), &WimaJoinedAreaData::pathChanged, this,
+ &GeneratorBase::generatorChanged);
+ disconnect(this->distance(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ disconnect(this->deltaAlpha(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ disconnect(this->minLength(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ disconnect(this, &CircularGenerator::referenceChanged, this,
+ &GeneratorBase::generatorChanged);
+ this->_connectionsEstablished = false;
+ }
+}
+
+Fact *CircularGenerator::distance() { return &_distance; }
+
+Fact *CircularGenerator::deltaAlpha() { return &_deltaAlpha; }
+
+Fact *CircularGenerator::minLength() { return &_minLength; }
+
+bool circularTransects(const snake::FPoint &reference,
+ const snake::FPolygon &polygon,
+ const std::vector &tiles,
+ snake::Length deltaR, snake::Angle deltaAlpha,
+ snake::Length minLength, snake::Transects &transects) {
+ auto s1 = std::chrono::high_resolution_clock::now();
+
+ // Check preconitions
+ if (polygon.outer().size() >= 3) {
+ using namespace boost::units;
+ // Convert geo polygon to ENU polygon.
+ std::string error;
+ // Check validity.
+ if (!bg::is_valid(polygon, error)) {
+ qCDebug(CircularGeneratorLog) << "circularTransects(): "
+ "invalid polygon.";
+ qCDebug(CircularGeneratorLog) << error.c_str();
+ std::stringstream ss;
+ ss << bg::wkt(polygon);
+ qCDebug(CircularGeneratorLog) << ss.str().c_str();
+ } else {
+ // Calculate polygon distances and angles.
+ std::vector distances;
+ distances.reserve(polygon.outer().size());
+ std::vector angles;
+ angles.reserve(polygon.outer().size());
+ //#ifdef DEBUG_CIRCULAR_SURVEY
+ // qCDebug(CircularGeneratorLog) << "circularTransects():";
+ //#endif
+ for (const auto &p : polygon.outer()) {
+ snake::Length distance = bg::distance(reference, p) * si::meter;
+ distances.push_back(distance);
+ snake::Angle alpha = (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
+ alpha = alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
+ angles.push_back(alpha);
+ //#ifdef DEBUG_CIRCULAR_SURVEY
+ // qCDebug(CircularGeneratorLog) << "distances, angles,
+ // coordinates:"; qCDebug(CircularGeneratorLog) <<
+ // to_string(distance).c_str(); qCDebug(CircularGeneratorLog)
+ // << to_string(snake::Degree(alpha)).c_str();
+ // qCDebug(CircularGeneratorLog) << "x = " << p.get<0>() << "y
+ // = "
+ // << p.get<1>();
+ //#endif
+ }
+
+ auto rMin = deltaR; // minimal circle radius
+ snake::Angle alpha1(0 * degree::degree);
+ snake::Angle alpha2(360 * degree::degree);
+ // Determine r_min by successive approximation
+ if (!bg::within(reference, polygon.outer())) {
+ rMin = bg::distance(reference, polygon) * si::meter;
+ }
+
+ auto rMax = (*std::max_element(distances.begin(),
+ distances.end())); // maximal circle radius
+
+ // Scale parameters and coordinates.
+ const auto rMinScaled =
+ ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
+ const auto deltaRScaled =
+ ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
+ auto referenceScaled = ClipperLib::IntPoint{
+ ClipperLib::cInt(std::round(reference.get<0>() * CLIPPER_SCALE)),
+ ClipperLib::cInt(std::round(reference.get<1>() * CLIPPER_SCALE))};
+
+ // Generate circle sectors.
+ auto rScaled = rMinScaled;
+ const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
+ vector sectors(nTran, ClipperLib::Path());
+ const auto nSectors =
+ long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
+ //#ifdef DEBUG_CIRCULAR_SURVEY
+ // qCDebug(CircularGeneratorLog) << "circularTransects(): sector
+ // parameres:"; qCDebug(CircularGeneratorLog) << "alpha1: " <<
+ // to_string(snake::Degree(alpha1)).c_str();
+ // qCDebug(CircularGeneratorLog) << "alpha2:
+ // "
+ // << to_string(snake::Degree(alpha2)).c_str();
+ // qCDebug(CircularGeneratorLog) << "n: "
+ // << to_string((alpha2 - alpha1) / deltaAlpha).c_str();
+ // qCDebug(CircularGeneratorLog)
+ // << "nSectors: " << nSectors; qCDebug(CircularGeneratorLog) <<
+ // "rMin: " << to_string(rMin).c_str();
+ // qCDebug(CircularGeneratorLog)
+ // << "rMax: " << to_string(rMax).c_str();
+ // qCDebug(CircularGeneratorLog) << "nTran: " << nTran;
+ //#endif
+ using ClipperCircle =
+ GenericCircle;
+ for (auto §or : sectors) {
+ ClipperCircle circle(rScaled, referenceScaled);
+ approximate(circle, nSectors, sector);
+ rScaled += deltaRScaled;
+ }
+ // Clip sectors to polygonENU.
+ ClipperLib::Path polygonClipper;
+ snake::FPolygon shrinked;
+ snake::offsetPolygon(polygon, shrinked, -0.3);
+ auto &outer = shrinked.outer();
+ 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));
+ polygonClipper.push_back(ClipperLib::IntPoint{x, y});
+ }
+ ClipperLib::Clipper clipper;
+ clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
+ clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
+ ClipperLib::PolyTree transectsClipper;
+ clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
+ ClipperLib::pftNonZero, ClipperLib::pftNonZero);
+
+ // Subtract holes.
+ if (tiles.size() > 0) {
+ vector processedTiles;
+ for (const auto &tile : tiles) {
+ ClipperLib::Path path;
+ for (const auto &v : tile.outer()) {
+ path.push_back(ClipperLib::IntPoint{
+ static_cast(v.get<0>() * CLIPPER_SCALE),
+ static_cast(v.get<1>() * CLIPPER_SCALE)});
+ }
+ processedTiles.push_back(std::move(path));
+ }
+
+ clipper.Clear();
+ for (const auto &child : transectsClipper.Childs) {
+ clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
+ }
+ clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
+ transectsClipper.Clear();
+ clipper.Execute(ClipperLib::ctDifference, transectsClipper,
+ ClipperLib::pftNonZero, ClipperLib::pftNonZero);
+ }
+
+ // Extract transects from PolyTree and convert them to
+ // BoostLineString
+ for (const auto &child : transectsClipper.Childs) {
+ snake::FLineString transect;
+ transect.reserve(child->Contour.size());
+ for (const auto &vertex : child->Contour) {
+ auto x = static_cast(vertex.X) / CLIPPER_SCALE;
+ auto y = static_cast(vertex.Y) / CLIPPER_SCALE;
+ transect.push_back(snake::FPoint(x, y));
+ }
+ transects.push_back(transect);
+ }
+
+ // Join sectors which where slit due to clipping.
+ const double th = 0.01;
+ for (auto ito = transects.begin(); ito < transects.end(); ++ito) {
+ for (auto iti = ito + 1; iti < transects.end(); ++iti) {
+ auto dist1 = bg::distance(ito->front(), iti->front());
+ if (dist1 < th) {
+ snake::FLineString temp;
+ for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
+ temp.push_back(*it);
+ }
+ temp.insert(temp.end(), ito->begin(), ito->end());
+ *ito = temp;
+ transects.erase(iti);
+ break;
+ }
+ auto dist2 = bg::distance(ito->front(), iti->back());
+ if (dist2 < th) {
+ snake::FLineString temp;
+ temp.insert(temp.end(), iti->begin(), iti->end());
+ temp.insert(temp.end(), ito->begin(), ito->end());
+ *ito = temp;
+ transects.erase(iti);
+ break;
+ }
+ auto dist3 = bg::distance(ito->back(), iti->front());
+ if (dist3 < th) {
+ snake::FLineString temp;
+ temp.insert(temp.end(), ito->begin(), ito->end());
+ temp.insert(temp.end(), iti->begin(), iti->end());
+ *ito = temp;
+ transects.erase(iti);
+ break;
+ }
+ auto dist4 = bg::distance(ito->back(), iti->back());
+ if (dist4 < th) {
+ snake::FLineString temp;
+ temp.insert(temp.end(), ito->begin(), ito->end());
+ for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
+ temp.push_back(*it);
+ }
+ *ito = temp;
+ transects.erase(iti);
+ break;
+ }
+ }
+ }
+
+ // Remove short transects
+ for (auto it = transects.begin(); it < transects.end();) {
+ if (bg::length(*it) < minLength.value()) {
+ it = transects.erase(it);
+ } else {
+ ++it;
+ }
+ }
+
+ qCDebug(CircularGeneratorLog)
+ << "circularTransects(): transect gen. time: "
+ << std::chrono::duration_cast(
+ std::chrono::high_resolution_clock::now() - s1)
+ .count()
+ << " ms";
+ return true;
+ }
+ }
+ return false;
+}
+
+} // namespace routing
diff --git a/src/Wima/Snake/CircularGenerator.h b/src/Wima/Snake/CircularGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..d4458a2292ea9b90317d2be338f0848b6fe56503
--- /dev/null
+++ b/src/Wima/Snake/CircularGenerator.h
@@ -0,0 +1,63 @@
+#include "GeneratorBase.h"
+
+#include
+
+namespace routing {
+
+class CircularGenerator : public GeneratorBase {
+ Q_OBJECT
+public:
+ CircularGenerator(QObject *parent = nullptr);
+ CircularGenerator(Data d, QObject *parent = nullptr);
+
+ Q_PROPERTY(QGeoCoordinate reference READ reference WRITE setReference NOTIFY
+ referenceChanged)
+ Q_PROPERTY(Fact *distance READ distance CONSTANT)
+ Q_PROPERTY(Fact *deltaAlpha READ deltaAlpha CONSTANT)
+ Q_PROPERTY(Fact *minLength READ minLength CONSTANT)
+
+ virtual QString editorQml() override;
+ virtual QString mapVisualQml() override;
+
+ virtual QString name() override;
+ virtual QString abbreviation() override;
+
+ virtual bool get(Generator &generator) override;
+
+ QGeoCoordinate reference() const;
+ Fact *distance();
+ Fact *deltaAlpha();
+ Fact *minLength();
+
+ void setReference(const QGeoCoordinate &reference);
+
+ static const char *settingsGroup;
+ static const char *distanceName;
+ static const char *deltaAlphaName;
+ static const char *minLengthName;
+ static const char *refPointLongitudeName;
+ static const char *refPointLatitudeName;
+ static const char *refPointAltitudeName;
+
+signals:
+ void referenceChanged();
+
+public slots:
+ Q_INVOKABLE void resetReferenceIfInvalid();
+ Q_INVOKABLE void resetReference();
+
+protected:
+ virtual void establishConnections() override;
+ virtual void deleteConnections() override;
+
+private:
+ bool _connectionsEstablished;
+
+ QGeoCoordinate _reference;
+ QMap _metaDataMap;
+ SettingsFact _distance;
+ SettingsFact _deltaAlpha;
+ SettingsFact _minLength;
+};
+
+} // namespace routing
diff --git a/src/Wima/Snake/GeneratorBase.cc b/src/Wima/Snake/GeneratorBase.cc
new file mode 100644
index 0000000000000000000000000000000000000000..bc315e54b9bd9319d433208f20a75bf29a53c5bd
--- /dev/null
+++ b/src/Wima/Snake/GeneratorBase.cc
@@ -0,0 +1,31 @@
+#include "GeneratorBase.h"
+
+namespace routing {
+
+GeneratorBase::GeneratorBase(QObject *parent)
+ : GeneratorBase(nullptr, parent) {}
+
+GeneratorBase::GeneratorBase(GeneratorBase::Data d, QObject *parent)
+ : QObject(parent), _d(d) {
+ establishConnections();
+}
+
+GeneratorBase::~GeneratorBase() {}
+
+QString GeneratorBase::editorQml() { return QStringLiteral(""); }
+
+QString GeneratorBase::mapVisualQml() { return QStringLiteral(""); }
+
+GeneratorBase::Data GeneratorBase::data() const { return _d; }
+
+void GeneratorBase::setData(const Data &d) {
+ deleteConnections();
+ _d = d;
+ establishConnections();
+}
+
+void GeneratorBase::establishConnections() {}
+
+void GeneratorBase::deleteConnections() {}
+
+} // namespace routing
diff --git a/src/Wima/Snake/GeneratorBase.h b/src/Wima/Snake/GeneratorBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..89fd78fef245107e59d1e7db948ed7b1954fa532
--- /dev/null
+++ b/src/Wima/Snake/GeneratorBase.h
@@ -0,0 +1,47 @@
+#pragma once
+
+#include
+
+#include
+#include
+
+#include "snake.h"
+
+#include "Wima/WimaPlanData.h"
+
+namespace routing {
+
+class GeneratorBase : public QObject {
+ Q_OBJECT
+public:
+ using Data = std::shared_ptr;
+ using Generator = std::function;
+
+ explicit GeneratorBase(QObject *parent = nullptr);
+ explicit GeneratorBase(Data d, QObject *parent = nullptr);
+ ~GeneratorBase();
+
+ Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
+ Q_PROPERTY(QString mapVisualQml READ mapVisualQml CONSTANT)
+
+ virtual QString editorQml();
+ virtual QString mapVisualQml();
+
+ virtual QString name() = 0;
+ virtual QString abbreviation() = 0;
+
+ virtual bool get(Generator &generator) = 0;
+
+ Data data() const;
+ void setData(const Data &d);
+
+signals:
+ void generatorChanged();
+
+protected:
+ virtual void establishConnections();
+ virtual void deleteConnections();
+ Data _d;
+};
+
+} // namespace routing
diff --git a/src/Wima/Snake/LinearGenerator.cpp b/src/Wima/Snake/LinearGenerator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4a81934750c4eb232f4a05fad2f7cadc164fba2b
--- /dev/null
+++ b/src/Wima/Snake/LinearGenerator.cpp
@@ -0,0 +1,334 @@
+#include "LinearGenerator.h"
+
+#include "QGCLoggingCategory.h"
+QGC_LOGGING_CATEGORY(LinearGeneratorLog, "LinearGeneratorLog")
+
+#define CLIPPER_SCALE 1000000
+#include "clipper/clipper.hpp"
+
+#include "SnakeTile.h"
+#include "Wima/RoutingThread.h"
+
+namespace routing {
+
+bool linearTransects(const snake::FPolygon &polygon,
+ const std::vector &tiles,
+ snake::Length distance, snake::Angle angle,
+ snake::Length minLength, snake::Transects &transects);
+
+const char *LinearGenerator::settingsGroup = "LinearGenerator";
+const char *LinearGenerator::distanceName = "TransectDistance";
+const char *LinearGenerator::alphaName = "Alpha";
+const char *LinearGenerator::minLengthName = "MinLength";
+
+LinearGenerator::LinearGenerator(QObject *parent)
+ : LinearGenerator(nullptr, parent) {}
+
+LinearGenerator::LinearGenerator(GeneratorBase::Data d, QObject *parent)
+ : GeneratorBase(d, parent),
+ _metaDataMap(FactMetaData::createMapFromJsonFile(
+ QStringLiteral(":/json/LinearGenerator.SettingsGroup.json"), this)),
+ _distance(settingsGroup, _metaDataMap[distanceName]),
+ _alpha(settingsGroup, _metaDataMap[alphaName]),
+ _minLength(settingsGroup, _metaDataMap[minLengthName]) {
+ establishConnections();
+}
+
+QString LinearGenerator::editorQml() {
+ return QStringLiteral("LinearGeneratorEditor.qml");
+}
+
+QString LinearGenerator::name() { return QStringLiteral("Linear Generator"); }
+
+QString LinearGenerator::abbreviation() { return QStringLiteral("L. Gen."); }
+
+bool LinearGenerator::get(Generator &generator) {
+ if (_d) {
+ if (this->_d->isValid()) {
+ // Prepare data.
+ auto origin = this->_d->origin();
+ origin.setAltitude(0);
+ if (!origin.isValid()) {
+ qCDebug(LinearGeneratorLog) << "get(): origin invalid." << origin;
+ }
+
+ auto geoPolygon = this->_d->measurementArea().coordinateList();
+ for (auto &v : geoPolygon) {
+ if (v.isValid()) {
+ v.setAltitude(0);
+ } else {
+ qCDebug(LinearGeneratorLog) << "get(): measurement area invalid.";
+ for (const auto &w : geoPolygon) {
+ qCDebug(LinearGeneratorLog) << w;
+ }
+ return false;
+ }
+ }
+ auto pPolygon = std::make_shared();
+ snake::areaToEnu(origin, geoPolygon, *pPolygon);
+
+ // Progress and tiles.
+ const auto &progress = this->_d->measurementArea().progress();
+ const auto *tiles = this->_d->measurementArea().tiles();
+ auto pTiles = std::make_shared>();
+ if (progress.size() == tiles->count()) {
+ for (int i = 0; i < tiles->count(); ++i) {
+ if (progress[i] == 100) {
+ const auto *tile = tiles->value(i);
+ if (tile != nullptr) {
+ snake::FPolygon tileENU;
+ snake::areaToEnu(origin, tile->coordinateList(), tileENU);
+ pTiles->push_back(std::move(tileENU));
+ } else {
+ qCDebug(LinearGeneratorLog) << "get(): tile == nullptr";
+ return false;
+ }
+ }
+ }
+ } else {
+ qCDebug(LinearGeneratorLog)
+ << "get(): progress.size() != tiles->count().";
+ return false;
+ }
+
+ auto geoDepot = this->_d->serviceArea().depot();
+ if (!geoDepot.isValid()) {
+ qCDebug(LinearGeneratorLog) << "get(): depot invalid." << geoDepot;
+ return false;
+ }
+ snake::FPoint depot;
+ snake::toENU(origin, geoDepot, depot);
+
+ // Fetch transect parameter.
+ auto distance =
+ snake::Length(this->_distance.rawValue().toDouble() * bu::si::meter);
+ auto minLength =
+ snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
+ auto alpha =
+ snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);
+ generator = [depot, pPolygon, pTiles, distance, alpha,
+ minLength](snake::Transects &transects) -> bool {
+ bool value = linearTransects(*pPolygon, *pTiles, distance, alpha,
+ minLength, transects);
+ transects.insert(transects.begin(), snake::FLineString{depot});
+ return value;
+ };
+ return true;
+ } else {
+ qCDebug(LinearGeneratorLog) << "get(): data invalid.";
+ return false;
+ }
+ } else {
+ qCDebug(LinearGeneratorLog) << "get(): data member not set.";
+ return false;
+ }
+}
+
+Fact *LinearGenerator::distance() { return &_distance; }
+
+Fact *LinearGenerator::alpha() { return &_alpha; }
+
+Fact *LinearGenerator::minLength() { return &_minLength; }
+
+void LinearGenerator::establishConnections() {
+ if (this->_d && !this->_connectionsEstablished) {
+ connect(this->_d.get(), &WimaPlanData::originChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::progressChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::tileDataChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(), &WimaMeasurementAreaData::pathChanged,
+ this, &GeneratorBase::generatorChanged);
+ connect(&this->_d->serviceArea(), &WimaServiceAreaData::depotChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->joinedArea(), &WimaJoinedAreaData::pathChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->distance(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->alpha(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->minLength(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ this->_connectionsEstablished = true;
+ }
+}
+
+void LinearGenerator::deleteConnections() {
+ if (this->_d && this->_connectionsEstablished) {
+ connect(this->_d.get(), &WimaPlanData::originChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::progressChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(),
+ &WimaMeasurementAreaData::tileDataChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->measurementArea(), &WimaMeasurementAreaData::pathChanged,
+ this, &GeneratorBase::generatorChanged);
+ connect(&this->_d->serviceArea(), &WimaServiceAreaData::depotChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(&this->_d->joinedArea(), &WimaJoinedAreaData::pathChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->distance(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->alpha(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ connect(this->minLength(), &Fact::rawValueChanged, this,
+ &GeneratorBase::generatorChanged);
+ this->_connectionsEstablished = false;
+ }
+}
+
+bool linearTransects(const snake::FPolygon &polygon,
+ const std::vector &tiles,
+ snake::Length distance, snake::Angle angle,
+ snake::Length minLength, snake::Transects &transects) {
+ namespace tr = bg::strategy::transform;
+ auto s1 = std::chrono::high_resolution_clock::now();
+
+ // Check preconitions
+ if (polygon.outer().size() >= 3) {
+ // Convert to ENU system.
+ std::string error;
+ // Check validity.
+ if (!bg::is_valid(polygon, error)) {
+ std::stringstream ss;
+ ss << bg::wkt(polygon);
+
+ qCDebug(LinearGeneratorLog) << "linearTransects(): "
+ "invalid polygon. "
+ << error.c_str() << ss.str().c_str();
+ } else {
+ tr::rotate_transformer rotate(angle.value() *
+ 180 / M_PI);
+ // Rotate polygon by angle and calculate bounding box.
+ snake::FPolygon polygonENURotated;
+ bg::transform(polygon.outer(), polygonENURotated.outer(), rotate);
+ snake::FBox 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::FPoint v1{x0, y0 + i * distance.value()};
+ snake::FPoint v2{x1, y0 + i * distance.value()};
+ snake::FLineString transect;
+ transect.push_back(v1);
+ transect.push_back(v2);
+ // transform back
+ snake::FLineString 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;
+ qCDebug(LinearGeneratorLog)
+ << "linearTransects(): " << ss.str().c_str();
+ return false;
+ }
+
+ // Convert measurement area to clipper path.
+ snake::FPolygon shrinked;
+ snake::offsetPolygon(polygon, 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);
+
+ // Subtract holes.
+ if (tiles.size() > 0) {
+ vector processedTiles;
+ for (const auto &tile : tiles) {
+ ClipperLib::Path path;
+ for (const auto &v : tile.outer()) {
+ path.push_back(ClipperLib::IntPoint{
+ static_cast(v.get<0>() * CLIPPER_SCALE),
+ static_cast(v.get<1>() * CLIPPER_SCALE)});
+ }
+ processedTiles.push_back(std::move(path));
+ }
+
+ clipper.Clear();
+ for (const auto &child : clippedTransecs.Childs) {
+ clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
+ }
+ clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
+ clippedTransecs.Clear();
+ clipper.Execute(ClipperLib::ctDifference, clippedTransecs,
+ ClipperLib::pftNonZero, ClipperLib::pftNonZero);
+ }
+
+ // Extract transects from PolyTree and convert them to BoostLineString
+ for (const auto &child : clippedTransecs.Childs) {
+ const auto &clipperTransect = child->Contour;
+ snake::FPoint v1{
+ static_cast(clipperTransect[0].X) / CLIPPER_SCALE,
+ static_cast(clipperTransect[0].Y) / CLIPPER_SCALE};
+ snake::FPoint v2{
+ static_cast(clipperTransect[1].X) / CLIPPER_SCALE,
+ static_cast(clipperTransect[1].Y) / CLIPPER_SCALE};
+
+ snake::FLineString transect{v1, v2};
+ if (bg::length(transect) >= minLength.value()) {
+ transects.push_back(transect);
+ }
+ }
+
+ if (transects.size() == 0) {
+ std::stringstream ss;
+ ss << "Not able to generatetransects. Parameter: minLength = "
+ << minLength << std::endl;
+ qCDebug(LinearGeneratorLog)
+ << "linearTransects(): " << ss.str().c_str();
+ return false;
+ }
+
+ qCDebug(LinearGeneratorLog)
+ << "linearTransects(): time: "
+ << std::chrono::duration_cast(
+ std::chrono::high_resolution_clock::now() - s1)
+ .count()
+ << " ms";
+ return true;
+ }
+ }
+ return false;
+}
+} // namespace routing
diff --git a/src/Wima/Snake/LinearGenerator.h b/src/Wima/Snake/LinearGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..ba63cf51a322f7c6e3d0b3cdc30ae3a6b5c6183e
--- /dev/null
+++ b/src/Wima/Snake/LinearGenerator.h
@@ -0,0 +1,46 @@
+#include "GeneratorBase.h"
+
+#include
+
+namespace routing {
+
+class LinearGenerator : public GeneratorBase {
+ Q_OBJECT
+public:
+ LinearGenerator(QObject *parent = nullptr);
+ LinearGenerator(Data d, QObject *parent = nullptr);
+
+ Q_PROPERTY(Fact *distance READ distance CONSTANT)
+ Q_PROPERTY(Fact *alpha READ alpha CONSTANT)
+ Q_PROPERTY(Fact *minLength READ minLength CONSTANT)
+
+ virtual QString editorQml() override;
+
+ virtual QString name() override;
+ virtual QString abbreviation() override;
+
+ virtual bool get(Generator &generator) override;
+
+ Fact *distance();
+ Fact *alpha();
+ Fact *minLength();
+
+ static const char *settingsGroup;
+ static const char *distanceName;
+ static const char *alphaName;
+ static const char *minLengthName;
+
+protected:
+ virtual void establishConnections() override;
+ virtual void deleteConnections() override;
+
+private:
+ bool _connectionsEstablished;
+
+ QMap _metaDataMap;
+ SettingsFact _distance;
+ SettingsFact _alpha;
+ SettingsFact _minLength;
+};
+
+} // namespace routing
diff --git a/src/Wima/Snake/NemoInterface.cpp b/src/Wima/Snake/NemoInterface.cpp
index f5187778dc3088a804aca08d352b0bdeb6bee21b..95b5d753e59e4d0c008cf86bcfbf75809361fe86 100644
--- a/src/Wima/Snake/NemoInterface.cpp
+++ b/src/Wima/Snake/NemoInterface.cpp
@@ -2,6 +2,7 @@
#include "SnakeTilesLocal.h"
#include "QGCApplication.h"
+#include "QGCLoggingCategory.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
@@ -26,6 +27,8 @@
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
+QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
+
#define EVENT_TIMER_INTERVAL 100 // ms
auto static timeoutInterval = std::chrono::milliseconds(3000);
@@ -160,28 +163,28 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
if (tile != nullptr) {
if (tile->coordinateList().size() > 0) {
if (tile->coordinateList().first().isValid()) {
- this->ENUOrigin = tile->coordinateList().first();
- const auto &origin = this->ENUOrigin;
- this->tilesENU.polygons().clear();
- for (int i = 0; i < tileData.tiles.count(); ++i) {
- obj = tileData.tiles.get(i);
- tile = qobject_cast(obj);
- if (tile != nullptr) {
- SnakeTileLocal tileENU;
- snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
- this->tilesENU.polygons().push_back(std::move(tileENU));
- } else {
- qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
- break;
- }
+ this->ENUOrigin = tile->coordinateList().first();
+ const auto &origin = this->ENUOrigin;
+ this->tilesENU.polygons().clear();
+ for (int i = 0; i < tileData.tiles.count(); ++i) {
+ obj = tileData.tiles.get(i);
+ tile = qobject_cast(obj);
+ if (tile != nullptr) {
+ SnakeTileLocal tileENU;
+ snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
+ this->tilesENU.polygons().push_back(std::move(tileENU));
+ } else {
+ qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
+ break;
}
- } else {
- qWarning() << "NemoInterface::Impl::setTileData(): Origin invalid.";
}
} else {
- qWarning() << "NemoInterface::Impl::setTileData(): tile empty.";
+ qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
}
+ } else {
+ qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
}
+ }
} else {
this->tileData.clear();
@@ -300,12 +303,17 @@ void NemoInterface::Impl::doTopicServiceSetup() {
std::make_unique(rapidjson::kObjectType));
auto &origin = this->ENUOrigin;
rapidjson::Value jOrigin(rapidjson::kObjectType);
- bool ret = geographic_msgs::geo_point::toJson(origin, jOrigin,
- pDoc->GetAllocator());
lk.unlock();
- Q_ASSERT(ret);
- (void)ret;
+ if (geographic_msgs::geo_point::toJson(origin, jOrigin,
+ pDoc->GetAllocator())) {
+ lk.unlock();
pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
+ } else {
+ lk.unlock();
+ qCWarning(NemoInterfaceLog)
+ << "/snake/get_origin service: could not create json document.";
+ }
+
return pDoc;
});
@@ -318,11 +326,17 @@ void NemoInterface::Impl::doTopicServiceSetup() {
JsonDocUPtr pDoc(
std::make_unique(rapidjson::kObjectType));
rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
- bool ret = jsk_recognition_msgs::polygon_array::toJson(
- this->tilesENU, jSnakeTiles, pDoc->GetAllocator());
- Q_ASSERT(ret);
- (void)ret;
+
+ if (jsk_recognition_msgs::polygon_array::toJson(
+ this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
+ lk.unlock();
pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
+ } else {
+ lk.unlock();
+ qCWarning(NemoInterfaceLog)
+ << "/snake/get_tiles service: could not create json document.";
+ }
+
return pDoc;
});
}
@@ -336,7 +350,7 @@ void NemoInterface::Impl::loop() {
} else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
!this->topicServiceSetupDone) {
this->doTopicServiceSetup();
- this->topicServiceSetupDone = true;
+ this->topicServiceSetupDone = true;
this->setStatus(STATUS::WEBSOCKET_DETECTED);
} else if (this->pRosBridge->isRunning() &&
@@ -379,22 +393,26 @@ void NemoInterface::Impl::publishTilesENU() {
using namespace ros_bridge::messages;
JsonDocUPtr jSnakeTiles(
std::make_unique(rapidjson::kObjectType));
- bool ret = jsk_recognition_msgs::polygon_array::toJson(
- this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
- Q_ASSERT(ret);
- (void)ret;
+ if (jsk_recognition_msgs::polygon_array::toJson(
+ this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) {
this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
+ } else {
+ qCWarning(NemoInterfaceLog)
+ << "Impl::publishTilesENU: could not create json document.";
+ }
}
void NemoInterface::Impl::publishENUOrigin() {
using namespace ros_bridge::messages;
JsonDocUPtr jOrigin(
std::make_unique(rapidjson::kObjectType));
- bool ret = geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
- jOrigin->GetAllocator());
- Q_ASSERT(ret);
- (void)ret;
+ if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
+ jOrigin->GetAllocator())) {
this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+ } else {
+ qCWarning(NemoInterfaceLog)
+ << "Impl::publishENUOrigin: could not create json document.";
+ }
}
bool NemoInterface::Impl::setStatus(NemoInterface::STATUS s) {
@@ -421,7 +439,7 @@ void NemoInterface::stop() { this->pImpl->stop(); }
void NemoInterface::publishTileData() { this->pImpl->publishTileData(); }
void NemoInterface::requestProgress() {
- qWarning() << "NemoInterface::requestProgress(): dummy.";
+ qCWarning(NemoInterfaceLog) << "requestProgress(): dummy.";
}
void NemoInterface::setTileData(const TileData &tileData) {
diff --git a/src/Wima/Snake/SnakeTile.cpp b/src/Wima/Snake/SnakeTile.cpp
index ba2ffcbaf8b6dc568dd76a1e607072bf21a3ee99..397a2e917bcbd6281144aabb62ae42f4964c20f8 100644
--- a/src/Wima/Snake/SnakeTile.cpp
+++ b/src/Wima/Snake/SnakeTile.cpp
@@ -18,8 +18,6 @@ QString SnakeTile::type() const { return "Tile"; }
SnakeTile *SnakeTile::Clone() const { return new SnakeTile(*this); }
SnakeTile &SnakeTile::operator=(const SnakeTile &other) {
- this->assign(other);
+ this->WimaAreaData::operator=(other);
return *this;
}
-
-void SnakeTile::assign(const SnakeTile &other) { WimaAreaData::assign(other); }
diff --git a/src/Wima/Snake/json/CircularGenerator.SettingsGroup.json b/src/Wima/Snake/json/CircularGenerator.SettingsGroup.json
new file mode 100644
index 0000000000000000000000000000000000000000..001a3df5fcfa545657a0fe98b32ce8dfc8032365
--- /dev/null
+++ b/src/Wima/Snake/json/CircularGenerator.SettingsGroup.json
@@ -0,0 +1,30 @@
+[
+{
+ "name": "TransectDistance",
+ "shortDescription": "The distance between transects.",
+ "type": "double",
+ "units": "m",
+ "min": 0.3,
+ "decimalPlaces": 1,
+ "defaultValue": 5.0
+},
+{
+ "name": "DeltaAlpha",
+ "shortDescription": "Angle discretisation.",
+ "type": "double",
+ "units": "Deg",
+ "min": 0.3,
+ "max": 90,
+ "decimalPlaces": 1,
+ "defaultValue": 5.0
+},
+{
+ "name": "MinLength",
+ "shortDescription": "The minimal transect length.",
+ "type": "double",
+ "units": "m",
+ "min": 0.3,
+ "decimalPlaces": 1,
+ "defaultValue": 5.0
+}
+]
diff --git a/src/Wima/Snake/json/LinearGenerator.SettingsGroup.json b/src/Wima/Snake/json/LinearGenerator.SettingsGroup.json
new file mode 100644
index 0000000000000000000000000000000000000000..cc3bc7130e880d8a52eaffb91a3d289a92929476
--- /dev/null
+++ b/src/Wima/Snake/json/LinearGenerator.SettingsGroup.json
@@ -0,0 +1,30 @@
+[
+{
+ "name": "TransectDistance",
+ "shortDescription": "The distance between transects.",
+ "type": "double",
+ "units": "m",
+ "min": 0.3,
+ "decimalPlaces": 1,
+ "defaultValue": 5.0
+},
+{
+ "name": "Alpha",
+ "shortDescription": "Transect angle.",
+ "type": "double",
+ "units": "Deg",
+ "min": 0,
+ "max": 180,
+ "decimalPlaces": 1,
+ "defaultValue": 0.0
+},
+{
+ "name": "MinLength",
+ "shortDescription": "The minimal transect length.",
+ "type": "double",
+ "units": "m",
+ "min": 0.3,
+ "decimalPlaces": 1,
+ "defaultValue": 5.0
+}
+]
diff --git a/src/Wima/Snake/snake.cpp b/src/Wima/Snake/snake.cpp
index c7517cb5c46e0e02c92de2969067b344e3f71f56..8886d35d43adcf3124f29f2d53702f9411a75820 100644
--- a/src/Wima/Snake/snake.cpp
+++ b/src/Wima/Snake/snake.cpp
@@ -760,7 +760,17 @@ bool route(const FPolygon &area, const Transects &transects,
if (std::isinf(dist)) {
std::vector route;
if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) {
- par.errorString = "Distance matrix calculation failed.";
+ std::stringstream ss;
+ ss << "Distance matrix calculation failed. connection graph: "
+ << connectionGraph << std::endl;
+ ss << "area: " << bg::wkt(area) << std::endl;
+ ss << "transects:" << std::endl;
+ for (const auto &t : transects) {
+
+ ss << bg::wkt(t) << std::endl;
+ }
+
+ par.errorString = ss.str();
return false;
}
(void)route;
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index 7a9aa89ffc37dc2ad13ad0dda92661838f6b9736..7c9a050b06138c90293b8c9827802221699fb58b 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -230,74 +230,53 @@ void WimaController::planDataChangedHandler() {
_serviceArea = WimaServiceAreaData();
_corridor = WimaCorridorData();
_joinedArea = WimaJoinedAreaData();
+ _planDataValid = false;
emit visualItemsChanged();
emit missionItemsChanged();
emit waypointPathChanged();
- _planDataValid = false;
-
+ // Extract areas.
auto planData = WimaBridge::instance()->planData();
- // extract list with WimaAreas
- QList areaList = planData.areaList();
-
- int areaCounter = 0;
- const int numAreas = 4; // extract only numAreas Areas, if there are more
- // they are invalid and ignored
- for (int i = 0; i < areaList.size(); i++) {
- const WimaAreaData *areaData = areaList[i];
-
- if (areaData->type() ==
- WimaServiceAreaData::typeString) { // is it a service area?
- _serviceArea = *qobject_cast(areaData);
- areaCounter++;
- _areas.append(&_serviceArea);
-
- continue;
- }
- if (areaData->type() ==
- WimaMeasurementAreaData::typeString) { // is it a measurement area?
- _measurementArea =
- *qobject_cast(areaData);
- areaCounter++;
- _areas.append(&_measurementArea);
+ // Measurement Area.
+ if (planData.measurementArea().coordinateList().size() >= 3) {
+ _measurementArea = planData.measurementArea();
+ _areas.append(&_measurementArea);
- continue;
- }
+ // Service Area.
+ if (planData.serviceArea().coordinateList().size() >= 3) {
+ _serviceArea = planData.serviceArea();
+ _areas.append(&_serviceArea);
- if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
- _corridor = *qobject_cast(areaData);
- areaCounter++;
- //_visualItems.append(&_corridor); // not needed
+ _WMSettings.setHomePosition(
+ QGeoCoordinate(_serviceArea.depot().latitude(),
+ _serviceArea.depot().longitude(), 0));
- continue;
- }
+ // Joined Area.
+ if (planData.joinedArea().coordinateList().size() >= 3) {
+ _joinedArea = planData.joinedArea();
+ _areas.append(&_joinedArea);
- if (areaData->type() ==
- WimaJoinedAreaData::typeString) { // is it a corridor?
- _joinedArea = *qobject_cast(areaData);
- areaCounter++;
- _areas.append(&_joinedArea);
+ _planDataValid = true;
- continue;
+ // Corridor.
+ if (planData.corridor().coordinateList().size() >= 3) {
+ _corridor = planData.corridor();
+ }
+ }
}
-
- if (areaCounter >= numAreas)
- break;
}
- if (areaCounter != numAreas) {
- Q_ASSERT(false);
- return;
+ if (_planDataValid) {
+ emit visualItemsChanged();
+ } else {
+ _areas.clear();
+ _measurementArea = WimaMeasurementAreaData();
+ _serviceArea = WimaServiceAreaData();
+ _corridor = WimaCorridorData();
+ _joinedArea = WimaJoinedAreaData();
}
-
- emit visualItemsChanged();
-
- _WMSettings.setHomePosition(QGeoCoordinate(
- _serviceArea.depot().latitude(), _serviceArea.depot().longitude(), 0));
-
- _planDataValid = true;
}
void WimaController::progressChangedHandler() {
diff --git a/src/Wima/WimaPlanData.cc b/src/Wima/WimaPlanData.cc
index 51be4cc28e4182a099be3366ac49b77cd89911ea..11c55862df727c2b1283a622e57fee2725c50c69 100644
--- a/src/Wima/WimaPlanData.cc
+++ b/src/Wima/WimaPlanData.cc
@@ -7,94 +7,116 @@ WimaPlanData::WimaPlanData(const WimaPlanData &other, QObject *parent)
*this = other;
}
-/*!
- * \fn WimaPlanData &WimaPlanData::operator=(const WimaPlanData &other)
- *
- * Copies the data area list of \a other to the calling \c WimaPlanData object.
- * Returns a reference to the calling \c WimaPlanData object.
- */
WimaPlanData &WimaPlanData::operator=(const WimaPlanData &other) {
- // copy wima areas
- QList areaList = other.areaList();
- _areaList.clear();
- for (int i = 0; i < areaList.size(); i++) {
- const WimaAreaData *areaData = areaList[i];
- // determine area type and append
- if (areaData->type() == WimaJoinedAreaData::typeString) {
- this->append(*qobject_cast(areaData));
- } else if (areaData->type() == WimaServiceAreaData::typeString) {
- this->append(*qobject_cast(areaData));
- } else if (areaData->type() == WimaMeasurementAreaData::typeString) {
- this->append(*qobject_cast(areaData));
- } else if (areaData->type() == WimaCorridorData::typeString) {
- this->append(*qobject_cast(areaData));
- }
- }
+ this->set(other.measurementArea());
+ this->set(other.serviceArea());
+ this->set(other.joinedArea());
+ this->set(other.corridor());
return *this;
}
-/*!
- * \fn void WimaPlanData::append(const WimaAreaData &areaData)
- *
- * Adds a WimaAreaData item.
- */
-void WimaPlanData::append(const WimaJoinedAreaData &areaData) {
- _joinedArea = areaData;
+void WimaPlanData::set(const WimaJoinedAreaData &areaData) {
+ if (_joinedArea != areaData) {
+ _joinedArea = areaData;
+ emit joinedAreaChanged();
+ }
+}
+
+void WimaPlanData::set(const WimaServiceAreaData &areaData) {
+ if (_serviceArea != areaData) {
+ _serviceArea = areaData;
+ emit serviceAreaChanged();
+ }
+}
- if (!_areaList.contains(&_joinedArea)) {
- _areaList.append(&_joinedArea);
+void WimaPlanData::set(const WimaCorridorData &areaData) {
+ if (_corridor != areaData) {
+ _corridor = areaData;
+ emit corridorChanged();
}
}
-/*!
- * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
- *
- * Adds a WimaServiceAreaData item.
- */
-void WimaPlanData::append(const WimaServiceAreaData &areaData) {
- _serviceArea = areaData;
+void WimaPlanData::set(const WimaMeasurementAreaData &areaData) {
+ if (_measurementArea != areaData) {
+ _measurementArea = areaData;
+ emit measurementAreaChanged();
- if (!_areaList.contains(&_serviceArea)) {
- _areaList.append(&_serviceArea);
+ if (_measurementArea.coordinateList().size() > 0) {
+ setOrigin(_measurementArea.coordinateList().first());
+ } else {
+ setOrigin(QGeoCoordinate());
+ }
}
}
-/*!
- * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
- *
- * Adds a WimaCorridorData item.
- */
-void WimaPlanData::append(const WimaCorridorData &areaData) {
- _corridor = areaData;
+void WimaPlanData::set(const WimaJoinedArea &areaData) {
+ if (_joinedArea != areaData) {
+ _joinedArea = areaData;
+ emit joinedAreaChanged();
+ }
+}
- if (!_areaList.contains(&_corridor)) {
- _areaList.append(&_corridor);
+void WimaPlanData::set(const WimaServiceArea &areaData) {
+ if (_serviceArea != areaData) {
+ _serviceArea = areaData;
+ emit serviceAreaChanged();
}
}
-/*!
- * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
- *
- * Adds a WimaMeasurementAreaData item.
- */
-void WimaPlanData::append(const WimaMeasurementAreaData &areaData) {
- _measurementArea = areaData;
+void WimaPlanData::set(const WimaCorridor &areaData) {
+ if (_corridor != areaData) {
+ _corridor = areaData;
+ emit corridorChanged();
+ }
+}
- if (!_areaList.contains(&_measurementArea)) {
- _areaList.append(&_measurementArea);
+void WimaPlanData::set(const WimaMeasurementArea &areaData) {
+ if (_measurementArea != areaData) {
+ _measurementArea = areaData;
+ emit measurementAreaChanged();
+
+ if (_measurementArea.coordinateList().size() > 0) {
+ setOrigin(_measurementArea.coordinateList().first());
+ } else {
+ setOrigin(QGeoCoordinate());
+ }
}
}
-/*!
- * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
- *
- * Clears all stored objects
- */
-void WimaPlanData::clear() { _areaList.clear(); }
+void WimaPlanData::clear() { *this = WimaPlanData(); }
+
+QGeoCoordinate WimaPlanData::origin() { return _origin; }
+
+bool WimaPlanData::isValid() {
+ return _measurementArea.coordinateList().size() >= 3 &&
+ _serviceArea.coordinateList().size() >= 3 && _origin.isValid();
+}
+
+const WimaJoinedAreaData &WimaPlanData::joinedArea() const {
+ return this->_joinedArea;
+}
+
+const WimaServiceAreaData &WimaPlanData::serviceArea() const {
+ return this->_serviceArea;
+}
+
+const WimaCorridorData &WimaPlanData::corridor() const {
+ return this->_corridor;
+}
-const QList &WimaPlanData::areaList() const {
- return _areaList;
+const WimaMeasurementAreaData &WimaPlanData::measurementArea() const {
+ return this->_measurementArea;
+}
+
+WimaJoinedAreaData &WimaPlanData::joinedArea() { return this->_joinedArea; }
+
+WimaServiceAreaData &WimaPlanData::serviceArea() { return this->_serviceArea; }
+
+WimaCorridorData &WimaPlanData::corridor() { return this->_corridor; }
+
+WimaMeasurementAreaData &WimaPlanData::measurementArea() {
+ return this->_measurementArea;
}
bool WimaPlanData::operator==(const WimaPlanData &other) const {
@@ -107,14 +129,9 @@ bool WimaPlanData::operator!=(const WimaPlanData &other) const {
return !(*this == other);
}
-/*!
- * \class WimaPlanData
- * \brief Class storing data generated by the \c WimaPlaner class.
- *
- * This class is designed to store data generated by the \c WimaPlaner class and
- * meant for data exchange between the \c WimaController and the \c WimaPlanner.
- * It stores a QList of \c WimaAreaData objects, called area data list,
- * containing the data of serveral \c WimaAreas generated by the \c WimaPlaner.
- *
- * \sa QList
- */
+void WimaPlanData::setOrigin(const QGeoCoordinate &origin) {
+ if (this->_origin != origin) {
+ this->_origin = origin;
+ emit originChanged();
+ }
+}
diff --git a/src/Wima/WimaPlanData.h b/src/Wima/WimaPlanData.h
index 869855f2889e701d73d438e57f262acb4a431a0a..3b2445b0835056ef8c4479e5c9d8c6a87382918a 100644
--- a/src/Wima/WimaPlanData.h
+++ b/src/Wima/WimaPlanData.h
@@ -18,33 +18,47 @@ public:
WimaPlanData &operator=(const WimaPlanData &other);
// Member Methodes
- 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
- //! \note Takes owenership of MissionItems*
- void append(const QList &missionItems);
+ void set(const WimaJoinedAreaData &areaData);
+ void set(const WimaServiceAreaData &areaData);
+ void set(const WimaCorridorData &areaData);
+ void set(const WimaMeasurementAreaData &areaData);
+
+ void set(const WimaJoinedArea &areaData);
+ void set(const WimaServiceArea &areaData);
+ void set(const WimaCorridor &areaData);
+ void set(const WimaMeasurementArea &areaData);
void clear();
- const QList &areaList() const;
- const QList> &transects() const;
- const QList &missionItems() const;
+ const WimaJoinedAreaData &joinedArea() const;
+ const WimaServiceAreaData &serviceArea() const;
+ const WimaCorridorData &corridor() const;
+ const WimaMeasurementAreaData &measurementArea() const;
+
+ WimaJoinedAreaData &joinedArea();
+ WimaServiceAreaData &serviceArea();
+ WimaCorridorData &corridor();
+ WimaMeasurementAreaData &measurementArea();
+
+ QGeoCoordinate origin();
+ bool isValid();
bool operator==(const WimaPlanData &other) const;
bool operator!=(const WimaPlanData &other) const;
signals:
- void areaListChanged();
+ void joinedAreaChanged();
+ void serviceAreaChanged();
+ void corridorChanged();
+ void measurementAreaChanged();
+ void originChanged();
private:
+ void setOrigin(const QGeoCoordinate &origin);
+
WimaJoinedAreaData _joinedArea;
WimaServiceAreaData _serviceArea;
WimaCorridorData _corridor;
WimaMeasurementAreaData _measurementArea;
- QList _areaList;
+
+ QGeoCoordinate _origin;
};
diff --git a/src/Wima/WimaPlanData_old.cc b/src/Wima/WimaPlanData_old.cc
deleted file mode 100644
index 394c53c25ce55c9f2b477b91487cec47854261e7..0000000000000000000000000000000000000000
--- a/src/Wima/WimaPlanData_old.cc
+++ /dev/null
@@ -1,127 +0,0 @@
-#include "WimaPlanData.h"
-
-WimaPlanData::WimaPlanData(QObject *parent) : QObject(parent) {}
-
-WimaPlanData::WimaPlanData(const WimaPlanData &other, QObject *parent)
- : QObject(parent) {
- *this = other;
-}
-
-/*!
- * \fn WimaPlanData &WimaPlanData::operator=(const WimaPlanData &other)
- *
- * Copies the data area list of \a other to the calling \c WimaPlanData object.
- * Returns a reference to the calling \c WimaPlanData object.
- */
-WimaPlanData &WimaPlanData::operator=(const WimaPlanData &other) {
- // copy wima areas
- QList areaList = other.areaList();
- _areaList.clear();
- for (int i = 0; i < areaList.size(); i++) {
- const WimaAreaData *areaData = areaList[i];
- // determine area type and append
- if (areaData->type() == WimaJoinedAreaData::typeString) {
- this->append(*qobject_cast(areaData));
- } else if (areaData->type() == WimaServiceAreaData::typeString) {
- this->append(*qobject_cast(areaData));
- } else if (areaData->type() == WimaMeasurementAreaData::typeString) {
- this->append(*qobject_cast(areaData));
- } else if (areaData->type() == WimaCorridorData::typeString) {
- this->append(*qobject_cast(areaData));
- }
- }
-
- // copy mission items
- _missionItems = other.missionItems();
-
- return *this;
-}
-
-/*!
- * \fn void WimaPlanData::append(const WimaAreaData &areaData)
- *
- * Adds a WimaAreaData item.
- */
-void WimaPlanData::append(const WimaJoinedAreaData &areaData) {
- _joinedArea = areaData;
-
- if (!_areaList.contains(&_joinedArea)) {
- _areaList.append(&_joinedArea);
- }
-}
-
-/*!
- * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
- *
- * Adds a WimaServiceAreaData item.
- */
-void WimaPlanData::append(const WimaServiceAreaData &areaData) {
- _serviceArea = areaData;
-
- if (!_areaList.contains(&_serviceArea)) {
- _areaList.append(&_serviceArea);
- }
-}
-
-/*!
- * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
- *
- * Adds a WimaCorridorData item.
- */
-void WimaPlanData::append(const WimaCorridorData &areaData) {
- _corridor = areaData;
-
- if (!_areaList.contains(&_corridor)) {
- _areaList.append(&_corridor);
- }
-}
-
-/*!
- * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
- *
- * Adds a WimaMeasurementAreaData item.
- */
-void WimaPlanData::append(const WimaMeasurementAreaData &areaData) {
- _measurementArea = areaData;
-
- if (!_areaList.contains(&_measurementArea)) {
- _areaList.append(&_measurementArea);
- }
-}
-
-void WimaPlanData::append(const QList &missionItems) {
- for (auto *item : missionItems) {
- item->setParent(this);
- _missionItems.append(item);
- }
-}
-
-/*!
- * \fn void WimaPlanData::append(const WimaServiceAreaData &areaData)
- *
- * Clears all stored objects
- */
-void WimaPlanData::clear() {
- _areaList.clear();
- _missionItems.clear();
-}
-
-const QList &WimaPlanData::areaList() const {
- return _areaList;
-}
-
-const QList &WimaPlanData::missionItems() const {
- return _missionItems;
-}
-
-/*!
- * \class WimaPlanData
- * \brief Class storing data generated by the \c WimaPlaner class.
- *
- * This class is designed to store data generated by the \c WimaPlaner class and
- * meant for data exchange between the \c WimaController and the \c WimaPlanner.
- * It stores a QList of \c WimaAreaData objects, called area data list,
- * containing the data of serveral \c WimaAreas generated by the \c WimaPlaner.
- *
- * \sa QList
- */
diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc
index 5e4672cfe41078fa139ca0ed97caee8cce358ed7..3d9dd4c753998d7b83561ff8f4e7ec9798ef0168 100644
--- a/src/Wima/WimaPlaner.cc
+++ b/src/Wima/WimaPlaner.cc
@@ -38,9 +38,7 @@ const char *WimaPlaner::missionItemsName = "MissionItems";
WimaPlaner::WimaPlaner(QObject *parent)
: QObject(parent), _masterController(nullptr), _missionController(nullptr),
- _currentAreaIndex(-1), _copyMAreaToSurvey(true), _copySAreaToSurvey(true),
- _corridorChanged(true), _joinedArea(this), _arrivalPathLength(0),
- _returnPathLength(0), _survey(nullptr), _surveyChanged(true),
+ _currentAreaIndex(-1), _joinedArea(this), _survey(nullptr),
_synchronized(false), _nemoInterface(this),
_stateMachine(new StateMachine), _areasMonitored(false),
_missionControllerMonitored(false), _progressLocked(false) {
@@ -246,7 +244,7 @@ void WimaPlaner::removeAll() {
removeArea(0);
changesApplied = true;
}
- // Reset Items.
+
_measurementArea = WimaMeasurementArea();
_joinedArea = WimaJoinedArea();
_serviceArea = WimaServiceArea();
@@ -356,8 +354,6 @@ void WimaPlaner::_update() {
}
// establish connections
- _survey->setRefPoint(_measurementArea.center());
- _survey->setHidePolygon(true);
connect(_survey, &CircularSurvey::calculatingChanged, this,
&WimaPlaner::CSCalculatingChangedHandler);
connect(_survey, &CircularSurvey::missionItemReady, this,
@@ -366,17 +362,7 @@ void WimaPlaner::_update() {
&WimaPlaner::CSDestroyedHandler);
}
- // update survey area
- disconnect(_survey, &CircularSurvey::calculatingChanged, this,
- &WimaPlaner::CSCalculatingChangedHandler);
- _survey->setMeasurementArea(this->_measurementArea);
- _survey->setJoinedArea(this->_joinedArea);
- _survey->setDepot(this->_serviceArea.depot());
- connect(_survey, &CircularSurvey::calculatingChanged, this,
- &WimaPlaner::CSCalculatingChangedHandler);
-
- // Folloing statement just for completeness.
- this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED);
+ (void)toPlanData(this->_survey->planData());
} break; // STATE::NEEDS_SURVEY_UPDATE
case STATE::WAITING_FOR_SURVEY_UPDATE: {
@@ -444,7 +430,7 @@ void WimaPlaner::_update() {
.data());
return;
}
- _arrivalPathLength = path.size() - 1;
+
for (int i = 1; i < path.count() - 1; i++) {
(void)_missionController->insertSimpleMissionItem(
path[i], missionItems->count() - 1);
@@ -462,8 +448,7 @@ void WimaPlaner::_update() {
.data());
return;
}
- _returnPathLength =
- path.size() - 1; // -1: fist item is last measurement point
+
for (int i = 1; i < path.count() - 1; i++) {
(void)_missionController->insertSimpleMissionItem(
path[i], missionItems->count());
@@ -772,7 +757,6 @@ bool WimaPlaner::loadFromFile(const QString &filename) {
for (int i = 0; i < missionItems->count(); i++) {
_survey = missionItems->value(i);
if (_survey != nullptr) {
- _survey->setHidePolygon(true);
connect(_survey, &CircularSurvey::calculatingChanged, this,
&WimaPlaner::CSCalculatingChangedHandler);
connect(_survey, &CircularSurvey::missionItemReady, this,
@@ -953,13 +937,11 @@ void WimaPlaner::setInteractive() {
* \sa WimaController, WimaPlanData
*/
bool WimaPlaner::toPlanData(WimaPlanData &planData) {
-
- // store areas
- planData.append(WimaMeasurementAreaData(_measurementArea));
- planData.append(WimaServiceAreaData(_serviceArea));
- planData.append(WimaCorridorData(_corridor));
- planData.append(WimaJoinedAreaData(_joinedArea));
- return true;
+ planData.set(_measurementArea);
+ planData.set(_serviceArea);
+ planData.set(_corridor);
+ planData.set(_joinedArea);
+ return planData.isValid();
}
#ifndef NDEBUG
diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h
index 2682bc95e2cbb681dd6d44b560b43b425fb40941..f6e786681e1a33fb159dfae5ffff5ba90fcb0a73 100644
--- a/src/Wima/WimaPlaner.h
+++ b/src/Wima/WimaPlaner.h
@@ -172,27 +172,13 @@ private:
int _currentAreaIndex;
QString _currentFile;
- bool _joinedAreaValid;
WimaMeasurementArea _measurementArea;
- bool _copyMAreaToSurvey;
WimaServiceArea _serviceArea;
- bool _copySAreaToSurvey;
WimaCorridor _corridor;
- bool _corridorChanged;
- // contains all visible areas
- QmlObjectListModel _visualItems;
- // joined area fromed by _measurementArea, _serviceArea, _corridor
WimaJoinedArea _joinedArea;
- // path from takeoff to first measurement point
- unsigned long _arrivalPathLength;
- // path from last measurement point to land
- unsigned long _returnPathLength;
+ QmlObjectListModel _visualItems; // all areas
CircularSurvey *_survey;
- bool _surveyChanged;
- // sync stuff
- bool _synchronized; // true if planData is synchronized with
- // wimaController
#ifndef NDEBUG
QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents
@@ -206,4 +192,6 @@ private:
bool _areasMonitored;
bool _missionControllerMonitored;
bool _progressLocked;
+ bool _synchronized; // true if planData is synchronized with
+ // wimaController
};
diff --git a/src/WimaView/CircularGeneratorEditor.qml b/src/WimaView/CircularGeneratorEditor.qml
new file mode 100644
index 0000000000000000000000000000000000000000..c70751f87fed7c85a8aded01e4bc25e7181ca8e3
--- /dev/null
+++ b/src/WimaView/CircularGeneratorEditor.qml
@@ -0,0 +1,43 @@
+import QtQuick 2.0
+
+import QtQuick.Layouts 1.11
+
+import QGroundControl.Controls 1.0
+import QGroundControl.FactControls 1.0
+import QGroundControl.ScreenTools 1.0
+
+GridLayout {
+
+ property var generator // CircularGenerator
+
+ property real _margin: ScreenTools.defaultFontPixelWidth / 2
+
+ columnSpacing: _margin
+ rowSpacing: _margin
+ columns: 2
+
+ QGCLabel { text: qsTr("Distance") }
+ FactTextField {
+ fact: generator.distance
+ Layout.fillWidth: true
+ }
+
+ QGCLabel { text: qsTr("Delta Alpha") }
+ FactTextField {
+ fact: generator.deltaAlpha
+ Layout.fillWidth: true
+ }
+
+ QGCLabel { text: qsTr("Min. Length") }
+ FactTextField {
+ fact: generator.minLength
+ Layout.fillWidth: true
+ }
+
+ QGCButton {
+ text: qsTr("Reset Ref.")
+ onClicked: generator.resetReference();
+ Layout.fillWidth: true
+ Layout.columnSpan: 2
+ }
+}
diff --git a/src/WimaView/CircularGeneratorMapVisual.qml b/src/WimaView/CircularGeneratorMapVisual.qml
new file mode 100644
index 0000000000000000000000000000000000000000..6a7fea20642669f18bb3d9536095bff358b5bce2
--- /dev/null
+++ b/src/WimaView/CircularGeneratorMapVisual.qml
@@ -0,0 +1,87 @@
+import QtQuick 2.0
+import Wima 1.0
+import QGroundControl 1.0
+
+Item {
+ id: _root
+
+ visible: true
+
+ // Expects the following properties:
+ property var map ///< Map control to place item in
+ property var qgcView ///< QGCView to use for popping dialogs
+ property var generator
+ property bool checked: false
+
+ property var _referenceComponent: undefined
+
+ signal clicked()
+
+ onVisibleChanged: {
+ if (visible){
+ _addRefPoint()
+ } else {
+ _destroyRefPoint()
+ }
+ }
+
+ Component.onCompleted: {
+ if (visible){
+ _addRefPoint()
+ }
+ }
+
+ Component.onDestruction: {
+ _destroyRefPoint()
+ }
+
+ function _addRefPoint(){
+ if (!_referenceComponent){
+ _referenceComponent = refPointComponent.createObject(_root)
+ map.addMapItem(_referenceComponent)
+ }
+ }
+
+ function _destroyRefPoint(){
+ if (_referenceComponent){
+ map.removeMapItem(_referenceComponent)
+ _referenceComponent.destroy()
+ _referenceComponent = undefined
+ }
+ }
+
+ // Ref. point (Base Station)
+ Component {
+ id: refPointComponent
+
+ DragCoordinate {
+ id: dragCoordinate
+
+ property var ref: _root.generator.reference
+
+ map: _root.map
+ qgcView: _root.qgcView
+ z: QGroundControl.zOrderMapItems
+ checked: _root.checked
+ coordinate: ref
+
+ onDragReleased: {
+ syncAndBind()
+ }
+ Component.onCompleted: {
+ syncAndBind()
+ }
+ onClicked: {
+ _root.clicked()
+ }
+
+ function syncAndBind(){
+ if (coordinate.latitude !== ref.latitude ||
+ coordinate.longitude !== ref.longitude){
+ _root.generator.reference = coordinate
+ }
+ coordinate = Qt.binding(function(){return _root.generator.reference})
+ }
+ }
+ }
+}
diff --git a/src/PlanView/CircularSurveyItemEditor.qml b/src/WimaView/CircularSurveyItemEditor.qml
similarity index 56%
rename from src/PlanView/CircularSurveyItemEditor.qml
rename to src/WimaView/CircularSurveyItemEditor.qml
index 2bc9fb7a0a63fc051bca2559126edb12ea520d93..fc0cb381565e5aa2c32db8d4b5fc385b6d627fdc 100644
--- a/src/PlanView/CircularSurveyItemEditor.qml
+++ b/src/WimaView/CircularSurveyItemEditor.qml
@@ -25,29 +25,24 @@ Rectangle {
//property real availableWidth ///< Width for control
//property var missionItem ///< Mission Item for editor
- property real _margin: ScreenTools.defaultFontPixelWidth / 2
- property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5
- property var _vehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle
- property real _cameraMinTriggerInterval: missionItem.cameraCalc.minTriggerInterval.rawValue
+ property real _margin: ScreenTools.defaultFontPixelWidth / 2
- function polygonCaptureStarted() {
- missionItem.clearPolygon()
- }
+ property var _generator: missionItem.generator
+ property var _generatorEditor: undefined
- function polygonCaptureFinished(coordinates) {
- for (var i=0; i