Skip to content

Q
qgroundcontrol
Commit 053f9768 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

snake::route() now a lot faster

parent 05f67e26
Hide whitespace changes
Inline Side-by-side
Showing with 3306 additions and 1025 deletions
Paths/KlingenbachTest.wima
View file @ 053f9768
......@@ -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,
......
Paths/Routing error.wima 0 → 100644
View file @ 053f9768
{
"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
}
}
Paths/Routing error2.wima 0 → 100644
View file @ 053f9768
{
"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
}
}
Paths/test.plan 0 → 100644
View file @ 053f9768
{
"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
}
qgroundcontrol.pro
View file @ 053f9768
......@@ -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
}
......
src/Wima/CircularSurvey.cc
View file @ 053f9768
......@@ -5,7 +5,7 @@
#include "QGCApplication.h"
// Wima
#include "snake.h"
#define CLIPPER_SCALE 10000
#define CLIPPER_SCALE 1000000
#include "clipper/clipper.hpp"
template <int k> 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<CoordInfo_t> 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<CoordInfo_t> 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<std::chrono::milliseconds>(
......@@ -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<double>(vertex.X) / CLIPPER_SCALE;
auto y = static_cast<double>(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<bg::degree, double, 2, 2> 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<bg::degree, double, 2, 2> 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<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
snake::BoostPoint v2{
snake::FPoint v2{
static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(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);
}
......
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ 053f9768
......@@ -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<BoostPolygon> tilesENU;
std::vector<FPolygon> 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);
......
src/Wima/RoutingThread.cpp
View file @ 053f9768
......@@ -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::milliseconds>(
// 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::milliseconds>(
// 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
......
src/Wima/RoutingThread.h
View file @ 053f9768
......@@ -11,7 +11,7 @@
#include <mutex>
struct RoutingData {
snake::BoostLineString route;
snake::FLineString route;
snake::Transects transects;
std::vector<snake::TransectInfo> 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;
......
src/Wima/Snake/snake.cpp
View file @ 053f9768
......@@ -12,7 +12,7 @@
#include <boost/geometry/geometries/polygon.hpp>
#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 &center) {
void polygonCenter(const FPolygon &polygon, FPoint &center) {
using namespace mapbox;
if (polygon.outer().empty())
return;
......@@ -53,7 +54,7 @@ void polygonCenter(const BoostPolygon &polygon, BoostPoint &center) {
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<BoostPolygon2D>(convex_hull) << endl;
//# Compute edges (x2-x1,y2-y1)
std::vector<BoostPoint> edges;
std::vector<FPoint> 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<bg::degree, double, 2, 2> rotate(angle * 180 /
M_PI);
BoostPolygon hull_rotated;
FPolygon hull_rotated;
bg::transform(convex_hull, hull_rotated, rotate);
// cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated)
// << endl;
bg::model::box<BoostPoint> box;
bg::model::box<FPoint> box;
bg::envelope(hull_rotated, box);
// cout << "Bounding box: " <<
// bg::wkt<bg::model::box<BoostPoint2D>>(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<bg::degree, double, 2, 2> 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<double> 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<BoostPolygon> result;
bg::model::multi_polygon<FPolygon> 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<double> &graph) {
size_t n = graph.getN();
void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices,
Matrix<double> &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<double>::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<size_t> &elementPath,
std::function<double(const size_t, const size_t)> 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<double>::infinity();
};
// The list with all Nodes (elements)
std::vector<Node> 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<size_t> 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<double>::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<double> &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<size_t> 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<size_t> path;
if (!dijkstraAlgorithm(n, i, j, path, d, distance)) {
return false;
}
// cout << "(" << i << "," << j << ") d: " << d << endl;
......@@ -346,41 +263,16 @@ bool toDistanceMatrix(Matrix<double> &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<double> &graph, size_t startIndex,
size_t endIndex, std::vector<size_t> &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<BoostPolygon> &tiles,
BoundingBox &bbox, string &errorString) {
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &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<double, 2, 2> 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<BoostPolygon> boost_tiles;
std::deque<FPolygon> 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<BoostPolygon> sol;
BoostPolygon partialArea = mArea;
std::deque<FPolygon> 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<BoostPolygon *> &areas,
BoostPolygon &joinedArea) {
bool joinedArea(const std::vector<FPolygon *> &areas, FPolygon &joinedArea) {
if (areas.size() < 1)
return false;
joinedArea = *areas[0];
......@@ -569,7 +460,7 @@ bool joinedArea(const std::vector<BoostPolygon *> &areas,
for (size_t i = 1; i < areas.size(); ++i)
idxList.push_back(i);
std::deque<BoostPolygon> sol;
std::deque<FPolygon> 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<BoostPolygon> &tiles,
const FPolygon &mArea,
const std::vector<FPolygon> &tiles,
const Progress &p, Transects &t,
string &errorString) {
// Rotate measurement area by angle and calculate bounding box.
BoostPolygon mAreaRotated;
FPolygon mAreaRotated;
trans::rotate_transformer<bg::degree, double, 2, 2> 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<bg::degree, double, 2, 2> 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<BoostPolygon> processedTiles;
vector<FPolygon> 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<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
BoostPoint v2{static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};
FPoint v1{static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
FPoint v2{static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
static_cast<double>(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<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
};
bool generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset, size_t n0,
RoutingDataModel &dataModel, Matrix<double> &graph) {
bool route(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
std::function<bool()> 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<IPoint> vertices;
std::vector<std::pair<std::size_t, std::size_t>> disjointNodes;
std::vector<VirtualNode> nodeList;
std::vector<NodeToTransect> 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<double> 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<double>::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<IntType> 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<std::size_t> 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<IntType> 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<IntType>::max();
routingMatrix(j, i) = std::numeric_limits<IntType>::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<int64> 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::milliseconds>(
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<double> 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::milliseconds>(
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<size_t> 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<size_t> 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::milliseconds>(
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> &transectInfo, Route &r,
std::function<bool()> stop, string &errorString) {
bool route_old(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
std::function<bool()> 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<std::size_t>(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<double> 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::milliseconds>(
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<double> 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::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms"
<< endl;
#endif
Matrix<int64_t> 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<size_t> &idxArray,
std::vector<BoostPoint> &path) {
std::vector<FPoint> &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> &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> &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<size_t> &elementPath, double &length,
std::function<double(size_t, size_t)> distanceDij) {
if (startIndex >= numElements || endIndex >= numElements) {
length = std::numeric_limits<double>::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<std::size_t>::max();
double distance = std::numeric_limits<double>::infinity();
};
// The list with all Nodes (elements)
std::vector<Node> 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<size_t> 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<double>::infinity();
std::size_t minDistIndex_WS =
std::numeric_limits<std::size_t>::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<std::size_t>::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<std::size_t>::max()) {
if (elementPath.size() > 0 &&
elementPath[0] == startIndex) { // check if starting point was reached
break;
} else { // some error
length = std::numeric_limits<double>::infinity();
elementPath.clear();
return false;
}
} else {
elementPath.insert(elementPath.begin(), e);
// Update Node
e = nodeList[e].predecessorIndex;
}
}
return true;
}
bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
const size_t endIndex,
std::vector<size_t> &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);
}
src/Wima/Snake/snake.h
View file @ 053f9768
......@@ -27,59 +27,68 @@ namespace snake {
// Geometry stuff.
//=========================================================================
typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
// typedef std::vector<BoostPoint> BoostPointList;
typedef bg::model::polygon<BoostPoint> BoostPolygon;
typedef bg::model::linestring<BoostPoint> BoostLineString;
typedef std::vector<std::vector<int64_t>> Int64Matrix;
typedef bg::model::box<BoostPoint> BoostBox;
template <class T> class Matrix {
// Double geometry.
using FloatType = double;
typedef bg::model::point<double, 2, bg::cs::cartesian> FPoint;
typedef bg::model::polygon<FPoint> FPolygon;
typedef bg::model::linestring<FPoint> FLineString;
typedef bg::model::box<FPoint> FBox;
// Integer geometry.
using IntType = long long;
using IPoint = bg::model::point<IntType, 2, bg::cs::cartesian>;
using IPolygon = bg::model::polygon<IPoint>;
using IRing = bg::model::ring<IPoint>;
using ILineString = bg::model::linestring<IPoint>;
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 T> class Matrix;
template <class DataType>
ostream &operator<<(ostream &os, const Matrix<DataType> &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 DataType> 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<DataType> &dt);
private:
size_t _elements;
size_t _m;
size_t _n;
bool _isInitialized;
std::vector<T> _matrix;
};
std::vector<DataType> _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 <class GeoPoint1, class GeoPoint2>
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 <class GeoPoint>
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 <class GeoPoint, class Container>
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 <class GeoPoint, class Container>
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 &center);
bool minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox);
void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset,
void polygonCenter(const FPolygon &polygon, FPoint &center);
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<double> &graph);
void graphFromPolygon(const FPolygon &polygon, const FLineString &vertices,
Matrix<double> &graph);
bool toDistanceMatrix(Matrix<double> &graph);
bool dijkstraAlgorithm(
const size_t numElements, size_t startIndex, size_t endIndex,
std::vector<size_t> &elementPath,
std::function<double(const size_t, const size_t)> distanceDij);
void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex,
size_t endIndex, std::vector<size_t> &pathIdx);
bool dijkstraAlgorithm(size_t numElements, size_t startIndex, size_t endIndex,
std::vector<size_t> &elementPath, double &length,
std::function<double(size_t, size_t)> distanceDij);
bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
const size_t endIndex, std::vector<size_t> &pathIdx);
typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
......@@ -197,21 +205,21 @@ typedef bu::quantity<bu::si::plane_angle> Angle;
typedef bu::quantity<bu::si::plane_angle> Radian;
typedef bu::quantity<bu::degree::plane_angle> Degree;
bool joinedArea(const std::vector<BoostPolygon *> &areas, BoostPolygon &jArea);
bool joinedArea(const BoostPolygon &mArea, const BoostPolygon &sArea,
const BoostPolygon &corridor, BoostPolygon &jArea,
bool joinedArea(const std::vector<FPolygon *> &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<BoostPolygon> &tiles,
BoundingBox &bbox, std::string &errorString);
bool tiles(const FPolygon &area, Length tileHeight, Length tileWidth,
Area minTileArea, std::vector<FPolygon> &tiles, BoundingBox &bbox,
std::string &errorString);
using Transects = vector<BoostLineString>;
using Transects = vector<FLineString>;
using Progress = vector<int>;
using Route = BoostLineString;
using Route = FLineString;
bool transectsFromScenario(Length distance, Length minLength, Angle angle,
const BoostPolygon &mArea,
const std::vector<BoostPolygon> &tiles,
const FPolygon &mArea,
const std::vector<FPolygon> &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> &transectInfo, Route &r,
string &errorString);
bool route(const BoostPolygon &area, const Transects &transects,
bool route(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
std::function<bool(void)> stop, string &errorString);
bool route_old(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
string &errorString);
bool route_old(const FPolygon &area, const Transects &transects,
std::vector<TransectInfo> &transectInfo, Route &r,
std::function<bool(void)> 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
......
src/Wima/WimaController.cc
View file @ 053f9768
......@@ -18,7 +18,7 @@
#include "QVector3D"
#include <QScopedPointer>
#define CLIPPER_SCALE 10000
#define CLIPPER_SCALE 1000000
#include "clipper/clipper.hpp"
#include <memory>
......@@ -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<std::vector<snake::BoostPolygon>>();
auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
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<const SnakeTile *>(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<snake::Transects>();
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<double>(v.X) / CLIPPER_SCALE,
snake::FPoint c{static_cast<double>(v.X) / CLIPPER_SCALE,
static_cast<double>(v.Y) / CLIPPER_SCALE};
transect.push_back(c);
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment