From 5e62e791512b7db8aaafb56fec2029a8a2d59639 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Mon, 12 Oct 2020 18:24:37 +0200 Subject: [PATCH] wima planer state machine added --- Paths/temp1.plan | 988 ++++++++++++++++++ Paths/temp2.plan | 988 ++++++++++++++++++ Paths/temp3.plan | 988 ++++++++++++++++++ qgroundcontrol.pro | 2 + src/Wima/CircularSurvey.cc | 458 ++++---- src/Wima/CircularSurvey.h | 19 +- src/Wima/Geometry/GenericCircle.h | 10 +- src/Wima/Geometry/WimaAreaData.cc | 45 +- src/Wima/Geometry/WimaAreaData.h | 12 +- src/Wima/Geometry/WimaMeasurementArea.cc | 132 ++- src/Wima/Geometry/WimaMeasurementArea.h | 18 +- src/Wima/Geometry/WimaMeasurementAreaData.cc | 36 +- src/Wima/Geometry/WimaMeasurementAreaData.h | 12 + src/Wima/RoutingThread.cpp | 59 +- src/Wima/Snake/NemoInterface.cpp | 20 +- src/Wima/Snake/NemoInterface.h | 8 +- src/Wima/StateMachine.cpp | 277 +++++ src/Wima/StateMachine.h | 55 + src/Wima/WimaPlaner.cc | 604 ++++++----- src/Wima/WimaPlaner.h | 35 +- src/WimaView/CircularSurveyMapVisual.qml | 4 + src/WimaView/WimaMeasurementAreaMapVisual.qml | 71 +- src/WimaView/WimaToolBar.qml | 2 +- src/WimaView/WimaView.qml | 6 + 24 files changed, 4180 insertions(+), 669 deletions(-) create mode 100644 Paths/temp1.plan create mode 100644 Paths/temp2.plan create mode 100644 Paths/temp3.plan create mode 100644 src/Wima/StateMachine.cpp create mode 100644 src/Wima/StateMachine.h diff --git a/Paths/temp1.plan b/Paths/temp1.plan new file mode 100644 index 000000000..0eb1e5655 --- /dev/null +++ b/Paths/temp1.plan @@ -0,0 +1,988 @@ +{ + "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, + 5 + ], + "type": "SimpleItem" + }, + { + "Alpha": 23, + "MinLength": 5, + "NumRuns": 1, + "ReferencePointAlt": 0, + "ReferencePointLat": 47.76807182520187, + "ReferencePointLong": 16.530610531894183, + "Run": 1, + "TransectDistance": 2, + "TransectStyleComplexItem": { + "CameraCalc": { + "AdjustedFootprintFrontal": 25, + "AdjustedFootprintSide": 25, + "CameraName": "Manual (no camera specs)", + "DistanceToSurface": 15, + "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.76782117293054, + 16.53045952207802, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76801762181985, + 16.531145995440625, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812651838593, + 16.53090512987649, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768125250928335, + 16.530839239105784, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768187963203616, + 16.531058383792516, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768204997344334, + 16.531049622605572, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812144975848, + 16.530757670470322, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811764852164, + 16.530676101820056, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768222031484335, + 16.53104086139957, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768239065614615, + 16.53103210018785, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768113847226864, + 16.530594533208365, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811004588309, + 16.530512964621895, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825609974418, + 16.531023338983744, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76827313388202, + 16.53101457776058, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810624447235, + 16.53043139603398, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815568510607, + 16.530399305509537, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76832423627323, + 16.530988294070095, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834127040819, + 16.53097953282404, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76822245220941, + 16.530564330637993, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 21, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76831125125834, + 16.530806346203697, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768358304533436, + 16.530970771572267, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768307202146545, + 16.53099705529708, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809864148597, + 16.530268258893802, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768102443012644, + 16.530349827431266, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76829016801015, + 16.531005816531692, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768170929071175, + 16.531067144987073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812500283788, + 16.530906658047133, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809124406109, + 16.53085697602152, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815389493801, + 16.53107590617591, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 31, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768136860795146, + 16.531084667345684, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76805748523563, + 16.53080729404687, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76802372639744, + 16.53075761214986, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811982666054, + 16.531093428523086, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 35, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810279252523, + 16.53110218969476, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798996754654, + 16.530707930317146, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 37, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76795620865596, + 16.530658248522045, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 38, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768085758380195, + 16.53111095084737, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 39, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806872423443, + 16.5311197120076, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76792244976166, + 16.53060856681793, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 41, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76788869084566, + 16.53055888517811, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768051690105956, + 16.531128473148772, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 43, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76803465596775, + 16.53113723429756, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 44, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.767854931889964, + 16.530509203589244, + 15 + ], + "type": "SimpleItem" + } + ], + "Refly90Degrees": false, + "TurnAroundDistance": 10, + "VisualTransectPoints": [ + [ + 47.76782117293054, + 16.53045952207802 + ], + [ + 47.76801762181985, + 16.531145995440625 + ], + [ + 47.76812651838593, + 16.53090512987649 + ], + [ + 47.768125250928335, + 16.530839239105784 + ], + [ + 47.768187963203616, + 16.531058383792516 + ], + [ + 47.768204997344334, + 16.531049622605572 + ], + [ + 47.76812144975848, + 16.530757670470322 + ], + [ + 47.76811764852164, + 16.530676101820056 + ], + [ + 47.768222031484335, + 16.53104086139957 + ], + [ + 47.768239065614615, + 16.53103210018785 + ], + [ + 47.768113847226864, + 16.530594533208365 + ], + [ + 47.76811004588309, + 16.530512964621895 + ], + [ + 47.76825609974418, + 16.531023338983744 + ], + [ + 47.76827313388202, + 16.53101457776058 + ], + [ + 47.76810624447235, + 16.53043139603398 + ], + [ + 47.76815568510607, + 16.530399305509537 + ], + [ + 47.76832423627323, + 16.530988294070095 + ], + [ + 47.76834127040819, + 16.53097953282404 + ], + [ + 47.76822245220941, + 16.530564330637993 + ], + [ + 47.76831125125834, + 16.530806346203697 + ], + [ + 47.768358304533436, + 16.530970771572267 + ], + [ + 47.768307202146545, + 16.53099705529708 + ], + [ + 47.76809864148597, + 16.530268258893802 + ], + [ + 47.768102443012644, + 16.530349827431266 + ], + [ + 47.76829016801015, + 16.531005816531692 + ], + [ + 47.768170929071175, + 16.531067144987073 + ], + [ + 47.76812500283788, + 16.530906658047133 + ], + [ + 47.76809124406109, + 16.53085697602152 + ], + [ + 47.76815389493801, + 16.53107590617591 + ], + [ + 47.768136860795146, + 16.531084667345684 + ], + [ + 47.76805748523563, + 16.53080729404687 + ], + [ + 47.76802372639744, + 16.53075761214986 + ], + [ + 47.76811982666054, + 16.531093428523086 + ], + [ + 47.76810279252523, + 16.53110218969476 + ], + [ + 47.76798996754654, + 16.530707930317146 + ], + [ + 47.76795620865596, + 16.530658248522045 + ], + [ + 47.768085758380195, + 16.53111095084737 + ], + [ + 47.76806872423443, + 16.5311197120076 + ], + [ + 47.76792244976166, + 16.53060856681793 + ], + [ + 47.76788869084566, + 16.53055888517811 + ], + [ + 47.768051690105956, + 16.531128473148772 + ], + [ + 47.76803465596775, + 16.53113723429756 + ], + [ + 47.767854931889964, + 16.530509203589244 + ] + ], + "version": 1 + }, + "Type": 1, + "Variant": 0, + "complexItemType": "CircularSurvey", + "polygon": [ + [ + 47.76809580679245, + 16.530246122817612 + ], + [ + 47.76823933601322, + 16.53060087654427 + ], + [ + 47.7683711160486, + 16.530967006195464 + ], + [ + 47.7680076482754, + 16.531153949077463 + ], + [ + 47.7677855557718, + 16.530403347547246 + ], + [ + 47.768126518382985, + 16.5309051298743 + ] + ], + "type": "ComplexItem", + "version": 1 + }, + { + "AMSLAltAboveTerrain": 0, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 48, + "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/Paths/temp2.plan b/Paths/temp2.plan new file mode 100644 index 000000000..0eb1e5655 --- /dev/null +++ b/Paths/temp2.plan @@ -0,0 +1,988 @@ +{ + "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, + 5 + ], + "type": "SimpleItem" + }, + { + "Alpha": 23, + "MinLength": 5, + "NumRuns": 1, + "ReferencePointAlt": 0, + "ReferencePointLat": 47.76807182520187, + "ReferencePointLong": 16.530610531894183, + "Run": 1, + "TransectDistance": 2, + "TransectStyleComplexItem": { + "CameraCalc": { + "AdjustedFootprintFrontal": 25, + "AdjustedFootprintSide": 25, + "CameraName": "Manual (no camera specs)", + "DistanceToSurface": 15, + "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.76782117293054, + 16.53045952207802, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76801762181985, + 16.531145995440625, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812651838593, + 16.53090512987649, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768125250928335, + 16.530839239105784, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768187963203616, + 16.531058383792516, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768204997344334, + 16.531049622605572, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812144975848, + 16.530757670470322, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811764852164, + 16.530676101820056, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768222031484335, + 16.53104086139957, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768239065614615, + 16.53103210018785, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768113847226864, + 16.530594533208365, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811004588309, + 16.530512964621895, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825609974418, + 16.531023338983744, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76827313388202, + 16.53101457776058, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810624447235, + 16.53043139603398, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815568510607, + 16.530399305509537, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76832423627323, + 16.530988294070095, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834127040819, + 16.53097953282404, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76822245220941, + 16.530564330637993, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 21, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76831125125834, + 16.530806346203697, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768358304533436, + 16.530970771572267, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768307202146545, + 16.53099705529708, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809864148597, + 16.530268258893802, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768102443012644, + 16.530349827431266, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76829016801015, + 16.531005816531692, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768170929071175, + 16.531067144987073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812500283788, + 16.530906658047133, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809124406109, + 16.53085697602152, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815389493801, + 16.53107590617591, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 31, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768136860795146, + 16.531084667345684, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76805748523563, + 16.53080729404687, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76802372639744, + 16.53075761214986, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811982666054, + 16.531093428523086, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 35, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810279252523, + 16.53110218969476, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798996754654, + 16.530707930317146, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 37, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76795620865596, + 16.530658248522045, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 38, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768085758380195, + 16.53111095084737, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 39, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806872423443, + 16.5311197120076, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76792244976166, + 16.53060856681793, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 41, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76788869084566, + 16.53055888517811, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768051690105956, + 16.531128473148772, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 43, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76803465596775, + 16.53113723429756, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 44, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.767854931889964, + 16.530509203589244, + 15 + ], + "type": "SimpleItem" + } + ], + "Refly90Degrees": false, + "TurnAroundDistance": 10, + "VisualTransectPoints": [ + [ + 47.76782117293054, + 16.53045952207802 + ], + [ + 47.76801762181985, + 16.531145995440625 + ], + [ + 47.76812651838593, + 16.53090512987649 + ], + [ + 47.768125250928335, + 16.530839239105784 + ], + [ + 47.768187963203616, + 16.531058383792516 + ], + [ + 47.768204997344334, + 16.531049622605572 + ], + [ + 47.76812144975848, + 16.530757670470322 + ], + [ + 47.76811764852164, + 16.530676101820056 + ], + [ + 47.768222031484335, + 16.53104086139957 + ], + [ + 47.768239065614615, + 16.53103210018785 + ], + [ + 47.768113847226864, + 16.530594533208365 + ], + [ + 47.76811004588309, + 16.530512964621895 + ], + [ + 47.76825609974418, + 16.531023338983744 + ], + [ + 47.76827313388202, + 16.53101457776058 + ], + [ + 47.76810624447235, + 16.53043139603398 + ], + [ + 47.76815568510607, + 16.530399305509537 + ], + [ + 47.76832423627323, + 16.530988294070095 + ], + [ + 47.76834127040819, + 16.53097953282404 + ], + [ + 47.76822245220941, + 16.530564330637993 + ], + [ + 47.76831125125834, + 16.530806346203697 + ], + [ + 47.768358304533436, + 16.530970771572267 + ], + [ + 47.768307202146545, + 16.53099705529708 + ], + [ + 47.76809864148597, + 16.530268258893802 + ], + [ + 47.768102443012644, + 16.530349827431266 + ], + [ + 47.76829016801015, + 16.531005816531692 + ], + [ + 47.768170929071175, + 16.531067144987073 + ], + [ + 47.76812500283788, + 16.530906658047133 + ], + [ + 47.76809124406109, + 16.53085697602152 + ], + [ + 47.76815389493801, + 16.53107590617591 + ], + [ + 47.768136860795146, + 16.531084667345684 + ], + [ + 47.76805748523563, + 16.53080729404687 + ], + [ + 47.76802372639744, + 16.53075761214986 + ], + [ + 47.76811982666054, + 16.531093428523086 + ], + [ + 47.76810279252523, + 16.53110218969476 + ], + [ + 47.76798996754654, + 16.530707930317146 + ], + [ + 47.76795620865596, + 16.530658248522045 + ], + [ + 47.768085758380195, + 16.53111095084737 + ], + [ + 47.76806872423443, + 16.5311197120076 + ], + [ + 47.76792244976166, + 16.53060856681793 + ], + [ + 47.76788869084566, + 16.53055888517811 + ], + [ + 47.768051690105956, + 16.531128473148772 + ], + [ + 47.76803465596775, + 16.53113723429756 + ], + [ + 47.767854931889964, + 16.530509203589244 + ] + ], + "version": 1 + }, + "Type": 1, + "Variant": 0, + "complexItemType": "CircularSurvey", + "polygon": [ + [ + 47.76809580679245, + 16.530246122817612 + ], + [ + 47.76823933601322, + 16.53060087654427 + ], + [ + 47.7683711160486, + 16.530967006195464 + ], + [ + 47.7680076482754, + 16.531153949077463 + ], + [ + 47.7677855557718, + 16.530403347547246 + ], + [ + 47.768126518382985, + 16.5309051298743 + ] + ], + "type": "ComplexItem", + "version": 1 + }, + { + "AMSLAltAboveTerrain": 0, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 48, + "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/Paths/temp3.plan b/Paths/temp3.plan new file mode 100644 index 000000000..0eb1e5655 --- /dev/null +++ b/Paths/temp3.plan @@ -0,0 +1,988 @@ +{ + "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, + 5 + ], + "type": "SimpleItem" + }, + { + "Alpha": 23, + "MinLength": 5, + "NumRuns": 1, + "ReferencePointAlt": 0, + "ReferencePointLat": 47.76807182520187, + "ReferencePointLong": 16.530610531894183, + "Run": 1, + "TransectDistance": 2, + "TransectStyleComplexItem": { + "CameraCalc": { + "AdjustedFootprintFrontal": 25, + "AdjustedFootprintSide": 25, + "CameraName": "Manual (no camera specs)", + "DistanceToSurface": 15, + "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.76782117293054, + 16.53045952207802, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76801762181985, + 16.531145995440625, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812651838593, + 16.53090512987649, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768125250928335, + 16.530839239105784, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768187963203616, + 16.531058383792516, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768204997344334, + 16.531049622605572, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812144975848, + 16.530757670470322, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811764852164, + 16.530676101820056, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768222031484335, + 16.53104086139957, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768239065614615, + 16.53103210018785, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768113847226864, + 16.530594533208365, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811004588309, + 16.530512964621895, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825609974418, + 16.531023338983744, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76827313388202, + 16.53101457776058, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810624447235, + 16.53043139603398, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815568510607, + 16.530399305509537, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76832423627323, + 16.530988294070095, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834127040819, + 16.53097953282404, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76822245220941, + 16.530564330637993, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 21, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76831125125834, + 16.530806346203697, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768358304533436, + 16.530970771572267, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768307202146545, + 16.53099705529708, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809864148597, + 16.530268258893802, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768102443012644, + 16.530349827431266, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76829016801015, + 16.531005816531692, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768170929071175, + 16.531067144987073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812500283788, + 16.530906658047133, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809124406109, + 16.53085697602152, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815389493801, + 16.53107590617591, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 31, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768136860795146, + 16.531084667345684, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76805748523563, + 16.53080729404687, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76802372639744, + 16.53075761214986, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811982666054, + 16.531093428523086, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 35, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810279252523, + 16.53110218969476, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798996754654, + 16.530707930317146, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 37, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76795620865596, + 16.530658248522045, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 38, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768085758380195, + 16.53111095084737, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 39, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806872423443, + 16.5311197120076, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76792244976166, + 16.53060856681793, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 41, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76788869084566, + 16.53055888517811, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768051690105956, + 16.531128473148772, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 43, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76803465596775, + 16.53113723429756, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 44, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.767854931889964, + 16.530509203589244, + 15 + ], + "type": "SimpleItem" + } + ], + "Refly90Degrees": false, + "TurnAroundDistance": 10, + "VisualTransectPoints": [ + [ + 47.76782117293054, + 16.53045952207802 + ], + [ + 47.76801762181985, + 16.531145995440625 + ], + [ + 47.76812651838593, + 16.53090512987649 + ], + [ + 47.768125250928335, + 16.530839239105784 + ], + [ + 47.768187963203616, + 16.531058383792516 + ], + [ + 47.768204997344334, + 16.531049622605572 + ], + [ + 47.76812144975848, + 16.530757670470322 + ], + [ + 47.76811764852164, + 16.530676101820056 + ], + [ + 47.768222031484335, + 16.53104086139957 + ], + [ + 47.768239065614615, + 16.53103210018785 + ], + [ + 47.768113847226864, + 16.530594533208365 + ], + [ + 47.76811004588309, + 16.530512964621895 + ], + [ + 47.76825609974418, + 16.531023338983744 + ], + [ + 47.76827313388202, + 16.53101457776058 + ], + [ + 47.76810624447235, + 16.53043139603398 + ], + [ + 47.76815568510607, + 16.530399305509537 + ], + [ + 47.76832423627323, + 16.530988294070095 + ], + [ + 47.76834127040819, + 16.53097953282404 + ], + [ + 47.76822245220941, + 16.530564330637993 + ], + [ + 47.76831125125834, + 16.530806346203697 + ], + [ + 47.768358304533436, + 16.530970771572267 + ], + [ + 47.768307202146545, + 16.53099705529708 + ], + [ + 47.76809864148597, + 16.530268258893802 + ], + [ + 47.768102443012644, + 16.530349827431266 + ], + [ + 47.76829016801015, + 16.531005816531692 + ], + [ + 47.768170929071175, + 16.531067144987073 + ], + [ + 47.76812500283788, + 16.530906658047133 + ], + [ + 47.76809124406109, + 16.53085697602152 + ], + [ + 47.76815389493801, + 16.53107590617591 + ], + [ + 47.768136860795146, + 16.531084667345684 + ], + [ + 47.76805748523563, + 16.53080729404687 + ], + [ + 47.76802372639744, + 16.53075761214986 + ], + [ + 47.76811982666054, + 16.531093428523086 + ], + [ + 47.76810279252523, + 16.53110218969476 + ], + [ + 47.76798996754654, + 16.530707930317146 + ], + [ + 47.76795620865596, + 16.530658248522045 + ], + [ + 47.768085758380195, + 16.53111095084737 + ], + [ + 47.76806872423443, + 16.5311197120076 + ], + [ + 47.76792244976166, + 16.53060856681793 + ], + [ + 47.76788869084566, + 16.53055888517811 + ], + [ + 47.768051690105956, + 16.531128473148772 + ], + [ + 47.76803465596775, + 16.53113723429756 + ], + [ + 47.767854931889964, + 16.530509203589244 + ] + ], + "version": 1 + }, + "Type": 1, + "Variant": 0, + "complexItemType": "CircularSurvey", + "polygon": [ + [ + 47.76809580679245, + 16.530246122817612 + ], + [ + 47.76823933601322, + 16.53060087654427 + ], + [ + 47.7683711160486, + 16.530967006195464 + ], + [ + 47.7680076482754, + 16.531153949077463 + ], + [ + 47.7677855557718, + 16.530403347547246 + ], + [ + 47.768126518382985, + 16.5309051298743 + ] + ], + "type": "ComplexItem", + "version": 1 + }, + { + "AMSLAltAboveTerrain": 0, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 48, + "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 ec80cec53..2def2ccee 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -454,6 +454,7 @@ HEADERS += \ src/Wima/Snake/SnakeTileLocal.h \ src/Wima/Snake/SnakeTiles.h \ src/Wima/Snake/SnakeTilesLocal.h \ + src/Wima/StateMachine.h \ src/Wima/WaypointManager/AreaInterface.h \ src/Wima/WaypointManager/DefaultManager.h \ src/Wima/WaypointManager/GenericWaypointManager.h \ @@ -515,6 +516,7 @@ SOURCES += \ src/Wima/Snake/NemoInterface.cpp \ src/Wima/Snake/QNemoProgress.cc \ src/Wima/Snake/SnakeTile.cpp \ + src/Wima/StateMachine.cpp \ src/Wima/WaypointManager/AreaInterface.cpp \ src/Wima/WaypointManager/DefaultManager.cpp \ src/Wima/WaypointManager/GenericWaypointManager.cpp \ diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index 40387c2e2..ca369e21c 100644 --- a/src/Wima/CircularSurvey.cc +++ b/src/Wima/CircularSurvey.cc @@ -3,19 +3,25 @@ // QGC #include "JsonHelper.h" #include "QGCApplication.h" +#include "QGCLoggingCategory.h" // Wima #include "snake.h" #define CLIPPER_SCALE 1000000 #include "clipper/clipper.hpp" -template ClipperLib::cInt get(ClipperLib::IntPoint &p); -template <> ClipperLib::cInt get<0>(ClipperLib::IntPoint &p) { return p.X; } -template <> ClipperLib::cInt get<1>(ClipperLib::IntPoint &p) { return p.Y; } #include "Geometry/GenericCircle.h" +#include "Snake/SnakeTile.h" + // boost #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) {} @@ -30,15 +36,13 @@ constexpr typename std::underlying_type::type integral(T value) { return static_cast::type>(value); } -bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, - bool useDepot, const QList &polygon, - snake::Length deltaR, snake::Angle deltaAlpha, - snake::Length minLength, snake::Transects &transects); +bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR, + snake::Angle deltaAlpha, snake::Length minLength, + snake::Transects &transects); -bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, - bool useDepot, const QList &polygon, - snake::Length distance, snake::Angle angle, - snake::Length minLength, snake::Transects &transects); +bool linearTransects(const snake::FPolygon &polygon, snake::Length distance, + snake::Angle angle, snake::Length minLength, + snake::Transects &transects); const char *CircularSurvey::settingsGroup = "CircularSurvey"; const char *CircularSurvey::transectDistanceName = "TransectDistance"; @@ -80,10 +84,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, &CircularSurvey::_rebuildTransects); connect(this, &CircularSurvey::refPointChanged, this, &CircularSurvey::_rebuildTransects); - connect(this, &CircularSurvey::depotChanged, this, - &CircularSurvey::_rebuildTransects); - connect(this, &CircularSurvey::safeAreaChanged, this, - &CircularSurvey::_rebuildTransects); connect(&this->_type, &Fact::rawValueChanged, this, &CircularSurvey::_rebuildTransects); connect(&this->_variant, &Fact::rawValueChanged, this, @@ -93,19 +93,23 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, 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, &CircularSurvey::_setTransects); connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this, &CircularSurvey::calculatingChanged); - this->_transectsDirty = false; + this->_transectsDirty = true; } CircularSurvey::~CircularSurvey() {} -void CircularSurvey::resetReference() { - setRefPoint(_surveyAreaPolygon.center()); -} +void CircularSurvey::resetReference() { setRefPoint(_mArea.center()); } void CircularSurvey::reverse() { this->_state = STATE::REVERSE; @@ -133,12 +137,6 @@ QList CircularSurvey::variantNames() const { return _variantNames; } QList CircularSurvey::runNames() const { return _runNames; } -QGeoCoordinate CircularSurvey::depot() const { return this->_depot; } - -QList CircularSurvey::safeArea() const { - return this->_safeArea; -} - const QList> &CircularSurvey::rawTransects() const { return this->_rawTransects; } @@ -150,18 +148,31 @@ void CircularSurvey::setHidePolygon(bool hide) { } } -void CircularSurvey::setDepot(const QGeoCoordinate &depot) { - if (this->_depot != depot) { - this->_depot = depot; - this->_depot.setAltitude(0); - emit depotChanged(); +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(); } } -void CircularSurvey::setSafeArea(const QList &safeArea) { - if (this->_safeArea != safeArea) { - this->_safeArea = safeArea; - emit safeAreaChanged(); +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(); } } @@ -358,6 +369,8 @@ void CircularSurvey::_changeRun() { } void CircularSurvey::_updateWorker() { + // Mark transects as dirty. + this->_transectsDirty = true; // Reset data. this->_transects.clear(); this->_rawTransects.clear(); @@ -369,15 +382,51 @@ void CircularSurvey::_updateWorker() { // Prepare data. auto ref = this->_referencePoint; - auto polygon = this->_surveyAreaPolygon.coordinateList(); - for (auto &v : polygon) { + auto geoPolygon = this->_mArea.coordinateList(); + for (auto &v : geoPolygon) { v.setAltitude(0); } - auto safeArea = this->_safeArea; - for (auto &v : safeArea) { + 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(); + 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); + pPolygon->inners().push_back(std::move(tileENU.outer())); + } else { + qCWarning(CircularSurveyLog) + << "CS::_updateWorker(): progress.size() != tiles->count()."; + return; + } + } + } + } else { + qCWarning(CircularSurveyLog) + << "CS::_updateWorker(): progress.size() != tiles->count()."; + return; + } + bg::correct(*pPolygon); + + // Convert safe area. + auto geoDepot = this->_jArea.center(); + auto geoSafeArea = this->_jArea.coordinateList(); + if (!geoDepot.isValid() || !(geoSafeArea.size() >= 3)) { + return; + } + for (auto &v : geoSafeArea) { v.setAltitude(0); } - auto depot = this->_depot; + snake::FPolygon safeArea; + snake::areaToEnu(ref, geoSafeArea, safeArea); + snake::FPoint depot; + snake::toENU(ref, geoDepot, depot); // Routing par. RoutingParameter par; @@ -393,15 +442,8 @@ void CircularSurvey::_updateWorker() { } par.numRuns = this->_numRuns.rawValue().toUInt(); - // Convert safe area. auto &safeAreaENU = par.safeArea; - bool useDepot = false; - if (this->_depot.isValid() && this->_safeArea.size() >= 3) { - useDepot = true; - snake::areaToEnu(ref, safeArea, safeAreaENU); - } else { - snake::areaToEnu(ref, polygon, safeAreaENU); - } + snake::areaToEnu(ref, geoSafeArea, safeAreaENU); // Fetch transect parameter. auto distance = snake::Length(this->_transectDistance.rawValue().toDouble() * @@ -416,10 +458,11 @@ void CircularSurvey::_updateWorker() { // Clip angle. if (alpha >= snake::Angle(0.3 * bu::degree::degree) && alpha <= snake::Angle(45 * bu::degree::degree)) { - auto generator = [ref, depot, useDepot, polygon, distance, alpha, + auto generator = [depot, pPolygon, distance, alpha, minLength](snake::Transects &transects) -> bool { - return circularTransects(ref, depot, useDepot, polygon, distance, alpha, - minLength, transects); + transects.push_back(snake::FLineString{depot}); + return circularTransects(*pPolygon, distance, alpha, minLength, + transects); }; // Start routing worker. this->_pWorker->route(par, generator); @@ -431,20 +474,18 @@ void CircularSurvey::_updateWorker() { } } } else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) { - auto generator = [ref, depot, useDepot, polygon, distance, alpha, + auto generator = [depot, pPolygon, distance, alpha, minLength](snake::Transects &transects) -> bool { - return linearTransects(ref, depot, useDepot, polygon, distance, alpha, - minLength, transects); + transects.push_back(snake::FLineString{depot}); + return linearTransects(*pPolygon, distance, alpha, minLength, transects); }; // Start routing worker. this->_pWorker->route(par, generator); } else { - qWarning() + qCWarning(CircularSurveyLog) << "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:" << this->_type.rawValue().toUInt(); } - // Mark transects as dirty. - this->_transectsDirty = true; } void CircularSurvey::_changeVariantRunWorker() { @@ -494,9 +535,10 @@ void CircularSurvey::_changeVariantRunWorker() { } } else { // error - qWarning() << "Variant or run out of bounds (variant = " << variant - << ", run = " << run << ")."; - qWarning() << "Resetting variant and run."; + qCWarning(CircularSurveyLog) + << "Variant or run out of bounds (variant = " << variant + << ", run = " << run << ")."; + qCWarning(CircularSurveyLog) << "Resetting variant and run."; disconnect(&this->_variant, &Fact::rawValueChanged, this, &CircularSurvey::_changeVariant); @@ -552,14 +594,8 @@ void CircularSurvey::_storeWorker() { const auto &pRoutingData = this->_pRoutingData; const auto &ori = this->_referencePoint; const auto &transectsENU = pRoutingData->transects; - std::size_t startIdx = 0; - bool depotValid = false; - if (transectsENU.size() > 0 && transectsENU.front().size() == 1) { - depotValid = true; - startIdx = 1; - } QList> rawTransects; - for (std::size_t i = startIdx; i < transectsENU.size(); ++i) { + for (std::size_t i = 0; i < transectsENU.size(); ++i) { const auto &t = transectsENU[i]; rawTransects.append(QList()); auto trGeo = rawTransects.back(); @@ -585,7 +621,7 @@ void CircularSurvey::_storeWorker() { if (info.size() > 1) { // Find index of first waypoint. std::size_t idxFirst = 0; - const auto &infoFirst = depotValid ? info.at(1) : info.at(0); + const auto &infoFirst = info.at(1); const auto &firstTransect = transectsENU[infoFirst.index]; if (firstTransect.size() > 0) { const auto &firstWaypoint = @@ -623,13 +659,16 @@ void CircularSurvey::_storeWorker() { list.append(CoordInfo_t{c, CoordTypeInterior}); } } else { - qWarning() << "CS::_storeWorker(): lastTransect.size() == 0"; + qCWarning(CircularSurveyLog) + << "CS::_storeWorker(): lastTransect.size() == 0"; } } else { - qWarning() << "CS::_storeWorker(): firstTransect.size() == 0"; + qCWarning(CircularSurveyLog) + << "CS::_storeWorker(): firstTransect.size() == 0"; } } else { - qWarning() << "CS::_storeWorker(): transectsInfo.size() <= 1"; + qCWarning(CircularSurveyLog) + << "CS::_storeWorker(): transectsInfo.size() <= 1"; } } // Remove empty runs. @@ -713,52 +752,39 @@ bool CircularSurvey::readyForSave() const { double CircularSurvey::additionalTimeDelay() const { return 0; } void CircularSurvey::_rebuildTransectsPhase1(void) { - qWarning() << "_rebuildTransectsPhase1: TODO: remove depot valid stuff"; -#ifdef SHOW_CIRCULAR_SURVEY_TIME auto start = std::chrono::high_resolution_clock::now(); -#endif switch (this->_state) { case STATE::STORE: -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "CS::rebuildTransectsPhase1: store."; -#endif + qCWarning(CircularSurveyLog) << "CS::rebuildTransectsPhase1: store."; this->_storeWorker(); break; case STATE::VARIANT_CHANGE: -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "CS::rebuildTransectsPhase1: variant change."; -#endif + qCWarning(CircularSurveyLog) + << "CS::rebuildTransectsPhase1: variant change."; this->_changeVariantRunWorker(); break; case STATE::RUN_CHANGE: -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "CS::rebuildTransectsPhase1: run change."; -#endif + qCWarning(CircularSurveyLog) << "CS::rebuildTransectsPhase1: run change."; this->_changeVariantRunWorker(); break; case STATE::REVERSE: -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "CS::rebuildTransectsPhase1: reverse."; -#endif + qCWarning(CircularSurveyLog) << "CS::rebuildTransectsPhase1: reverse."; this->_reverseWorker(); break; case STATE::DEFAULT: -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "CS::rebuildTransectsPhase1: update."; -#endif + qCWarning(CircularSurveyLog) << "CS::rebuildTransectsPhase1: update."; this->_updateWorker(); break; } this->_state = STATE::DEFAULT; -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "CS::rebuildTransectsPhase1(): " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start) - .count() - << " ms"; -#endif + qCWarning(CircularSurveyLog) + << "CS::rebuildTransectsPhase1(): " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; } void CircularSurvey::_recalcComplexDistance() { @@ -798,54 +824,47 @@ bool CircularSurvey::calculating() const { return this->_pWorker->calculating(); } -bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, - bool useDepot, const QList &polygon, - snake::Length deltaR, snake::Angle deltaAlpha, - snake::Length minLength, snake::Transects &transects) { -#ifdef SHOW_CIRCULAR_SURVEY_TIME +bool circularTransects(const snake::FPolygon &polygon, snake::Length deltaR, + snake::Angle deltaAlpha, snake::Length minLength, + snake::Transects &transects) { auto s1 = std::chrono::high_resolution_clock::now(); -#endif + // Check preconitions - if (polygon.size() >= 3) { + if (polygon.outer().size() >= 3) { using namespace boost::units; // Convert geo polygon to ENU polygon. - snake::FPolygon polygonENU; - snake::FPoint originENU{0, 0}; - snake::FPoint depotENU{0, 0}; - snake::areaToEnu(ref, polygon, polygonENU); - snake::toENU(ref, ref, originENU); - snake::toENU(ref, depot, depotENU); + snake::FPoint origin{0, 0}; std::string error; // Check validity. - if (!bg::is_valid(polygonENU, error)) { -#ifdef DEBUG_CIRCULAR_SURVEY - qWarning() << "CS::circularTransects(): " - "invalid polygon."; - qWarning() << error.c_str(); + if (!bg::is_valid(polygon, error)) { + qCWarning(CircularSurveyLog) << "CS::circularTransects(): " + "invalid polygon."; + qCWarning(CircularSurveyLog) << error.c_str(); std::stringstream ss; - ss << bg::wkt(polygonENU); - qWarning() << ss.str().c_str(); -#endif + ss << bg::wkt(polygon); + qCWarning(CircularSurveyLog) << ss.str().c_str(); } else { // Calculate polygon distances and angles. std::vector distances; - distances.reserve(polygonENU.outer().size()); + distances.reserve(polygon.outer().size()); std::vector angles; - angles.reserve(polygonENU.outer().size()); + angles.reserve(polygon.outer().size()); //#ifdef DEBUG_CIRCULAR_SURVEY - // qWarning() << "CS::circularTransects():"; + // qCWarning(CircularSurveyLog) << "CS::circularTransects():"; //#endif - for (const auto &p : polygonENU.outer()) { - snake::Length distance = bg::distance(originENU, p) * si::meter; + 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 - // qWarning() << "distances, angles, coordinates:"; - // qWarning() << to_string(distance).c_str(); - // qWarning() << to_string(snake::Degree(alpha)).c_str(); - // qWarning() << "x = " << p.get<0>() << "y = " << p.get<1>(); + // 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 } @@ -853,8 +872,8 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, snake::Angle alpha1(0 * degree::degree); snake::Angle alpha2(360 * degree::degree); // Determine r_min by successive approximation - if (!bg::within(originENU, polygonENU)) { - rMin = bg::distance(originENU, polygonENU) * si::meter; + if (!bg::within(origin, polygon.outer())) { + rMin = bg::distance(origin, polygon) * si::meter; } auto rMax = (*std::max_element(distances.begin(), @@ -865,9 +884,9 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, 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(originENU.get<0>())), - ClipperLib::cInt(std::round(originENU.get<1>()))}; + auto originScaled = + ClipperLib::IntPoint{ClipperLib::cInt(std::round(origin.get<0>())), + ClipperLib::cInt(std::round(origin.get<1>()))}; // Generate circle sectors. auto rScaled = rMinScaled; @@ -876,15 +895,19 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, const auto nSectors = long(std::round(((alpha2 - alpha1) / deltaAlpha).value())); //#ifdef DEBUG_CIRCULAR_SURVEY - // qWarning() << "CS::circularTransects(): sector parameres:"; - // qWarning() << "alpha1: " << - // to_string(snake::Degree(alpha1)).c_str(); qWarning() << "alpha2: + // qCWarning(CircularSurveyLog) << "CS::circularTransects(): sector + // parameres:"; qCWarning(CircularSurveyLog) << "alpha1: " << + // to_string(snake::Degree(alpha1)).c_str(); + // qCWarning(CircularSurveyLog) << "alpha2: // " - // << to_string(snake::Degree(alpha2)).c_str(); qWarning() << "n: " - // << to_string((alpha2 - alpha1) / deltaAlpha).c_str(); qWarning() - // << "nSectors: " << nSectors; qWarning() << "rMin: " << - // to_string(rMin).c_str(); qWarning() << "rMax: " << - // to_string(rMax).c_str(); qWarning() << "nTran: " << nTran; + // << 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; @@ -896,7 +919,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, // Clip sectors to polygonENU. ClipperLib::Path polygonClipper; snake::FPolygon shrinked; - snake::offsetPolygon(polygonENU, shrinked, -0.3); + snake::offsetPolygon(polygon, shrinked, -0.3); auto &outer = shrinked.outer(); polygonClipper.reserve(outer.size()); for (auto it = outer.begin(); it < outer.end() - 1; ++it) { @@ -911,11 +934,31 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, clipper.Execute(ClipperLib::ctIntersection, transectsClipper, ClipperLib::pftNonZero, ClipperLib::pftNonZero); + // Subtract holes. + if (polygon.inners().size() > 0) { + vector processedTiles; + for (const auto &h : polygon.inners()) { + ClipperLib::Path path; + for (const auto &v : h) { + path.push_back(ClipperLib::IntPoint{ + static_cast(v.get<0>() * CLIPPER_SCALE), + static_cast(v.get<1>() * CLIPPER_SCALE)}); + } + processedTiles.push_back(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 - if (useDepot) { - transects.push_back(snake::FLineString{depotENU}); - } for (const auto &child : transectsClipper.Childs) { snake::FLineString transect; transect.reserve(child->Contour.size()); @@ -926,6 +969,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } 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) { @@ -972,9 +1016,9 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } } } + // Remove short transects - auto begin = useDepot ? transects.begin() + 1 : transects.begin(); - for (auto it = begin; it < transects.end();) { + for (auto it = transects.begin(); it < transects.end();) { if (bg::length(*it) < minLength.value()) { it = transects.erase(it); } else { @@ -982,85 +1026,42 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } } - if (!useDepot) { - // Move transect with min. distance to the front. - auto minDist = std::numeric_limits::max(); - auto minIt = transects.begin(); - bool reverse = false; - for (auto it = transects.begin(); it < transects.end(); ++it) { - auto distFront = bg::distance(originENU, it->front()); - auto distBack = bg::distance(originENU, it->back()); - if (distFront < minDist) { - minDist = distFront; - minIt = it; - reverse = false; - } - if (distBack < minDist) { - minDist = distBack; - minIt = it; - reverse = true; - } - } - // Swap and reverse (if necessary). - if (minIt != transects.begin()) { - auto minTransect = *minIt; - if (reverse) { - snake::FLineString rev; - for (auto it = minTransect.end() - 1; it >= minTransect.begin(); - --it) { - rev.push_back(*it); - } - minTransect = rev; - } - *minIt = *transects.begin(); - *transects.begin() = minTransect; - } - } -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "CS::circularTransects(): transect gen. time: " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - s1) - .count() - << " ms"; -#endif + qCWarning(CircularSurveyLog) + << "CS::circularTransects(): transect gen. time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - s1) + .count() + << " ms"; return true; } } return false; } -bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, - bool useDepot, const QList &polygon, - snake::Length distance, snake::Angle angle, - snake::Length minLength, snake::Transects &transects) { +bool linearTransects(const snake::FPolygon &polygon, snake::Length distance, + snake::Angle angle, snake::Length minLength, + snake::Transects &transects) { namespace tr = bg::strategy::transform; -#ifdef SHOW_CIRCULAR_SURVEY_TIME auto s1 = std::chrono::high_resolution_clock::now(); -#endif + // Check preconitions - if (polygon.size() >= 3) { + if (polygon.outer().size() >= 3) { // Convert to ENU system. - snake::FPolygon polygonENU; - snake::areaToEnu(origin, polygon, polygonENU); std::string error; // Check validity. - if (!bg::is_valid(polygonENU, error)) { -#ifdef DEBUG_CIRCULAR_SURVEY - qWarning() << "CS::circularTransects(): " - "invalid polygon."; - qWarning() << error.c_str(); + if (!bg::is_valid(polygon, error)) { std::stringstream ss; - ss << bg::wkt(polygonENU); - qWarning() << ss.str().c_str(); -#endif + ss << bg::wkt(polygon); + + qCWarning(CircularSurveyLog) << "CS::circularTransects(): " + "invalid polygon. " + << error.c_str() << ss.str().c_str(); } else { - snake::FPoint depotENU; - snake::toENU(origin, depot, depotENU); tr::rotate_transformer rotate(angle.value() * 180 / M_PI); // Rotate polygon by angle and calculate bounding box. snake::FPolygon polygonENURotated; - bg::transform(polygonENU, polygonENURotated, rotate); + bg::transform(polygon.outer(), polygonENURotated.outer(), rotate); snake::FBox box; boost::geometry::envelope(polygonENURotated, box); double x0 = box.min_corner().get<0>(); @@ -1101,13 +1102,14 @@ bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, std::stringstream ss; ss << "Not able to generate transects. Parameter: distance = " << distance << std::endl; - qWarning() << "CircularSurvey::linearTransects(): " << ss.str().c_str(); + qCWarning(CircularSurveyLog) + << "CircularSurvey::linearTransects(): " << ss.str().c_str(); return false; } // Convert measurement area to clipper path. snake::FPolygon shrinked; - snake::offsetPolygon(polygonENU, shrinked, -0.2); + snake::offsetPolygon(polygon, shrinked, -0.2); auto &outer = shrinked.outer(); ClipperLib::Path polygonClipper; for (auto vertex : outer) { @@ -1125,10 +1127,30 @@ bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, clipper.Execute(ClipperLib::ctIntersection, clippedTransecs, ClipperLib::pftNonZero, ClipperLib::pftNonZero); - // Extract transects from PolyTree and convert them to BoostLineString - if (useDepot) { - transects.push_back(snake::FLineString{depotENU}); + // Subtract holes. + if (polygon.inners().size() > 0) { + vector processedTiles; + for (const auto &h : polygon.inners()) { + ClipperLib::Path path; + for (const auto &v : h) { + path.push_back(ClipperLib::IntPoint{ + static_cast(v.get<0>() * CLIPPER_SCALE), + static_cast(v.get<1>() * CLIPPER_SCALE)}); + } + processedTiles.push_back(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{ @@ -1148,16 +1170,16 @@ bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, std::stringstream ss; ss << "Not able to generate transects. Parameter: minLength = " << minLength << std::endl; - qWarning() << "CircularSurvey::linearTransects(): " << ss.str().c_str(); + qCWarning(CircularSurveyLog) + << "CircularSurvey::linearTransects(): " << ss.str().c_str(); return false; } -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "CS::circularTransects(): transect gen. time: " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - s1) - .count() - << " ms"; -#endif + qCWarning(CircularSurveyLog) + << "CS::circularTransects(): transect gen. time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - s1) + .count() + << " ms"; return true; } } diff --git a/src/Wima/CircularSurvey.h b/src/Wima/CircularSurvey.h index 85168d7c2..d8528d8e6 100644 --- a/src/Wima/CircularSurvey.h +++ b/src/Wima/CircularSurvey.h @@ -6,6 +6,9 @@ #include "SettingsFact.h" #include "TransectStyleComplexItem.h" +#include "Geometry/WimaJoinedAreaData.h" +#include "Geometry/WimaMeasurementAreaData.h" + class RoutingThread; class RoutingData; @@ -50,8 +53,10 @@ public: // Property setters void setRefPoint(const QGeoCoordinate &refPt); void setHidePolygon(bool hide); - void setDepot(const QGeoCoordinate &depot); - void setSafeArea(const QList &safeArea); + void setMeasurementArea(const WimaMeasurementAreaData &mArea); + void setJoinedArea(const WimaJoinedAreaData &jArea); + void setMeasurementArea(const WimaMeasurementArea &mArea); + void setJoinedArea(const WimaJoinedArea &jArea); // Property getters QGeoCoordinate refPoint() const; @@ -68,7 +73,6 @@ public: QList variantNames() const; QList runNames() const; QGeoCoordinate depot() const; - QList safeArea() const; const QList> &rawTransects() const; // Overrides @@ -105,9 +109,10 @@ signals: void calculatingChanged(); void hidePolygonChanged(); void depotChanged(); - void safeAreaChanged(); void variantNamesChanged(); void runNamesChanged(); + void measurementAreaChanged(); + void joinedAreaChanged(); private slots: // Overrides from TransectStyleComplexItem @@ -147,14 +152,16 @@ private: SettingsFact _run; QList _runNames; + // Area data + WimaMeasurementAreaData _mArea; + WimaJoinedAreaData _jArea; + // Worker using PtrWorker = std::shared_ptr; PtrWorker _pWorker; PtrRoutingData _pRoutingData; // Routing data. - QGeoCoordinate _depot; - QList _safeArea; QList> _rawTransects; using Runs = QVector; QVector _variantVector; diff --git a/src/Wima/Geometry/GenericCircle.h b/src/Wima/Geometry/GenericCircle.h index 09b9740fc..8d35c785a 100644 --- a/src/Wima/Geometry/GenericCircle.h +++ b/src/Wima/Geometry/GenericCircle.h @@ -8,7 +8,7 @@ #include "boost/units/systems/angle/degrees.hpp" #include "boost/units/systems/si.hpp" -template auto get(Point &p); +template auto get(const Point &p); namespace bu = boost::units; @@ -112,11 +112,11 @@ void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2, } double a = a1; + using Point = + std::remove_cv_t>; auto x0 = get<0>(circ.origin()); auto y0 = get<1>(circ.origin()); auto r = circ.radius(); - using Point = - std::remove_cv_t>; using NumberType = std::remove_cv_t>; while (n--) { auto x = NumberType(x0 + r * std::cos(a)); @@ -139,11 +139,11 @@ void approximate(Circle &circ, long n, Container &c) { } double a = 0; + using Point = + std::remove_cv_t>; auto x0 = get<0>(circ.origin()); auto y0 = get<1>(circ.origin()); auto r = circ.radius(); - using Point = - std::remove_cv_t>; using NumberType = std::remove_cv_t>; while (n--) { auto x = NumberType(x0 + r * std::cos(a)); diff --git a/src/Wima/Geometry/WimaAreaData.cc b/src/Wima/Geometry/WimaAreaData.cc index 1c1bad8ce..56909032e 100644 --- a/src/Wima/Geometry/WimaAreaData.cc +++ b/src/Wima/Geometry/WimaAreaData.cc @@ -1,23 +1,17 @@ #include "WimaAreaData.h" -WimaAreaData::WimaAreaData(QObject *parent) : QObject(parent) { - _maxAltitude = 0; -} +WimaAreaData::WimaAreaData(QObject *parent) : QObject(parent) {} WimaAreaData::~WimaAreaData() {} -/*! - * \fn double WimaAreaData::maxAltitude() - * - * Returns the maximum altitude at which vehicles are allowed to fly. - */ -double WimaAreaData::maxAltitude() const { return _maxAltitude; } +bool WimaAreaData::operator==(const WimaAreaData &data) const { + return this->_path == data._path && this->_center == data._center; +} + +bool WimaAreaData::operator!=(const WimaAreaData &data) const { + return !this->operator==(data); +} -/*! - * \fn double WimaAreaData::maxAltitude() - * - * Returns the path (vertex list defining the \l {Simple Polygon}). - */ QVariantList WimaAreaData::path() const { return _path; } QGeoCoordinate WimaAreaData::center() const { return _center; } @@ -51,20 +45,6 @@ void WimaAreaData::clear() { _path.clear(); } -/*! - * \fn void WimaAreaData::setMaxAltitude(double maxAltitude) - * - * Sets the maximum altitude member to \a maxAltitude and emits the \c - * maxAltitudeChanged() signal if \a maxAltitude differs from the members value. - */ -void WimaAreaData::setMaxAltitude(double maxAltitude) { - if (!qFuzzyCompare(_maxAltitude, maxAltitude)) { - _maxAltitude = maxAltitude; - - emit maxAltitudeChanged(_maxAltitude); - } -} - void WimaAreaData::setPath(const QVariantList &coordinateList) { _path = coordinateList; _list.clear(); @@ -87,13 +67,11 @@ void WimaAreaData::setCenter(const QGeoCoordinate ¢er) { * Assigns \a other to the invoking object */ void WimaAreaData::assign(const WimaAreaData &other) { - setMaxAltitude(other.maxAltitude()); setPath(other.path()); setCenter(other.center()); } void WimaAreaData::assign(const WimaArea &other) { - setMaxAltitude(other.maxAltitude()); setPath(other.path()); setCenter(other.center()); } @@ -116,6 +94,13 @@ void WimaAreaData::setPath(const QList &coordinateList) { 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); +} + /*! * \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 71cd686c4..5003f89ed 100644 --- a/src/Wima/Geometry/WimaAreaData.h +++ b/src/Wima/Geometry/WimaAreaData.h @@ -20,7 +20,9 @@ public: WimaAreaData & operator=(const WimaAreaData &otherData) = delete; // avoid slicing - double maxAltitude() const; + bool operator==(const WimaAreaData &data) const; + bool operator!=(const WimaAreaData &data) const; + QVariantList path() const; QGeoCoordinate center() const; const QList &coordinateList() const; @@ -32,12 +34,10 @@ public: void clear(); signals: - void maxAltitudeChanged(double maxAltitude); void pathChanged(const QVariantList &coordinateList); void centerChanged(void); public slots: - void setMaxAltitude(double maxAltitude); void setPath(const QList &coordinateList); void setPath(const QVariantList &coordinateList); void setCenter(const QGeoCoordinate ¢er); @@ -51,8 +51,10 @@ private: // Member Variables // see WimaArea.h for explanation - double _maxAltitude; QVariantList _path; - QList _list; + mutable QList _list; QGeoCoordinate _center; }; + +bool operator==(const WimaAreaData &m1, const WimaArea &m2); +bool operator!=(const WimaAreaData &m1, const WimaArea &m2); diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc index 674e90545..d3dadcf32 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.cc +++ b/src/Wima/Geometry/WimaMeasurementArea.cc @@ -5,10 +5,14 @@ #include +#include "QGCLoggingCategory.h" + #ifndef SNAKE_MAX_TILES #define SNAKE_MAX_TILES 1000 #endif +QGC_LOGGING_CATEGORY(WimaMeasurementAreaLog, "WimaMeasurementAreaLog") + TileData::TileData() : tiles(this) {} TileData::~TileData() { tiles.clearAndDeleteContents(); } @@ -21,7 +25,7 @@ TileData &TileData::operator=(const TileData &other) { if (tile != nullptr) { this->tiles.append(new SnakeTile(*tile, this)); } else { - qWarning("TileData::operator=: nullptr"); + qCWarning(WimaMeasurementAreaLog) << "TileData::operator=: nullptr"; } } this->tileCenterPoints = other.tileCenterPoints; @@ -80,7 +84,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent) this /* QObject parent */)), _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], this /* QObject parent */)), - _calculating(false) { + _state(STATE::IDLE) { init(); } @@ -104,7 +108,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other, this /* QObject parent */)), _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], this /* QObject parent */)), - _calculating(false) { + _state(STATE::IDLE) { init(); } @@ -123,11 +127,11 @@ operator=(const WimaMeasurementArea &other) { WimaMeasurementArea::~WimaMeasurementArea() {} QString WimaMeasurementArea::mapVisualQML() const { - return "WimaMeasurementAreaMapVisual.qml"; + return QStringLiteral("WimaMeasurementAreaMapVisual.qml"); } QString WimaMeasurementArea::editorQML() const { - return "WimaMeasurementAreaEditor.qml"; + return QStringLiteral("WimaMeasurementAreaEditor.qml"); } Fact *WimaMeasurementArea::tileHeight() { return &_tileHeight; } @@ -146,7 +150,13 @@ QmlObjectListModel *WimaMeasurementArea::tiles() { return &this->_tileData.tiles; } -QVector WimaMeasurementArea::progress() { return this->_progress; } +const QVector &WimaMeasurementArea::progress() const { + return this->_progress; +} + +QVector WimaMeasurementArea::progressQml() const { + return this->_progress; +} const QmlObjectListModel *WimaMeasurementArea::tiles() const { return &this->_tileData.tiles; @@ -162,7 +172,7 @@ const TileData &WimaMeasurementArea::tileData() const { int WimaMeasurementArea::maxTiles() const { return SNAKE_MAX_TILES; } -bool WimaMeasurementArea::ready() const { return !_calculating; } +bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; } void WimaMeasurementArea::saveToJson(QJsonObject &json) { this->WimaArea::saveToJson(json); @@ -230,7 +240,7 @@ bool WimaMeasurementArea::loadFromJson(const QJsonObject &json, } bool WimaMeasurementArea::setProgress(const QVector &p) { - if (!_calculating) { + if (!ready()) { if (p.size() == this->tiles()->count() && this->_progress != p) { this->_progress = p; emit progressChanged(); @@ -246,18 +256,18 @@ bool WimaMeasurementArea::setProgress(const QVector &p) { void WimaMeasurementArea::doUpdate() { using namespace snake; using namespace boost::units; -#ifdef SNAKE_SHOW_TIME + auto start = std::chrono::high_resolution_clock::now(); -#endif + 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; - if (!this->_calculating && + if (this->_state != STATE::UPDATE && long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES && this->count() >= 3 && this->isSimplePolygon()) { - this->_calculating = true; + setState(STATE::UPDATE); auto polygon = this->coordinateList(); for (auto &v : polygon) { v.setAltitude(0); @@ -266,9 +276,8 @@ void WimaMeasurementArea::doUpdate() { this->_minTileArea.rawValue().toDouble() * si::meter * si::meter; auto *th = this->thread(); auto future = QtConcurrent::run([polygon, th, height, width, minArea] { -#ifdef SNAKE_SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); -#endif + DataPtr pData(new TileData()); // Convert to ENU system. QGeoCoordinate origin = polygon.first(); @@ -298,56 +307,67 @@ void WimaMeasurementArea::doUpdate() { } } pData->moveToThread(th); -#ifdef SNAKE_SHOW_TIME - qDebug() << "WimaMeasurementArea::doUpdate concurrent update execution " - "time: " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start) - .count() - << " ms"; -#endif + + qCDebug(WimaMeasurementAreaLog) + << "doUpdate(): update time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; + return pData; }); // QtConcurrent::run() this->_watcher.setFuture(future); } -#ifdef SNAKE_SHOW_TIME - qDebug() << "WimaMeasurementArea::doUpdate execution time: " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start) - .count() - << " ms"; -#endif + + qCDebug(WimaMeasurementAreaLog) + << "doUpdate(): execution time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; } void WimaMeasurementArea::deferUpdate() { - if (this->_timer.isActive()) { - this->_timer.stop(); - } - if (this->_tileData.size() > 0) { - this->_progress.clear(); - emit this->progressChanged(); - this->_tileData.clear(); - emit this->tilesChanged(); + if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) { + if (this->_state == STATE::IDLE) { + this->_progress.clear(); + this->_tileData.clear(); + emit this->progressChanged(); + emit this->tilesChanged(); + } + this->setState(STATE::DEFERED); + this->_timer.start(100); + } else if (this->_state == STATE::UPDATE) { + setState(STATE::RESTART); } - this->_timer.start(100); } void WimaMeasurementArea::storeTiles() { -#ifdef SNAKE_SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); -#endif - this->_tileData = *this->_watcher.result(); - this->_calculating = false; - // This is expensive. Drawing tiles is expensive too. - emit this->tilesChanged(); -#ifdef SNAKE_SHOW_TIME - qDebug() << "WimaMeasurementArea::storeTiles() execution time: " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start) - .count() - << " ms"; -#endif + + if (this->_state == STATE::UPDATE) { + + qCDebug(WimaMeasurementAreaLog) << "storeTiles(): update."; + + this->_tileData = *this->_watcher.result(); + // This is expensive. Drawing tiles is expensive too. + this->_progress = QVector(this->_tileData.tiles.count(), 0); + this->progressChanged(); + emit this->tilesChanged(); + setState(STATE::IDLE); + } else if (this->_state == STATE::RESTART) { + qCDebug(WimaMeasurementAreaLog) << "storeTiles(): restart."; + + doUpdate(); + } + qCDebug(WimaMeasurementAreaLog) + << "storeTiles() execution time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; } void WimaMeasurementArea::init() { @@ -368,6 +388,16 @@ void WimaMeasurementArea::init() { &WimaMeasurementArea::storeTiles); } +void WimaMeasurementArea::setState(WimaMeasurementArea::STATE s) { + if (this->_state != s) { + auto oldState = this->_state; + this->_state = s; + if (s == STATE::IDLE || oldState == STATE::IDLE) { + emit readyChanged(); + } + } +} + /*! * \class WimaMeasurementArea * \brief Class defining the area inside which the actual drone measurements diff --git a/src/Wima/Geometry/WimaMeasurementArea.h b/src/Wima/Geometry/WimaMeasurementArea.h index 9c4fa8c2b..77e4a9bf1 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.h +++ b/src/Wima/Geometry/WimaMeasurementArea.h @@ -26,6 +26,14 @@ public: class WimaMeasurementArea : public WimaArea { Q_OBJECT + + enum class STATE { + IDLE, + DEFERED, + UPDATE, + RESTART, + }; + public: WimaMeasurementArea(QObject *parent = nullptr); WimaMeasurementArea(const WimaMeasurementArea &other, @@ -41,7 +49,7 @@ public: Q_PROPERTY(Fact *showTiles READ showTiles CONSTANT) Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tilesChanged) Q_PROPERTY(int maxTiles READ maxTiles NOTIFY maxTilesChanged) - Q_PROPERTY(QVector progress READ progress NOTIFY progressChanged) + Q_PROPERTY(QVector progress READ progressQml NOTIFY progressChanged) // Overrides from WimaPolygon QString mapVisualQML(void) const; @@ -54,7 +62,8 @@ public: Fact *minTransectLength(); Fact *showTiles(); QmlObjectListModel *tiles(); - QVector progress(); + const QVector &progress() const; + QVector progressQml() const; const QmlObjectListModel *tiles() const; const QVariantList &tileCenterPoints() const; // List of QGeoCoordinate const TileData &tileData() const; @@ -83,6 +92,7 @@ signals: void tilesChanged(); void maxTilesChanged(); void progressChanged(); + void readyChanged(); public slots: bool setProgress(const QVector &p); @@ -95,6 +105,7 @@ private slots: private: // Member Methodes void init(); + void setState(STATE s); // Members QMap _metaDataMap; @@ -111,7 +122,8 @@ private: using DataPtr = std::shared_ptr; TileData _tileData; QFutureWatcher _watcher; - bool _calculating; + + STATE _state; QVector _progress; }; diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.cc b/src/Wima/Geometry/WimaMeasurementAreaData.cc index 879b0fde4..dcdc5debf 100644 --- a/src/Wima/Geometry/WimaMeasurementAreaData.cc +++ b/src/Wima/Geometry/WimaMeasurementAreaData.cc @@ -18,6 +18,18 @@ WimaMeasurementAreaData::WimaMeasurementAreaData( *this = other; } +bool WimaMeasurementAreaData:: +operator==(const WimaMeasurementAreaData &other) const { + return this->WimaAreaData::operator==(other) && + this->_tileData == other.tileData() && + this->center() == other.center(); +} + +bool WimaMeasurementAreaData:: +operator!=(const WimaMeasurementAreaData &other) const { + return !(*this == other); +} + /*! * \overload operator=(); * @@ -82,9 +94,31 @@ void WimaMeasurementAreaData::assign(const WimaMeasurementArea &other) { WimaAreaData::assign(other); if (other.ready()) { this->_tileData = other.tileData(); - qWarning() << "WimaMeasurementAreaData: add progress copy here."; + this->_progress = other.progress(); } else { qWarning() << "WimaMeasurementAreaData::assign(): WimaMeasurementArea not ready."; } } + +bool operator==(const WimaMeasurementAreaData &m1, + const WimaMeasurementArea &m2) { + return operator==(*static_cast(&m1), + *static_cast(&m2)) && + m1.tileData() == m2.tileData() && m1.progress() == m2.progress(); +} + +bool operator!=(const WimaMeasurementAreaData &m1, + const WimaMeasurementArea &m2) { + return !(m1 == m2); +} + +bool operator==(const WimaMeasurementArea &m1, + const WimaMeasurementAreaData &m2) { + return m2 == m1; +} + +bool operator!=(const WimaMeasurementArea &m1, + const WimaMeasurementAreaData &m2) { + return m2 != m1; +} diff --git a/src/Wima/Geometry/WimaMeasurementAreaData.h b/src/Wima/Geometry/WimaMeasurementAreaData.h index fccdf6dd3..5211d0686 100644 --- a/src/Wima/Geometry/WimaMeasurementAreaData.h +++ b/src/Wima/Geometry/WimaMeasurementAreaData.h @@ -18,6 +18,9 @@ public: WimaMeasurementAreaData &operator=(const WimaMeasurementAreaData &other); WimaMeasurementAreaData &operator=(const WimaMeasurementArea &other); + bool operator==(const WimaMeasurementAreaData &other) const; + bool operator!=(const WimaMeasurementAreaData &other) const; + QString type() const; WimaMeasurementAreaData *Clone() const { return new WimaMeasurementAreaData(*this); @@ -42,3 +45,12 @@ private: TileData _tileData; QVector _progress; }; + +bool operator==(const WimaMeasurementAreaData &m1, + const WimaMeasurementArea &m2); +bool operator!=(const WimaMeasurementAreaData &m1, + const WimaMeasurementArea &m2); +bool operator==(const WimaMeasurementArea &m1, + const WimaMeasurementAreaData &m2); +bool operator!=(const WimaMeasurementArea &m1, + const WimaMeasurementAreaData &m2); diff --git a/src/Wima/RoutingThread.cpp b/src/Wima/RoutingThread.cpp index 77e8f2127..20f05f516 100644 --- a/src/Wima/RoutingThread.cpp +++ b/src/Wima/RoutingThread.cpp @@ -4,6 +4,9 @@ // Qt #include +#include "QGCLoggingCategory.h" +QGC_LOGGING_CATEGORY(RoutingWorkerLog, "RoutingWorkerLog") + RoutingThread::RoutingThread(QObject *parent) : QThread(parent), _calculating(false), _stop(false), _restart(false) { @@ -41,16 +44,13 @@ void RoutingThread::route(const RoutingParameter &par, } void RoutingThread::run() { - qWarning() << "RoutingWorker::run(): thread start."; + qCWarning(RoutingWorkerLog) << "run(): thread start."; while (!this->_stop) { -#ifdef DEBUG_CIRCULAR_SURVEY - qWarning() << "RoutingWorker::run(): calculation " - "started."; -#endif + qCWarning(RoutingWorkerLog) << "run(): calculation " + "started."; // Copy input. -#ifdef SHOW_CIRCULAR_SURVEY_TIME auto start = std::chrono::high_resolution_clock::now(); -#endif + this->_calculating = true; emit calculatingChanged(); Lock lk(this->_mutex); @@ -67,10 +67,8 @@ void RoutingThread::run() { if (generator(transectsENU)) { // Check if generation was successful. if (transectsENU.size() == 0) { -#ifdef DEBUG_CIRCULAR_SURVEY - qWarning() << "RoutingWorker::run(): " - "not able to generate transects."; -#endif + qCWarning(RoutingWorkerLog) << "run(): " + "not able to generate transects."; } else { // Prepare data for routing. auto &solutionVector = pRouteData->solutionVector; @@ -95,38 +93,29 @@ void RoutingThread::run() { // Check if routing was successful. if ((!success || solutionVector.size() < 1) && !this->_restart) { -#ifdef DEBUG_CIRCULAR_SURVEY - qWarning() << "RoutingWorker::run(): " - "routing failed."; - qWarning() << snakePar.errorString.c_str(); -#endif + qCWarning(RoutingWorkerLog) << "run(): " + "routing failed. " + << snakePar.errorString.c_str(); } else if (this->_restart) { -#ifdef DEBUG_CIRCULAR_SURVEY - qWarning() << "RoutingWorker::run(): " - "restart requested."; -#endif + qCWarning(RoutingWorkerLog) << "run(): " + "restart requested."; } else { // Notify main thread. emit result(pRouteData); -#ifdef DEBUG_CIRCULAR_SURVEY - qWarning() << "RoutingWorker::run(): " - "concurrent update success."; -#endif + qCWarning(RoutingWorkerLog) << "run(): " + "concurrent update success."; } } } // end calculation -#ifdef DEBUG_CIRCULAR_SURVEY else { - qWarning() << "RoutingWorker::run(): generator() failed."; + qCWarning(RoutingWorkerLog) << "run(): generator() failed."; } -#endif -#ifdef SHOW_CIRCULAR_SURVEY_TIME - qWarning() << "RoutingWorker::run(): execution time: " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start) - .count() - << " ms"; -#endif + qCWarning(RoutingWorkerLog) + << "run(): execution time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; // Signal calulation end and set thread to sleep. this->_calculating = false; emit calculatingChanged(); @@ -136,5 +125,5 @@ void RoutingThread::run() { } this->_restart = false; } // main loop - qWarning() << "RoutingWorker::run(): thread end."; + qCWarning(RoutingWorkerLog) << "run(): thread end."; } diff --git a/src/Wima/Snake/NemoInterface.cpp b/src/Wima/Snake/NemoInterface.cpp index eb9388da6..c6fa880f3 100644 --- a/src/Wima/Snake/NemoInterface.cpp +++ b/src/Wima/Snake/NemoInterface.cpp @@ -48,7 +48,7 @@ public: void setAutoPublish(bool ap); void setHoldProgress(bool hp); - bool holdProgress(); + bool lockProgress(); void publishTileData(); NemoInterface::STATUS status(); @@ -88,7 +88,7 @@ private: // Internals std::atomic_bool running_; - std::atomic_bool holdProgress_; + std::atomic_bool lockProgress_; std::atomic_bool topicServiceSetupDone; ROSBridgePtr pRosBridge; QTimer loopTimer; @@ -110,7 +110,7 @@ StatusMap statusMap{ NemoInterface::Impl::Impl(NemoInterface *p) : status_(STATUS::NOT_CONNECTED), nextTimeout(TimePoint::max()), - running_(false), holdProgress_(false), topicServiceSetupDone(false), + running_(false), lockProgress_(false), topicServiceSetupDone(false), parent(p) { // ROS Bridge. @@ -189,11 +189,11 @@ bool NemoInterface::Impl::hasTileData(const TileData &tileData) const { } void NemoInterface::Impl::setHoldProgress(bool hp) { - if (this->holdProgress_ != hp) { - this->holdProgress_ = hp; - emit this->parent->holdProgressChanged(); + if (this->lockProgress_ != hp) { + this->lockProgress_ = hp; + emit this->parent->lockProgressChanged(); - if (!this->holdProgress_) { + if (!this->lockProgress_) { UniqueLock lk(this->progressMutex); if (this->qProgress != this->qProgressHolded) { this->qProgressHolded = this->qProgress; @@ -204,7 +204,7 @@ void NemoInterface::Impl::setHoldProgress(bool hp) { } } -bool NemoInterface::Impl::holdProgress() { return this->holdProgress_.load(); } +bool NemoInterface::Impl::lockProgress() { return this->lockProgress_.load(); } void NemoInterface::Impl::publishTileData() { std::lock(this->ENUOriginMutex, this->tilesENUMutex); @@ -263,7 +263,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() { UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock); int requiredSize = this->tilesENU.polygons().size(); - auto hold = this->holdProgress_.load(); + auto hold = this->lockProgress_.load(); auto &progressMsg = this->qProgress; if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) || progressMsg.progress().size() != @@ -474,4 +474,4 @@ QString NemoInterface::editorQml() { bool NemoInterface::running() { return this->pImpl->running(); } -bool NemoInterface::holdProgress() { return this->pImpl->holdProgress(); } +bool NemoInterface::lockProgress() { return this->pImpl->lockProgress(); } diff --git a/src/Wima/Snake/NemoInterface.h b/src/Wima/Snake/NemoInterface.h index 6a461b38b..14e8915ed 100644 --- a/src/Wima/Snake/NemoInterface.h +++ b/src/Wima/Snake/NemoInterface.h @@ -29,8 +29,8 @@ public: Q_PROPERTY(QVector progress READ progress NOTIFY progressChanged) Q_PROPERTY(QString editorQml READ editorQml CONSTANT) Q_PROPERTY(bool running READ running NOTIFY runningChanged) - Q_PROPERTY(bool holdProgress READ holdProgress WRITE setHoldProgress NOTIFY - holdProgressChanged) + Q_PROPERTY(bool lockProgress READ lockProgress WRITE setHoldProgress NOTIFY + lockProgressChanged) Q_INVOKABLE void start(); Q_INVOKABLE void stop(); @@ -48,13 +48,13 @@ public: QVector progress() const; QString editorQml(); bool running(); - bool holdProgress(); + bool lockProgress(); signals: void statusChanged(); void progressChanged(); void runningChanged(); - void holdProgressChanged(); + void lockProgressChanged(); private: PImpl pImpl; diff --git a/src/Wima/StateMachine.cpp b/src/Wima/StateMachine.cpp new file mode 100644 index 000000000..4cb6abdf6 --- /dev/null +++ b/src/Wima/StateMachine.cpp @@ -0,0 +1,277 @@ +#include "StateMachine.h" + +#include "QGCLoggingCategory.h" + +#include + +const QLoggingCategory &WimaPlanerLog(); + +namespace wima_planer_detail { + +template +constexpr typename std::underlying_type::type integral(T value) { + return static_cast::type>(value); +} + +StateMachine::StateMachine(QObject *parent) + : QObject(parent), _state(STATE::NEEDS_INIT) {} + +STATE StateMachine::state() { return this->_state; } + +void StateMachine::updateState(EVENT e) { + qCDebug(WimaPlanerLog) << "StateMachine::updateState(): event:" << e; + switch (this->_state) { + case STATE::NEEDS_INIT: + switch (e) { + case EVENT::INIT_DONE: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::SURVEY_UPDATE_TRIGGERED: + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_UPDATED: + break; + default: + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + break; + } + break; // STATE::NEEDS_INIT + case STATE::NEEDS_J_AREA_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + break; + case EVENT::J_AREA_UPDATED: + setState(STATE::NEEDS_SURVEY_UPDATE); + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + case EVENT::SURVEY_UPDATE_TRIGGERED: + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_UPDATED: + break; + default: + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + break; + } + break; // STATE::NEEDS_J_AREA_UPDATE + case STATE::NEEDS_SURVEY_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + setState(STATE::WAITING_FOR_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATED: + case EVENT::PATH_UPDATED: + break; + default: + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + break; + } + break; // STATE::NEEDS_SURVEY_UPDATE + case STATE::WAITING_FOR_SURVEY_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + setState(STATE::NEEDS_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + break; + case EVENT::SURVEY_UPDATED: + setState(STATE::NEEDS_PATH_UPDATE); + case EVENT::PATH_UPDATED: + break; + default: + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + break; + } + break; // STATE::WAYTING_FOR_SURVEY_UPDATE + case STATE::NEEDS_PATH_UPDATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + setState(STATE::NEEDS_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + setState(STATE::WAITING_FOR_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATED: + break; + case EVENT::PATH_UPDATED: + setState(STATE::UP_TO_DATE); + break; + default: + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + break; + } + break; // STATE::NEEDS_PATH_UPDATE + case STATE::UP_TO_DATE: + switch (e) { + case EVENT::INIT_DONE: + case EVENT::M_AREA_PATH_CHANGED: + case EVENT::S_AREA_PATH_CHANGED: + case EVENT::CORRIDOR_PATH_CHANGED: + setState(STATE::NEEDS_J_AREA_UPDATE); + break; + case EVENT::M_AREA_TILES_CHANGED: + case EVENT::M_AREA_PROGRESS_CHANGED: + case EVENT::J_AREA_UPDATED: + case EVENT::DEPOT_CHANGED: + case EVENT::SURVEY_DESTROYED: + setState(STATE::NEEDS_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + setState(STATE::WAITING_FOR_SURVEY_UPDATE); + break; + case EVENT::SURVEY_UPDATED: + setState(STATE::NEEDS_PATH_UPDATE); + break; + case EVENT::PATH_UPDATED: + break; + default: + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown event: " << e; + Q_ASSERT(false); + break; + } + break; // STATE::UP_TO_DATE + default: + qCCritical(WimaPlanerLog) + << "StateMachine::updateState: Unknown state: " << this->_state; + Q_ASSERT(false); + break; + } +} + +bool StateMachine::upToDate() { return this->_state == STATE::UP_TO_DATE; } + +void StateMachine::setState(STATE s) { + if (this->_state != s) { + auto oldState = this->_state; + this->_state = s; + emit stateChanged(); + if (oldState == STATE::UP_TO_DATE || s == STATE::UP_TO_DATE) { + emit upToDateChanged(); + } + qCDebug(WimaPlanerLog) << "StateMachine::setState():" << oldState << "->" + << s; + } +} + +QDebug &operator<<(QDebug &ds, STATE s) { + switch (s) { + + case STATE::NEEDS_INIT: + ds << "NEEDS_INIT"; + break; + case STATE::NEEDS_J_AREA_UPDATE: + ds << "NEEDS_J_AREA_UPDATE"; + break; + case STATE::NEEDS_SURVEY_UPDATE: + ds << "NEEDS_SURVEY_UPDATE"; + break; + case STATE::WAITING_FOR_SURVEY_UPDATE: + ds << "WAITING_FOR_SURVEY_UPDATE"; + break; + case STATE::NEEDS_PATH_UPDATE: + ds << "NEEDS_PATH_UPDATE"; + break; + case STATE::UP_TO_DATE: + ds << "UP_TO_DATE"; + break; + } + return ds; +} + +QDebug &operator<<(QDebug &ds, EVENT s) { + switch (s) { + case EVENT::INIT_DONE: + ds << "INIT_DONE"; + break; + case EVENT::M_AREA_PATH_CHANGED: + ds << "M_AREA_PATH_CHANGED"; + break; + case EVENT::S_AREA_PATH_CHANGED: + ds << "S_AREA_PATH_CHANGED"; + break; + case EVENT::CORRIDOR_PATH_CHANGED: + ds << "CORRIDOR_PATH_CHANGED"; + break; + case EVENT::M_AREA_TILES_CHANGED: + ds << "M_AREA_TILES_CHANGED"; + break; + case EVENT::M_AREA_PROGRESS_CHANGED: + ds << "M_AREA_PROGRESS_CHANGED"; + break; + case EVENT::J_AREA_UPDATED: + ds << "J_AREA_UPDATED"; + break; + case EVENT::DEPOT_CHANGED: + ds << "DEPOT_CHANGED"; + break; + case EVENT::SURVEY_DESTROYED: + ds << "SURVEY_DESTROYED"; + break; + case EVENT::SURVEY_UPDATE_TRIGGERED: + ds << "SURVEY_UPDATE_TRIGGERED"; + break; + case EVENT::SURVEY_UPDATED: + ds << "SURVEY_UPDATED"; + break; + case EVENT::PATH_UPDATED: + ds << "PATH_UPDATED"; + break; + } + return ds; +} + +} // namespace wima_planer_detail diff --git a/src/Wima/StateMachine.h b/src/Wima/StateMachine.h new file mode 100644 index 000000000..2f6500627 --- /dev/null +++ b/src/Wima/StateMachine.h @@ -0,0 +1,55 @@ +#pragma once + +#include +#include + +namespace wima_planer_detail { + +enum class STATE { + NEEDS_INIT, + NEEDS_J_AREA_UPDATE, + NEEDS_SURVEY_UPDATE, + WAITING_FOR_SURVEY_UPDATE, + NEEDS_PATH_UPDATE, + UP_TO_DATE +}; + +QDebug &operator<<(QDebug &ds, STATE s); + +enum class EVENT { + INIT_DONE, + M_AREA_PATH_CHANGED, + S_AREA_PATH_CHANGED, + CORRIDOR_PATH_CHANGED, + M_AREA_TILES_CHANGED, + M_AREA_PROGRESS_CHANGED, + J_AREA_UPDATED, + DEPOT_CHANGED, + SURVEY_DESTROYED, + SURVEY_UPDATE_TRIGGERED, + SURVEY_UPDATED, + PATH_UPDATED, +}; + +QDebug &operator<<(QDebug &ds, EVENT s); + +class StateMachine : public QObject { + Q_OBJECT +public: + explicit StateMachine(QObject *parent = nullptr); + + STATE state(); + void updateState(EVENT e); + bool upToDate(); + +signals: + void stateChanged(); + void upToDateChanged(); + +private: + void setState(STATE s); + + STATE _state; +}; + +} // namespace wima_planer_detail diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 36ed60bd6..d34309265 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -4,6 +4,7 @@ #include "MissionSettingsItem.h" #include "PlanMasterController.h" #include "QGCApplication.h" +#include "QGCLoggingCategory.h" #include "QGCMapPolygon.h" #include "SimpleMissionItem.h" @@ -16,40 +17,39 @@ #include "Geometry/WimaAreaData.h" #include "WimaBridge.h" +#include "StateMachine.h" +using namespace wima_planer_detail; + +#include + +QGC_LOGGING_CATEGORY(WimaPlanerLog, "WimaPlanerLog") + +class CommandRAII { + std::function f; + +public: + CommandRAII(const std::function &fun) : f(fun) {} + ~CommandRAII() { f(); } +}; + const char *WimaPlaner::wimaFileExtension = "wima"; const char *WimaPlaner::areaItemsName = "AreaItems"; const char *WimaPlaner::missionItemsName = "MissionItems"; WimaPlaner::WimaPlaner(QObject *parent) : QObject(parent), _masterController(nullptr), _missionController(nullptr), - _currentAreaIndex(-1), _wimaBridge(nullptr), _mAreaChanged(true), - _sAreaChanged(true), _corridorChanged(true), _joinedArea(this), - _arrivalPathLength(0), _returnPathLength(0), _TSComplexItem(nullptr), - _surveyChanged(true), _synchronized(false), _needsUpdate(true), - _nemoInterface(this) { + _currentAreaIndex(-1), _wimaBridge(nullptr), _copyMAreaToSurvey(true), + _copySAreaToSurvey(true), _corridorChanged(true), _joinedArea(this), + _arrivalPathLength(0), _returnPathLength(0), _survey(nullptr), + _surveyChanged(true), _synchronized(false), _nemoInterface(this), + _stateMachine(new StateMachine) { connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::updatePolygonInteractivity); - connect(&this->_measurementArea, &WimaArea::pathChanged, [this] { - this->_mAreaChanged = true; - this->setNeedsUpdate(true); - }); - connect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, [this] { - this->_nemoInterface.setTileData(this->_measurementArea.tileData()); - }); - connect(&this->_serviceArea, &WimaArea::pathChanged, [this] { - this->_sAreaChanged = true; - this->setNeedsUpdate(true); - }); - connect(&this->_serviceArea, &WimaServiceArea::depotChanged, [this] { - this->_sAreaChanged = true; - this->setNeedsUpdate(true); - }); - connect(&this->_corridor, &WimaArea::pathChanged, [this] { - this->_corridorChanged = true; - this->setNeedsUpdate(true); - }); + // Enable monitoring (state update) + enableMonitoring(); + #ifndef NDEBUG // for debugging and testing purpose, remove if not needed anymore connect(&_autoLoadTimer, &QTimer::timeout, this, @@ -62,8 +62,14 @@ WimaPlaner::WimaPlaner(QObject *parent) connect(&this->_nemoInterface, &NemoInterface::progressChanged, [this] { this->_measurementArea.setProgress(this->_nemoInterface.progress()); }); + + // StateMachine + connect(this->_stateMachine.get(), &StateMachine::upToDateChanged, this, + &WimaPlaner::needsUpdateChanged); } +WimaPlaner::~WimaPlaner() {} + PlanMasterController *WimaPlaner::masterController() { return _masterController; } @@ -135,7 +141,7 @@ void WimaPlaner::setWimaBridge(WimaBridge *bridge) { bool WimaPlaner::synchronized() { return _synchronized; } -bool WimaPlaner::needsUpdate() { return _needsUpdate; } +bool WimaPlaner::needsUpdate() { return !this->_stateMachine->upToDate(); } WimaPlaner *WimaPlaner::thisPointer() { return this; } @@ -144,7 +150,8 @@ void WimaPlaner::removeArea(int index) { WimaArea *area = qobject_cast(_visualItems.removeAt(index)); if (area == nullptr) { - qWarning("WimaPlaner::removeArea(): nullptr catched, internal error."); + qCWarning(WimaPlanerLog) + << "removeArea(): nullptr catched, internal error."; return; } area->clear(); @@ -167,7 +174,7 @@ void WimaPlaner::removeArea(int index) { updatePolygonInteractivity(_currentAreaIndex); } } else { - qWarning("Index out of bounds!"); + qCWarning(WimaPlanerLog) << "removeArea(): Index out of bounds!"; } } @@ -224,62 +231,67 @@ void WimaPlaner::removeAll() { _currentFile = ""; - _TSComplexItem = nullptr; + _survey = nullptr; emit currentFileChanged(); if (changesApplied) emit visualItemsChanged(); } -bool WimaPlaner::update() { - this->_synchronized = false; - emit synchronizedChanged(); +void WimaPlaner::update() { this->_update(); } - // ====================== update joined area ====================== - // check if at least service area and measurement area are available - if (_visualItems.indexOf(&_serviceArea) == -1 || - _visualItems.indexOf(&_measurementArea) == -1) - return false; +void WimaPlaner::_update() { + setSynchronized(false); - // Check if polygons have at least three vertices - if (_serviceArea.count() < 3) { - qgcApp()->showMessage(tr("Service area has less than three vertices.")); - return false; - } + switch (this->_stateMachine->state()) { + case STATE::NEEDS_INIT: { + this->_stateMachine->updateState(EVENT::INIT_DONE); + this->_update(); + } break; - if (_measurementArea.count() < 3) { - qgcApp()->showMessage(tr("Measurement area has less than three vertices.")); - return false; - } + case STATE::NEEDS_J_AREA_UPDATE: { + // check if at least service area and measurement area are available + if (_visualItems.indexOf(&_serviceArea) == -1 || + _visualItems.indexOf(&_measurementArea) == -1) + return; - // Check for simple polygons - if (!_serviceArea.isSimplePolygon()) { - qgcApp()->showMessage(tr("Service area is not a simple polygon. " - "Only simple polygons allowed.\n")); - return false; - } + // Check if polygons have at least three vertices + if (_serviceArea.count() < 3) { + qgcApp()->showMessage(tr("Service area has less than three vertices.")); + return; + } - if (!_corridor.isSimplePolygon() && _corridor.count() > 0) { - qgcApp()->showMessage(tr("Corridor is not a simple polygon. Only " - "simple polygons allowed.\n")); - return false; - } + if (_measurementArea.count() < 3) { + qgcApp()->showMessage( + tr("Measurement area has less than three vertices.")); + return; + } - if (!_measurementArea.isSimplePolygon()) { - qgcApp()->showMessage(tr("Measurement area is not a simple " - "polygon. Only simple polygons allowed.\n")); - return false; - } + // Check for simple polygons + if (!_serviceArea.isSimplePolygon()) { + qgcApp()->showMessage(tr("Service area is not a simple polygon. " + "Only simple polygons allowed.\n")); + return; + } - if (!_serviceArea.containsCoordinate(_serviceArea.depot())) { - qgcApp()->showMessage(tr("Depot not inside service area.")); - return false; - } + if (!_corridor.isSimplePolygon() && _corridor.count() > 0) { + qgcApp()->showMessage(tr("Corridor is not a simple polygon. Only " + "simple polygons allowed.\n")); + return; + } + + if (!_measurementArea.isSimplePolygon()) { + qgcApp()->showMessage(tr("Measurement area is not a simple " + "polygon. Only simple polygons allowed.\n")); + return; + } + + if (!_serviceArea.containsCoordinate(_serviceArea.depot())) { + qgcApp()->showMessage(tr("Depot not inside service area.")); + return; + } - // Join areas. - bool jAreaChanged = - this->_mAreaChanged || this->_sAreaChanged || this->_corridorChanged; - if (jAreaChanged) { + // Join areas. _joinedArea.setPath(_serviceArea.path()); if (_corridor.count() >= 3) { _joinedArea.join(_corridor); @@ -289,169 +301,208 @@ bool WimaPlaner::update() { tr("Not able to join areas. Service and measurement area" " must be overlapping, or connected through a " "corridor.")); - return false; + return; } - } + this->_stateMachine->updateState(EVENT::J_AREA_UPDATED); + this->_update(); + } break; // STATE::NEEDS_J_AREA_UPDATE - // =========================================================== - // ====================== update survey ====================== - bool updateSurveyArea = jAreaChanged; - // extract old survey data - QmlObjectListModel *missionItems = _missionController->visualItems(); - int surveyIndex = missionItems->indexOf(_TSComplexItem); - // create survey item if not yet present - if (surveyIndex == -1) { - _missionController->insertComplexMissionItem( - _missionController->circularSurveyComplexItemName(), - _measurementArea.center(), missionItems->count()); - _TSComplexItem = qobject_cast( - missionItems->get(missionItems->count() - 1)); - - if (_TSComplexItem == nullptr) { - qWarning("WimaPlaner::update(): survey == nullptr"); - return false; - } + case STATE::NEEDS_SURVEY_UPDATE: { + // Need to insert Survey? + QmlObjectListModel *missionItems = _missionController->visualItems(); + int surveyIndex = missionItems->indexOf(_survey); + // Create survey item if not yet present. + if (surveyIndex < 0) { + _missionController->insertComplexMissionItem( + _missionController->circularSurveyComplexItemName(), + _measurementArea.center(), missionItems->count()); + _survey = qobject_cast( + missionItems->get(missionItems->count() - 1)); + + if (_survey == nullptr) { + qCWarning(WimaPlanerLog) << "_survey == nullptr"; + return; + } - // establish connections - _TSComplexItem->setRefPoint(_measurementArea.center()); - _TSComplexItem->setHidePolygon(true); - connect(_TSComplexItem, &CircularSurvey::missionItemReady, [this] { - this->_surveyChanged = true; - this->setNeedsUpdate(true); - }); - updateSurveyArea = true; - } + // establish connections + _survey->setRefPoint(_measurementArea.center()); + _survey->setHidePolygon(true); + connect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + connect(_survey, &CircularSurvey::missionItemReady, this, + &WimaPlaner::CSMissionItemReadyHandler); + connect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); + } - if (updateSurveyArea) { // update survey area - _TSComplexItem->surveyAreaPolygon()->clear(); - _TSComplexItem->surveyAreaPolygon()->appendVertices( - _measurementArea.coordinateList()); - _TSComplexItem->setDepot(this->_serviceArea.depot()); - _TSComplexItem->setSafeArea(this->_joinedArea.coordinateList()); - this->_mAreaChanged = false; - this->_sAreaChanged = false; - this->_corridorChanged = false; - } else if (this->_surveyChanged) { - - // ========================================================== - // ====================== update paths ====================== - - surveyIndex = missionItems->indexOf(_TSComplexItem); - - if (surveyIndex == -1) { - qWarning("WimaPlaner::update(): no survey item"); - return false; - } + disconnect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + _survey->setMeasurementArea(this->_measurementArea); + _survey->setJoinedArea(this->_joinedArea); + connect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + + // Folloing statement just for completeness. + this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED); + } break; // STATE::NEEDS_SURVEY_UPDATE + + case STATE::WAITING_FOR_SURVEY_UPDATE: { + } break; + + case STATE::NEEDS_PATH_UPDATE: { + // Check if survey is present. + QmlObjectListModel *missionItems = _missionController->visualItems(); + int surveyIndex = missionItems->indexOf(_survey); + if (surveyIndex < 0) { + this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED); + this->_update(); + } else { - // remove old arrival and return path - int size = missionItems->count(); - for (int i = surveyIndex + 1; i < size; i++) - _missionController->removeMissionItem(surveyIndex + 1); - for (int i = surveyIndex - 1; i > 1; i--) - _missionController->removeMissionItem(i); - - // set home position to serArea center - MissionSettingsItem *settingsItem = - qobject_cast(missionItems->get(0)); - if (settingsItem == nullptr) { - qWarning("WimaPlaner::update(): settingsItem == nullptr"); - return false; - } - // set altitudes - QGeoCoordinate point = _serviceArea.depot(); - point.setAltitude(0); - _serviceArea.setDepot(point); - point = _measurementArea.center(); - point.setAltitude(0); - _measurementArea.setCenter(point); - point = _corridor.center(); - point.setAltitude(0); - _corridor.setCenter(point); - settingsItem->setCoordinate(_serviceArea.depot()); - - // set takeoff position - bool setCommandNeeded = false; - if (missionItems->count() < 3) { - setCommandNeeded = true; - _missionController->insertSimpleMissionItem(_serviceArea.depot(), 1); - } - SimpleMissionItem *takeOffItem = - qobject_cast(missionItems->get(1)); - if (takeOffItem == nullptr) { - qWarning("WimaPlaner::update(): takeOffItem == nullptr"); - return false; - } - if (setCommandNeeded) - _missionController->setTakeoffCommand(*takeOffItem); - takeOffItem->setCoordinate(_serviceArea.depot()); + // Remove old arrival and return path. + int size = missionItems->count(); + for (int i = surveyIndex + 1; i < size; i++) + _missionController->removeMissionItem(surveyIndex + 1); + for (int i = surveyIndex - 1; i > 1; i--) + _missionController->removeMissionItem(i); + + // set home position to serArea center + MissionSettingsItem *settingsItem = + qobject_cast(missionItems->get(0)); + if (settingsItem == nullptr) { + qCWarning(WimaPlanerLog) << "update(): settingsItem == nullptr"; + return; + } - if (_TSComplexItem->visualTransectPoints().size() == 0) { - qWarning("WimaPlaner::update(): survey no points."); - return false; - } + // set altitudes + auto depot = _serviceArea.depot(); + depot.setAltitude(0); + settingsItem->setCoordinate(depot); - // calculate path from take off to survey - QGeoCoordinate start = _serviceArea.depot(); - QGeoCoordinate end = _TSComplexItem->coordinate(); -#ifdef QT_DEBUG -// if (!_visualItems.contains(&_joinedArea)) -//_visualItems.append(&_joinedArea); -#endif - QVector path; - if (!shortestPath(start, end, path)) { - qgcApp()->showMessage(QString(tr("Not able to calculate path from " - "takeoff position to measurement area.")) - .toLocal8Bit() - .data()); - return false; - } - _arrivalPathLength = path.size() - 1; - int sequenceNumber = 0; - for (int i = 1; i < path.count() - 1; i++) { - sequenceNumber = _missionController->insertSimpleMissionItem( - path[i], missionItems->count() - 1); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - } + // set takeoff position + bool setCommandNeeded = false; + if (missionItems->count() < 3) { + setCommandNeeded = true; + _missionController->insertSimpleMissionItem(depot, 1); + } + SimpleMissionItem *takeOffItem = + qobject_cast(missionItems->get(1)); + if (takeOffItem == nullptr) { + qCWarning(WimaPlanerLog) << "update(): takeOffItem == nullptr"; + return; + } + if (setCommandNeeded) + _missionController->setTakeoffCommand(*takeOffItem); + takeOffItem->setCoordinate(depot); - // calculate return path - start = _TSComplexItem->exitCoordinate(); - end = _serviceArea.depot(); - path.clear(); - if (!shortestPath(start, end, path)) { - qgcApp()->showMessage(QString(tr("Not able to calculate the path from " - "measurement area to landing position.")) - .toLocal8Bit() - .data()); - return false; - } - _returnPathLength = - path.size() - 1; // -1: fist item is last measurement point - for (int i = 1; i < path.count() - 1; i++) { + if (_survey->visualTransectPoints().size() == 0) { + qCWarning(WimaPlanerLog) << "update(): survey no points"; + return; + } + + // calculate path from take off to survey + QGeoCoordinate start = depot; + QGeoCoordinate end = _survey->coordinate(); + QVector path; + if (!shortestPath(start, end, path)) { + qgcApp()->showMessage( + QString(tr("Not able to calculate path from " + "takeoff position to measurement area.")) + .toLocal8Bit() + .data()); + return; + } + _arrivalPathLength = path.size() - 1; + int sequenceNumber = 0; + for (int i = 1; i < path.count() - 1; i++) { + sequenceNumber = _missionController->insertSimpleMissionItem( + path[i], missionItems->count() - 1); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + } + + // calculate return path + start = _survey->exitCoordinate(); + end = depot; + path.clear(); + if (!shortestPath(start, end, path)) { + qgcApp()->showMessage( + QString(tr("Not able to calculate the path from " + "measurement area to landing position.")) + .toLocal8Bit() + .data()); + return; + } + _returnPathLength = + path.size() - 1; // -1: fist item is last measurement point + for (int i = 1; i < path.count() - 1; i++) { + sequenceNumber = _missionController->insertSimpleMissionItem( + path[i], missionItems->count()); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + } + + // create land position item sequenceNumber = _missionController->insertSimpleMissionItem( - path[i], missionItems->count()); + depot, missionItems->count()); _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - } - - // create land position item - sequenceNumber = _missionController->insertSimpleMissionItem( - _serviceArea.depot(), missionItems->count()); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - SimpleMissionItem *landItem = qobject_cast( - missionItems->get(missionItems->count() - 1)); - if (landItem == nullptr) { - qWarning("WimaPlaner::calcArrivalAndReturnPath(): landItem == nullptr"); - return false; - } else { + SimpleMissionItem *landItem = qobject_cast( + missionItems->get(missionItems->count() - 1)); + if (landItem == nullptr) { + qCWarning(WimaPlanerLog) << "update(): landItem == nullptr"; + return; + } if (!_missionController->setLandCommand(*landItem)) - return false; + return; + + this->_stateMachine->updateState(EVENT::PATH_UPDATED); } - this->_surveyChanged = false; - setNeedsUpdate(false); + } break; // STATE::NEEDS_PATH_UPDATE + + case STATE::UP_TO_DATE: { + } break; // STATE::UP_TO_DATE + + } // switch +} + +void WimaPlaner::CSDestroyedHandler() { + this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED); +} + +void WimaPlaner::CSMissionItemReadyHandler() { + this->_stateMachine->updateState(EVENT::SURVEY_UPDATED); + this->_update(); +} + +void WimaPlaner::CSCalculatingChangedHandler() { + if (this->_survey->calculating()) { + this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED); } +} + +void WimaPlaner::mAreaPathChangedHandler() { + this->_stateMachine->updateState(EVENT::M_AREA_PATH_CHANGED); +} + +void WimaPlaner::mAreaTilesChangedHandler() { + this->_nemoInterface.setTileData(this->_measurementArea.tileData()); + this->_stateMachine->updateState(EVENT::M_AREA_TILES_CHANGED); +} + +void WimaPlaner::mAreaProgressChangedHandler() { + this->_stateMachine->updateState(EVENT::M_AREA_PROGRESS_CHANGED); +} + +void WimaPlaner::sAreaPathChangedHandler() { + this->_stateMachine->updateState(EVENT::S_AREA_PATH_CHANGED); +} + +void WimaPlaner::corridorPathChangedHandler() { + this->_stateMachine->updateState(EVENT::CORRIDOR_PATH_CHANGED); +} - return true; +void WimaPlaner::depotChangedHandler() { + this->_stateMachine->updateState(EVENT::DEPOT_CHANGED); } + void WimaPlaner::saveToCurrent() { saveToFile(_currentFile); } void WimaPlaner::saveToFile(const QString &filename) { @@ -498,7 +549,22 @@ void WimaPlaner::saveToFile(const QString &filename) { bool WimaPlaner::loadFromCurrent() { return loadFromFile(_currentFile); } bool WimaPlaner::loadFromFile(const QString &filename) { -#define debug 0 + // Remove obsolete connections. + disableMonitoring(); + CommandRAII onExit([this] { this->enableMonitoring(); }); + // disconnect old survey + if (_survey != nullptr) { + disconnect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + disconnect(_survey, &CircularSurvey::missionItemReady, this, + &WimaPlaner::CSMissionItemReadyHandler); + disconnect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); + } + + setSynchronized(false); + + // Precondition. QString errorString; QString errorMessage = tr("Error loading Plan file (%1). %2").arg(filename).arg("%1"); @@ -593,21 +659,15 @@ bool WimaPlaner::loadFromFile(const QString &filename) { QJsonObject missionObject = json[missionItemsName].toObject(); - // qWarning() << json[missionItemsName].type(); - QJsonDocument missionJsonDoc = QJsonDocument(missionObject); // create temporary file with missionItems QFile temporaryFile; QString cropedFileName = filename.section("/", 0, -2); -#if debug - qWarning() << cropedFileName; -#endif QString temporaryFileName; for (int i = 0;; i++) { temporaryFileName = cropedFileName + QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension); - // qWarning() << temporaryFileName; if (!QFile::exists(temporaryFileName)) { temporaryFile.setFileName(temporaryFileName); @@ -617,48 +677,40 @@ bool WimaPlaner::loadFromFile(const QString &filename) { } if (i > 1000) { - qWarning( - "WimaPlaner::loadFromFile(): not able to create temporary file."); + qCWarning(WimaPlanerLog) + << "loadFromFile(): not able to create temporary file."; return false; } } - // qWarning() << missionJsonDoc.toVariant().toString(); temporaryFile.write(missionJsonDoc.toJson()); temporaryFile.close(); // load from temporary file _masterController->loadFromFile(temporaryFileName); QmlObjectListModel *missionItems = _missionController->visualItems(); - _TSComplexItem = nullptr; + _survey = nullptr; for (int i = 0; i < missionItems->count(); i++) { - _TSComplexItem = missionItems->value(i); - if (_TSComplexItem != nullptr) { - _TSComplexItem->setHidePolygon(true); - connect(_TSComplexItem, &CircularSurvey::missionItemReady, [this] { - this->_surveyChanged = true; - this->setNeedsUpdate(true); - }); + _survey = missionItems->value(i); + if (_survey != nullptr) { + _survey->setHidePolygon(true); + connect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + connect(_survey, &CircularSurvey::missionItemReady, this, + &WimaPlaner::CSMissionItemReadyHandler); + connect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); break; } } - if (!update()) - return false; - // remove temporary file if (!temporaryFile.remove()) { - qWarning( - "WimaPlaner::loadFromFile(): not able to remove temporary file."); + qCWarning(WimaPlanerLog) + << "WimaPlaner::loadFromFile(): not able to remove " + "temporary file."; } - - this->_synchronized = false; - emit synchronizedChanged(); return true; - - } else if (fileInfo.suffix() == AppSettings::planFileExtension) { - _masterController->loadFromFile(filename); - return true; // might be wrong return value } else { errorString += QString(tr("File extension not supported.\n")); qgcApp()->showMessage(errorMessage.arg(errorString)); @@ -678,25 +730,20 @@ void WimaPlaner::updatePolygonInteractivity(int index) { void WimaPlaner::synchronize() { if (_wimaBridge != nullptr) { - if (readyForSynchronization()) { + if (!needsUpdate()) { auto planData = toPlanData(); if (planData) { (void)_wimaBridge->setWimaPlanData(planData); - this->_synchronized = true; - emit synchronizedChanged(); + setSynchronized(true); } else { - qWarning("WimaPlaner::uploadToContainer(): error creating plan data."); + qCWarning(WimaPlanerLog) << "error creating plan data."; } } } else { - qWarning("WimaPlaner::uploadToContainer(): no container assigned."); + qCWarning(WimaPlanerLog) << "no container assigned."; } } -bool WimaPlaner::readyForSynchronization() { - return !_needsUpdate && _measurementArea.ready(); -} - bool WimaPlaner::shortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) { @@ -715,13 +762,43 @@ bool WimaPlaner::shortestPath(const QGeoCoordinate &start, return retVal; } -void WimaPlaner::setNeedsUpdate(bool needsUpdate) { - if (this->_needsUpdate != needsUpdate) { - this->_needsUpdate = needsUpdate; - emit needsUpdateChanged(); +void WimaPlaner::setSynchronized(bool s) { + if (this->_synchronized != s) { + this->_synchronized = s; + emit this->synchronizedChanged(); } } +void WimaPlaner::enableMonitoring() { + connect(&this->_measurementArea, &WimaArea::pathChanged, this, + &WimaPlaner::mAreaPathChangedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, this, + &WimaPlaner::mAreaTilesChangedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::progressChanged, this, + &WimaPlaner::mAreaProgressChangedHandler); + connect(&this->_serviceArea, &WimaArea::pathChanged, this, + &WimaPlaner::sAreaPathChangedHandler); + connect(&this->_serviceArea, &WimaServiceArea::depotChanged, this, + &WimaPlaner::depotChangedHandler); + connect(&this->_corridor, &WimaArea::pathChanged, this, + &WimaPlaner::corridorPathChangedHandler); +} + +void WimaPlaner::disableMonitoring() { + disconnect(&this->_measurementArea, &WimaArea::pathChanged, this, + &WimaPlaner::mAreaPathChangedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, this, + &WimaPlaner::mAreaTilesChangedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::progressChanged, + this, &WimaPlaner::mAreaProgressChangedHandler); + disconnect(&this->_serviceArea, &WimaArea::pathChanged, this, + &WimaPlaner::sAreaPathChangedHandler); + disconnect(&this->_serviceArea, &WimaServiceArea::depotChanged, this, + &WimaPlaner::depotChangedHandler); + disconnect(&this->_corridor, &WimaArea::pathChanged, this, + &WimaPlaner::corridorPathChangedHandler); +} + void WimaPlaner::resetAllInteractive() { // Marks all areas as inactive (area.interactive == false) int itemCount = _visualItems.count(); @@ -758,13 +835,12 @@ QSharedPointer WimaPlaner::toPlanData() { // convert mission items to mavlink commands if (_missionController && _missionController->visualItems()) { - int surveyIndex = - _missionController->visualItems()->indexOf(_TSComplexItem); + int surveyIndex = _missionController->visualItems()->indexOf(_survey); if (surveyIndex > 0) { QList missionItems; - _TSComplexItem->appendMissionItems(missionItems, nullptr); + _survey->appendMissionItems(missionItems, nullptr); planData->append(missionItems); - planData->setTransects(this->_TSComplexItem->rawTransects()); + planData->setTransects(this->_survey->rawTransects()); return planData; } } @@ -795,7 +871,7 @@ QJsonDocument WimaPlaner::saveToJson(FileType fileType) { WimaArea *area = qobject_cast(_visualItems.get(i)); if (area == nullptr) { - qWarning("WimaPlaner::saveToJson(): Internal error, area == nullptr!"); + qCWarning(WimaPlanerLog) << "saveing, area == nullptr!"; return QJsonDocument(); } diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 03fb4b945..03003fa73 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -2,6 +2,7 @@ #include "QmlObjectListModel.h" #include +#include #include #include "Geometry/WimaCorridor.h" @@ -22,6 +23,10 @@ class MissionController; class PlanMasterController; class WimaBridge; +namespace wima_planer_detail { +class StateMachine; +} + class WimaPlaner : public QObject { Q_OBJECT @@ -29,6 +34,7 @@ class WimaPlaner : public QObject { public: WimaPlaner(QObject *parent = nullptr); + ~WimaPlaner(); template WimaPlaner(T t, QObject *parent = nullptr) = delete; template WimaPlaner(T t) = delete; @@ -85,10 +91,9 @@ public: /// MissionController Q_INVOKABLE void removeAll(); /// Recalculates vehicle corridor, flight path, etc. - Q_INVOKABLE bool update(); + Q_INVOKABLE void update(); /// Pushes the generated mission data to the wimaController. Q_INVOKABLE void synchronize(); - Q_INVOKABLE bool readyForSynchronization(); Q_INVOKABLE void saveToCurrent(); Q_INVOKABLE void saveToFile(const QString &filename); Q_INVOKABLE bool loadFromCurrent(); @@ -114,6 +119,17 @@ signals: private slots: void updatePolygonInteractivity(int index); + void _update(); + void CSDestroyedHandler(); + void CSMissionItemReadyHandler(); + void CSCalculatingChangedHandler(); + void mAreaPathChangedHandler(); + void mAreaTilesChangedHandler(); + void mAreaProgressChangedHandler(); + void sAreaPathChangedHandler(); + void corridorPathChangedHandler(); + void depotChangedHandler(); + #ifndef NDEBUG void autoLoadMission(void); #endif @@ -121,6 +137,7 @@ private slots: private: signals: void joinedAreaValidChanged(); + void stateChanged(); private: // Member Functions @@ -128,7 +145,9 @@ private: bool shortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path); - void setNeedsUpdate(bool needsUpdate); + void setSynchronized(bool s); + void enableMonitoring(); + void disableMonitoring(); // Member Variables PlanMasterController *_masterController; @@ -140,9 +159,9 @@ private: bool _joinedAreaValid; WimaMeasurementArea _measurementArea; - bool _mAreaChanged; + bool _copyMAreaToSurvey; WimaServiceArea _serviceArea; - bool _sAreaChanged; + bool _copySAreaToSurvey; WimaCorridor _corridor; bool _corridorChanged; // contains all visible areas @@ -154,12 +173,11 @@ private: // path from last measurement point to land unsigned long _returnPathLength; - CircularSurvey *_TSComplexItem; + CircularSurvey *_survey; bool _surveyChanged; // sync stuff bool _synchronized; // true if planData is synchronized with // wimaController - bool _needsUpdate; // gets set by updateMission and calcArrivalAndReturnPath #ifndef NDEBUG QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents @@ -167,4 +185,7 @@ private: #endif NemoInterface _nemoInterface; + + // State + QScopedPointer _stateMachine; }; diff --git a/src/WimaView/CircularSurveyMapVisual.qml b/src/WimaView/CircularSurveyMapVisual.qml index 22bd9d63f..83f949795 100644 --- a/src/WimaView/CircularSurveyMapVisual.qml +++ b/src/WimaView/CircularSurveyMapVisual.qml @@ -65,6 +65,7 @@ Item { function _destroyEntryCoordinate(){ if (_entryCoordinate){ + map.removeMapItem(_entryCoordinate) _entryCoordinate.destroy() _entryCoordinate = undefined } @@ -72,6 +73,7 @@ Item { function _destroyExitCoordinate(){ if (_exitCoordinate){ + map.removeMapItem(_exitCoordinate) _exitCoordinate.destroy() _exitCoordinate = undefined } @@ -79,6 +81,7 @@ Item { function _destroyRefPoint(){ if (_refPoint){ + map.removeMapItem(_refPoint) _refPoint.destroy() _refPoint = undefined } @@ -86,6 +89,7 @@ Item { function _destroyTransectsComponent(){ if (_transectsComponent){ + map.removeMapItem(_transectsComponent) _transectsComponent.destroy() _transectsComponent = undefined } diff --git a/src/WimaView/WimaMeasurementAreaMapVisual.qml b/src/WimaView/WimaMeasurementAreaMapVisual.qml index b2f1585b0..e7ba55df6 100644 --- a/src/WimaView/WimaMeasurementAreaMapVisual.qml +++ b/src/WimaView/WimaMeasurementAreaMapVisual.qml @@ -110,51 +110,64 @@ Item { // Add Snake tiles to the map Component { id: tileComponent - MapPolygon{ - path: [] - opacity: 0.6 - z: 2 + + Item{ + id: root + + property MapPolygon polygon + + MapPolygon{ + id:mapPolygon + path: [] + } + + Component.onCompleted: { + polygon = mapPolygon + map.addMapItem(mapPolygon) + } + + Component.onDestruction: { + map.removeMapItem(mapPolygon) + } } } + function getColor(progress) { + if (progress < 25) + return "transparent" + if (progress < 50) + return "orange" + if (progress < 75) + return "yellow" + if (progress < 100) + return "greenyellow" + return "limegreen" + } Repeater { id: progressRepeater property bool enable: areaItem.showTiles.value - model: enable ? areaItem.tiles : 0 - - function getColor(i) { - var progress = areaItem.progress[i] - if (progress < 25) - return "transparent" - if (progress < 50) - return "orange" - if (progress < 75) - return "yellow" - if (progress < 100) - return "greenyellow" - return "limegreen" - } + model: enable ? areaItem.tiles : [] Item{ property var _tileComponent + property int _progress: _root.areaItem.progress.length - function addVisuals() { + Component.onCompleted: { _tileComponent = tileComponent.createObject(map) - map.addMapItem(_tileComponent) - _tileComponent.path = object.path - _tileComponent.color = progressRepeater.getColor(index) - } - function removeVisuals() { - _tileComponent.destroy() + _tileComponent.polygon.path = + Qt.binding(function(){return object.path}) + _tileComponent.polygon.opacity = 0.6 + _tileComponent.polygon.border.color = "black" + _tileComponent.polygon.border.width = 1 + _tileComponent.polygon.color = + Qt.binding(function(){return getColor(_progress)}) } - Component.onCompleted: { - addVisuals() - } Component.onDestruction: { - removeVisuals() + _tileComponent.destroy() } + } } } diff --git a/src/WimaView/WimaToolBar.qml b/src/WimaView/WimaToolBar.qml index 358986c93..9a26c954b 100644 --- a/src/WimaView/WimaToolBar.qml +++ b/src/WimaView/WimaToolBar.qml @@ -252,7 +252,7 @@ Rectangle { rowSpacing: _rowSpacing columnSpacing: _labelToValueSpacing Layout.alignment: Qt.AlignVCenter | Qt.AlignHCenter - // visible: _batteryInfoAvailable + visible: _batteryInfoAvailable QGCLabel { text: qsTr("Battery") diff --git a/src/WimaView/WimaView.qml b/src/WimaView/WimaView.qml index d4df2d1be..a9777e9e3 100644 --- a/src/WimaView/WimaView.qml +++ b/src/WimaView/WimaView.qml @@ -966,6 +966,12 @@ QGCView { checked: true visible: QGroundControl.corePlugin.options.enablePlanViewSelector anchors.verticalCenter: parent.verticalCenter + + onCheckedChanged:{ + if (!checked){ + wimaPlaner.update() + } + } } QGCRadioButton { id: planElementMission -- 2.22.0