diff --git a/Paths/KlingenbachTest.wima b/Paths/KlingenbachTest.wima index 5db0c77e552bc5a2767eaae1eaafe61647a23c1f..1cb19f361b42ca2f5f7de8d5390726c697b17df4 100644 --- a/Paths/KlingenbachTest.wima +++ b/Paths/KlingenbachTest.wima @@ -60,8 +60,8 @@ 16.530403347547246 ], [ - 47.76799814678123, - 16.530388491662848 + 47.768126518382985, + 16.5309051298743 ] ] } @@ -103,7 +103,7 @@ "ReferencePointAlt": 0, "ReferencePointLat": 47.76807182520187, "ReferencePointLong": 16.530610531894183, - "TransectDistance": 2, + "TransectDistance": 10, "TransectStyleComplexItem": { "CameraCalc": { "AdjustedFootprintFrontal": 25, @@ -128,8 +128,8 @@ 0, 0, null, - 47.76780546458091, - 16.530404628981014, + 47.7677958621741, + 16.53051039683979, 15 ], "type": "SimpleItem" @@ -144,8 +144,8 @@ 0, 0, null, - 47.7680176218918, - 16.531145995840845, + 47.76795620865596, + 16.530658248522045, 15 ], "type": "SimpleItem" @@ -160,8 +160,8 @@ 0, 0, null, - 47.76803465648041, - 16.53113723392403, + 47.768085758380195, + 16.53111095084737, 15 ], "type": "SimpleItem" @@ -176,8 +176,8 @@ 0, 0, null, - 47.76782462170033, - 16.530403289516762, + 47.768170929071175, + 16.531067144987073, 15 ], "type": "SimpleItem" @@ -192,8 +192,8 @@ 0, 0, null, - 47.767843780618456, - 16.530401950051516, + 47.76812500283788, + 16.530906658047133, 15 ], "type": "SimpleItem" @@ -208,8 +208,8 @@ 0, 0, null, - 47.76805169016891, - 16.53112847333554, + 47.768170929071175, + 16.531067144987073, 15 ], "type": "SimpleItem" @@ -224,8 +224,8 @@ 0, 0, null, - 47.768068723856686, - 16.531119712741326, + 47.76811004588309, + 16.530512964621895, 15 ], "type": "SimpleItem" @@ -240,8 +240,8 @@ 0, 0, null, - 47.767862938637116, - 16.53040061191935, + 47.76811004588309, + 16.530512964621895, 15 ], "type": "SimpleItem" @@ -256,8 +256,8 @@ 0, 0, null, - 47.76788209665569, - 16.53039927245214, + 47.76825609974418, + 16.531023338983744, 15 ], "type": "SimpleItem" @@ -272,8 +272,8 @@ 0, 0, null, - 47.76808575754375, - 16.53111095080734, + 47.76834127040819, + 16.53097953282404, 15 ], "type": "SimpleItem" @@ -288,8 +288,8 @@ 0, 0, null, - 47.76810279212948, - 16.53110218886764, + 47.76822245220941, + 16.530564330637993, 15 ], "type": "SimpleItem" @@ -304,8 +304,8 @@ 0, 0, null, - 47.76790125467419, - 16.530397934318007, + 47.76834127040819, + 16.53097953282404, 15 ], "type": "SimpleItem" @@ -320,8 +320,8 @@ 0, 0, null, - 47.767920412692604, - 16.530396594848835, + 47.76795620865596, + 16.530658248522045, 15 ], "type": "SimpleItem" @@ -336,456 +336,8 @@ 0, 0, null, - 47.76811982671451, - 16.531093428256273, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 16, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768136860399416, - 16.53108466630512, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 17, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76793957161033, - 16.530395256712723, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 18, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.767958730527994, - 16.53039391724158, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 19, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768153894983, - 16.53107590568231, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 20, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76817092866645, - 16.531067145053775, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 21, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76797788764677, - 16.530392579103516, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 22, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.767997046564254, - 16.530391239630404, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 23, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.7681879623492, - 16.53105838308546, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 24, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76820499693062, - 16.531049622445487, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 25, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768011359505685, - 16.530372972324884, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 26, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76802514809256, - 16.530352870682762, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 27, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76822203061193, - 16.531040860465723, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 28, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.7682390651919, - 16.53103209981431, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 29, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768038937575305, - 16.530332769030014, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 30, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76805272615508, - 16.53031266736664, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 31, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768256098871774, - 16.531023337823104, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 32, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768273133450315, - 16.531014577160242, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 33, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768066514731295, - 16.53029256569264, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 34, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76808030330396, - 16.530272464008018, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 35, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76829016802814, - 16.531005815157602, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 36, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76830720170584, - 16.530997054483304, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 37, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76809409187308, - 16.530252363646827, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 38, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76815568462939, - 16.530399306029828, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 39, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768324235382835, - 16.53098829246921, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 40, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76831125036794, - 16.530806343268754, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 41, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.76835830363405, - 16.530970769757932, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 42, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.7683412699585, - 16.53097953178347, - 15 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 16, - "doJumpId": 43, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.768222452326334, - 16.530564330637993, + 47.7677958621741, + 16.53051039683979, 15 ], "type": "SimpleItem" @@ -795,172 +347,60 @@ "TurnAroundDistance": 10, "VisualTransectPoints": [ [ - 47.76780546458091, - 16.530404628981014 - ], - [ - 47.7680176218918, - 16.531145995840845 - ], - [ - 47.76803465648041, - 16.53113723392403 - ], - [ - 47.76782462170033, - 16.530403289516762 - ], - [ - 47.767843780618456, - 16.530401950051516 - ], - [ - 47.76805169016891, - 16.53112847333554 - ], - [ - 47.768068723856686, - 16.531119712741326 - ], - [ - 47.767862938637116, - 16.53040061191935 - ], - [ - 47.76788209665569, - 16.53039927245214 - ], - [ - 47.76808575754375, - 16.53111095080734 - ], - [ - 47.76810279212948, - 16.53110218886764 - ], - [ - 47.76790125467419, - 16.530397934318007 - ], - [ - 47.767920412692604, - 16.530396594848835 - ], - [ - 47.76811982671451, - 16.531093428256273 - ], - [ - 47.768136860399416, - 16.53108466630512 - ], - [ - 47.76793957161033, - 16.530395256712723 - ], - [ - 47.767958730527994, - 16.53039391724158 + 47.7677958621741, + 16.53051039683979 ], [ - 47.768153894983, - 16.53107590568231 + 47.76795620865596, + 16.530658248522045 ], [ - 47.76817092866645, - 16.531067145053775 + 47.768085758380195, + 16.53111095084737 ], [ - 47.76797788764677, - 16.530392579103516 + 47.768170929071175, + 16.531067144987073 ], [ - 47.767997046564254, - 16.530391239630404 + 47.76812500283788, + 16.530906658047133 ], [ - 47.7681879623492, - 16.53105838308546 + 47.768170929071175, + 16.531067144987073 ], [ - 47.76820499693062, - 16.531049622445487 + 47.76811004588309, + 16.530512964621895 ], [ - 47.768011359505685, - 16.530372972324884 + 47.76811004588309, + 16.530512964621895 ], [ - 47.76802514809256, - 16.530352870682762 + 47.76825609974418, + 16.531023338983744 ], [ - 47.76822203061193, - 16.531040860465723 + 47.76834127040819, + 16.53097953282404 ], [ - 47.7682390651919, - 16.53103209981431 - ], - [ - 47.768038937575305, - 16.530332769030014 - ], - [ - 47.76805272615508, - 16.53031266736664 - ], - [ - 47.768256098871774, - 16.531023337823104 - ], - [ - 47.768273133450315, - 16.531014577160242 - ], - [ - 47.768066514731295, - 16.53029256569264 - ], - [ - 47.76808030330396, - 16.530272464008018 - ], - [ - 47.76829016802814, - 16.531005815157602 - ], - [ - 47.76830720170584, - 16.530997054483304 - ], - [ - 47.76809409187308, - 16.530252363646827 - ], - [ - 47.76815568462939, - 16.530399306029828 - ], - [ - 47.768324235382835, - 16.53098829246921 - ], - [ - 47.76831125036794, - 16.530806343268754 + 47.76822245220941, + 16.530564330637993 ], [ - 47.76835830363405, - 16.530970769757932 + 47.76834127040819, + 16.53097953282404 ], [ - 47.7683412699585, - 16.53097953178347 + 47.76795620865596, + 16.530658248522045 ], [ - 47.768222452326334, - 16.530564330637993 + 47.7677958621741, + 16.53051039683979 ] ], "version": 1 @@ -989,20 +429,20 @@ 16.530403347547246 ], [ - 47.76799814678123, - 16.530388491662848 + 47.768126518382985, + 16.5309051298743 ] ], "type": "ComplexItem", "version": 1 }, { - "AMSLAltAboveTerrain": null, + "AMSLAltAboveTerrain": 0, "Altitude": 0, "AltitudeMode": 1, "autoContinue": true, "command": 21, - "doJumpId": 47, + "doJumpId": 19, "frame": 3, "params": [ 0, diff --git a/Paths/Routing error.wima b/Paths/Routing error.wima new file mode 100644 index 0000000000000000000000000000000000000000..b8fc3b3e94d0d0ad62632b6e8af92ea7ea1764c5 --- /dev/null +++ b/Paths/Routing error.wima @@ -0,0 +1,1034 @@ +{ + "AreaItems": [ + { + "AreaType": "Service Area", + "BorderPolygonOffset": 6, + "DepotAltitude": 0, + "DepotLatitude": 47.76779586216649, + "DepotLongitude": 16.530510396830728, + "ShowBorderPolygon": 0, + "maxAltitude": 30, + "polygon": [ + [ + 47.76809291417393, + 16.53012652393579 + ], + [ + 47.7681907801866, + 16.53040358920316 + ], + [ + 47.767783696604035, + 16.530655731490725 + ], + [ + 47.76771642095403, + 16.53041704413795 + ] + ] + }, + { + "AreaType": "Measurement Area", + "BorderPolygonOffset": 6, + "MinTileArea": 10, + "MinTransectLength": 1, + "ShowBorderPolygon": 0, + "ShowTiles": true, + "TileHeight": 5, + "TileWidth": 5, + "TransectDistance": 3, + "maxAltitude": 30, + "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 + ] + ] + } + ], + "MissionItems": { + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 3, + "hoverSpeed": 1, + "items": [ + { + "autoContinue": true, + "command": 22, + "doJumpId": 1, + "frame": 3, + "params": [ + 15, + 0, + 0, + null, + 47.76779586216649, + 16.530510396830728, + 5 + ], + "type": "SimpleItem" + }, + { + "Alpha": 23, + "MinLength": 5, + "ReferencePointAlt": 0, + "ReferencePointLat": 47.76807182520187, + "ReferencePointLong": 16.530610531894183, + "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.768125250928335, + 16.530839239105784, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768187963203616, + 16.531058383792516, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768204997344334, + 16.531049622605572, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812144975848, + 16.530757670470322, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811764852164, + 16.530676101820056, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768222031484335, + 16.53104086139957, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768239065614615, + 16.53103210018785, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768113847226864, + 16.530594533208365, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811004588309, + 16.530512964621895, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825609974418, + 16.531023338983744, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76827313388202, + 16.53101457776058, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810624447235, + 16.53043139603398, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815568510607, + 16.530399305509537, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76832423627323, + 16.530988294070095, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834127040819, + 16.53097953282404, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76822245220941, + 16.530564330637993, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76831125125834, + 16.530806346203697, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 21, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768358304533436, + 16.530970771572267, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768307202146545, + 16.53099705529708, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809864148597, + 16.530268258893802, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768102443012644, + 16.530349827431266, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76829016801015, + 16.531005816531692, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768170929071175, + 16.531067144987073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812500283788, + 16.530906658047133, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76809124406109, + 16.53085697602152, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815389493801, + 16.53107590617591, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768136860795146, + 16.531084667345684, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 31, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76805748523563, + 16.53080729404687, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76802372639744, + 16.53075761214986, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811982666054, + 16.531093428523086, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810279252523, + 16.53110218969476, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 35, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76798996754654, + 16.530707930317146, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76795620865596, + 16.530658248522045, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 37, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768085758380195, + 16.53111095084737, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 38, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806872423443, + 16.5311197120076, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 39, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76792244976166, + 16.53060856681793, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76788869084566, + 16.53055888517811, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 41, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768051690105956, + 16.531128473148772, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76803465596775, + 16.53113723429756, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 43, + "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.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, + "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": null, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 47, + "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/Routing error2.wima b/Paths/Routing error2.wima new file mode 100644 index 0000000000000000000000000000000000000000..571d965b65f3d6545d2ac1876788e97abe0ad20c --- /dev/null +++ b/Paths/Routing error2.wima @@ -0,0 +1,1034 @@ +{ + "AreaItems": [ + { + "AreaType": "Service Area", + "BorderPolygonOffset": 6, + "DepotAltitude": 0, + "DepotLatitude": 47.76807026847266, + "DepotLongitude": 16.5302609042063, + "ShowBorderPolygon": 0, + "maxAltitude": 30, + "polygon": [ + [ + 47.76809291417393, + 16.53012652393579 + ], + [ + 47.7681907801866, + 16.53040358920316 + ], + [ + 47.767783696604035, + 16.530655731490725 + ], + [ + 47.76771642095403, + 16.53041704413795 + ] + ] + }, + { + "AreaType": "Measurement Area", + "BorderPolygonOffset": 6, + "MinTileArea": 10, + "MinTransectLength": 1, + "ShowBorderPolygon": 0, + "ShowTiles": true, + "TileHeight": 5, + "TileWidth": 5, + "TransectDistance": 3, + "maxAltitude": 30, + "polygon": [ + [ + 47.76809580679245, + 16.530246122817612 + ], + [ + 47.76823933601322, + 16.53060087654427 + ], + [ + 47.7683711160486, + 16.530967006195464 + ], + [ + 47.7680076482754, + 16.531153949077463 + ], + [ + 47.7677855557718, + 16.530403347547246 + ], + [ + 47.76803790826872, + 16.53059609924395 + ] + ] + } + ], + "MissionItems": { + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 3, + "hoverSpeed": 1, + "items": [ + { + "autoContinue": true, + "command": 22, + "doJumpId": 1, + "frame": 3, + "params": [ + 15, + 0, + 0, + null, + 47.76807026847266, + 16.5302609042063, + 5 + ], + "type": "SimpleItem" + }, + { + "Alpha": 23, + "MinLength": 5, + "ReferencePointAlt": 0, + "ReferencePointLat": 47.76807182520187, + "ReferencePointLong": 16.530610531894183, + "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.76809569830049, + 16.530257974342632, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768307202146545, + 16.53099705529708, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76832423627323, + 16.530988294070095, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815568511506, + 16.530399305536218, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76808853988583, + 16.53030124484071, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76829016801015, + 16.531005816531692, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76831125125834, + 16.530806346203697, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768358304533436, + 16.530970771572267, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834127040819, + 16.53097953282404, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76822245220941, + 16.530564330637993, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76796170862731, + 16.53054089736257, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811982666054, + 16.531093428523086, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76810279252523, + 16.53110218969476, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76793670095285, + 16.530521795991955, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76786167791875, + 16.530464491976645, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 17, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768051690105956, + 16.531128473148772, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76803465596775, + 16.53113723429756, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 19, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76783667023114, + 16.53044539066594, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76781166254923, + 16.530426289386895, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 21, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76801762181985, + 16.531145995440625, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806872423443, + 16.5311197120076, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 23, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.767886685603074, + 16.530483593292324, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76791169327511, + 16.530502694626307, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 25, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768085758380195, + 16.53111095084737, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768136860795146, + 16.531084667345684, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 27, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7679867162895, + 16.530559998751496, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76801172395737, + 16.530579100172073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 29, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76815389493801, + 16.53107590617591, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768170929071175, + 16.531067144987073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 31, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768036731612966, + 16.530598201597627, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76804558902823, + 16.530560867713245, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 33, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768187963203616, + 16.531058383792516, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768204997344334, + 16.531049622605572, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 35, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76805274754079, + 16.530517597273068, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768059906046055, + 16.53047432680768, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 37, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768222031484335, + 16.53104086139957, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 38, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768239065614615, + 16.53103210018785, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 39, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76806706452599, + 16.530431056330418, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76807422299861, + 16.530387785854614, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 41, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825609974418, + 16.531023338983744, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76827313388202, + 16.53101457776058, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 43, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768081381445874, + 16.530344515366945, + 15 + ], + "type": "SimpleItem" + } + ], + "Refly90Degrees": false, + "TurnAroundDistance": 10, + "VisualTransectPoints": [ + [ + 47.76809569830049, + 16.530257974342632 + ], + [ + 47.768307202146545, + 16.53099705529708 + ], + [ + 47.76832423627323, + 16.530988294070095 + ], + [ + 47.76815568511506, + 16.530399305536218 + ], + [ + 47.76808853988583, + 16.53030124484071 + ], + [ + 47.76829016801015, + 16.531005816531692 + ], + [ + 47.76831125125834, + 16.530806346203697 + ], + [ + 47.768358304533436, + 16.530970771572267 + ], + [ + 47.76834127040819, + 16.53097953282404 + ], + [ + 47.76822245220941, + 16.530564330637993 + ], + [ + 47.76796170862731, + 16.53054089736257 + ], + [ + 47.76811982666054, + 16.531093428523086 + ], + [ + 47.76810279252523, + 16.53110218969476 + ], + [ + 47.76793670095285, + 16.530521795991955 + ], + [ + 47.76786167791875, + 16.530464491976645 + ], + [ + 47.768051690105956, + 16.531128473148772 + ], + [ + 47.76803465596775, + 16.53113723429756 + ], + [ + 47.76783667023114, + 16.53044539066594 + ], + [ + 47.76781166254923, + 16.530426289386895 + ], + [ + 47.76801762181985, + 16.531145995440625 + ], + [ + 47.76806872423443, + 16.5311197120076 + ], + [ + 47.767886685603074, + 16.530483593292324 + ], + [ + 47.76791169327511, + 16.530502694626307 + ], + [ + 47.768085758380195, + 16.53111095084737 + ], + [ + 47.768136860795146, + 16.531084667345684 + ], + [ + 47.7679867162895, + 16.530559998751496 + ], + [ + 47.76801172395737, + 16.530579100172073 + ], + [ + 47.76815389493801, + 16.53107590617591 + ], + [ + 47.768170929071175, + 16.531067144987073 + ], + [ + 47.768036731612966, + 16.530598201597627 + ], + [ + 47.76804558902823, + 16.530560867713245 + ], + [ + 47.768187963203616, + 16.531058383792516 + ], + [ + 47.768204997344334, + 16.531049622605572 + ], + [ + 47.76805274754079, + 16.530517597273068 + ], + [ + 47.768059906046055, + 16.53047432680768 + ], + [ + 47.768222031484335, + 16.53104086139957 + ], + [ + 47.768239065614615, + 16.53103210018785 + ], + [ + 47.76806706452599, + 16.530431056330418 + ], + [ + 47.76807422299861, + 16.530387785854614 + ], + [ + 47.76825609974418, + 16.531023338983744 + ], + [ + 47.76827313388202, + 16.53101457776058 + ], + [ + 47.768081381445874, + 16.530344515366945 + ] + ], + "version": 1 + }, + "Type": 1, + "complexItemType": "CircularSurvey", + "polygon": [ + [ + 47.76809580679245, + 16.530246122817612 + ], + [ + 47.76823933601322, + 16.53060087654427 + ], + [ + 47.7683711160486, + 16.530967006195464 + ], + [ + 47.7680076482754, + 16.531153949077463 + ], + [ + 47.7677855557718, + 16.530403347547246 + ], + [ + 47.76803790826872, + 16.53059609924395 + ] + ], + "type": "ComplexItem", + "version": 1 + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 47, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76807026847266, + 16.5302609042063, + 0 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.76807026847266, + 16.5302609042063, + 178 + ], + "vehicleType": 2, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 + } +} diff --git a/Paths/test.plan b/Paths/test.plan new file mode 100644 index 0000000000000000000000000000000000000000..8944dac47b8e2848b31c89d3c3a533698f56ba11 --- /dev/null +++ b/Paths/test.plan @@ -0,0 +1,405 @@ +{ + "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, + "ReferencePointAlt": 0, + "ReferencePointLat": 47.76807182520187, + "ReferencePointLong": 16.530610531894183, + "TransectDistance": 10, + "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.7677958621741, + 16.53051039683979, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76795620865596, + 16.530658248522045, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768085758380195, + 16.53111095084737, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768170929071175, + 16.531067144987073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76812500283788, + 16.530906658047133, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.768170929071175, + 16.531067144987073, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811004588309, + 16.530512964621895, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76811004588309, + 16.530512964621895, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76825609974418, + 16.531023338983744, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 11, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834127040819, + 16.53097953282404, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76822245220941, + 16.530564330637993, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 13, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76834127040819, + 16.53097953282404, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.76795620865596, + 16.530658248522045, + 15 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 16, + "doJumpId": 15, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.7677958621741, + 16.53051039683979, + 15 + ], + "type": "SimpleItem" + } + ], + "Refly90Degrees": false, + "TurnAroundDistance": 10, + "VisualTransectPoints": [ + [ + 47.7677958621741, + 16.53051039683979 + ], + [ + 47.76795620865596, + 16.530658248522045 + ], + [ + 47.768085758380195, + 16.53111095084737 + ], + [ + 47.768170929071175, + 16.531067144987073 + ], + [ + 47.76812500283788, + 16.530906658047133 + ], + [ + 47.768170929071175, + 16.531067144987073 + ], + [ + 47.76811004588309, + 16.530512964621895 + ], + [ + 47.76811004588309, + 16.530512964621895 + ], + [ + 47.76825609974418, + 16.531023338983744 + ], + [ + 47.76834127040819, + 16.53097953282404 + ], + [ + 47.76822245220941, + 16.530564330637993 + ], + [ + 47.76834127040819, + 16.53097953282404 + ], + [ + 47.76795620865596, + 16.530658248522045 + ], + [ + 47.7677958621741, + 16.53051039683979 + ] + ], + "version": 1 + }, + "Type": 1, + "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": 19, + "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 49544aa75fd5843a75bdf966097e30e31c131a73..4e38d2ebbd6e240674ccc0475fa99e8ed0103091 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -28,8 +28,8 @@ QGCROOT = $$PWD DebugBuild { DESTDIR = $${OUT_PWD}/debug DEFINES += DEBUG - DEFINES += SNAKE_SHOW_TIME - DEFINES += DEBUG_SRTL + #DEFINES += SNAKE_SHOW_TIME + #DEFINES += DEBUG_SRTL #DEFINES += SNAKE_DEBUG DEFINES += SHOW_CIRCULAR_SURVEY_TIME DEFINES += DEBUG_CIRCULAR_SURVEY @@ -38,9 +38,10 @@ DebugBuild { else { DESTDIR = $${OUT_PWD}/release #DEFINES += ROS_BRIDGE_DEBUG - #DEFINES += SHOW_CIRCULAR_SURVEY_TIME - #DEFINES += SNAKE_SHOW_TIME - DEFINES += DEBUG_SRTL + DEFINES += SHOW_CIRCULAR_SURVEY_TIME + DEFINES += SNAKE_SHOW_TIME + #DEFINES += SNAKE_DEBUG + #DEFINES += DEBUG_SRTL DEFINES += NDEBUG } diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index c45be2c6689695c773be1ac3a6780cb1d25e1207..f0c7ea9756b2deb10ac6abdcea10010ecbb5821f 100644 --- a/src/Wima/CircularSurvey.cc +++ b/src/Wima/CircularSurvey.cc @@ -5,7 +5,7 @@ #include "QGCApplication.h" // Wima #include "snake.h" -#define CLIPPER_SCALE 10000 +#define CLIPPER_SCALE 1000000 #include "clipper/clipper.hpp" template ClipperLib::cInt get(ClipperLib::IntPoint &p); @@ -344,6 +344,7 @@ bool CircularSurvey::readyForSave() const { double CircularSurvey::additionalTimeDelay() const { return 0; } void CircularSurvey::_rebuildTransectsPhase1(void) { + qWarning() << "_rebuildTransectsPhase1: TODO: remove depot valid stuff"; // Store result of former calculation. if (this->_needsStoring) { #ifdef SHOW_CIRCULAR_SURVEY_TIME @@ -356,6 +357,7 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { _loadedMissionItemsParent->deleteLater(); _loadedMissionItemsParent = nullptr; } + bool error = false; // Store raw transects. const auto &transectsENU = this->_workerOutput->transects; const auto &ori = this->_referencePoint; @@ -375,46 +377,70 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { } // Store route. const auto &transectsInfo = this->_workerOutput->transectsInfo; - const auto &route = this->_workerOutput->route; - // Find index of first waypoint. - std::size_t idxFirst = 0; - const auto &infoFirst = transectsInfo.front(); - const auto &firstTransect = transectsENU[infoFirst.index]; - const auto &firstWaypoint = - infoFirst.reversed ? firstTransect.back() : firstTransect.front(); - double th = 0.001; - for (std::size_t i = 0; i < route.size(); ++i) { - auto dist = bg::distance(route[i], firstWaypoint); - if (dist < th) { - idxFirst = i; - break; - } - } - // Find index of last waypoint. - std::size_t idxLast = route.size() - 1; - const auto &infoLast = transectsInfo.back(); - const auto &lastTransect = transectsENU[infoLast.index]; - const auto &lastWaypoint = - infoLast.reversed ? lastTransect.front() : lastTransect.back(); - for (long i = route.size() - 1; i >= 0; --i) { - auto dist = bg::distance(route[i], lastWaypoint); - if (dist < th) { - idxLast = i; - break; + if (transectsInfo.size() > 1) { + const auto &route = this->_workerOutput->route; + // Find index of first waypoint. + std::size_t idxFirst = 0; + const auto &infoFirst = + this->depot().isValid() ? transectsInfo.at(1) : transectsInfo.at(0); + const auto &firstTransect = transectsENU[infoFirst.index]; + if (firstTransect.size() > 0) { + const auto &firstWaypoint = + infoFirst.reversed ? firstTransect.back() : firstTransect.front(); + double th = 0.01; + for (std::size_t i = 0; i < route.size(); ++i) { + auto dist = bg::distance(route[i], firstWaypoint); + if (dist < th) { + idxFirst = i; + break; + } + } + // Find index of last waypoint. + std::size_t idxLast = route.size() - 1; + const auto &infoLast = transectsInfo.at(transectsInfo.size() - 2); + const auto &lastTransect = transectsENU[infoLast.index]; + if (lastTransect.size() > 0) { + const auto &lastWaypoint = + infoLast.reversed ? lastTransect.front() : lastTransect.back(); + for (long i = route.size() - 1; i >= 0; --i) { + auto dist = bg::distance(route[i], lastWaypoint); + if (dist < th) { + idxLast = i; + break; + } + } + // Convert to geo coordinates. + QList list; + for (std::size_t i = idxFirst; i <= idxLast; ++i) { + auto &vertex = route[i]; + QGeoCoordinate c; + snake::fromENU(ori, vertex, c); + list.append(CoordInfo_t{c, CoordTypeInterior}); + } + this->_transects.append(std::move(list)); + } else { + qWarning() + << "CS::rebuildTransectsPhase1(): lastTransect.size() == 0"; + error = true; + } + } else { + qWarning() << "CS::rebuildTransectsPhase1(): firstTransect.size() == 0"; + error = true; } + } else { + qWarning() << "CS::rebuildTransectsPhase1(): transectsInfo.size() <= 1"; + error = true; } - // Convert to geo coordinates. - QList list; - for (std::size_t i = idxFirst; i <= idxLast; ++i) { - auto &vertex = route[i]; - QGeoCoordinate c; - snake::fromENU(ori, vertex, c); - list.append(CoordInfo_t{c, CoordTypeInterior}); - } - this->_transects.append(list); - // Mark transect as stored and ready. + this->_needsStoring = false; - this->_transectsDirty = false; + if (!error) { + // Mark transect as stored and ready. + this->_transectsDirty = false; + } else { // clear up + this->_rawTransects.clear(); + this->_transects.clear(); + } + #ifdef SHOW_CIRCULAR_SURVEY_TIME qWarning() << "CS::rebuildTransectsPhase1(): store: " << std::chrono::duration_cast( @@ -455,7 +481,7 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { v.setAltitude(0); } auto depot = this->_depot; - snake::BoostPolygon safeAreaENU; + snake::FPolygon safeAreaENU; bool useDepot = false; if (this->_depot.isValid() && this->_safeArea.size() >= 3) { useDepot = true; @@ -470,6 +496,7 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { auto alpha = snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree); // Select survey type. + this->_needsStoring = false; if (this->_type.rawValue().toUInt() == integral(Type::Circular)) { if (alpha >= snake::Angle(0.3 * bu::degree::degree) && alpha <= snake::Angle(45 * bu::degree::degree)) { @@ -554,9 +581,9 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, if (polygon.size() >= 3) { using namespace boost::units; // Convert geo polygon to ENU polygon. - snake::BoostPolygon polygonENU; - snake::BoostPoint originENU{0, 0}; - snake::BoostPoint depotENU{0, 0}; + 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); @@ -639,7 +666,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } // Clip sectors to polygonENU. ClipperLib::Path polygonClipper; - snake::BoostPolygon shrinked; + snake::FPolygon shrinked; snake::offsetPolygon(polygonENU, shrinked, -0.3); auto &outer = shrinked.outer(); polygonClipper.reserve(outer.size()); @@ -658,15 +685,15 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, // Extract transects from PolyTree and convert them to // BoostLineString if (useDepot) { - transects.push_back(snake::BoostLineString{depotENU}); + transects.push_back(snake::FLineString{depotENU}); } for (const auto &child : transectsClipper.Childs) { - snake::BoostLineString transect; + snake::FLineString transect; transect.reserve(child->Contour.size()); for (const auto &vertex : child->Contour) { auto x = static_cast(vertex.X) / CLIPPER_SCALE; auto y = static_cast(vertex.Y) / CLIPPER_SCALE; - transect.push_back(snake::BoostPoint(x, y)); + transect.push_back(snake::FPoint(x, y)); } transects.push_back(transect); } @@ -676,7 +703,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, for (auto iti = ito + 1; iti < transects.end(); ++iti) { auto dist1 = bg::distance(ito->front(), iti->front()); if (dist1 < th) { - snake::BoostLineString temp; + snake::FLineString temp; for (auto it = iti->end() - 1; it >= iti->begin(); --it) { temp.push_back(*it); } @@ -687,7 +714,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } auto dist2 = bg::distance(ito->front(), iti->back()); if (dist2 < th) { - snake::BoostLineString temp; + snake::FLineString temp; temp.insert(temp.end(), iti->begin(), iti->end()); temp.insert(temp.end(), ito->begin(), ito->end()); *ito = temp; @@ -696,7 +723,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } auto dist3 = bg::distance(ito->back(), iti->front()); if (dist3 < th) { - snake::BoostLineString temp; + snake::FLineString temp; temp.insert(temp.end(), ito->begin(), ito->end()); temp.insert(temp.end(), iti->begin(), iti->end()); *ito = temp; @@ -705,7 +732,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, } auto dist4 = bg::distance(ito->back(), iti->back()); if (dist4 < th) { - snake::BoostLineString temp; + snake::FLineString temp; temp.insert(temp.end(), ito->begin(), ito->end()); for (auto it = iti->end() - 1; it >= iti->begin(); --it) { temp.push_back(*it); @@ -749,7 +776,7 @@ bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, if (minIt != transects.begin()) { auto minTransect = *minIt; if (reverse) { - snake::BoostLineString rev; + snake::FLineString rev; for (auto it = minTransect.end() - 1; it >= minTransect.begin(); --it) { rev.push_back(*it); @@ -784,7 +811,7 @@ bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, // Check preconitions if (polygon.size() >= 3) { // Convert to ENU system. - snake::BoostPolygon polygonENU; + snake::FPolygon polygonENU; snake::areaToEnu(origin, polygon, polygonENU); std::string error; // Check validity. @@ -798,14 +825,14 @@ bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, qWarning() << ss.str().c_str(); #endif } else { - snake::BoostPoint depotENU; + 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::BoostPolygon polygonENURotated; + snake::FPolygon polygonENURotated; bg::transform(polygonENU, polygonENURotated, rotate); - snake::BoostBox box; + snake::FBox box; boost::geometry::envelope(polygonENURotated, box); double x0 = box.min_corner().get<0>(); double y0 = box.min_corner().get<1>(); @@ -818,13 +845,13 @@ bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, transectsClipper.reserve(num_t); for (size_t i = 0; i < num_t; ++i) { // calculate transect - snake::BoostPoint v1{x0, y0 + i * distance.value()}; - snake::BoostPoint v2{x1, y0 + i * distance.value()}; - snake::BoostLineString transect; + snake::FPoint v1{x0, y0 + i * distance.value()}; + snake::FPoint v2{x1, y0 + i * distance.value()}; + snake::FLineString transect; transect.push_back(v1); transect.push_back(v2); // transform back - snake::BoostLineString temp_transect; + snake::FLineString temp_transect; tr::rotate_transformer rotate_back( -angle.value() * 180 / M_PI); bg::transform(transect, temp_transect, rotate_back); @@ -850,7 +877,7 @@ bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, } // Convert measurement area to clipper path. - snake::BoostPolygon shrinked; + snake::FPolygon shrinked; snake::offsetPolygon(polygonENU, shrinked, -0.2); auto &outer = shrinked.outer(); ClipperLib::Path polygonClipper; @@ -871,18 +898,18 @@ bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot, // Extract transects from PolyTree and convert them to BoostLineString if (useDepot) { - transects.push_back(snake::BoostLineString{depotENU}); + transects.push_back(snake::FLineString{depotENU}); } for (const auto &child : clippedTransecs.Childs) { const auto &clipperTransect = child->Contour; - snake::BoostPoint v1{ + snake::FPoint v1{ static_cast(clipperTransect[0].X) / CLIPPER_SCALE, static_cast(clipperTransect[0].Y) / CLIPPER_SCALE}; - snake::BoostPoint v2{ + snake::FPoint v2{ static_cast(clipperTransect[1].X) / CLIPPER_SCALE, static_cast(clipperTransect[1].Y) / CLIPPER_SCALE}; - snake::BoostLineString transect{v1, v2}; + snake::FLineString transect{v1, v2}; if (bg::length(transect) >= minLength.value()) { transects.push_back(transect); } diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc index 3ec518ea6b228b9fccf8fd1e7e54c7fa3dc001f1..18fddf1858edbbe78b427fe17c53217b1f10b2a4 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.cc +++ b/src/Wima/Geometry/WimaMeasurementArea.cc @@ -259,9 +259,9 @@ void WimaMeasurementArea::doUpdate() { DataPtr pData(new TileData()); // Convert to ENU system. QGeoCoordinate origin = polygon.first(); - BoostPolygon polygonENU; + FPolygon polygonENU; areaToEnu(origin, polygon, polygonENU); - std::vector tilesENU; + std::vector tilesENU; BoundingBox bbox; std::string errorString; // Generate tiles. @@ -277,7 +277,7 @@ void WimaMeasurementArea::doUpdate() { } pData->tiles.append(geoTile); // Calculate center. - snake::BoostPoint center; + snake::FPoint center; snake::polygonCenter(t, center); QGeoCoordinate geoCenter; fromENU(origin, center, geoCenter); diff --git a/src/Wima/RoutingThread.cpp b/src/Wima/RoutingThread.cpp index 8461ea773c6f8135a2c1182e51364f82fb0f98ce..0a241e14d91767fc1092f6eb1cd03dad3aaacfba 100644 --- a/src/Wima/RoutingThread.cpp +++ b/src/Wima/RoutingThread.cpp @@ -23,7 +23,7 @@ RoutingThread::~RoutingThread() { bool RoutingThread::calculating() const { return this->_calculating; } -void RoutingThread::route(const snake::BoostPolygon &safeArea, +void RoutingThread::route(const snake::FPolygon &safeArea, const RoutingThread::Generator &generator) { // Sample input. Lock lk(this->_mutex); @@ -81,9 +81,37 @@ void RoutingThread::run() { return restart || expired; }; std::string errorString; - // Route transects; + // Route transects + //#ifdef SHOW_CIRCULAR_SURVEY_TIME + // auto s = std::chrono::high_resolution_clock::now(); + //#endif + // snake::route_old(safeAreaENU, transectsENU, transectsInfo, + // route, + // stopLambda, errorString); + //#ifdef SHOW_CIRCULAR_SURVEY_TIME + // qWarning() << "RoutingWorker::run(): route_old time: " + // << + // std::chrono::duration_cast( + // std::chrono::high_resolution_clock::now() - + // s) .count() + // << " ms"; + //#endif + //#ifdef SHOW_CIRCULAR_SURVEY_TIME + // s = std::chrono::high_resolution_clock::now(); + //#endif + // transectsInfo.clear(); + // route.clear(); + // errorString.clear(); bool success = snake::route(safeAreaENU, transectsENU, transectsInfo, route, stopLambda, errorString); + //#ifdef SHOW_CIRCULAR_SURVEY_TIME + // qWarning() << "RoutingWorker::run(): route time: " + // << + // std::chrono::duration_cast( + // std::chrono::high_resolution_clock::now() - + // s) .count() + // << " ms"; + //#endif // Check if routing was successful. if ((!success || route.size() < 3) && !this->_restart) { #ifdef DEBUG_CIRCULAR_SURVEY diff --git a/src/Wima/RoutingThread.h b/src/Wima/RoutingThread.h index b725a1805cefc0c68e671129ddf698b23c86d6df..d17409bcd99a73587f0f615f6846016d30ba2943 100644 --- a/src/Wima/RoutingThread.h +++ b/src/Wima/RoutingThread.h @@ -11,7 +11,7 @@ #include struct RoutingData { - snake::BoostLineString route; + snake::FLineString route; snake::Transects transects; std::vector transectsInfo; }; @@ -35,7 +35,7 @@ public: bool calculating() const; public slots: - void route(const snake::BoostPolygon &safeArea, const Generator &generator); + void route(const snake::FPolygon &safeArea, const Generator &generator); signals: void result(PtrRoutingData pTransects); @@ -48,7 +48,7 @@ private: mutable std::mutex _mutex; mutable std::condition_variable _cv; // Internal data - snake::BoostPolygon _safeArea; + snake::FPolygon _safeArea; Generator _generator; // transect generator // State std::atomic_bool _calculating; diff --git a/src/Wima/Snake/snake.cpp b/src/Wima/Snake/snake.cpp index 71dee537857b2d1ba11c6b99eed83ef9615438b6..3c565df6c16631438cb68d4bae7ad3805291b511 100644 --- a/src/Wima/Snake/snake.cpp +++ b/src/Wima/Snake/snake.cpp @@ -12,7 +12,7 @@ #include #include "clipper/clipper.hpp" -#define CLIPPER_SCALE 10000 +#define CLIPPER_SCALE 1000000 #include "ortools/constraint_solver/routing.h" #include "ortools/constraint_solver/routing_enums.pb.h" @@ -31,11 +31,12 @@ namespace trans = bg::strategy::transform; BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(bg::cs::cartesian) namespace snake { +static const IntType stdScale = 1000000; //========================================================================= // Geometry stuff. //========================================================================= -void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) { +void polygonCenter(const FPolygon &polygon, FPoint ¢er) { using namespace mapbox; if (polygon.outer().empty()) return; @@ -53,7 +54,7 @@ void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) { center.set<1>(c.y); } -bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { +bool minimalBoundingBox(const FPolygon &polygon, BoundingBox &minBBox) { /* Find the minimum-area bounding box of a set of 2D points @@ -97,20 +98,20 @@ bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { if (polygon.outer().empty() || polygon.outer().size() < 3) return false; - BoostPolygon convex_hull; + FPolygon convex_hull; bg::convex_hull(polygon, convex_hull); // cout << "Convex hull: " << bg::wkt(convex_hull) << endl; //# Compute edges (x2-x1,y2-y1) - std::vector edges; + std::vector edges; const auto &convex_hull_outer = convex_hull.outer(); for (long i = 0; i < long(convex_hull_outer.size()) - 1; ++i) { - BoostPoint p1 = convex_hull_outer.at(i); - BoostPoint p2 = convex_hull_outer.at(i + 1); + FPoint p1 = convex_hull_outer.at(i); + FPoint p2 = convex_hull_outer.at(i + 1); double edge_x = p2.get<0>() - p1.get<0>(); double edge_y = p2.get<1>() - p1.get<1>(); - edges.push_back(BoostPoint{edge_x, edge_y}); + edges.push_back(FPoint{edge_x, edge_y}); } // cout << "Edges: "; @@ -144,19 +145,19 @@ bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { trans::rotate_transformer rotate(angle * 180 / M_PI); - BoostPolygon hull_rotated; + FPolygon hull_rotated; bg::transform(convex_hull, hull_rotated, rotate); // cout << "Convex hull rotated: " << bg::wkt(hull_rotated) // << endl; - bg::model::box box; + bg::model::box box; bg::envelope(hull_rotated, box); // cout << "Bounding box: " << // bg::wkt>(box) << endl; //# print "Rotated hull points are \n", rot_points - BoostPoint min_corner = box.min_corner(); - BoostPoint max_corner = box.max_corner(); + FPoint min_corner = box.min_corner(); + FPoint max_corner = box.max_corner(); double min_x = min_corner.get<0>(); double max_x = max_corner.get<0>(); double min_y = min_corner.get<1>(); @@ -184,11 +185,11 @@ bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { minBBox.height = height; minBBox.corners.clear(); - minBBox.corners.outer().push_back(BoostPoint{min_x, min_y}); - minBBox.corners.outer().push_back(BoostPoint{min_x, max_y}); - minBBox.corners.outer().push_back(BoostPoint{max_x, max_y}); - minBBox.corners.outer().push_back(BoostPoint{max_x, min_y}); - minBBox.corners.outer().push_back(BoostPoint{min_x, min_y}); + minBBox.corners.outer().push_back(FPoint{min_x, min_y}); + minBBox.corners.outer().push_back(FPoint{min_x, max_y}); + minBBox.corners.outer().push_back(FPoint{max_x, max_y}); + minBBox.corners.outer().push_back(FPoint{max_x, min_y}); + minBBox.corners.outer().push_back(FPoint{min_x, min_y}); } // cout << endl << endl; } @@ -196,14 +197,14 @@ bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { // Transform corners of minimal bounding box. trans::rotate_transformer rotate(-minBBox.angle * 180 / M_PI); - BoostPolygon rotated_polygon; + FPolygon rotated_polygon; bg::transform(minBBox.corners, rotated_polygon, rotate); minBBox.corners = rotated_polygon; return true; } -void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, +void offsetPolygon(const FPolygon &polygon, FPolygon &polygonOffset, double offset) { bg::strategy::buffer::distance_symmetric distance_strategy(offset); bg::strategy::buffer::join_miter join_strategy(3); @@ -211,7 +212,7 @@ void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, bg::strategy::buffer::point_square point_strategy; bg::strategy::buffer::side_straight side_strategy; - bg::model::multi_polygon result; + bg::model::multi_polygon result; bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy); @@ -220,125 +221,41 @@ void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, polygonOffset = result[0]; } -void graphFromPolygon(const BoostPolygon &polygon, - const BoostLineString &vertices, Matrix &graph) { - size_t n = graph.getN(); +void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices, + Matrix &graph) { + size_t n = graph.n(); for (size_t i = 0; i < n; ++i) { - BoostPoint v1 = vertices[i]; + FPoint v1 = vertices[i]; for (size_t j = i + 1; j < n; ++j) { - BoostPoint v2 = vertices[j]; - BoostLineString path{v1, v2}; + FPoint v2 = vertices[j]; + FLineString path{v1, v2}; double distance = 0; if (!bg::within(path, polygon)) distance = std::numeric_limits::infinity(); else distance = bg::length(path); - - graph.set(i, j, distance); - graph.set(j, i, distance); + graph(i, j) = distance; + graph(j, i) = distance; } } } -bool dijkstraAlgorithm( - const size_t numElements, size_t startIndex, size_t endIndex, - std::vector &elementPath, - std::function distanceDij) { - if (startIndex >= numElements || endIndex >= numElements || - endIndex == startIndex) { - return false; - } - // Node struct - // predecessorIndex is the index of the predecessor node - // (nodeList[predecessorIndex]) distance is the distance between the node and - // the start node node number is stored by the position in nodeList - struct Node { - int predecessorIndex = -1; - double distance = std::numeric_limits::infinity(); - }; - - // The list with all Nodes (elements) - std::vector nodeList(numElements); - // This list will be initalized with indices referring to the elements of - // nodeList. Elements will be successively remove during the execution of the - // Dijkstra Algorithm. - std::vector workingSet(numElements); - - // append elements to node list - for (size_t i = 0; i < numElements; ++i) - workingSet[i] = i; - - nodeList[startIndex].distance = 0; - - // Dijkstra Algorithm - // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus - while (workingSet.size() > 0) { - // serach Node with minimal distance - double minDist = std::numeric_limits::infinity(); - int minDistIndex_WS = -1; // WS = workinSet - for (size_t i = 0; i < workingSet.size(); ++i) { - const int nodeIndex = workingSet.at(i); - const double dist = nodeList.at(nodeIndex).distance; - if (dist < minDist) { - minDist = dist; - minDistIndex_WS = i; - } - } - if (minDistIndex_WS == -1) - return false; - - size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList - workingSet.erase(workingSet.begin() + minDistIndex_WS); - if (indexU_NL == endIndex) // shortest path found - break; - - const double distanceU = nodeList.at(indexU_NL).distance; - // update distance - for (size_t i = 0; i < workingSet.size(); ++i) { - int indexV_NL = workingSet[i]; // NL = nodeList - Node *v = &nodeList[indexV_NL]; - double dist = distanceDij(indexU_NL, indexV_NL); - // is ther an alternative path which is shorter? - double alternative = distanceU + dist; - if (alternative < v->distance) { - v->distance = alternative; - v->predecessorIndex = indexU_NL; - } - } - } - // end Djikstra Algorithm - - // reverse assemble path - int e = endIndex; - while (1) { - if (e == -1) { - if (elementPath[0] == startIndex) // check if starting point was reached - break; - return false; - } - elementPath.insert(elementPath.begin(), e); - - // Update Node - e = nodeList[e].predecessorIndex; - } - return true; -} - bool toDistanceMatrix(Matrix &graph) { - size_t n = graph.getN(); + size_t n = graph.n(); - auto distance = [graph](size_t i, size_t j) { return graph.get(i, j); }; + auto distance = [&graph](size_t i, size_t j) -> double { + return graph(i, j); + }; - std::vector path; for (size_t i = 0; i < n; ++i) { for (size_t j = i + 1; j < n; ++j) { - double d = graph.get(i, j); + double d = graph(i, j); if (!std::isinf(d)) continue; - path.clear(); - if (!dijkstraAlgorithm(n, i, j, path, distance)) { + std::vector path; + if (!dijkstraAlgorithm(n, i, j, path, d, distance)) { return false; } // cout << "(" << i << "," << j << ") d: " << d << endl; @@ -346,41 +263,16 @@ bool toDistanceMatrix(Matrix &graph) { // for (auto idx : path) // cout << idx << " "; // cout << endl; - - d = 0; - for (long k = 0; k < long(path.size()) - 1; ++k) { - size_t idx0 = path[k]; - size_t idx1 = path[k + 1]; - double d0 = graph.get(idx0, idx1); - assert(std::isinf(d0) == false); - d += d0; - } - - graph.set(i, j, d); - graph.set(j, i, d); + graph(i, j) = d; + graph(j, i) = d; } } return true; } -void shortestPathFromGraph(const Matrix &graph, size_t startIndex, - size_t endIndex, std::vector &pathIdx) { - - if (!std::isinf(graph.get(startIndex, endIndex))) { - pathIdx.push_back(startIndex); - pathIdx.push_back(endIndex); - } else { - auto distance = [graph](size_t i, size_t j) { return graph.get(i, j); }; - bool ret = dijkstraAlgorithm(graph.getN(), startIndex, endIndex, pathIdx, - distance); - assert(ret); - (void)ret; - } -} - -bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth, - Area minTileArea, std::vector &tiles, - BoundingBox &bbox, string &errorString) { +bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth, + Area minTileArea, std::vector &tiles, BoundingBox &bbox, + string &errorString) { if (area.outer().empty() || area.outer().size() < 4) { errorString = "Area has to few vertices."; return false; @@ -405,7 +297,7 @@ bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth, return false; double bboxWidth = bbox.width; double bboxHeight = bbox.height; - BoostPoint origin = bbox.corners.outer()[0]; + FPoint origin = bbox.corners.outer()[0]; // cout << "Origin: " << origin[0] << " " << origin[1] << endl; // Transform _mArea polygon to bounding box coordinate system. @@ -413,8 +305,8 @@ bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth, bbox.angle * 180 / M_PI); trans::translate_transformer translate(-origin.get<0>(), -origin.get<1>()); - BoostPolygon translated_polygon; - BoostPolygon rotated_polygon; + FPolygon translated_polygon; + FPolygon rotated_polygon; boost::geometry::transform(area, translated_polygon, translate); boost::geometry::transform(translated_polygon, rotated_polygon, rotate); bg::correct(rotated_polygon); @@ -442,23 +334,23 @@ bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth, double y_min = tileHeight.value() * j; double y_max = y_min + tileHeight.value(); - BoostPolygon tile_unclipped; - tile_unclipped.outer().push_back(BoostPoint{x_min, y_min}); - tile_unclipped.outer().push_back(BoostPoint{x_min, y_max}); - tile_unclipped.outer().push_back(BoostPoint{x_max, y_max}); - tile_unclipped.outer().push_back(BoostPoint{x_max, y_min}); - tile_unclipped.outer().push_back(BoostPoint{x_min, y_min}); + FPolygon tile_unclipped; + tile_unclipped.outer().push_back(FPoint{x_min, y_min}); + tile_unclipped.outer().push_back(FPoint{x_min, y_max}); + tile_unclipped.outer().push_back(FPoint{x_max, y_max}); + tile_unclipped.outer().push_back(FPoint{x_max, y_min}); + tile_unclipped.outer().push_back(FPoint{x_min, y_min}); - std::deque boost_tiles; + std::deque boost_tiles; if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, boost_tiles)) continue; - for (BoostPolygon t : boost_tiles) { + for (FPolygon t : boost_tiles) { if (bg::area(t) > minTileArea.value()) { // Transform boost_tile to world coordinate system. - BoostPolygon rotated_tile; - BoostPolygon translated_tile; + FPolygon rotated_tile; + FPolygon translated_tile; boost::geometry::transform(t, rotated_tile, rotate_back); boost::geometry::transform(rotated_tile, translated_tile, translate_back); @@ -481,8 +373,8 @@ bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth, return true; } -bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea, - const BoostPolygon &corridor, BoostPolygon &jArea, +bool joinedArea(const FPolygon &mArea, const FPolygon &sArea, + const FPolygon &corridor, FPolygon &jArea, std::string &errorString) { // Measurement area and service area overlapping? bool overlapingSerMeas = bg::intersects(mArea, sArea) ? true : false; @@ -501,8 +393,8 @@ bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea, } // Are areas joinable? - std::deque sol; - BoostPolygon partialArea = mArea; + std::deque sol; + FPolygon partialArea = mArea; if (overlapingSerMeas) { if (corridor_is_connection) { bg::union_(partialArea, corridor, sol); @@ -511,7 +403,7 @@ bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea, bg::union_(partialArea, corridor, sol); } else { std::stringstream ss; - auto printPoint = [&ss](const BoostPoint &p) { + auto printPoint = [&ss](const FPoint &p) { ss << " (" << p.get<0>() << ", " << p.get<1>() << ")"; }; ss << "Areas are not overlapping." << std::endl; @@ -540,7 +432,7 @@ bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea, jArea = sol[0]; } else { std::stringstream ss; - auto printPoint = [&ss](const BoostPoint &p) { + auto printPoint = [&ss](const FPoint &p) { ss << " (" << p.get<0>() << ", " << p.get<1>() << ")"; }; ss << "Areas not joinable." << std::endl; @@ -560,8 +452,7 @@ bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea, return true; } -bool joinedArea(const std::vector &areas, - BoostPolygon &joinedArea) { +bool joinedArea(const std::vector &areas, FPolygon &joinedArea) { if (areas.size() < 1) return false; joinedArea = *areas[0]; @@ -569,7 +460,7 @@ bool joinedArea(const std::vector &areas, for (size_t i = 1; i < areas.size(); ++i) idxList.push_back(i); - std::deque sol; + std::deque sol; while (idxList.size() > 0) { bool success = false; for (auto it = idxList.begin(); it != idxList.end(); ++it) { @@ -599,17 +490,17 @@ void BoundingBox::clear() { } bool transectsFromScenario(Length distance, Length minLength, Angle angle, - const BoostPolygon &mArea, - const std::vector &tiles, + const FPolygon &mArea, + const std::vector &tiles, const Progress &p, Transects &t, string &errorString) { // Rotate measurement area by angle and calculate bounding box. - BoostPolygon mAreaRotated; + FPolygon mAreaRotated; trans::rotate_transformer rotate(angle.value() * 180 / M_PI); bg::transform(mArea, mAreaRotated, rotate); - BoostBox box; + FBox box; boost::geometry::envelope(mAreaRotated, box); double x0 = box.min_corner().get<0>(); double y0 = box.min_corner().get<1>(); @@ -622,13 +513,13 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle, transectsClipper.reserve(num_t); for (size_t i = 0; i < num_t; ++i) { // calculate transect - BoostPoint v1{x0, y0 + i * distance.value()}; - BoostPoint v2{x1, y0 + i * distance.value()}; - BoostLineString transect; + FPoint v1{x0, y0 + i * distance.value()}; + FPoint v2{x1, y0 + i * distance.value()}; + FLineString transect; transect.push_back(v1); transect.push_back(v2); // transform back - BoostLineString temp_transect; + FLineString temp_transect; trans::rotate_transformer rotate_back( -angle.value() * 180 / M_PI); bg::transform(transect, temp_transect, rotate_back); @@ -677,7 +568,7 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle, // Calculate processed tiles (_progress[i] == 100) and subtract them from // measurement area. size_t numTiles = p.size(); - vector processedTiles; + vector processedTiles; for (size_t i = 0; i < numTiles; ++i) { if (p[i] == 100) { processedTiles.push_back(tiles[i]); @@ -714,12 +605,12 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle, // Extract transects from PolyTree and convert them to BoostLineString for (const auto &child : transects->Childs) { const auto &clipperTransect = child->Contour; - BoostPoint v1{static_cast(clipperTransect[0].X) / CLIPPER_SCALE, - static_cast(clipperTransect[0].Y) / CLIPPER_SCALE}; - BoostPoint v2{static_cast(clipperTransect[1].X) / CLIPPER_SCALE, - static_cast(clipperTransect[1].Y) / CLIPPER_SCALE}; + FPoint v1{static_cast(clipperTransect[0].X) / CLIPPER_SCALE, + static_cast(clipperTransect[0].Y) / CLIPPER_SCALE}; + FPoint v2{static_cast(clipperTransect[1].X) / CLIPPER_SCALE, + static_cast(clipperTransect[1].Y) / CLIPPER_SCALE}; - BoostLineString transect{v1, v2}; + FLineString transect{v1, v2}; if (bg::length(transect) >= minLength.value()) { t.push_back(transect); } @@ -735,64 +626,331 @@ bool transectsFromScenario(Length distance, Length minLength, Angle angle, return true; } -struct RoutingDataModel { - Matrix distanceMatrix; - long numVehicles; - RoutingIndexManager::NodeIndex depot; -}; - -bool generateRoutingModel(const BoostLineString &vertices, - const BoostPolygon &polygonOffset, size_t n0, - RoutingDataModel &dataModel, Matrix &graph) { +bool route(const FPolygon &area, const Transects &transects, + std::vector &transectInfo, Route &r, + std::function stop, string &errorString) { #ifdef SNAKE_SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); #endif - graphFromPolygon(polygonOffset, vertices, graph); + //================================================================ + // Create routing model. + //================================================================ + // Use integer polygons to increase numerical robustness. + // Convert area; + IPolygon intArea; + for (const auto &v : area.outer()) { + auto p = float2Int(v); + intArea.outer().push_back(p); + } + for (const auto &ring : area.inners()) { + IRing intRing; + for (const auto &v : ring) { + auto p = float2Int(v); + intRing.push_back(p); + } + intArea.inners().push_back(std::move(intRing)); + } + + // Helper classes. + struct VirtualNode { + VirtualNode(std::size_t f, std::size_t t) : fromIndex(f), toIndex(t) {} + std::size_t fromIndex; // index for leaving node + std::size_t toIndex; // index for entering node + }; + struct NodeToTransect { + NodeToTransect(std::size_t i, bool r) : transectsIndex(i), reversed(r) {} + std::size_t transectsIndex; // transects index + bool reversed; // transect reversed? + }; + // Create vertex and node list + std::vector vertices; + std::vector> disjointNodes; + std::vector nodeList; + std::vector nodeToTransectList; + for (std::size_t i = 0; i < transects.size(); ++i) { + const auto &t = transects[i]; + // Copy line edges only. + if (t.size() == 1 || i == 0) { + auto p = float2Int(t.back()); + vertices.push_back(p); + nodeToTransectList.emplace_back(i, false); + auto idx = vertices.size() - 1; + nodeList.emplace_back(idx, idx); + } else if (t.size() > 1) { + auto p1 = float2Int(t.front()); + auto p2 = float2Int(t.back()); + vertices.push_back(p1); + vertices.push_back(p2); + nodeToTransectList.emplace_back(i, false); + nodeToTransectList.emplace_back(i, true); + auto fromIdx = vertices.size() - 1; + auto toIdx = fromIdx - 1; + nodeList.emplace_back(fromIdx, toIdx); + nodeList.emplace_back(toIdx, fromIdx); + disjointNodes.emplace_back(toIdx, fromIdx); + } else { // transect empty + std::cout << "ignoring empty transect with index " << i << std::endl; + } + } +#ifdef SNAKE_DEBUG + // Print. + std::cout << "nodeToTransectList:" << std::endl; + std::cout << "node:transectIndex:reversed" << std::endl; + std::size_t c = 0; + for (const auto &n2t : nodeToTransectList) { + std::cout << c++ << ":" << n2t.transectsIndex << ":" << n2t.reversed + << std::endl; + } + std::cout << "nodeList:" << std::endl; + std::cout << "node:fromIndex:toIndex" << std::endl; + c = 0; + for (const auto &n : nodeList) { + std::cout << c++ << ":" << n.fromIndex << ":" << n.toIndex << std::endl; + } + std::cout << "disjoint nodes:" << std::endl; + std::cout << "number:nodes" << std::endl; + c = 0; + for (const auto &d : disjointNodes) { + std::cout << c++ << ":" << d.first << "," << d.second << std::endl; + } +#endif + + // Add polygon vertices. + for (auto &v : intArea.outer()) { + vertices.push_back(v); + } + for (auto &ring : intArea.inners()) { + for (auto &v : ring) { + vertices.push_back(v); + } + } + + // Create connection graph (inf == no connection between vertices). + // Note: graph is not symmetric. + auto n = vertices.size(); + // Matrix must be double since integers don't have infinity and nan + Matrix connectionGraph(n, n); + for (std::size_t i = 0; i < n; ++i) { + auto &fromVertex = vertices[i]; + for (std::size_t j = 0; j < n; ++j) { + auto &toVertex = vertices[j]; + ILineString line{fromVertex, toVertex}; + if (bg::covered_by(line, intArea)) { + connectionGraph(i, j) = bg::length(line); + } else { + connectionGraph(i, j) = std::numeric_limits::infinity(); + } + } + } +#ifdef SNAKE_DEBUG + std::cout << "connection grah:" << std::endl; + std::cout << connectionGraph << std::endl; +#endif + + // Create distance matrix. + auto distLambda = [&connectionGraph](std::size_t i, std::size_t j) -> double { + return connectionGraph(i, j); + }; + auto nNodes = nodeList.size(); + Matrix distanceMatrix(nNodes, nNodes); + for (std::size_t i = 0; i < nNodes; ++i) { + distanceMatrix(i, i) = 0; + for (std::size_t j = i + 1; j < nNodes; ++j) { + auto dist = connectionGraph(i, j); + if (std::isinf(dist)) { + std::vector route; + if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) { + errorString = "Distance matrix calculation failed."; + return false; + } + (void)route; + } + distanceMatrix(i, j) = dist; + distanceMatrix(j, i) = dist; + } + } +#ifdef SNAKE_DEBUG + std::cout << "distance matrix:" << std::endl; + std::cout << distanceMatrix << std::endl; +#endif + + // Create (asymmetric) routing matrix. + Matrix routingMatrix(nNodes, nNodes); + for (std::size_t i = 0; i < nNodes; ++i) { + auto fromNode = nodeList[i]; + for (std::size_t j = 0; j < nNodes; ++j) { + auto toNode = nodeList[j]; + routingMatrix(i, j) = distanceMatrix(fromNode.fromIndex, toNode.toIndex); + } + } + // Insert max for disjoint nodes. + for (const auto &d : disjointNodes) { + auto i = d.first; + auto j = d.second; + routingMatrix(i, j) = std::numeric_limits::max(); + routingMatrix(j, i) = std::numeric_limits::max(); + } +#ifdef SNAKE_DEBUG + std::cout << "routing matrix:" << std::endl; + std::cout << routingMatrix << std::endl; +#endif + + // Create Routing Index Manager. + long numVehicles = 1; + RoutingIndexManager::NodeIndex depot(0); + RoutingIndexManager manager(nNodes, numVehicles, depot); + // Create Routing Model. + RoutingModel routing(manager); + // Create and register a transit callback. + const int transitCallbackIndex = routing.RegisterTransitCallback( + [&routingMatrix, &manager](int64 from_index, int64 to_index) -> int64 { + // Convert from routing variable Index to distance matrix NodeIndex. + auto from_node = manager.IndexToNode(from_index).value(); + auto to_node = manager.IndexToNode(to_index).value(); + return routingMatrix(from_node, to_node); + }); + // Define cost of each arc. + routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex); + + // Define disjunctions. +#ifdef SNAKE_DEBUG + std::cout << "disjunctions:" << std::endl; +#endif + for (const auto &d : disjointNodes) { + auto i = d.first; + auto j = d.second; +#ifdef SNAKE_DEBUG + std::cout << i << "," << j << std::endl; +#endif + auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i)); + auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(j)); + std::vector disj{idx0, idx1}; + routing.AddDisjunction(disj, -1 /*force cardinality*/, 1 /*cardinality*/); + } + + // Set first solution heuristic. + auto searchParameters = DefaultRoutingSearchParameters(); + searchParameters.set_first_solution_strategy( + FirstSolutionStrategy::PATH_CHEAPEST_ARC); + // Set costume limit. + auto *solver = routing.solver(); + auto *limit = solver->MakeCustomLimit(stop); + routing.AddSearchMonitor(limit); #ifdef SNAKE_SHOW_TIME auto delta = std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start); - cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" - << endl; + cout << "create routing model: " << delta.count() << " ms" << endl; #endif - Matrix distanceMatrix(graph); + + //================================================================ + // Solve model. + //================================================================ #ifdef SNAKE_SHOW_TIME start = std::chrono::high_resolution_clock::now(); #endif - if (!toDistanceMatrix(distanceMatrix)) { + const Assignment *solution = routing.SolveWithParameters(searchParameters); +#ifdef SNAKE_SHOW_TIME + delta = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start); + cout << "solve routing model: " << delta.count() << " ms" << endl; +#endif + + if (!solution || solution->Size() <= 1) { + errorString = "Not able to solve the routing problem."; + return false; + } else if (stop()) { + errorString = "User terminated."; + return false; + } + + //================================================================ + // Construc route. + //================================================================ +#ifdef SNAKE_SHOW_TIME + start = std::chrono::high_resolution_clock::now(); +#endif + // Create index list. + auto index = routing.Start(0); + std::vector route_idx; + route_idx.push_back(manager.IndexToNode(index).value()); + while (!routing.IsEnd(index)) { + index = solution->Value(routing.NextVar(index)); + route_idx.push_back(manager.IndexToNode(index).value()); + } + +#ifdef SNAKE_DEBUG + // Print route. + std::cout << "route_idx.size() = " << route_idx.size() << std::endl; + std::cout << "route: "; + for (const auto &idx : route_idx) { + std::cout << idx << ", "; + } + std::cout << std::endl; +#endif + + if (route_idx.size() < 2) { + errorString = "Error while assembling route."; return false; } + + // Construct route. + for (size_t i = 0; i < route_idx.size() - 1; ++i) { + size_t nodeIndex0 = route_idx[i]; + size_t nodeIndex1 = route_idx[i + 1]; + const auto &n2t0 = nodeToTransectList[nodeIndex0]; + transectInfo.emplace_back(n2t0.transectsIndex, n2t0.reversed); + // Copy transect to route. + const auto &t = transects[n2t0.transectsIndex]; + if (n2t0.reversed) { // transect reversal needed? + for (auto it = t.end() - 1; it > t.begin(); --it) { + r.push_back(*it); + } + } else { + for (auto it = t.begin(); it < t.end() - 1; ++it) { + r.push_back(*it); + } + } + // Connect transects. + std::vector idxList; + if (!shortestPathFromGraph(connectionGraph, nodeList[nodeIndex0].fromIndex, + nodeList[nodeIndex1].toIndex, idxList)) { + errorString = "Error while assembling route."; + return false; + } + if (i != route_idx.size() - 2) { + idxList.pop_back(); + } + for (auto idx : idxList) { + auto p = int2Float(vertices[idx]); + r.push_back(p); + } + } + // Append last transect info. + const auto &n2t0 = nodeToTransectList.back(); + transectInfo.emplace_back(n2t0.transectsIndex, n2t0.reversed); #ifdef SNAKE_SHOW_TIME delta = std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start); - cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" - << endl; + cout << "reconstruct route: " << delta.count() << " ms" << endl; #endif - dataModel.distanceMatrix.setDimension(n0, n0); - for (size_t i = 0; i < n0; ++i) { - dataModel.distanceMatrix.set(i, i, 0); - for (size_t j = i + 1; j < n0; ++j) { - dataModel.distanceMatrix.set( - i, j, int64_t(distanceMatrix.get(i, j) * CLIPPER_SCALE)); - dataModel.distanceMatrix.set( - j, i, int64_t(distanceMatrix.get(i, j) * CLIPPER_SCALE)); - } + if (r.size() < 2) { + errorString = "Route empty."; + return false; } - dataModel.numVehicles = 1; - dataModel.depot = 0; return true; } -bool route(const BoostPolygon &area, const Transects &transects, - std::vector &transectInfo, Route &r, - std::function stop, string &errorString) { +bool route_old(const FPolygon &area, const Transects &transects, + std::vector &transectInfo, Route &r, + std::function stop, string &errorString) { //======================================= // Route Transects using Google or-tools. //======================================= // Create vertex list; - BoostLineString vertices; + FLineString vertices; size_t n0 = 0; for (const auto &t : transects) { n0 += std::min(t.size(), 2); @@ -823,52 +981,59 @@ bool route(const BoostPolygon &area, const Transects &transects, vertices.push_back(vertex); } size_t n1 = vertices.size(); - if (stop()) { - errorString = "User termination."; - return false; - } // Generate routing model. - RoutingDataModel dataModel; Matrix connectionGraph(n1, n1); - if (stop()) { - errorString = "User termination."; - return false; - } // Offset joined area. - BoostPolygon areaOffset; + FPolygon areaOffset; offsetPolygon(area, areaOffset, detail::offsetConstant); + // Generate routing model. #ifdef SNAKE_SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); #endif - if (!generateRoutingModel(vertices, areaOffset, n0, dataModel, - connectionGraph)) { - errorString = "Routing model generation failed."; - return false; - } + graphFromPolygon(areaOffset, vertices, connectionGraph); #ifdef SNAKE_SHOW_TIME auto delta = std::chrono::duration_cast( std::chrono::high_resolution_clock::now() - start); - cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" + cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl; #endif - if (stop()) { - errorString = "User termination."; + Matrix distanceMatrix(connectionGraph); +#ifdef SNAKE_SHOW_TIME + start = std::chrono::high_resolution_clock::now(); +#endif + if (!toDistanceMatrix(distanceMatrix)) { + errorString = "Error while generating distance matrix."; return false; } +#ifdef SNAKE_SHOW_TIME + delta = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start); + cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" + << endl; +#endif + Matrix dm(n0, n0); + for (size_t i = 0; i < n0; ++i) { + dm(i, i) = 0; + for (size_t j = i + 1; j < n0; ++j) { + dm(i, j) = int64_t(distanceMatrix(i, j) * CLIPPER_SCALE); + dm(j, i) = int64_t(distanceMatrix(i, j) * CLIPPER_SCALE); + } + } // Create Routing Index Manager. - RoutingIndexManager manager(dataModel.distanceMatrix.getN(), - dataModel.numVehicles, dataModel.depot); + RoutingIndexManager manager( + dm.n() /*num indices*/, 1 /*vehicles*/, + RoutingIndexManager::NodeIndex(0) /*depot index*/); // Create Routing Model. RoutingModel routing(manager); // Create and register a transit callback. const int transitCallbackIndex = routing.RegisterTransitCallback( - [&dataModel, &manager](int64 from_index, int64 to_index) -> int64 { + [&dm, &manager](int64 from_index, int64 to_index) -> int64 { // Convert from routing variable Index to distance matrix NodeIndex. auto from_node = manager.IndexToNode(from_index).value(); auto to_node = manager.IndexToNode(to_index).value(); - return dataModel.distanceMatrix.get(from_node, to_node); + return dm(from_node, to_node); }); // Define cost of each arc. @@ -941,7 +1106,7 @@ bool route(const BoostPolygon &area, const Transects &transects, // Helper Lambda. auto idx2Vertex = [&vertices](const std::vector &idxArray, - std::vector &path) { + std::vector &path) { for (auto idx : idxArray) path.push_back(vertices[idx]); }; @@ -957,7 +1122,7 @@ bool route(const BoostPolygon &area, const Transects &transects, !info1.front ? true : false /*transect reversed?*/); transectInfo.push_back(trInfo); if (!info1.front) { // transect reversal needed? - BoostLineString reversedTransect; + FLineString reversedTransect; const auto &t = transects[info1.index]; for (auto it = t.end() - 1; it >= t.begin(); --it) { reversedTransect.push_back(*it); @@ -984,21 +1149,153 @@ bool route(const BoostPolygon &area, const Transects &transects, return true; } -bool route(const BoostPolygon &area, const Transects &transects, +bool route_old(const FPolygon &area, const Transects &transects, + std::vector &transectInfo, Route &r, + string &errorString) { + auto stop = [] { return false; }; + return route_old(area, transects, transectInfo, r, stop, errorString); +} + +bool route(const FPolygon &area, const Transects &transects, std::vector &transectInfo, Route &r, string &errorString) { auto stop = [] { return false; }; return route(area, transects, transectInfo, r, stop, errorString); } +FPoint int2Float(const IPoint &ip) { return int2Float(ip, stdScale); } + +FPoint int2Float(const IPoint &ip, IntType scale) { + return FPoint{FloatType(ip.get<0>()) / scale, FloatType(ip.get<1>()) / scale}; +} + +IPoint float2Int(const FPoint &ip) { return float2Int(ip, stdScale); } + +IPoint float2Int(const FPoint &ip, IntType scale) { + return IPoint{IntType(std::llround(ip.get<0>() * scale)), + IntType(std::llround(ip.get<1>() * scale))}; +} + +bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex, + std::vector &elementPath, double &length, + std::function distanceDij) { + if (startIndex >= numElements || endIndex >= numElements) { + length = std::numeric_limits::infinity(); + return false; + } else if (endIndex == startIndex) { + length = 0; + elementPath.push_back(startIndex); + return true; + } + + // Node struct + // predecessorIndex is the index of the predecessor node + // (nodeList[predecessorIndex]) distance is the distance between the node and + // the start node node number is stored by the position in nodeList + struct Node { + std::size_t predecessorIndex = std::numeric_limits::max(); + double distance = std::numeric_limits::infinity(); + }; + + // The list with all Nodes (elements) + std::vector nodeList(numElements); + // This list will be initalized with indices referring to the elements of + // nodeList. Elements will be successively remove during the execution of the + // Dijkstra Algorithm. + std::vector workingSet(numElements); + + // append elements to node list + for (size_t i = 0; i < numElements; ++i) + workingSet[i] = i; + + nodeList[startIndex].distance = 0; + + // Dijkstra Algorithm + // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus + while (workingSet.size() > 0) { + // serach Node with minimal distance + auto minDist = std::numeric_limits::infinity(); + std::size_t minDistIndex_WS = + std::numeric_limits::max(); // WS = workinSet + for (size_t i = 0; i < workingSet.size(); ++i) { + const auto nodeIndex = workingSet.at(i); + const auto dist = nodeList.at(nodeIndex).distance; + if (dist < minDist) { + minDist = dist; + minDistIndex_WS = i; + } + } + if (minDistIndex_WS == std::numeric_limits::max()) + return false; + + size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList + workingSet.erase(workingSet.begin() + minDistIndex_WS); + if (indexU_NL == endIndex) // shortest path found + break; + + const auto distanceU = nodeList.at(indexU_NL).distance; + // update distance + for (size_t i = 0; i < workingSet.size(); ++i) { + auto indexV_NL = workingSet[i]; // NL = nodeList + Node *v = &nodeList[indexV_NL]; + auto dist = distanceDij(indexU_NL, indexV_NL); + // is ther an alternative path which is shorter? + auto alternative = distanceU + dist; + if (alternative < v->distance) { + v->distance = alternative; + v->predecessorIndex = indexU_NL; + } + } + } + // end Djikstra Algorithm + + // reverse assemble path + auto e = endIndex; + length = nodeList[e].distance; + while (true) { + if (e == std::numeric_limits::max()) { + if (elementPath.size() > 0 && + elementPath[0] == startIndex) { // check if starting point was reached + break; + } else { // some error + length = std::numeric_limits::infinity(); + elementPath.clear(); + return false; + } + } else { + elementPath.insert(elementPath.begin(), e); + // Update Node + e = nodeList[e].predecessorIndex; + } + } + return true; +} + +bool shortestPathFromGraph(const Matrix &graph, const size_t startIndex, + const size_t endIndex, + std::vector &pathIdx) { + if (!std::isinf(graph(startIndex, endIndex))) { + pathIdx.push_back(startIndex); + pathIdx.push_back(endIndex); + } else { + auto distance = [&graph](size_t i, size_t j) -> double { + return graph(i, j); + }; + double d = 0; + if (!dijkstraAlgorithm(graph.n(), startIndex, endIndex, pathIdx, d, + distance)) { + return false; + } + } + return true; +} + } // namespace snake -bool boost::geometry::model::operator==(snake::BoostPoint p1, - snake::BoostPoint p2) { +bool boost::geometry::model::operator==(snake::FPoint p1, snake::FPoint p2) { return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>()); } -bool boost::geometry::model::operator!=(snake::BoostPoint p1, - snake::BoostPoint p2) { +bool boost::geometry::model::operator!=(snake::FPoint p1, snake::FPoint p2) { return !(p1 == p2); } diff --git a/src/Wima/Snake/snake.h b/src/Wima/Snake/snake.h index c21c6ee4b3648c5369ffe633d6a1148fda427e5d..31ffc3e24bce685fcf5638565863394f845596b5 100644 --- a/src/Wima/Snake/snake.h +++ b/src/Wima/Snake/snake.h @@ -27,59 +27,68 @@ namespace snake { // Geometry stuff. //========================================================================= -typedef bg::model::point BoostPoint; -// typedef std::vector BoostPointList; -typedef bg::model::polygon BoostPolygon; -typedef bg::model::linestring BoostLineString; -typedef std::vector> Int64Matrix; -typedef bg::model::box BoostBox; - -template class Matrix { +// Double geometry. +using FloatType = double; +typedef bg::model::point FPoint; +typedef bg::model::polygon FPolygon; +typedef bg::model::linestring FLineString; +typedef bg::model::box FBox; + +// Integer geometry. +using IntType = long long; +using IPoint = bg::model::point; +using IPolygon = bg::model::polygon; +using IRing = bg::model::ring; +using ILineString = bg::model::linestring; + +FPoint int2Float(const IPoint &ip); +FPoint int2Float(const IPoint &ip, IntType scale); +IPoint float2Int(const FPoint &ip); +IPoint float2Int(const FPoint &ip, IntType scale); + +template class Matrix; + +template +ostream &operator<<(ostream &os, const Matrix &matrix) { + for (std::size_t i = 0; i < matrix.m(); ++i) { + for (std::size_t j = 0; j < matrix.n(); ++j) { + os << "(" << i << "," << j << "):" << matrix(i, j) << std::endl; + } + } + return os; +} + +// Matrix +template class Matrix { public: - Matrix() : _elements(0), _m(0), _n(0), _isInitialized(false) {} - Matrix(size_t m, size_t n) : Matrix(m, n, T{0}) {} - Matrix(size_t m, size_t n, T value) - : _elements(m * n), _m(m), _n(n), _isInitialized(true) { - assert((m > 0) || (n > 0)); - _matrix.resize(_elements, value); + using value_type = DataType; + Matrix(std::size_t m, std::size_t n) : _m(m), _n(n) { _matrix.resize(m * n); } + Matrix(std::size_t m, std::size_t n, DataType value) : _m(m), _n(n) { + _matrix.resize(m * n, value); } - double get(size_t i, size_t j) const { - assert(_isInitialized); + DataType &operator()(std::size_t i, std::size_t j) { assert(i < _m); assert(j < _n); return _matrix[i * _m + j]; } - size_t getM() const { return _m; } - size_t getN() const { return _n; } - - void set(size_t i, size_t j, const T &value) { - assert(_isInitialized); + const DataType &operator()(std::size_t i, std::size_t j) const { assert(i < _m); assert(j < _n); - _matrix[i * _m + j] = value; + return _matrix[i * _m + j]; } - void setDimension(size_t m, size_t n, const T &value) { - assert((m > 0) || (n > 0)); - assert(!_isInitialized); - _m = m; - _n = n; - _elements = n * m; - _matrix.resize(_elements, value); - _isInitialized = true; - } + std::size_t m() const { return _n; } + std::size_t n() const { return _n; } - void setDimension(size_t m, size_t n) { setDimension(m, n, T{0}); } + friend ostream &operator<<<>(ostream &os, const Matrix &dt); private: - size_t _elements; - size_t _m; - size_t _n; - bool _isInitialized; - std::vector _matrix; -}; + std::vector _matrix; + const std::size_t _m; + const std::size_t _n; +}; // Matrix struct BoundingBox { BoundingBox(); @@ -89,11 +98,11 @@ struct BoundingBox { double width; double height; double angle; - BoostPolygon corners; + FPolygon corners; }; template -void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, BoostPoint &out) { +void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, FPoint &out) { static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), @@ -125,7 +134,7 @@ void toENU(const GeoPoint1 &origin, const GeoPoint2 &in, Point &out) { } template -void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) { +void fromENU(const GeoPoint &origin, const FPoint &in, GeoPoint &out) { static GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), @@ -148,9 +157,9 @@ void areaToEnu(const GeoPoint &origin, const Container1 &in, Container2 &out) { } template -void areaToEnu(const GeoPoint &origin, const Container &in, BoostPolygon &out) { +void areaToEnu(const GeoPoint &origin, const Container &in, FPolygon &out) { for (auto &vertex : in) { - BoostPoint p; + FPoint p; toENU(origin, vertex, p); out.outer().push_back(p); } @@ -168,8 +177,7 @@ void areaFromEnu(const GeoPoint &origin, Container1 &in, } template -void areaFromEnu(const GeoPoint &origin, BoostPolygon &in, - const Container &out) { +void areaFromEnu(const GeoPoint &origin, FPolygon &in, const Container &out) { for (auto &vertex : in.outer()) { typename Container::value_type p; fromENU(origin, vertex, p); @@ -177,19 +185,19 @@ void areaFromEnu(const GeoPoint &origin, BoostPolygon &in, } } -void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er); -bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox); -void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, +void polygonCenter(const FPolygon &polygon, FPoint ¢er); +bool minimalBoundingBox(const FPolygon &polygon, BoundingBox &minBBox); +void offsetPolygon(const FPolygon &polygon, FPolygon &polygonOffset, double offset); -void graphFromPolygon(const BoostPolygon &polygon, - const BoostLineString &vertices, Matrix &graph); +void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices, + Matrix &graph); bool toDistanceMatrix(Matrix &graph); -bool dijkstraAlgorithm( - const size_t numElements, size_t startIndex, size_t endIndex, - std::vector &elementPath, - std::function distanceDij); -void shortestPathFromGraph(const Matrix &graph, size_t startIndex, - size_t endIndex, std::vector &pathIdx); +bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex, + std::vector &elementPath, double &length, + std::function distanceDij); + +bool shortestPathFromGraph(const Matrix &graph, const size_t startIndex, + const size_t endIndex, std::vector &pathIdx); typedef bu::quantity Length; typedef bu::quantity Area; @@ -197,21 +205,21 @@ typedef bu::quantity Angle; typedef bu::quantity Radian; typedef bu::quantity Degree; -bool joinedArea(const std::vector &areas, BoostPolygon &jArea); -bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea, - const BoostPolygon &corridor, BoostPolygon &jArea, +bool joinedArea(const std::vector &areas, FPolygon &jArea); +bool joinedArea(const FPolygon &mArea, const FPolygon &sArea, + const FPolygon &corridor, FPolygon &jArea, std::string &errorString); -bool tiles(const BoostPolygon &area, Length tileHeight, Length tileWidth, - Area minTileArea, std::vector &tiles, - BoundingBox &bbox, std::string &errorString); +bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth, + Area minTileArea, std::vector &tiles, BoundingBox &bbox, + std::string &errorString); -using Transects = vector; +using Transects = vector; using Progress = vector; -using Route = BoostLineString; +using Route = FLineString; bool transectsFromScenario(Length distance, Length minLength, Angle angle, - const BoostPolygon &mArea, - const std::vector &tiles, + const FPolygon &mArea, + const std::vector &tiles, const Progress &p, Transects &t, string &errorString); @@ -220,13 +228,20 @@ struct TransectInfo { size_t index; bool reversed; }; -bool route(const BoostPolygon &area, const Transects &transects, +bool route(const FPolygon &area, const Transects &transects, std::vector &transectInfo, Route &r, string &errorString); -bool route(const BoostPolygon &area, const Transects &transects, +bool route(const FPolygon &area, const Transects &transects, std::vector &transectInfo, Route &r, std::function stop, string &errorString); +bool route_old(const FPolygon &area, const Transects &transects, + std::vector &transectInfo, Route &r, + string &errorString); +bool route_old(const FPolygon &area, const Transects &transects, + std::vector &transectInfo, Route &r, + std::function stop, string &errorString); + namespace detail { const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. @@ -238,8 +253,8 @@ namespace boost { namespace geometry { namespace model { -bool operator==(snake::BoostPoint p1, snake::BoostPoint p2); -bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2); +bool operator==(snake::FPoint p1, snake::FPoint p2); +bool operator!=(snake::FPoint p1, snake::FPoint p2); } // namespace model } // namespace geometry diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index d1a8c35b208d332ae9687affecf944d0dfc0dfbb..d5290caa5ba21c7d4c78de5842c30793c650087b 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -18,7 +18,7 @@ #include "QVector3D" #include -#define CLIPPER_SCALE 10000 +#define CLIPPER_SCALE 1000000 #include "clipper/clipper.hpp" #include @@ -906,7 +906,7 @@ void WimaController::_updateRoute() { this->_origin = this->_joinedArea.coordinateList().first(); this->_origin.setAltitude(0); const auto &origin = this->_origin; - auto pTiles = std::make_shared>(); + auto pTiles = std::make_shared>(); std::size_t numTiles = 0; for (int i = 0; i < progress.size(); ++i) { if (progress[i] == 100) { @@ -914,7 +914,7 @@ void WimaController::_updateRoute() { const auto *obj = tiles->get(i); const auto *tile = qobject_cast(obj); if (tile != nullptr) { - snake::BoostPolygon t; + snake::FPolygon t; snake::areaToEnu(origin, tile->coordinateList(), t); pTiles->push_back(std::move(t)); } else { @@ -928,18 +928,18 @@ void WimaController::_updateRoute() { for (auto &v : safeArea) { v.setAltitude(0); } - snake::BoostPolygon safeAreaENU; + snake::FPolygon safeAreaENU; snake::areaToEnu(origin, safeArea, safeAreaENU); const auto &depot = this->_serviceArea.depot(); - snake::BoostPoint depotENU; + snake::FPoint depotENU; snake::toENU(origin, depot, depotENU); // Fetch geo transects and convert to ENU. const auto &geoTransects = this->_rawTransects; auto pUnclippedTransects = std::make_shared(); for (auto &geoTransect : geoTransects) { - snake::BoostLineString t; + snake::FLineString t; for (auto &geoVertex : geoTransect) { - snake::BoostPoint v; + snake::FPoint v; snake::toENU(origin, geoVertex, v); t.push_back(v); } @@ -983,11 +983,11 @@ void WimaController::_updateRoute() { ClipperLib::pftNonZero, ClipperLib::pftNonZero); // Extract transects from PolyTree and convert them to // BoostLineString - transects.push_back(snake::BoostLineString{depotENU}); + transects.push_back(snake::FLineString{depotENU}); for (const auto &child : clippedTransecs.Childs) { - snake::BoostLineString transect; + snake::FLineString transect; for (const auto &v : child->Contour) { - snake::BoostPoint c{static_cast(v.X) / CLIPPER_SCALE, + snake::FPoint c{static_cast(v.X) / CLIPPER_SCALE, static_cast(v.Y) / CLIPPER_SCALE}; transect.push_back(c); }