Skip to content

Q
qgroundcontrol
Commit 0510d157 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

snake working, gui still stuttering sometimes

parent 50929c20
Hide whitespace changes
Inline Side-by-side
Showing with 3112 additions and 1599 deletions
Paths/KlingenbachGross.wima 0 → 100644
View file @ 0510d157
{
"AreaItems": [
{
"AreaType": "Measurement Area",
"BorderPolygonOffset": 6,
"BottomLayerAltitude": 5,
"LayerDistance": 1,
"NumberOfLayers": 1,
"ShowBorderPolygon": 0,
"maxAltitude": 30,
"polygon": [
[
47.76846606616059,
16.529820081373487
],
[
47.76872241472738,
16.530837352593494
],
[
47.76798694569257,
16.531146687122508
],
[
47.76777418124312,
16.530481637145055
]
]
},
{
"AreaType": "Service Area",
"BorderPolygonOffset": 6,
"ShowBorderPolygon": 0,
"maxAltitude": 30,
"polygon": [
[
47.76777317830783,
16.530413969584316
],
[
47.76785089069791,
16.530667046992818
],
[
47.76775614544286,
16.5307494063876
],
[
47.76766140001518,
16.530447230100037
]
]
}
],
"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.76776040370392,
16.530569413178814,
8
],
"type": "SimpleItem"
},
{
"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.768644821858615,
16.53052943984308,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 3,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76863335716058,
16.53064612872492,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 4,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76862466704312,
16.530763346866273,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 5,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768618762330405,
16.530880948263555,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 6,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768527520688615,
16.530919324033228,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 7,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76853460520799,
16.530761042994456,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 8,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768546537105244,
16.530603407906614,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 9,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76856329151763,
16.530446747237274,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 10,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76858483353362,
16.530291387423606,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 11,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76852994880375,
16.530073587664386,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 12,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76849951312842,
16.53024717765454,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 13,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76847491313001,
16.530422803641923,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 14,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76845620960195,
16.530600031605875,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 15,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768443448765865,
16.530778423566808,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 16,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768436662157335,
16.530957538668577,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 17,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76834614163562,
16.530995611138827,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 18,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76835201161128,
16.53080463643282,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 19,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76836442845143,
16.53061435689921,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 20,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76838335975491,
16.53042526906374,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 21,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76840875612134,
16.530237866342457,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 22,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76844055128007,
16.530052637754178,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 23,
"frame": 3,
"params": [
0,
0,
0,
null,
47.7684786622631,
16.529870066644435,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 24,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768376274234,
16.52990593722282,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 25,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768340721559866,
16.530089431777824,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 26,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76831128621317,
16.530275309020222,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 27,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76828803964311,
16.530463117765336,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 28,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76827103827668,
16.530652402140113,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 29,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768260323381746,
16.53084270268965,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 30,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768255920966844,
16.531033557492464,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 31,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768165967629955,
16.53107139140768,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 32,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76816861051905,
16.53089350356298,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 33,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76817655483528,
16.53071596530955,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 34,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768189784935466,
16.530539126238857,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 35,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76820827476815,
16.530363334565596,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 36,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76823198792491,
16.53018893644203,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 37,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76826087771206,
16.530016275276378,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 38,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76815014481609,
16.530122154136524,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 39,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76812264138942,
16.53031697710711,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 40,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768101459028976,
16.53051354077119,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 41,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768086646632426,
16.530711391378407,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 42,
"frame": 3,
"params": [
0,
0,
0,
null,
47.7680782383929,
16.530910072207597,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 43,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768076253720125,
16.531109124621103,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 44,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76798674683302,
16.531146065535864,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 45,
"frame": 3,
"params": [
0,
0,
0,
null,
47.767987262588385,
16.530960627626218,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 46,
"frame": 3,
"params": [
0,
0,
0,
null,
47.767993180227045,
16.530775397272773,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 47,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768004488625024,
16.53059072267118,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 48,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76802116652483,
16.53040695097241,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 49,
"frame": 3,
"params": [
0,
0,
0,
null,
47.768043182575376,
16.530224427630138,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 50,
"frame": 3,
"params": [
0,
0,
0,
null,
47.767939333015754,
16.530323724888724,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 51,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76792089791219,
16.530506008454143,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 52,
"frame": 3,
"params": [
0,
0,
0,
null,
47.767907597891394,
16.53068927891659,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 53,
"frame": 3,
"params": [
0,
0,
0,
null,
47.767899456183414,
16.530873216172655,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 54,
"frame": 3,
"params": [
0,
0,
0,
null,
47.7678211595422,
16.53062847989455,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 55,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76783809632257,
16.530420523817295,
15
],
"type": "SimpleItem"
}
],
"Refly90Degrees": false,
"TurnAroundDistance": 10,
"VisualTransectPoints": [
[
47.768644821858615,
16.53052943984308
],
[
47.76863335716058,
16.53064612872492
],
[
47.76862466704312,
16.530763346866273
],
[
47.768618762330405,
16.530880948263555
],
[
47.768527520688615,
16.530919324033228
],
[
47.76853460520799,
16.530761042994456
],
[
47.768546537105244,
16.530603407906614
],
[
47.76856329151763,
16.530446747237274
],
[
47.76858483353362,
16.530291387423606
],
[
47.76852994880375,
16.530073587664386
],
[
47.76849951312842,
16.53024717765454
],
[
47.76847491313001,
16.530422803641923
],
[
47.76845620960195,
16.530600031605875
],
[
47.768443448765865,
16.530778423566808
],
[
47.768436662157335,
16.530957538668577
],
[
47.76834614163562,
16.530995611138827
],
[
47.76835201161128,
16.53080463643282
],
[
47.76836442845143,
16.53061435689921
],
[
47.76838335975491,
16.53042526906374
],
[
47.76840875612134,
16.530237866342457
],
[
47.76844055128007,
16.530052637754178
],
[
47.7684786622631,
16.529870066644435
],
[
47.768376274234,
16.52990593722282
],
[
47.768340721559866,
16.530089431777824
],
[
47.76831128621317,
16.530275309020222
],
[
47.76828803964311,
16.530463117765336
],
[
47.76827103827668,
16.530652402140113
],
[
47.768260323381746,
16.53084270268965
],
[
47.768255920966844,
16.531033557492464
],
[
47.768165967629955,
16.53107139140768
],
[
47.76816861051905,
16.53089350356298
],
[
47.76817655483528,
16.53071596530955
],
[
47.768189784935466,
16.530539126238857
],
[
47.76820827476815,
16.530363334565596
],
[
47.76823198792491,
16.53018893644203
],
[
47.76826087771206,
16.530016275276378
],
[
47.76815014481609,
16.530122154136524
],
[
47.76812264138942,
16.53031697710711
],
[
47.768101459028976,
16.53051354077119
],
[
47.768086646632426,
16.530711391378407
],
[
47.7680782383929,
16.530910072207597
],
[
47.768076253720125,
16.531109124621103
],
[
47.76798674683302,
16.531146065535864
],
[
47.767987262588385,
16.530960627626218
],
[
47.767993180227045,
16.530775397272773
],
[
47.768004488625024,
16.53059072267118
],
[
47.76802116652483,
16.53040695097241
],
[
47.768043182575376,
16.530224427630138
],
[
47.767939333015754,
16.530323724888724
],
[
47.76792089791219,
16.530506008454143
],
[
47.767907597891394,
16.53068927891659
],
[
47.767899456183414,
16.530873216172655
],
[
47.7678211595422,
16.53062847989455
],
[
47.76783809632257,
16.530420523817295
]
],
"version": 1
},
"complexItemType": "circularSurvey",
"deltaAlpha": 3,
"deltaR": 10,
"fixedDirection": false,
"polygon": [
[
47.76846606616059,
16.529820081373487
],
[
47.76872241472738,
16.530837352593494
],
[
47.76798694569257,
16.531146687122508
],
[
47.76777418124312,
16.530481637145055
]
],
"referencePointAlt": 0,
"referencePointLat": 47.77086091347008,
"referencePointLong": 16.531071041719485,
"reverse": false,
"transectMinLength": 15,
"type": "ComplexItem",
"version": 1
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 0,
"AltitudeMode": 1,
"autoContinue": true,
"command": 21,
"doJumpId": 59,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76776040370392,
16.530569413178814,
0
],
"type": "SimpleItem"
}
],
"plannedHomePosition": [
47.76776040370392,
16.530569413178814,
178
],
"vehicleType": 2,
"version": 2
},
"rallyPoints": {
"points": [
],
"version": 2
},
"version": 1
}
}
Paths/KlingenbachKlein.wima 0 → 100644
View file @ 0510d157
{
"AreaItems": [
{
"AreaType": "Measurement Area",
"BorderPolygonOffset": 6,
"BottomLayerAltitude": 5,
"LayerDistance": 1,
"NumberOfLayers": 1,
"ShowBorderPolygon": 0,
"maxAltitude": 30,
"polygon": [
[
47.767875513636454,
16.53043285498356
],
[
47.767996632271384,
16.530579999131646
],
[
47.76784223516915,
16.53070246118625
],
[
47.76777418124312,
16.530481637145055
]
]
},
{
"AreaType": "Service Area",
"BorderPolygonOffset": 6,
"ShowBorderPolygon": 0,
"maxAltitude": 30,
"polygon": [
[
47.76777317830783,
16.530413969584316
],
[
47.76785089069791,
16.530667046992818
],
[
47.76775614544286,
16.5307494063876
],
[
47.76766140001518,
16.530447230100037
]
]
}
],
"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.76776040370392,
16.530569413178814,
8
],
"type": "SimpleItem"
},
{
"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.767881573082065,
16.530440216459883,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 3,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76787098888673,
16.530563032107995,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 4,
"frame": 3,
"params": [
0,
0,
0,
null,
47.7678626911699,
16.530686236245202,
15
],
"type": "SimpleItem"
}
],
"Refly90Degrees": false,
"TurnAroundDistance": 10,
"VisualTransectPoints": [
[
47.767881573082065,
16.530440216459883
],
[
47.76787098888673,
16.530563032107995
],
[
47.7678626911699,
16.530686236245202
]
],
"version": 1
},
"complexItemType": "circularSurvey",
"deltaAlpha": 3,
"deltaR": 5,
"fixedDirection": false,
"polygon": [
[
47.767875513636454,
16.53043285498356
],
[
47.767996632271384,
16.530579999131646
],
[
47.76784223516915,
16.53070246118625
],
[
47.76777418124312,
16.530481637145055
]
],
"referencePointAlt": 0,
"referencePointLat": 47.77086091347008,
"referencePointLong": 16.531071041719485,
"reverse": false,
"transectMinLength": 15,
"type": "ComplexItem",
"version": 1
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 0,
"AltitudeMode": 1,
"autoContinue": true,
"command": 21,
"doJumpId": 8,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76776040370392,
16.530569413178814,
0
],
"type": "SimpleItem"
}
],
"plannedHomePosition": [
47.76776040370392,
16.530569413178814,
178
],
"vehicleType": 2,
"version": 2
},
"rallyPoints": {
"points": [
],
"version": 2
},
"version": 1
}
}
qgroundcontrol.pro
View file @ 0510d157
......@@ -29,6 +29,7 @@ DebugBuild {
DESTDIR = $${OUT_PWD}/debug
DEFINES += DEBUG
DEFINES += SNAKE_SHOW_TIME
DEFINES += SNAKE_DEBUG
#DEFINES += ROS_BRIDGE_DEBUG
}
else {
......@@ -366,7 +367,7 @@ INCLUDEPATH += \
src/QtLocationPlugin \
src/QtLocationPlugin/QMLControl \
src/Settings \
src/Snake \
src/Wima/Snake \
src/Terrain \
src/VehicleSetup \
src/ViewWidgets \
......@@ -410,29 +411,28 @@ FORMS += \
#
HEADERS += \
src/Snake/clipper/clipper.hpp \
src/Snake/mapbox/feature.hpp \
src/Snake/mapbox/geometry.hpp \
src/Snake/mapbox/geometry/box.hpp \
src/Snake/mapbox/geometry/empty.hpp \
src/Snake/mapbox/geometry/envelope.hpp \
src/Snake/mapbox/geometry/for_each_point.hpp \
src/Snake/mapbox/geometry/geometry.hpp \
src/Snake/mapbox/geometry/line_string.hpp \
src/Snake/mapbox/geometry/multi_line_string.hpp \
src/Snake/mapbox/geometry/multi_point.hpp \
src/Snake/mapbox/geometry/multi_polygon.hpp \
src/Snake/mapbox/geometry/point.hpp \
src/Snake/mapbox/geometry/point_arithmetic.hpp \
src/Snake/mapbox/geometry/polygon.hpp \
src/Snake/mapbox/geometry_io.hpp \
src/Snake/mapbox/optional.hpp \
src/Snake/mapbox/polylabel.hpp \
src/Snake/mapbox/recursive_wrapper.hpp \
src/Snake/mapbox/variant.hpp \
src/Snake/mapbox/variant_io.hpp \
src/Snake/snake.h \
src/Snake/snake_typedefs.h \
src/Wima/Snake/clipper/clipper.hpp \
src/Wima/Snake/mapbox/feature.hpp \
src/Wima/Snake/mapbox/geometry.hpp \
src/Wima/Snake/mapbox/geometry/box.hpp \
src/Wima/Snake/mapbox/geometry/empty.hpp \
src/Wima/Snake/mapbox/geometry/envelope.hpp \
src/Wima/Snake/mapbox/geometry/for_each_point.hpp \
src/Wima/Snake/mapbox/geometry/geometry.hpp \
src/Wima/Snake/mapbox/geometry/line_string.hpp \
src/Wima/Snake/mapbox/geometry/multi_line_string.hpp \
src/Wima/Snake/mapbox/geometry/multi_point.hpp \
src/Wima/Snake/mapbox/geometry/multi_polygon.hpp \
src/Wima/Snake/mapbox/geometry/point.hpp \
src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp \
src/Wima/Snake/mapbox/geometry/polygon.hpp \
src/Wima/Snake/mapbox/geometry_io.hpp \
src/Wima/Snake/mapbox/optional.hpp \
src/Wima/Snake/mapbox/polylabel.hpp \
src/Wima/Snake/mapbox/recursive_wrapper.hpp \
src/Wima/Snake/mapbox/variant.hpp \
src/Wima/Snake/mapbox/variant_io.hpp \
src/Wima/Snake/snake.h \
src/Wima/Geometry/GenericPolygon.h \
src/Wima/Geometry/GenericPolygonArray.h \
src/Wima/Geometry/GeoPoint3D.h \
......@@ -500,8 +500,8 @@ HEADERS += \
src/comm/ros_bridge/include/topic_subscriber.h \
src/comm/utilities.h
SOURCES += \
src/Snake/clipper/clipper.cpp \
src/Snake/snake.cpp \
src/Wima/Snake/clipper/clipper.cpp \
src/Wima/Snake/snake.cpp \
src/Wima/Geometry/GeoPoint3D.cpp \
src/Wima/Snake/NemoInterface.cpp \
src/Wima/Snake/QNemoProgress.cc \
......
src/FlightDisplay/FlightDisplayViewMap.qml
View file @ 0510d157
......@@ -243,10 +243,8 @@ FlightMap {
// Add Snake tiles center points to the map
MapItemView {
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
model: _enable ? wimaController.snakeTileCenterPoints : 0
delegate: MapCircle{
......@@ -260,7 +258,7 @@ FlightMap {
function getColor(progress) {
if (progress < 50)
return "red"
return "orangered"
if (progress < 100)
return "orange"
......@@ -271,10 +269,8 @@ FlightMap {
// Add Snake tiles to the map
MapItemView {
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
model: _enable ? wimaController.snakeTiles : 0
delegate: MapPolygon{
......
src/FlightDisplay/FlightDisplayWimaMenu.qml
View file @ 0510d157
......@@ -98,6 +98,7 @@ Item {
property var enableWimaFact: wimaController.enableWimaController
property bool enableWimaBoolean: enableWimaFact.value
property var enableSnakeFact: wimaController.enableSnake
onAccept: {
if (enableWimaBoolean) {
......@@ -107,6 +108,7 @@ Item {
} else {
enableWimaFact.value = true
enableWimaMouseArea.enabled = false
enableSnakeCheckBox.checkedState = false;
timer.stop()
}
}
......@@ -184,6 +186,7 @@ Item {
spacing: ScreenTools.defaultFontPixelHeight * 0.5
FactCheckBox {
id: enableSnakeCheckBox
text: wimaController.enableSnake.value ?
qsTr("Disable Snake")
: qsTr("Enable Snake")
......
src/FlightMap/MapItems/WimaPlanMapItems.qml
View file @ 0510d157
......@@ -30,29 +30,8 @@ Item {
property var _map: map
property var _missionLineViewComponent
property var zoom: map.zoomLevel
property bool showItems: true
// onZoomChanged: {
// console.log('zoomLevel')
// console.log(zoom)
// }
function hideNumber() {
if (zoom > 19.5) {
return 1
} else if (zoom > 19) {
return 2
} else if (zoom > 17) {
return 4
} else if (zoom > 14) {
return 8
} else {
return -1
}
}
Component {
id: missionLineViewComponent
......@@ -64,7 +43,6 @@ Item {
}
}
Repeater {
model: largeMapView ? ( showItems ? missionItems : 0) : 0
......@@ -72,22 +50,10 @@ Item {
map: _map
color: _root.color
zOrder: _root.zOrderWP
visible: isVisible(index) && _root.visible
function isVisible(index) {
var num = hideNumber()
if (num > 0) {
return ((index+num) % num) == 0 ? true : false
} else {
return false
}
}
visible: _root.visible
}
}
Component.onCompleted: {
_missionLineViewComponent = missionLineViewComponent.createObject(map)
if (_missionLineViewComponent.status === Component.Error)
......@@ -98,6 +64,4 @@ Item {
Component.onDestruction: {
_missionLineViewComponent.destroy()
}
}
src/Snake/snake_typedefs.h deleted 100644 → 0
View file @ 50929c20
#pragma once
#include <vector>
#include <array>
#include <boost/geometry.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/angle/degrees.hpp>
namespace bg = boost::geometry;
namespace bu = boost::units;
namespace snake {
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;
typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
typedef bu::quantity<bu::si::plane_angle> Angle;
}
namespace boost{
namespace geometry{
namespace model {
bool operator==(snake::BoostPoint p1, snake::BoostPoint p2){
return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
}
bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2){
return !(p1 == p2);
}
} // namespace model
} // namespace geometry
} // namespace boost
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ 0510d157
#include "WimaMeasurementArea.h"
const char* WimaMeasurementArea::settingsGroup = "MeasurementArea";
const char* WimaMeasurementArea::bottomLayerAltitudeName = "BottomLayerAltitude";
const char* WimaMeasurementArea::numberOfLayersName = "NumberOfLayers";
const char* WimaMeasurementArea::layerDistanceName = "LayerDistance";
const char* WimaMeasurementArea::WimaMeasurementAreaName = "Measurement Area";
const char *WimaMeasurementArea::settingsGroup = "MeasurementArea";
const char *WimaMeasurementArea::bottomLayerAltitudeName =
"BottomLayerAltitude";
const char *WimaMeasurementArea::numberOfLayersName = "NumberOfLayers";
const char *WimaMeasurementArea::layerDistanceName = "LayerDistance";
const char *WimaMeasurementArea::WimaMeasurementAreaName = "Measurement Area";
WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
: WimaArea (parent)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"), this /* QObject parent */))
, _bottomLayerAltitude (SettingsFact(settingsGroup, _metaDataMap[bottomLayerAltitudeName], this /* QObject parent */))
, _numberOfLayers (SettingsFact(settingsGroup, _metaDataMap[numberOfLayersName], this /* QObject parent */))
, _layerDistance (SettingsFact(settingsGroup, _metaDataMap[layerDistanceName], this /* QObject parent */))
{
init();
: WimaArea(parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"),
this /* QObject parent */)),
_bottomLayerAltitude(SettingsFact(settingsGroup,
_metaDataMap[bottomLayerAltitudeName],
this /* QObject parent */)),
_numberOfLayers(SettingsFact(settingsGroup,
_metaDataMap[numberOfLayersName],
this /* QObject parent */)),
_layerDistance(SettingsFact(settingsGroup,
_metaDataMap[layerDistanceName],
this /* QObject parent */)) {
init();
}
WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other, QObject *parent)
: WimaArea (other, parent)
, _metaDataMap (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"), this /* QObject parent */))
, _bottomLayerAltitude (SettingsFact(settingsGroup, _metaDataMap[bottomLayerAltitudeName], this /* QObject parent */))
, _numberOfLayers (SettingsFact(settingsGroup, _metaDataMap[numberOfLayersName], this /* QObject parent */))
, _layerDistance (SettingsFact(settingsGroup, _metaDataMap[layerDistanceName], this /* QObject parent */))
{
init();
WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
QObject *parent)
: WimaArea(other, parent),
_metaDataMap(FactMetaData::createMapFromJsonFile(
QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"),
this /* QObject parent */)),
_bottomLayerAltitude(SettingsFact(settingsGroup,
_metaDataMap[bottomLayerAltitudeName],
this /* QObject parent */)),
_numberOfLayers(SettingsFact(settingsGroup,
_metaDataMap[numberOfLayersName],
this /* QObject parent */)),
_layerDistance(SettingsFact(settingsGroup,
_metaDataMap[layerDistanceName],
this /* QObject parent */)) {
init();
}
/*!
......@@ -33,106 +47,115 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other, QObje
*
* Calls the inherited operator WimaArea::operator=().
*/
WimaMeasurementArea &WimaMeasurementArea::operator=(const WimaMeasurementArea &other)
{
WimaArea::operator=(other);
WimaMeasurementArea &WimaMeasurementArea::
operator=(const WimaMeasurementArea &other) {
WimaArea::operator=(other);
return *this;
}
QString WimaMeasurementArea::mapVisualQML() const {
return "WimaMeasurementAreaMapVisual.qml";
}
return *this;
QString WimaMeasurementArea::editorQML() const {
return "WimaMeasurementAreaEditor.qml";
}
void WimaMeasurementArea::saveToJson(QJsonObject &json)
{
this->WimaArea::saveToJson(json);
json[bottomLayerAltitudeName] = _bottomLayerAltitude.rawValue().toDouble();
json[numberOfLayersName] = _numberOfLayers.rawValue().toInt();
json[layerDistanceName] = _layerDistance.rawValue().toDouble();
json[areaTypeName] = WimaMeasurementAreaName;
Fact *WimaMeasurementArea::tileHeight() { return &_tileHeight; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) {
this->WimaArea::saveToJson(json);
json[bottomLayerAltitudeName] = _bottomLayerAltitude.rawValue().toDouble();
json[numberOfLayersName] = _numberOfLayers.rawValue().toInt();
json[layerDistanceName] = _layerDistance.rawValue().toDouble();
json[areaTypeName] = WimaMeasurementAreaName;
}
bool WimaMeasurementArea::loadFromJson(const QJsonObject &json, QString& errorString)
{
if (this->WimaArea::loadFromJson(json, errorString)) {
bool retVal = true;
if ( json.contains(bottomLayerAltitudeName) && json[bottomLayerAltitudeName].isDouble() ) {
_bottomLayerAltitude.setRawValue(json[bottomLayerAltitudeName].toDouble());
} else {
errorString.append(tr("Could not load Bottom Layer Altitude!\n"));
retVal = false;
}
if ( json.contains(numberOfLayersName) && json[numberOfLayersName].isDouble() ) {
_numberOfLayers.setRawValue(json[numberOfLayersName].toInt());
} else {
errorString.append(tr("Could not load Number of Layers!\n"));
retVal = false;
}
if ( json.contains(layerDistanceName) && json[layerDistanceName].isDouble() ) {
_layerDistance.setRawValue(json[layerDistanceName].toDouble());
} else {
errorString.append(tr("Could not load Layer Distance!\n"));
retVal = false;
}
return retVal;
bool WimaMeasurementArea::loadFromJson(const QJsonObject &json,
QString &errorString) {
if (this->WimaArea::loadFromJson(json, errorString)) {
bool retVal = true;
if (json.contains(bottomLayerAltitudeName) &&
json[bottomLayerAltitudeName].isDouble()) {
_bottomLayerAltitude.setRawValue(
json[bottomLayerAltitudeName].toDouble());
} else {
return false;
errorString.append(tr("Could not load Bottom Layer Altitude!\n"));
retVal = false;
}
}
void WimaMeasurementArea::setBottomLayerAltitude(double altitude)
{
if ( !qFuzzyCompare(_bottomLayerAltitude.rawValue().toDouble(), altitude) ) {
_bottomLayerAltitude.setRawValue(altitude);
if (json.contains(numberOfLayersName) &&
json[numberOfLayersName].isDouble()) {
_numberOfLayers.setRawValue(json[numberOfLayersName].toInt());
} else {
errorString.append(tr("Could not load Number of Layers!\n"));
retVal = false;
}
emit bottomLayerAltitudeChanged();
if (json.contains(layerDistanceName) &&
json[layerDistanceName].isDouble()) {
_layerDistance.setRawValue(json[layerDistanceName].toDouble());
} else {
errorString.append(tr("Could not load Layer Distance!\n"));
retVal = false;
}
return retVal;
} else {
return false;
}
}
void WimaMeasurementArea::setNumberOfLayers(double numLayers)
{
if ( !qFuzzyCompare(_numberOfLayers.rawValue().toDouble(), numLayers) ) {
_numberOfLayers.setRawValue(numLayers);
void WimaMeasurementArea::setBottomLayerAltitude(double altitude) {
if (!qFuzzyCompare(_bottomLayerAltitude.rawValue().toDouble(), altitude)) {
_bottomLayerAltitude.setRawValue(altitude);
emit numberOfLayersChanged();
}
emit bottomLayerAltitudeChanged();
}
}
void WimaMeasurementArea::setLayerDistance(double layerDistance)
{
if ( !qFuzzyCompare(_layerDistance.rawValue().toDouble(), layerDistance) ) {
_layerDistance.setRawValue(layerDistance);
void WimaMeasurementArea::setNumberOfLayers(double numLayers) {
if (!qFuzzyCompare(_numberOfLayers.rawValue().toDouble(), numLayers)) {
_numberOfLayers.setRawValue(numLayers);
emit layerDistanceChanged();
}
emit numberOfLayersChanged();
}
}
void print(const WimaMeasurementArea &area)
{
QString message;
print(area, message);
qWarning() << message;
void WimaMeasurementArea::setLayerDistance(double layerDistance) {
if (!qFuzzyCompare(_layerDistance.rawValue().toDouble(), layerDistance)) {
_layerDistance.setRawValue(layerDistance);
emit layerDistanceChanged();
}
}
void print(const WimaMeasurementArea &area) {
QString message;
print(area, message);
qWarning() << message;
}
void print(const WimaMeasurementArea &area, QString outputStr)
{
print(static_cast<const WimaArea&>(area), outputStr);
outputStr.append(QString("Bottom Layer Altitude: %1\n").arg(area._bottomLayerAltitude.rawValue().toDouble()));
outputStr.append(QString("Number of Layers: %1\n").arg(area._numberOfLayers.rawValue().toInt()));
outputStr.append(QString("Layer Distance: %1\n").arg(area._layerDistance.rawValue().toDouble()));
void print(const WimaMeasurementArea &area, QString outputStr) {
print(static_cast<const WimaArea &>(area), outputStr);
outputStr.append(QString("Bottom Layer Altitude: %1\n")
.arg(area._bottomLayerAltitude.rawValue().toDouble()));
outputStr.append(QString("Number of Layers: %1\n")
.arg(area._numberOfLayers.rawValue().toInt()));
outputStr.append(QString("Layer Distance: %1\n")
.arg(area._layerDistance.rawValue().toDouble()));
}
void WimaMeasurementArea::init()
{
this->setObjectName(WimaMeasurementAreaName);
void WimaMeasurementArea::init() {
this->setObjectName(WimaMeasurementAreaName);
}
/*!
* \class WimaMeasurementArea
* \brief Class defining the area inside which the actual drone measurements are performed.
* \brief Class defining the area inside which the actual drone measurements are
* performed.
*
* \sa WimaArea, WimaController, WimaPlaner
*/
src/Wima/Geometry/WimaMeasurementArea.h
View file @ 0510d157
......@@ -7,66 +7,53 @@
#include "SettingsFact.h"
class WimaMeasurementArea : public WimaArea
{
Q_OBJECT
class WimaMeasurementArea : public WimaArea {
Q_OBJECT
public:
WimaMeasurementArea(QObject* parent = nullptr);
WimaMeasurementArea(const WimaMeasurementArea &other, QObject *parent = nullptr);
WimaMeasurementArea &operator=(const WimaMeasurementArea &other);
Q_PROPERTY(Fact* bottomLayerAltitude READ bottomLayerAltitudeFact CONSTANT)
Q_PROPERTY(Fact* numberOfLayers READ numberOfLayersFact CONSTANT)
Q_PROPERTY(Fact* layerDistance READ layerDistanceFact CONSTANT)
// Overrides from WimaPolygon
QString mapVisualQML (void) const { return "WimaMeasurementAreaMapVisual.qml";}
QString editorQML (void) const { return "WimaMeasurementAreaEditor.qml";}
// Property accessors
Fact* bottomLayerAltitudeFact (void) { return &_bottomLayerAltitude;}
Fact* numberOfLayersFact (void) { return &_numberOfLayers;}
Fact* layerDistanceFact (void) { return &_layerDistance;}
double bottomLayerAltitude (void) const { return _bottomLayerAltitude.rawValue().toDouble();}
int numberOfLayers (void) const { return _numberOfLayers.rawValue().toInt();}
double layerDistance (void) const { return _layerDistance.rawValue().toDouble();}
// Member Methodes
void saveToJson(QJsonObject& json);
bool loadFromJson(const QJsonObject& json, QString &errorString);
// Friends
friend void print(const WimaMeasurementArea& area, QString outputStr);
friend void print(const WimaMeasurementArea& area);
// Static Variables
static const char* settingsGroup;
static const char* bottomLayerAltitudeName;
static const char* numberOfLayersName;
static const char* layerDistanceName;
static const char* WimaMeasurementAreaName;
WimaMeasurementArea(QObject *parent = nullptr);
WimaMeasurementArea(const WimaMeasurementArea &other,
QObject *parent = nullptr);
WimaMeasurementArea &operator=(const WimaMeasurementArea &other);
Q_PROPERTY(Fact *borderPolygonOffset READ borderPolygonOffsetFact CONSTANT)
// Overrides from WimaPolygon
QString mapVisualQML(void) const {
return "WimaMeasurementAreaMapVisual.qml";
}
QString editorQML(void) const { return "WimaMeasurementAreaEditor.qml"; }
// Member Methodes
void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString);
// Friends
friend void print(const WimaMeasurementArea &area, QString outputStr);
friend void print(const WimaMeasurementArea &area);
// Static Variables
static const char *settingsGroup;
static const char *tileHeightName;
static const char *tileWidthName;
static const char *minTileAreaName;
static const char *transcetDistanceName;
static const char *minTransectLength;
static const char *WimaMeasurementAreaName;
signals:
void bottomLayerAltitudeChanged (void);
void numberOfLayersChanged (void);
void layerDistanceChanged (void);
public slots:
void setBottomLayerAltitude (double altitude);
void setNumberOfLayers (double numLayers);
void setLayerDistance (double layerDistance);
private:
// Member Methodes
void init();
// Member Methodes
void init();
// Members
QMap<QString, FactMetaData*> _metaDataMap;
// Members
QMap<QString, FactMetaData *> _metaDataMap;
SettingsFact _bottomLayerAltitude;
SettingsFact _numberOfLayers;
SettingsFact _layerDistance;
SettingsFact _tileWidth;
SettingsFact _tileHeight;
SettingsFact _minTileArea;
SettingsFact _transectDistance;
SettingsFact _minTransectLength;
};
src/Wima/Snake/SnakeDataManager_old.cc deleted 100644 → 0
View file @ 50929c20
#include "SnakeDataManager.h"
#include <QGeoCoordinate>
#include <QMutexLocker>
#include "QGCApplication.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"
#include <memory>
#include <mutex>
#include <shared_mutex>
#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "snake.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#define EVENT_TIMER_INTERVAL 500 // ms
#define TIMEOUT 3000 // ms
using QVariantList = QList<QVariant>;
using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;
using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
class SnakeDataManager::SnakeImpl {
public:
SnakeImpl(SnakeDataManager *p)
: rosBridgeEnabeled(false), topicServiceSetupDone(false),
lineDistance(1 * si::meter), minTransectLength(1 * si::meter),
calcInProgress(false), parent(p) {
}
bool precondition() const;
void resetOutput();
bool doTopicServiceSetup();
// Private data.
ROSBridgePtr pRosBridge;
bool rosBridgeEnabeled;
bool topicServiceSetupDone;
QTimer eventTimer;
QTimer timeout;
mutable std::shared_timed_mutex mutex;
// Scenario
snake::Scenario scenario;
Length lineDistance;
Length minTransectLength;
QList<QGeoCoordinate> mArea;
QList<QGeoCoordinate> sArea;
QList<QGeoCoordinate> corridor;
QGeoCoordinate ENUOrigin;
SnakeTiles tiles;
QmlObjectListModel tilesQml;
QVariantList tileCenterPoints;
SnakeTilesLocal tilesENU;
QVector<QPointF> tileCenterPointsENU;
// Waypoints
QVector<QGeoCoordinate> waypoints;
QVector<QGeoCoordinate> arrivalPath;
QVector<QGeoCoordinate> returnPath;
QVector<QPointF> waypointsENU;
QVector<QPointF> arrivalPathENU;
QVector<QPointF> returnPathENU;
QString errorMessage;
// Other
std::atomic_bool calcInProgress;
QNemoProgress qProgress;
std::vector<int> progress;
QNemoHeartbeat heartbeat;
SnakeDataManager *parent;
};
bool SnakeDataManager::SnakeImpl::precondition() const { return true; }
//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void SnakeDataManager::SnakeImpl::resetOutput() {
this->waypoints.clear();
this->arrivalPath.clear();
this->returnPath.clear();
this->ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
this->waypointsENU.clear();
this->arrivalPathENU.clear();
this->returnPathENU.clear();
this->tilesQml.clearAndDeleteContents();
this->tiles.polygons().clear();
this->tileCenterPoints.clear();
this->tilesENU.polygons().clear();
this->tileCenterPointsENU.clear();
this->errorMessage.clear();
}
template <class Callable> class CommandRAII {
public:
CommandRAII(Callable &fun) : _fun(fun) {}
~CommandRAII() { _fun(); }
private:
Callable _fun;
};
SnakeDataManager::SnakeDataManager(QObject *parent)
: QThread(parent), pImpl(std::make_unique<SnakeImpl>(this))
{}
SnakeDataManager::~SnakeDataManager() {}
void SnakeDataManager::setMeasurementArea(
const QList<QGeoCoordinate> &measurementArea) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->mArea = measurementArea;
for (auto &vertex : this->pImpl->mArea) {
vertex.setAltitude(0);
}
}
void SnakeDataManager::setServiceArea(
const QList<QGeoCoordinate> &serviceArea) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->sArea = serviceArea;
for (auto &vertex : this->pImpl->sArea) {
vertex.setAltitude(0);
}
}
void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->corridor = corridor;
for (auto &vertex : this->pImpl->corridor) {
vertex.setAltitude(0);
}
}
const QmlObjectListModel *SnakeDataManager::tiles() const {
SharedLock lk(this->pImpl->mutex);
return &this->pImpl->tilesQml;
}
QVariantList SnakeDataManager::tileCenterPoints() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->tileCenterPoints;
}
QNemoProgress SnakeDataManager::nemoProgress() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->qProgress;
}
int SnakeDataManager::nemoStatus() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->heartbeat.status();
}
bool SnakeDataManager::calcInProgress() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->calcInProgress.load();
}
QString SnakeDataManager::errorMessage() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->errorMessage;
}
bool SnakeDataManager::success() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->errorMessage.isEmpty() ? true : false;
}
QVector<QGeoCoordinate> SnakeDataManager::waypoints() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->waypoints;
}
QVector<QGeoCoordinate> SnakeDataManager::arrivalPath() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->arrivalPath;
}
QVector<QGeoCoordinate> SnakeDataManager::returnPath() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->returnPath;
}
Length SnakeDataManager::lineDistance() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->lineDistance;
}
void SnakeDataManager::setLineDistance(Length lineDistance) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->lineDistance = lineDistance;
}
Area SnakeDataManager::minTileArea() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.minTileArea();
}
void SnakeDataManager::setMinTileArea(Area minTileArea) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setMinTileArea(minTileArea);
}
Length SnakeDataManager::tileHeight() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.tileHeight();
}
void SnakeDataManager::setTileHeight(Length tileHeight) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setTileHeight(tileHeight);
}
Length SnakeDataManager::tileWidth() const {
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.tileWidth();
}
void SnakeDataManager::setTileWidth(Length tileWidth) {
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setTileWidth(tileWidth);
}
void SnakeDataManager::enableRosBridge() {
this->pImpl->rosBridgeEnabeled = true;
}
void SnakeDataManager::disableRosBride() {
this->pImpl->rosBridgeEnabeled = false;
}
void SnakeDataManager::run() {
#ifndef NDEBUG
auto startTime = std::chrono::high_resolution_clock::now();
qDebug() << "SnakeDataManager::run()";
#endif
this->pImpl->calcInProgress.store(true);
emit calcInProgressChanged(this->pImpl->calcInProgress.load());
auto onExit = [this, &startTime] {
#ifndef NDEBUG
qDebug() << "SnakeDataManager::run() execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - startTime)
.count()
<< " ms";
#endif
this->pImpl->calcInProgress.store(false);
emit calcInProgressChanged(this->pImpl->calcInProgress.load());
};
CommandRAII<decltype(onExit)> onExitRAII(onExit);
UniqueLock lk(this->pImpl->mutex);
this->pImpl->resetOutput();
if (!this->pImpl->precondition())
return;
if (this->pImpl->mArea.size() < 3) {
this->pImpl->errorMessage = "Measurement area invalid: size < 3.";
return;
}
if (this->pImpl->sArea.size() < 3) {
this->pImpl->errorMessage = "Service area invalid: size < 3.";
return;
}
this->pImpl->ENUOrigin = this->pImpl->mArea.front();
auto &origin = this->pImpl->ENUOrigin;
qDebug() << "SnakeDataManager::run(): origin: " << origin.latitude() << " "
<< origin.longitude() << " " << origin.altitude();
// Update measurement area.
auto &mAreaEnu = this->pImpl->scenario.measurementArea();
auto &mArea = this->pImpl->mArea;
mAreaEnu.clear();
for (auto geoVertex : mArea) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
mAreaEnu.outer().push_back(p);
}
// Update service area.
auto &sAreaEnu = this->pImpl->scenario.serviceArea();
auto &sArea = this->pImpl->sArea;
sAreaEnu.clear();
for (auto geoVertex : sArea) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
sAreaEnu.outer().push_back(p);
}
// Update corridor.
auto &corridorEnu = this->pImpl->scenario.corridor();
auto &corridor = this->pImpl->corridor;
corridor.clear();
for (auto geoVertex : corridor) {
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
corridorEnu.outer().push_back(p);
}
if (!this->pImpl->scenario.update()) {
this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
return;
}
// Store scenario data.
{
// Get tiles.
const auto &tiles = this->pImpl->scenario.tiles();
const auto &centerPoints = this->pImpl->scenario.tileCenterPoints();
for (unsigned int i = 0; i < tiles.size(); ++i) {
const auto &tile = tiles[i];
SnakeTile geoTile;
SnakeTileLocal enuTile;
for (size_t i = tile.outer().size(); i < tile.outer().size() - 1; ++i) {
auto &p = tile.outer()[i];
QPointF enuVertex(p.get<0>(), p.get<1>());
QGeoCoordinate geoVertex;
snake::fromENU(origin, p, geoVertex);
enuTile.polygon().points().push_back(enuVertex);
geoTile.push_back(geoVertex);
}
const auto &boostPoint = centerPoints[i];
QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>());
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostPoint, geoVertex);
geoTile.setCenter(geoVertex);
this->pImpl->tilesQml.append(new SnakeTile(geoTile));
this->pImpl->tiles.polygons().push_back(geoTile);
this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
this->pImpl->tilesENU.polygons().push_back(enuTile);
this->pImpl->tileCenterPointsENU.push_back(enuVertex);
}
}
// Create transects.
std::string errorString;
snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle *
degree::degree);
snake::flight_plan::Transects transects;
transects.push_back(bg::model::linestring<snake::BoostPoint>{
this->pImpl->scenario.homePositon()});
bool value = snake::flight_plan::transectsFromScenario(
this->pImpl->lineDistance, this->pImpl->minTransectLength, alpha,
this->pImpl->scenario, this->pImpl->progress, transects, errorString);
if (!value) {
this->pImpl->errorMessage = errorString.c_str();
return;
}
// Route transects
snake::flight_plan::Transects transectsRouted;
snake::flight_plan::Route route;
value =
snake::flight_plan::route(this->pImpl->scenario.joinedArea(), transects,
transectsRouted, route, errorString);
if (!value) {
this->pImpl->errorMessage = errorString.c_str();
return;
}
// Store arrival path.
const auto &firstWaypoint = transectsRouted.front().front();
long startIdx = 0;
for (long i = 0; i < long(route.size()); ++i) {
const auto &boostVertex = route[i];
if (boostVertex == firstWaypoint) {
startIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->arrivalPathENU.push_back(enuVertex);
this->pImpl->arrivalPath.push_back(geoVertex);
}
// Store return path.
long endIdx = 0;
const auto &lastWaypoint = transectsRouted.back().back();
for (long i = route.size() - 1; i >= 0; --i) {
const auto &boostVertex = route[i];
if (boostVertex == lastWaypoint) {
endIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->returnPathENU.push_back(enuVertex);
this->pImpl->returnPath.push_back(geoVertex);
}
// Store waypoints.
for (long i = startIdx; i <= endIdx; ++i) {
const auto &boostVertex = route[i];
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->waypointsENU.push_back(enuVertex);
this->pImpl->waypoints.push_back(geoVertex);
}
}
src/Wima/Snake/SnakeThread.cc
View file @ 0510d157
......@@ -39,7 +39,7 @@ public:
Input()
: tileWidth(5 * si::meter), tileHeight(5 * si::meter),
minTileArea(1 * si::meter * si::meter), lineDistance(2 * si::meter),
minTileArea(5 * si::meter * si::meter), lineDistance(2 * si::meter),
minTransectLength(1 * si::meter), scenarioChanged(true),
progressChanged(true), routeParametersChanged(true) {}
......@@ -490,54 +490,55 @@ void SnakeThread::run() {
this->pImpl->output.returnPathENU.clear();
this->pImpl->output.waypoints.clear();
this->pImpl->output.waypointsENU.clear();
// // Store arrival path.
// const auto &firstWaypoint = transectsRouted.front().front();
// long startIdx = 0;
// for (long i = 0; i < long(route.size()); ++i) {
// const auto &boostVertex = route[i];
// if (boostVertex == firstWaypoint) {
// startIdx = i;
// break;
// }
// QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, boostVertex, geoVertex);
// this->pImpl->output.arrivalPathENU.push_back(enuVertex);
// this->pImpl->output.arrivalPath.push_back(geoVertex);
// }
// // Store return path.
// long endIdx = 0;
// const auto &lastWaypoint = transectsRouted.back().back();
// for (long i = route.size() - 1; i >= 0; --i) {
// const auto &boostVertex = route[i];
// if (boostVertex == lastWaypoint) {
// endIdx = i;
// break;
// }
// QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, boostVertex, geoVertex);
// this->pImpl->output.returnPathENU.push_back(enuVertex);
// this->pImpl->output.returnPath.push_back(geoVertex);
// }
// // Store waypoints.
// for (long i = startIdx; i <= endIdx; ++i) {
// const auto &boostVertex = route[i];
// QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, boostVertex, geoVertex);
// this->pImpl->output.waypointsENU.push_back(enuVertex);
// this->pImpl->output.waypoints.push_back(geoVertex);
// }
for (const auto &t : transects) {
for (const auto &v : t) {
QPointF enuVertex{v.get<0>(), v.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, v, geoVertex);
this->pImpl->output.waypointsENU.push_back(enuVertex);
this->pImpl->output.waypoints.push_back(geoVertex);
// Store arrival path.
const auto &firstWaypoint = transectsRouted.front().front();
long startIdx = 0;
for (long i = 0; i < long(route.size()); ++i) {
const auto &boostVertex = route[i];
if (boostVertex == firstWaypoint) {
startIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->output.arrivalPathENU.push_back(enuVertex);
this->pImpl->output.arrivalPath.push_back(geoVertex);
}
// Store return path.
long endIdx = 0;
const auto &lastWaypoint = transectsRouted.back().back();
for (long i = route.size() - 1; i >= 0; --i) {
const auto &boostVertex = route[i];
if (boostVertex == lastWaypoint) {
endIdx = i;
break;
}
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->output.returnPathENU.push_back(enuVertex);
this->pImpl->output.returnPath.push_back(geoVertex);
}
// Store waypoints.
for (long i = startIdx; i <= endIdx; ++i) {
const auto &boostVertex = route[i];
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
this->pImpl->output.waypointsENU.push_back(enuVertex);
this->pImpl->output.waypoints.push_back(geoVertex);
}
// Store waypoints.
// for (const auto &t : transects) {
// for (const auto &v : t) {
// QPointF enuVertex{v.get<0>(), v.get<1>()};
// QGeoCoordinate geoVertex;
// snake::fromENU(origin, v, geoVertex);
// this->pImpl->output.waypointsENU.push_back(enuVertex);
// this->pImpl->output.waypoints.push_back(geoVertex);
// }
// }
this->pImpl->output.waypointsUpdated = true;
}
}
src/Wima/Snake/route.cpp 0 → 100644
View file @ 0510d157
bool flight_plan::route(const BoostPolygon &area,
const flight_plan::Transects &transects,
Transects &transectsRouted, flight_plan::Route &route,
string &errorString) {
//=======================================
// Route Transects using Google or-tools.
//=======================================
// Create vertex list;
BoostLineString vertices;
size_t n0 = 0;
for (const auto &t : transects) {
n0 += std::min<std::size_t>(t.size(), 2);
}
vertices.reserve(n0);
struct TransectInfo {
TransectInfo(size_t n, bool f) : index(n), front(f) {}
size_t index;
bool front;
};
std::vector<TransectInfo> transectInfoList;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
vertices.push_back(t.front());
transectInfoList.push_back(TransectInfo{i, true});
if (t.size() >= 2) {
vertices.push_back(t.back());
transectInfoList.push_back(TransectInfo{i, false});
}
}
for (long i = 0; i < long(area.outer().size()) - 1; ++i) {
vertices.push_back(area.outer()[i]);
}
for (auto &ring : area.inners()) {
for (auto &vertex : ring)
vertices.push_back(vertex);
}
size_t n1 = vertices.size();
// Generate routing model.
RoutingDataModel dataModel;
Matrix<double> connectionGraph(n1, n1);
// Offset joined area.
BoostPolygon areaOffset;
offsetPolygon(area, areaOffset, detail::offsetConstant);
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
generateRoutingModel(vertices, areaOffset, n0, dataModel, 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"
<< endl;
#endif
// Create Routing Index Manager.
RoutingIndexManager manager(dataModel.distanceMatrix.getN(),
dataModel.numVehicles, dataModel.depot);
// 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 {
// 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);
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);
// Define Constraints (this constraints have a huge impact on the
// solving time, improvments could be done, e.g. SearchFilter).
#ifdef SNAKE_DEBUG
std::cout << "snake::route(): Constraints:" << std::endl;
#endif
Solver *solver = routing.solver();
size_t index = 0;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
#ifdef SNAKE_DEBUG
std::cout << "i = " << i << ": t.size() = " << t.size() << std::endl;
#endif
if (t.size() >= 2) {
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index));
auto idx1 =
manager.NodeToIndex(RoutingIndexManager::NodeIndex(index + 1));
auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
auto c = solver->MakeNonEquality(cond0, cond1);
solver->AddConstraint(c);
#ifdef SNAKE_DEBUG
std::cout << "( next(" << index << ") == " << index + 1 << " ) != ( next("
<< index + 1 << ") == " << index << " )" << std::endl;
#endif
index += 2;
} else {
index += 1;
}
}
// Setting first solution heuristic.
RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
google::protobuf::Duration *tMax =
new google::protobuf::Duration(); // seconds
tMax->set_seconds(10);
searchParameters.set_allocated_time_limit(tMax);
// Solve the problem.
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
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 << "Execution time routing.SolveWithParameters(): " << delta.count()
<< " ms" << endl;
#endif
if (!solution || solution->Size() <= 1) {
errorString = "Not able to solve the routing problem.";
return false;
}
// Extract index list from solution.
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());
}
// Helper Lambda.
auto idx2Vertex = [&vertices](const std::vector<size_t> &idxArray,
std::vector<BoostPoint> &path) {
for (auto idx : idxArray)
path.push_back(vertices[idx]);
};
// Construct route.
for (size_t i = 0; i < route_idx.size() - 1; ++i) {
size_t idx0 = route_idx[i];
size_t idx1 = route_idx[i + 1];
const auto &info1 = transectInfoList[idx0];
const auto &info2 = transectInfoList[idx1];
if (info1.index == info2.index) { // same transect?
if (!info1.front) { // transect reversal needed?
BoostLineString reversedTransect;
const auto &t = transects[info1.index];
for (auto it = t.end() - 1; it >= t.begin(); --it) {
reversedTransect.push_back(*it);
}
transectsRouted.push_back(reversedTransect);
for (auto it = reversedTransect.begin();
it < reversedTransect.end() - 1; ++it) {
route.push_back(*it);
}
} else {
const auto &t = transects[info1.index];
for (auto it = t.begin(); it < t.end() - 1; ++it) {
route.push_back(*it);
}
transectsRouted.push_back(t);
}
} else {
std::vector<size_t> idxList;
shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
if (i != route_idx.size() - 2) {
idxList.pop_back();
}
idx2Vertex(idxList, route);
}
}
return true;
}
src/Snake/snake.cpp → src/Wima/Snake/snake.cpp
View file @ 0510d157
......@@ -444,6 +444,7 @@ bool Scenario::update() {
bg::correct(_mArea);
bg::correct(_sArea);
bg::correct(_corridor);
polygonCenter(_sArea, _homePosition);
if (!_calculateJoinedArea())
return false;
if (!_calculateBoundingBox())
......@@ -965,20 +966,28 @@ bool flight_plan::route(const BoostPolygon &area,
// Define Constraints (this constraints have a huge impact on the
// solving time, improvments could be done, e.g. SearchFilter).
#ifdef SNAKE_DEBUG
std::cout << "snake::route(): Constraints:" << std::endl;
#endif
Solver *solver = routing.solver();
size_t index = 0;
for (size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
#ifdef SNAKE_DEBUG
std::cout << "i = " << i << ": t.size() = " << t.size() << std::endl;
#endif
if (t.size() >= 2) {
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index));
auto idx1 =
manager.NodeToIndex(RoutingIndexManager::NodeIndex(index + 1));
auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
vector<IntVar *> conds{cond0, cond1};
auto c = solver->MakeAllDifferent(conds);
solver->MakeRejectFilter();
auto c = solver->MakeNonEquality(cond0, cond1);
solver->AddConstraint(c);
#ifdef SNAKE_DEBUG
std::cout << "( next(" << index << ") == " << index + 1 << " ) != ( next("
<< index + 1 << ") == " << index << " )" << std::endl;
#endif
index += 2;
} else {
index += 1;
......
src/Snake/snake.h → src/Wima/Snake/snake.h
View file @ 0510d157
......@@ -242,7 +242,7 @@ bool route(const BoostPolygon &area, const Transects &transects,
namespace detail {
const double offsetConstant =
0.5; // meter, polygon offset to compenstate for numerical inaccurracies.
1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
} // namespace snake
......
src/Wima/Snake/snake_old.cpp 0 → 100644
View file @ 0510d157
bool FlightPlan::generate(double lineDistance, double minTransectLength)
{
_waypointsENU.clear();
_waypoints.clear();
_arrivalPathIdx.clear();
_returnPathIdx.clear();
#ifndef NDEBUG
_PathVertices.clear();
#endif
#ifdef SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
if (!_generateTransects(lineDistance, minTransectLength))
return false;
#ifdef SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
cout << endl;
cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl;
#endif
//=======================================
// Route Transects using Google or-tools.
//=======================================
// Offset joined area.
const BoostPolygon &jArea = _scenario.getJoineAreaENU();
BoostPolygon jAreaOffset;
offsetPolygon(jArea, jAreaOffset, detail::offsetConstant);
// Create vertex list;
BoostLineString vertices;
size_t n_t = _transects.size()*2;
size_t n0 = n_t+1;
vertices.reserve(n0);
for (auto lstring : _transects){
for (auto vertex : lstring){
vertices.push_back(vertex);
}
}
vertices.push_back(_scenario.getHomePositonENU());
for (long i=0; i<long(jArea.outer().size())-1; ++i) {
vertices.push_back(jArea.outer()[i]);
}
for (auto ring : jArea.inners()) {
for (auto vertex : ring)
vertices.push_back(vertex);
}
size_t n1 = vertices.size();
// Generate routing model.
RoutingDataModel_t dataModel;
Matrix<double> connectionGraph(n1, n1);
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
_generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
#ifdef SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl;
#endif
// Create Routing Index Manager.
RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles,
dataModel.depot);
// Create Routing Model.
RoutingModel routing(manager);
// Create and register a transit callback.
const int transit_callback_index = routing.RegisterTransitCallback(
[&dataModel, &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);
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);
// Define Constraints.
size_t n = _transects.size()*2;
Solver *solver = routing.solver();
for (size_t i=0; i<n; i=i+2){
// auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
// auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
// auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
// auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
// auto c = solver->MakeNonEquality(cond0, cond1);
// solver->AddConstraint(c);
// alternative
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
vector<IntVar*> conds{cond0, cond1};
auto c = solver->MakeAllDifferent(conds);
solver->MakeRejectFilter();
solver->AddConstraint(c);
}
// Setting first solution heuristic.
RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
google::protobuf::Duration *tMax = new google::protobuf::Duration(); // seconds
tMax->set_seconds(10);
searchParameters.set_allocated_time_limit(tMax);
// Solve the problem.
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
const Assignment* solution = routing.SolveWithParameters(searchParameters);
#ifdef SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl;
#endif
if (!solution || solution->Size() <= 1){
errorString = "Not able to solve the routing problem.";
return false;
}
// Extract waypoints from solution.
long index = routing.Start(0);
std::vector<size_t> route;
route.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)){
index = solution->Value(routing.NextVar(index));
route.push_back(manager.IndexToNode(index).value());
}
// Connect transects
#ifndef NDEBUG
_PathVertices = vertices;
#endif
{
_waypointsENU.push_back(vertices[route[0]]);
vector<size_t> pathIdx;
long arrivalPathLength = 0;
for (long i=0; i<long(route.size())-1; ++i){
size_t idx0 = route[i];
size_t idx1 = route[i+1];
pathIdx.clear();
shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
if ( i==0 )
arrivalPathLength = pathIdx.size();
for (size_t j=1; j<pathIdx.size(); ++j)
_waypointsENU.push_back(vertices[pathIdx[j]]);
}
long returnPathLength = pathIdx.size();
for (long i=returnPathLength; i > 0; --i)
_returnPathIdx.push_back(_waypointsENU.size()-i);
for (long i=0; i < arrivalPathLength; ++i)
_arrivalPathIdx.push_back(i);
}
// Back transform waypoints.
GeoPoint3D origin{_scenario.getOrigin()};
for (auto vertex : _waypointsENU) {
GeoPoint3D geoVertex;
fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
_waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
}
return true;
}
bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength)
{
_transects.clear();
if (_scenario.getTilesENU().size() != _progress.size()){
ostringstream strstream;
strstream << "Number of tiles ("
<< _scenario.getTilesENU().size()
<< ") is not equal to progress array length ("
<< _progress.size()
<< ")";
errorString = strstream.str();
return false;
}
// Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
size_t num_tiles = _progress.size();
vector<BoostPolygon> processedTiles;
{
const auto &tiles = _scenario.getTilesENU();
for (size_t i=0; i<num_tiles; ++i) {
if (_progress[i] == 100){
processedTiles.push_back(tiles[i]);
}
}
if (processedTiles.size() == num_tiles)
return true;
}
// Convert measurement area and tiles to clipper path.
ClipperLib::Path mAreaClipper;
for ( auto vertex : _scenario.getMeasurementAreaENU().outer() ){
mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
}
vector<ClipperLib::Path> processedTilesClipper;
for (auto t : processedTiles){
ClipperLib::Path path;
for (auto vertex : t.outer()){
path.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
}
processedTilesClipper.push_back(path);
}
const min_bbox_rt &bbox = _scenario.getMeasurementAreaBBoxENU();
double alpha = bbox.angle;
double x0 = bbox.corners.outer()[0].get<0>();
double y0 = bbox.corners.outer()[0].get<1>();
double bboxWidth = bbox.width;
double bboxHeight = bbox.height;
double delta = detail::offsetConstant;
size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects
vector<double> yCoords;
yCoords.reserve(num_t);
double y = -delta;
for (size_t i=0; i < num_t; ++i) {
yCoords.push_back(y);
y += lineDistance;
}
// Generate transects and convert them to clipper path.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-alpha*180/M_PI);
trans::translate_transformer<double, 2, 2> translate_back(x0, y0);
vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i=0; i < num_t; ++i) {
// calculate transect
BoostPoint v1{-delta, yCoords[i]};
BoostPoint v2{bboxWidth+delta, yCoords[i]};
BoostLineString transect;
transect.push_back(v1);
transect.push_back(v2);
// transform back
BoostLineString temp_transect;
bg::transform(transect, temp_transect, rotate_back);
transect.clear();
bg::transform(temp_transect, transect, translate_back);
ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(transect[0].get<0>()*CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(transect[0].get<1>()*CLIPPER_SCALE)};
ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(transect[1].get<0>()*CLIPPER_SCALE),
static_cast<ClipperLib::cInt>(transect[1].get<1>()*CLIPPER_SCALE)};
ClipperLib::Path path{c1, c2};
transectsClipper.push_back(path);
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib::Clipper clipper;
clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
ClipperLib::PolyTree clippedTransecsPolyTree1;
clipper.Execute(ClipperLib::ctIntersection, clippedTransecsPolyTree1, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper.Clear();
for (auto child : clippedTransecsPolyTree1.Childs)
clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
ClipperLib::PolyTree clippedTransecsPolyTree2;
clipper.Execute(ClipperLib::ctDifference, clippedTransecsPolyTree2, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Extract transects from PolyTree and convert them to BoostLineString
for (auto child : clippedTransecsPolyTree2.Childs){
ClipperLib::Path 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};
BoostLineString transect{v1, v2};
if (bg::length(transect) >= minTransectLength)
_transects.push_back(transect);
}
if (_transects.size() < 1)
return false;
return true;
}
void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset,
size_t n0,
FlightPlan::RoutingDataModel_t &dataModel,
Matrix<double> &graph)
{
#ifdef SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
graphFromPolygon(polygonOffset, vertices, graph);
#ifdef 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;
#endif
// cout << endl;
// for (size_t i=0; i<graph.size(); ++i){
// vector<double> &row = graph[i];
// for (size_t j=0; j<row.size();++j){
// cout << "(" << i << "," << j << "):" << row[j] << " ";
// }
// cout << endl;
// }
// cout << endl;
Matrix<double> distanceMatrix(graph);
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
toDistanceMatrix(distanceMatrix);
#ifdef 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
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));
}
}
dataModel.numVehicles = 1;
dataModel.depot = n0-1;
}
src/Wima/WimaController.cc
View file @ 0510d157
......@@ -138,6 +138,11 @@ WimaController::WimaController(QObject *parent)
connect(&_enableSnake, &Fact::rawValueChanged, this,
&WimaController::_enableSnakeChangedHandler);
_enableSnakeChangedHandler();
connect(&_enableWimaController, &Fact::rawValueChanged, [this] {
if (!this->_enableWimaController.rawValue().toBool()) {
this->_enableSnake.setCookedValue(QVariant(false));
}
});
// Snake Waypoint Manager.
connect(&_enableSnake, &Fact::rawValueChanged, this,
......@@ -193,7 +198,10 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
Fact *WimaController::altitude() { return &_altitude; }
QmlObjectListModel *WimaController::snakeTiles() { return &this->tiles; }
QmlObjectListModel *WimaController::snakeTiles() {
static QmlObjectListModel list;
return &list;
}
QVariantList WimaController::snakeTileCenterPoints() {
return _currentThread->tileCenterPoints();
......@@ -719,6 +727,9 @@ void WimaController::_initSmartRTL() {
}
void WimaController::_threadFinishedHandler() {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
if (!_snakeThread.errorMessage().isEmpty()) {
qDebug() << _snakeThread.errorMessage();
}
......@@ -735,11 +746,27 @@ void WimaController::_threadFinishedHandler() {
_nemoInterface.setTilesENU(_snakeThread.tilesENU());
_nemoInterface.setENUOrigin(_snakeThread.ENUOrigin());
}
#ifdef SNAKE_SHOW_TIME
auto end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): tiles: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
.count()
<< " ms" << std::endl;
start = std::chrono::high_resolution_clock::now();
#endif
if (_snakeThread.progressUpdated()) {
emit nemoProgressChanged();
}
#ifdef SNAKE_SHOW_TIME
end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): progress: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
.count()
<< " ms" << std::endl;
start = std::chrono::high_resolution_clock::now();
#endif
if (_snakeThread.waypointsUpdated()) {
// Copy waypoints to waypoint manager.
_snakeWM.clear();
......@@ -759,6 +786,14 @@ void WimaController::_threadFinishedHandler() {
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
#ifdef SNAKE_SHOW_TIME
end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): waypoints: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
.count()
<< " ms" << std::endl;
#endif
}
void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
......
src/WimaView/WimaMeasurementAreaEditor.qml
View file @ 0510d157
......@@ -21,14 +21,8 @@ Rectangle {
color: qgcPal.windowShadeDark
radius: _radius
// The following properties must be available up the hierarchy chain
//property real availableWidth ///< Width for control
//property var areaItem ///< Mission Item for editor
property real _margin: ScreenTools.defaultFontPixelWidth / 2
property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5
//property var polyline: areaItem.polyline
//property bool polylineInteractive: polyline.interactive
property bool polygonInteractive: areaItem.interactive
property var polygon: areaItem
property bool initNecesarry: true
......@@ -37,9 +31,6 @@ Rectangle {
polygon.interactive = polygonInteractive;
}
QGCPalette { id: qgcPal; colorGroupEnabled: true }
Column {
......@@ -68,70 +59,77 @@ Rectangle {
rowSpacing: _margin
columns: 2
QGCLabel { text: qsTr("Altitude") }
QGCLabel { text: qsTr("Offset") }
FactTextField {
fact: areaItem.bottomLayerAltitude
fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
}
FactCheckBox {
text: qsTr("Border Polygon")
fact: areaItem.showBorderPolygon
//enabled: !missionItem.followTerrain
}
Item {
height: ScreenTools.defaultFontPixelHeight / 2
width: 1
}
} // Column - Settings
SectionHeader {
id: snakeHeader
text: qsTr("Snake")
}
Column {
GridLayout {
anchors.left: parent.left
anchors.right: parent.right
columnSpacing: _margin
rowSpacing: _margin
columns: 2
QGCLabel { text: qsTr("Layers") }
QGCLabel { text: qsTr("Tile Height") }
FactTextField {
fact: areaItem.numberOfLayers
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Layer Dist.") }
QGCLabel { text: qsTr("Tile Width") }
FactTextField {
fact: areaItem.layerDistance
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Offset") }
QGCLabel { text: qsTr("Min. Tile Area") }
FactTextField {
fact: areaItem.borderPolygonOffset
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
}
FactCheckBox {
text: qsTr("Border Polygon")
fact: areaItem.showBorderPolygon
//enabled: !missionItem.followTerrain
}
QGCLabel { text: qsTr("Transect Distance") }
Item {
height: ScreenTools.defaultFontPixelHeight / 2
width: 1
}
} // Column - Settings
/*SectionHeader {
id: polylineHeader
text: qsTr("Gateway Poly Line")
}
FactTextField {
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
QGCButton {
id: polylineEditor
anchors.topMargin: _margin / 2
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
text: polylineInteractive ? "Done" : "Edit"
QGCLabel { text: qsTr("Min. Transect Length") }
onClicked: editPolyline()
}
FactTextField {
//fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
}
QGCButton {
id: swapEndpoints
anchors.topMargin: _margin / 2
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
text: "Swap End-Points"
onClicked: polyline.swapEndPoints()
}*/
} // Snake
SectionHeader {
id: statsHeader
......
src/WimaView/WimaServiceAreaEditor.qml
View file @ 0510d157
......@@ -21,41 +21,12 @@ Rectangle {
color: qgcPal.windowShadeDark
radius: _radius
// The following properties must be available up the hierarchy chain
//property real availableWidth ///< Width for control
//property var areaItem ///< Mission Item for editor
property real _margin: ScreenTools.defaultFontPixelWidth / 2
property real _fieldWidth: ScreenTools.defaultFontPixelWidth * 10.5
//property var polyline: areaItem.polyline
//property bool polylineInteractive: polyline.interactive
property bool polygonInteractive: areaItem.interactive
property var polygon: areaItem
property bool initNecesarry: true
/*onPolylineInteractiveChanged: {
polyline.interactive = polylineInteractive;
}*/
// onPolygonInteractiveChanged: {
// polygon.interactive = polygonInteractive;
// }
/*function editPolyline(){
if (polylineInteractive){
//polyline.interactive = false;
polylineInteractive = false;
//polygonInteractive = true;
}else{
//polyline.interactive = true;
polylineInteractive = true;
//polygonInteractive = false;
}
}*/
QGCPalette { id: qgcPal; colorGroupEnabled: true }
Column {
......@@ -90,28 +61,6 @@ Rectangle {
fact: areaItem.borderPolygonOffset
Layout.fillWidth: true
}
/*QGCLabel {
text: qsTr("Bottom Layer Altitude")
}
FactTextField {
fact: areaItem.bottomLayerAltitude
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Number of Layers") }
FactTextField {
fact: areaItem.numberOfLayers
Layout.fillWidth: true
}
QGCLabel { text: qsTr("Layer Distance") }
FactTextField {
fact: areaItem.layerDistance
Layout.fillWidth: true
}*/
}
Item {
......@@ -125,31 +74,6 @@ Rectangle {
fact: areaItem.showBorderPolygon
//enabled: !missionItem.followTerrain
}
// Column - Scan
/*SectionHeader {
id: polylineHeader
text: qsTr("Gateway Poly Line")
}
QGCButton {
id: polylineEditor
anchors.topMargin: _margin / 2
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
text: polylineInteractive ? "Done" : "Edit"
onClicked: editPolyline()
}
QGCButton {
id: swapEndpoints
anchors.topMargin: _margin / 2
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.rightMargin: ScreenTools.defaultFontPixelWidth
text: "Swap End-Points"
onClicked: polyline.swapEndPoints()
}*/
SectionHeader {
id: statsHeader
......@@ -161,9 +85,6 @@ Rectangle {
columnSpacing: ScreenTools.defaultFontPixelWidth
visible: statsHeader.checked
/*QGCLabel { text: qsTr("Layers") }
QGCLabel { text: areaItem.layers.valueString }*/
QGCLabel { text: qsTr("Area") }
QGCLabel { text: QGroundControl.squareMetersToAppSettingsAreaUnits(areaItem.area).toFixed(2) + " " + QGroundControl.appSettingsAreaUnitsString }
......
src/comm/ros_bridge/include/RosBridgeClient.cpp
View file @ 0510d157
#include "RosBridgeClient.h"
#include <chrono>
#include <functional>
#include <thread>
#include <future>
#include <regex>
#include <thread>
struct Task{
std::function<bool(void)> ready; // Condition under which command should be executed.
std::function<void(void)> execute; // Command to execute.
std::function<bool(void)> expired; // Returns true if the command is expired.
std::function<void(void)> clear_up; // operation to perform if task expired.
std::string name;
struct Task {
std::function<bool(void)>
ready; // Condition under which command should be executed.
std::function<void(void)> execute; // Command to execute.
std::function<bool(void)> expired; // Returns true if the command is expired.
std::function<void(void)> clear_up; // operation to perform if task expired.
std::string name;
};
void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr<WsClient> client, const std::__cxx11::string &message)
{
void RosbridgeWsClient::start(const std::__cxx11::string &client_name,
std::shared_ptr<WsClient> client,
const std::__cxx11::string &message) {
#ifndef ROS_BRIDGE_DEBUG
(void)client_name;
(void)client_name;
#endif
if (!client->on_open)
{
if (!client->on_open) {
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open = [client_name, message](
std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open =
[message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
connection->send(message);
};
}
connection->send(message);
};
}
#ifdef ROS_BRIDGE_DEBUG
if (!client->on_message)
{
client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
if (!client->on_message) {
client->on_message =
[client_name](std::shared_ptr<WsClient::Connection> /*connection*/,
std::shared_ptr<WsClient::InMessage> in_message) {
std::cout << client_name
<< ": Message received: " << in_message->string()
<< std::endl;
};
}
if (!client->on_close)
{
client->on_close = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, int status, const std::string & /*reason*/) {
std::cout << client_name << ": Closed connection with status code " << status << std::endl;
}
if (!client->on_close) {
client->on_close =
[client_name](std::shared_ptr<WsClient::Connection> /*connection*/,
int status, const std::string & /*reason*/) {
std::cout << client_name << ": Closed connection with status code "
<< status << std::endl;
};
}
if (!client->on_error)
{
// See http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html, Error Codes for error code meanings
client->on_error = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, const SimpleWeb::error_code &ec) {
std::cout << client_name << ": Error: " << ec << ", error message: " << ec.message() << std::endl;
}
if (!client->on_error) {
// See
// http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html,
// Error Codes for error code meanings
client->on_error =
[client_name](std::shared_ptr<WsClient::Connection> /*connection*/,
const SimpleWeb::error_code &ec) {
std::cout << client_name << ": Error: " << ec
<< ", error message: " << ec.message() << std::endl;
};
}
}
#endif
#ifdef ROS_BRIDGE_DEBUG
std::thread client_thread([client_name, client]() {
std::thread client_thread([client_name, client]() {
#else
std::thread client_thread([client]() {
std::thread client_thread([client]() {
#endif
client->start();
client->start();
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Terminated" << std::endl;
std::cout << client_name << ": Terminated" << std::endl;
#endif
client->on_open = NULL;
client->on_message = NULL;
client->on_close = NULL;
client->on_error = NULL;
client->on_open = NULL;
client->on_message = NULL;
client->on_close = NULL;
client->on_error = NULL;
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << " thread end" << std::endl;
std::cout << client_name << " thread end" << std::endl;
#endif
});
});
client_thread.detach();
client_thread.detach();
}
RosbridgeWsClient::RosbridgeWsClient(const std::string &server_port_path, bool run) :
server_port_path(server_port_path)
, isConnected(std::make_shared<std::atomic_bool>(false))
, stopped(std::make_shared<std::atomic_bool>(true))
{
if ( run )
this->run();
RosbridgeWsClient::RosbridgeWsClient(const std::string &server_port_path,
bool run)
: server_port_path(server_port_path),
isConnected(std::make_shared<std::atomic_bool>(false)),
stopped(std::make_shared<std::atomic_bool>(true)) {
if (run)
this->run();
}
RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_path) :
RosbridgeWsClient::RosbridgeWsClient(server_port_path, true)
{
}
RosbridgeWsClient::RosbridgeWsClient(
const std::__cxx11::string &server_port_path)
: RosbridgeWsClient::RosbridgeWsClient(server_port_path, true) {}
RosbridgeWsClient::~RosbridgeWsClient()
{
reset();
}
RosbridgeWsClient::~RosbridgeWsClient() { reset(); }
bool RosbridgeWsClient::connected(){
return isConnected->load();
}
bool RosbridgeWsClient::connected() { return isConnected->load(); }
void RosbridgeWsClient::run()
{
if ( !stopped->load() )
return;
stopped->store(false);
// Start periodic thread to monitor connection status, advertised topics etc.
periodic_thread = std::make_shared<std::thread> ([this] {
std::list<Task> task_list;
constexpr auto poll_interval = std::chrono::seconds(1);
auto poll_time_point = std::chrono::high_resolution_clock::now();
while ( !this->stopped->load() ) {
// ====================================================================================
// Add tasks.
// ====================================================================================
if ( std::chrono::high_resolution_clock::now() > poll_time_point) {
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
std::string reset_status_task_name = "reset_status_task";
// Add status task if necessary.
auto const it = std::find_if(task_list.begin(), task_list.end(),
[&reset_status_task_name](const Task &t){
return t.name == reset_status_task_name;
});
if ( it == task_list.end() ){
// Check connection status.
auto status_set = std::make_shared<std::atomic_bool>(false);
auto status_client = std::make_shared<WsClient>(this->server_port_path);
status_client->on_open = [status_set, this](std::shared_ptr<WsClient::Connection>) {
this->isConnected->store(true);
status_set->store(true);
};
std::thread status_thread([status_client]{
status_client->start();
status_client->on_open = NULL;
status_client->on_message = NULL;
status_client->on_close = NULL;
status_client->on_error = NULL;
});
status_thread.detach();
// Create task to reset isConnected after one second.
Task reset_task;
reset_task.name = reset_status_task_name;
// condition
auto now = std::chrono::high_resolution_clock::now();
const auto t_trigger = now + std::chrono::seconds(1);
reset_task.ready = [t_trigger]{
return std::chrono::high_resolution_clock::now() > t_trigger;
};
// command
reset_task.execute = [status_client, status_set, this]{
status_client->stop();
this->isConnected->store(false);
status_set->store(true);
};
// expired
reset_task.expired = [status_set]{
return status_set->load();
};
// clear up
reset_task.clear_up = [status_client, this]{
status_client->stop();
};
task_list.push_back(reset_task);
}
if ( this->isConnected->load() ){
// Add available topics task if neccessary.
std::string reset_topics_task_name = "reset_topics_task";
auto const topics_it = std::find_if(task_list.begin(), task_list.end(), [&reset_topics_task_name](const Task &t){
return t.name == reset_topics_task_name;
});
if ( topics_it == task_list.end() ){
// Call /rosapi/topics service.
auto topics_set = std::make_shared<std::atomic_bool>(false);
this->callService("/rosapi/topics", [topics_set, this](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message){
std::unique_lock<std::mutex> lk(this->mutex);
this->available_topics = in_message->string();
lk.unlock();
topics_set->store(true);
connection->send_close(1000);
});
// Create task to reset topics after one second.
Task reset_task;
reset_task.name = reset_topics_task_name;
// condition
auto now = std::chrono::high_resolution_clock::now();
auto t_trigger = now + std::chrono::seconds(1);
reset_task.ready = [t_trigger]{
return std::chrono::high_resolution_clock::now() > t_trigger;
};
// command
reset_task.execute = [topics_set, this]{
std::unique_lock<std::mutex> lk(this->mutex);
this->available_topics.clear();
lk.unlock();
topics_set->store(true);
};
// expired
reset_task.expired = [topics_set]{
return topics_set->load();
};
// clear up
reset_task.clear_up = [this]{
return;
};
task_list.push_back(reset_task);
}
// Add available services task if neccessary.
std::string reset_services_name = "reset_services_task";
auto const services_it = std::find_if(task_list.begin(), task_list.end(), [&reset_services_name](const Task &t){
return t.name == reset_services_name;
});
if ( services_it == task_list.end() ){
// Call /rosapi/services service.
auto services_set = std::make_shared<std::atomic_bool>(false);
this->callService("/rosapi/services", [this, services_set](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message){
std::unique_lock<std::mutex> lk(this->mutex);
this->available_services = in_message->string();
lk.unlock();
services_set->store(true);
connection->send_close(1000);
});
// Create task to reset services after one second.
Task reset_task;
reset_task.name = reset_services_name;
// condition
auto now = std::chrono::high_resolution_clock::now();
auto t_trigger = now + std::chrono::seconds(1);
reset_task.ready = [t_trigger]{
return std::chrono::high_resolution_clock::now() > t_trigger;
};
// command
reset_task.execute = [services_set, this]{
std::unique_lock<std::mutex> lk(this->mutex);
this->available_services.clear();
lk.unlock();
services_set->store(true);
};
// expired
reset_task.expired = [services_set]{
return services_set->load();
};
// clear up
reset_task.clear_up = [this]{
return;
};
task_list.push_back(reset_task);
}
} else {
std::lock_guard<std::mutex> lk(mutex);
available_topics.clear();
available_services.clear();
}
}
// ====================================================================================
// Process tasks.
// ====================================================================================
for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){
if ( !task_it->expired() ){
if ( task_it->ready() ){
task_it->execute();
task_it = task_list.erase(task_it);
} else {
++task_it;
}
} else {
task_it->clear_up();
task_it = task_list.erase(task_it);
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
void RosbridgeWsClient::run() {
if (!stopped->load())
return;
stopped->store(false);
// Start periodic thread to monitor connection status, advertised topics etc.
periodic_thread = std::make_shared<std::thread>([this] {
std::list<Task> task_list;
constexpr auto poll_interval = std::chrono::seconds(1);
auto poll_time_point = std::chrono::high_resolution_clock::now();
while (!this->stopped->load()) {
// ====================================================================================
// Add tasks.
// ====================================================================================
if (std::chrono::high_resolution_clock::now() > poll_time_point) {
poll_time_point =
std::chrono::high_resolution_clock::now() + poll_interval;
std::string reset_status_task_name = "reset_status_task";
// Add status task if necessary.
auto const it = std::find_if(task_list.begin(), task_list.end(),
[&reset_status_task_name](const Task &t) {
return t.name == reset_status_task_name;
});
if (it == task_list.end()) {
// Check connection status.
auto status_set = std::make_shared<std::atomic_bool>(false);
auto status_client =
std::make_shared<WsClient>(this->server_port_path);
status_client->on_open =
[status_set, this](std::shared_ptr<WsClient::Connection>) {
this->isConnected->store(true);
status_set->store(true);
};
std::thread status_thread(
[status_client] { status_client->start(); });
status_thread.detach();
// Create task to reset isConnected after one second.
Task reset_task;
reset_task.name = reset_status_task_name;
// condition
auto now = std::chrono::high_resolution_clock::now();
const auto t_trigger = now + std::chrono::seconds(1);
reset_task.ready = [t_trigger] {
return std::chrono::high_resolution_clock::now() > t_trigger;
};
// command
reset_task.execute = [status_client, status_set, this] {
status_client->stop();
this->isConnected->store(false);
status_set->store(true);
};
// expired
reset_task.expired = [status_set] { return status_set->load(); };
// clear up
reset_task.clear_up = [status_client, this] {
status_client->stop();
};
task_list.push_back(reset_task);
}
// Clear up remaining tasks.
for ( auto task_it = task_list.begin(); task_it != task_list.end(); ++task_it){
task_it->clear_up();
if (this->isConnected->load()) {
// Add available topics task if neccessary.
std::string reset_topics_task_name = "reset_topics_task";
auto const topics_it =
std::find_if(task_list.begin(), task_list.end(),
[&reset_topics_task_name](const Task &t) {
return t.name == reset_topics_task_name;
});
if (topics_it == task_list.end()) {
// Call /rosapi/topics service.
auto topics_set = std::make_shared<std::atomic_bool>(false);
this->callService(
"/rosapi/topics",
[topics_set,
this](std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message) {
std::unique_lock<std::mutex> lk(this->mutex);
this->available_topics = in_message->string();
lk.unlock();
topics_set->store(true);
connection->send_close(1000);
});
// Create task to reset topics after one second.
Task reset_task;
reset_task.name = reset_topics_task_name;
// condition
auto now = std::chrono::high_resolution_clock::now();
auto t_trigger = now + std::chrono::seconds(1);
reset_task.ready = [t_trigger] {
return std::chrono::high_resolution_clock::now() > t_trigger;
};
// command
reset_task.execute = [topics_set, this] {
std::unique_lock<std::mutex> lk(this->mutex);
this->available_topics.clear();
lk.unlock();
topics_set->store(true);
};
// expired
reset_task.expired = [topics_set] { return topics_set->load(); };
// clear up
reset_task.clear_up = [this] { return; };
task_list.push_back(reset_task);
}
// Add available services task if neccessary.
std::string reset_services_name = "reset_services_task";
auto const services_it =
std::find_if(task_list.begin(), task_list.end(),
[&reset_services_name](const Task &t) {
return t.name == reset_services_name;
});
if (services_it == task_list.end()) {
// Call /rosapi/services service.
auto services_set = std::make_shared<std::atomic_bool>(false);
this->callService(
"/rosapi/services",
[this, services_set](
std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message) {
std::unique_lock<std::mutex> lk(this->mutex);
this->available_services = in_message->string();
lk.unlock();
services_set->store(true);
connection->send_close(1000);
});
// Create task to reset services after one second.
Task reset_task;
reset_task.name = reset_services_name;
// condition
auto now = std::chrono::high_resolution_clock::now();
auto t_trigger = now + std::chrono::seconds(1);
reset_task.ready = [t_trigger] {
return std::chrono::high_resolution_clock::now() > t_trigger;
};
// command
reset_task.execute = [services_set, this] {
std::unique_lock<std::mutex> lk(this->mutex);
this->available_services.clear();
lk.unlock();
services_set->store(true);
};
// expired
reset_task.expired = [services_set] {
return services_set->load();
};
// clear up
reset_task.clear_up = [this] { return; };
task_list.push_back(reset_task);
}
} else {
std::lock_guard<std::mutex> lk(mutex);
available_topics.clear();
available_services.clear();
}
}
// ====================================================================================
// Process tasks.
// ====================================================================================
for (auto task_it = task_list.begin(); task_it != task_list.end();) {
if (!task_it->expired()) {
if (task_it->ready()) {
task_it->execute();
task_it = task_list.erase(task_it);
} else {
++task_it;
}
} else {
task_it->clear_up();
task_it = task_list.erase(task_it);
}
task_list.clear();
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
// Clear up remaining tasks.
for (auto task_it = task_list.begin(); task_it != task_list.end();
++task_it) {
task_it->clear_up();
}
task_list.clear();
#ifdef ROS_BRIDGE_DEBUG
std::cout << "periodic thread end" << std::endl;
std::cout << "periodic thread end" << std::endl;
#endif
});
});
}
void RosbridgeWsClient::stop()
{
if ( stopped->load() )
return;
stopped->store(true);
periodic_thread->join();
void RosbridgeWsClient::stop() {
if (stopped->load())
return;
stopped->store(true);
periodic_thread->join();
}
void RosbridgeWsClient::reset()
{
stop();
unsubscribeAll();
unadvertiseAll();
unadvertiseAllServices();
std::unique_lock<std::mutex> lk(mutex);
for (auto it = client_map.begin(); it != client_map.end(); ) {
std::string client_name = it->first;
lk.unlock();
removeClient(client_name);
lk.lock();
it = client_map.begin();
}
void RosbridgeWsClient::reset() {
stop();
unsubscribeAll();
unadvertiseAll();
unadvertiseAllServices();
std::unique_lock<std::mutex> lk(mutex);
for (auto it = client_map.begin(); it != client_map.end();) {
std::string client_name = it->first;
lk.unlock();
removeClient(client_name);
lk.lock();
it = client_map.begin();
}
}
void RosbridgeWsClient::addClient(const std::string &client_name)
{
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it == client_map.end())
{
client_map[client_name] = std::make_shared<WsClient>(server_port_path);
}
void RosbridgeWsClient::addClient(const std::string &client_name) {
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it =
client_map.find(client_name);
if (it == client_map.end()) {
client_map[client_name] = std::make_shared<WsClient>(server_port_path);
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << " has already been created" << std::endl;
}
else {
std::cerr << client_name << " has already been created" << std::endl;
}
#endif
}
std::shared_ptr<WsClient> RosbridgeWsClient::getClient(const std::string &client_name)
{
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
return it->second;
}
return NULL;
std::shared_ptr<WsClient>
RosbridgeWsClient::getClient(const std::string &client_name) {
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it =
client_map.find(client_name);
if (it != client_map.end()) {
return it->second;
}
return NULL;
}
void RosbridgeWsClient::stopClient(const std::string &client_name)
{
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
// Stop the client asynchronously in 100 ms.
// This is to ensure, that all threads involving the client have been launched.
std::shared_ptr<WsClient> client = it->second;
#ifdef ROS_BRIDGE_DEBUG
std::thread t([client, client_name](){
void RosbridgeWsClient::stopClient(const std::string &client_name) {
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it =
client_map.find(client_name);
if (it != client_map.end()) {
// Stop the client asynchronously in 100 ms.
// This is to ensure, that all threads involving the client have been
// launched.
std::shared_ptr<WsClient> client = it->second;
#ifdef ROS_BRIDGE_DEBUG
std::thread t([client, client_name]() {
#else
std::thread t([client](){
std::thread t([client]() {
#endif
std::this_thread::sleep_for(std::chrono::milliseconds(100));
client->stop();
// The next lines of code seem to cause a double free or corruption error, why?
std::this_thread::sleep_for(std::chrono::milliseconds(100));
client->stop();
// The next lines of code seem to cause a double free or corruption error,
// why?
// client->on_open = NULL;
// client->on_message = NULL;
// client->on_close = NULL;
// client->on_error = NULL;
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << " has been removed" << std::endl;
std::cout << client_name << " has been removed" << std::endl;
#endif
});
t.detach();
}
});
t.detach();
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << " has not been created" << std::endl;
}
else {
std::cerr << client_name << " has not been created" << std::endl;
}
#endif
}
void RosbridgeWsClient::removeClient(const std::string &client_name)
{
stopClient(client_name);
{
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
if (it != client_map.end())
{
client_map.erase(it);
}
void RosbridgeWsClient::removeClient(const std::string &client_name) {
stopClient(client_name);
{
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it =
client_map.find(client_name);
if (it != client_map.end()) {
client_map.erase(it);
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << " has not been created" << std::endl;
}
#endif
else {
std::cerr << client_name << " has not been created" << std::endl;
}
#endif
}
}
std::string RosbridgeWsClient::getAdvertisedTopics(){
std::lock_guard<std::mutex> lk(mutex);
return available_topics;
std::string RosbridgeWsClient::getAdvertisedTopics() {
std::lock_guard<std::mutex> lk(mutex);
return available_topics;
}
std::string RosbridgeWsClient::getAdvertisedServices(){
std::lock_guard<std::mutex> lk(mutex);
return available_services;
std::string RosbridgeWsClient::getAdvertisedServices() {
std::lock_guard<std::mutex> lk(mutex);
return available_services;
}
bool RosbridgeWsClient::topicAvailable(const std::string &topic){
std::lock_guard<std::mutex> lk(mutex);
bool RosbridgeWsClient::topicAvailable(const std::string &topic) {
std::lock_guard<std::mutex> lk(mutex);
#ifdef ROS_BRIDGE_DEBUG
std::cout << "checking if topic " << topic << " is available" << std::endl;
std::cout << "available topics: " << available_topics << std::endl;
std::cout << "checking if topic " << topic << " is available" << std::endl;
std::cout << "available topics: " << available_topics << std::endl;
#endif
size_t pos;
{
pos = available_topics.find(topic);
}
bool ret = pos != std::string::npos ? true : false;
size_t pos;
{ pos = available_topics.find(topic); }
bool ret = pos != std::string::npos ? true : false;
#ifdef ROS_BRIDGE_DEBUG
if ( ret ){
std::cout << "topic " << topic << " is available" << std::endl;
} else {
std::cout << "topic " << topic << " is not available" << std::endl;
}
if (ret) {
std::cout << "topic " << topic << " is available" << std::endl;
} else {
std::cout << "topic " << topic << " is not available" << std::endl;
}
#endif
return ret;
return ret;
}
void RosbridgeWsClient::advertise(const std::string &client_name, const std::string &topic, const std::string &type, const std::string &id)
{
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
if (it_client != client_map.end())
{
auto it_ser_top = std::find_if(service_topic_list.begin(),
service_topic_list.end(),
[topic](const EntryData &td){
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
void RosbridgeWsClient::advertise(const std::string &client_name,
const std::string &topic,
const std::string &type,
const std::string &id) {
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator
it_client = client_map.find(client_name);
if (it_client != client_map.end()) {
auto it_ser_top = std::find_if(
service_topic_list.begin(), service_topic_list.end(),
[topic](const EntryData &td) {
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top != service_topic_list.end()){
if (it_ser_top != service_topic_list.end()) {
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " already advertised" << std::endl;
std::cerr << "topic: " << topic << " already advertised" << std::endl;
#endif
return;
}
auto client = it_client->second;
std::weak_ptr<WsClient> wpClient = client;
service_topic_list.push_back(std::make_tuple(EntryType::AdvertisedTopic, topic, client_name, wpClient));
std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\"";
if (id.compare("") != 0)
{
message += ", \"id\":\"" + id + "\"";
}
message = "{" + message + "}";
return;
}
auto client = it_client->second;
std::weak_ptr<WsClient> wpClient = client;
service_topic_list.push_back(std::make_tuple(EntryType::AdvertisedTopic,
topic, client_name, wpClient));
std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic +
"\", \"type\":\"" + type + "\"";
if (id.compare("") != 0) {
message += ", \"id\":\"" + id + "\"";
}
message = "{" + message + "}";
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [this, topic, message, client_name](std::shared_ptr<WsClient::Connection> connection) {
client->on_open = [this, topic, message, client_name](
std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [this, topic, message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open = [this, topic, message](
std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
connection->send(message);
};
connection->send(message);
};
start(client_name, client, message);
}
start(client_name, client, message);
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
}
else {
std::cerr << client_name << "has not been created" << std::endl;
}
#endif
}
void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string &id){
std::lock_guard<std::mutex> lk(mutex);
auto it_ser_top = std::find_if(service_topic_list.begin(),
service_topic_list.end(),
[&topic](const EntryData &td){
void RosbridgeWsClient::unadvertise(const std::string &topic,
const std::string &id) {
std::lock_guard<std::mutex> lk(mutex);
auto it_ser_top = std::find_if(
service_topic_list.begin(), service_topic_list.end(),
[&topic](const EntryData &td) {
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top == service_topic_list.end()){
});
if (it_ser_top == service_topic_list.end()) {
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " not advertised" << std::endl;
std::cerr << "topic: " << topic << " not advertised" << std::endl;
#endif
return;
}
return;
}
std::string message = "\"op\":\"unadvertise\"";
if (id.compare("") != 0)
{
message += ", \"id\":\"" + id + "\"";
}
message += ", \"topic\":\"" + topic + "\"";
message = "{" + message + "}";
std::string message = "\"op\":\"unadvertise\"";
if (id.compare("") != 0) {
message += ", \"id\":\"" + id + "\"";
}
message += ", \"topic\":\"" + topic + "\"";
message = "{" + message + "}";
std::string client_name = "topic_unadvertiser" + topic;
auto client = std::make_shared<WsClient>(server_port_path);
std::string client_name = "topic_unadvertiser" + topic;
auto client = std::make_shared<WsClient>(server_port_path);
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open = [client_name, message](
std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open =
[message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
connection->send(message); // unadvertise
connection->send_close(1000);
};
connection->send(message); // unadvertise
connection->send_close(1000);
};
start(client_name, client, message);
service_topic_list.erase(it_ser_top);
start(client_name, client, message);
service_topic_list.erase(it_ser_top);
}
void RosbridgeWsClient::unadvertiseAll(){
for (auto entry : service_topic_list){
if ( std::get<integral(EntryEnum::EntryType)>(entry) == EntryType::AdvertisedTopic ){
unadvertise(std::get<integral(EntryEnum::ServiceTopicName)>(entry));
}
void RosbridgeWsClient::unadvertiseAll() {
for (auto entry : service_topic_list) {
if (std::get<integral(EntryEnum::EntryType)>(entry) ==
EntryType::AdvertisedTopic) {
unadvertise(std::get<integral(EntryEnum::ServiceTopicName)>(entry));
}
}
}
void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Document &msg, const std::string &id)
{
std::lock_guard<std::mutex> lk(mutex);
auto it_ser_top = std::find_if(service_topic_list.begin(),
service_topic_list.end(),
[&topic](const EntryData &td){
void RosbridgeWsClient::publish(const std::string &topic,
const rapidjson::Document &msg,
const std::string &id) {
std::lock_guard<std::mutex> lk(mutex);
auto it_ser_top = std::find_if(
service_topic_list.begin(), service_topic_list.end(),
[&topic](const EntryData &td) {
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top == service_topic_list.end() ){
});
if (it_ser_top == service_topic_list.end()) {
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " not yet advertised" << std::endl;
std::cerr << "topic: " << topic << " not yet advertised" << std::endl;
#endif
return;
}
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
msg.Accept(writer);
return;
}
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
msg.Accept(writer);
std::string client_name = "publish_client" + topic;
std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString();
std::string client_name = "publish_client" + topic;
std::string message = "\"op\":\"publish\", \"topic\":\"" + topic +
"\", \"msg\":" + strbuf.GetString();
if (id.compare("") != 0)
{
message += ", \"id\":\"" + id + "\"";
}
message = "{" + message + "}";
if (id.compare("") != 0) {
message += ", \"id\":\"" + id + "\"";
}
message = "{" + message + "}";
std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
std::shared_ptr<WsClient> publish_client =
std::make_shared<WsClient>(server_port_path);
#ifdef ROS_BRIDGE_DEBUG
publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
publish_client->on_open =
[message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#else
publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
publish_client->on_open =
[message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message." << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
std::cout << client_name << ": Sending message: " << message
<< std::endl;
#endif
connection->send(message);
// TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away
// TODO: This could be improved by creating a thread to keep publishing
// the message instead of closing it right away
connection->send_close(1000);
};
start(client_name, publish_client, message);
}
void RosbridgeWsClient::subscribe(const std::string &client_name, const std::string &topic, const InMessage &callback, const std::string &id, const std::string &type, int throttle_rate, int queue_length, int fragment_size, const std::string &compression)
{
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
if (it_client != client_map.end())
{
auto it_ser_top = std::find_if(service_topic_list.begin(),
service_topic_list.end(),
[topic](const EntryData &td){
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
};
start(client_name, publish_client, message);
}
void RosbridgeWsClient::subscribe(const std::string &client_name,
const std::string &topic,
const InMessage &callback,
const std::string &id,
const std::string &type, int throttle_rate,
int queue_length, int fragment_size,
const std::string &compression) {
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator
it_client = client_map.find(client_name);
if (it_client != client_map.end()) {
auto it_ser_top = std::find_if(
service_topic_list.begin(), service_topic_list.end(),
[topic](const EntryData &td) {
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top != service_topic_list.end()){
if (it_ser_top != service_topic_list.end()) {
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " already advertised" << std::endl;
std::cerr << "topic: " << topic << " already advertised" << std::endl;
#endif
return;
}
auto client = it_client->second;
std::weak_ptr<WsClient> wpClient = client;
service_topic_list.push_back(std::make_tuple(EntryType::SubscribedTopic, topic, client_name, wpClient));
std::string message = "\"op\":\"subscribe\", \"topic\":\"" + topic + "\"";
return;
}
auto client = it_client->second;
std::weak_ptr<WsClient> wpClient = client;
service_topic_list.push_back(std::make_tuple(EntryType::SubscribedTopic,
topic, client_name, wpClient));
if (id.compare("") != 0)
{
message += ", \"id\":\"" + id + "\"";
}
if (type.compare("") != 0)
{
message += ", \"type\":\"" + type + "\"";
}
if (throttle_rate > -1)
{
message += ", \"throttle_rate\":" + std::to_string(throttle_rate);
}
if (queue_length > -1)
{
message += ", \"queue_length\":" + std::to_string(queue_length);
}
if (fragment_size > -1)
{
message += ", \"fragment_size\":" + std::to_string(fragment_size);
}
if (compression.compare("none") == 0 || compression.compare("png") == 0)
{
message += ", \"compression\":\"" + compression + "\"";
}
message = "{" + message + "}";
std::string message = "\"op\":\"subscribe\", \"topic\":\"" + topic + "\"";
client->on_message = callback;
this->start(client_name, client, message); // subscribe to topic
if (id.compare("") != 0) {
message += ", \"id\":\"" + id + "\"";
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
if (type.compare("") != 0) {
message += ", \"type\":\"" + type + "\"";
}
if (throttle_rate > -1) {
message += ", \"throttle_rate\":" + std::to_string(throttle_rate);
}
if (queue_length > -1) {
message += ", \"queue_length\":" + std::to_string(queue_length);
}
if (fragment_size > -1) {
message += ", \"fragment_size\":" + std::to_string(fragment_size);
}
if (compression.compare("none") == 0 || compression.compare("png") == 0) {
message += ", \"compression\":\"" + compression + "\"";
}
message = "{" + message + "}";
client->on_message = callback;
this->start(client_name, client, message); // subscribe to topic
}
#ifdef ROS_BRIDGE_DEBUG
else {
std::cerr << client_name << "has not been created" << std::endl;
}
#endif
}
void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string &id){
std::lock_guard<std::mutex> lk(mutex);
auto it_ser_top = std::find_if(service_topic_list.begin(),
service_topic_list.end(),
[topic](const EntryData &td){
void RosbridgeWsClient::unsubscribe(const std::string &topic,
const std::string &id) {
std::lock_guard<std::mutex> lk(mutex);
auto it_ser_top = std::find_if(
service_topic_list.begin(), service_topic_list.end(),
[topic](const EntryData &td) {
return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top == service_topic_list.end()){
});
if (it_ser_top == service_topic_list.end()) {
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "topic: " << topic << " not advertised" << std::endl;
std::cerr << "topic: " << topic << " not advertised" << std::endl;
#endif
return;
}
return;
}
std::string message = "\"op\":\"unsubscribe\"";
if (id.compare("") != 0)
{
message += ", \"id\":\"" + id + "\"";
}
message += ", \"topic\":\"" + topic + "\"";
message = "{" + message + "}";
std::string message = "\"op\":\"unsubscribe\"";
if (id.compare("") != 0) {
message += ", \"id\":\"" + id + "\"";
}
message += ", \"topic\":\"" + topic + "\"";
message = "{" + message + "}";
std::string client_name = "topic_unsubscriber" + topic;
auto client = std::make_shared<WsClient>(server_port_path);
std::string client_name = "topic_unsubscriber" + topic;
auto client = std::make_shared<WsClient>(server_port_path);
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open = [client_name, message](
std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open =
[message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
connection->send(message); // unadvertise
connection->send_close(1000);
};
connection->send(message); // unadvertise
connection->send_close(1000);
};
start(client_name, client, message);
service_topic_list.erase(it_ser_top);
start(client_name, client, message);
service_topic_list.erase(it_ser_top);
}
void RosbridgeWsClient::unsubscribeAll(){
for (auto entry : service_topic_list){
if( std::get<integral(EntryEnum::EntryType)>(entry) == EntryType::SubscribedTopic ) {
unsubscribe(std::get<integral(EntryEnum::ServiceTopicName)>(entry));
}
void RosbridgeWsClient::unsubscribeAll() {
for (auto entry : service_topic_list) {
if (std::get<integral(EntryEnum::EntryType)>(entry) ==
EntryType::SubscribedTopic) {
unsubscribe(std::get<integral(EntryEnum::ServiceTopicName)>(entry));
}
}
}
void RosbridgeWsClient::advertiseService(const std::string &client_name, const std::string &service, const std::string &type, const InMessage &callback)
{
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
if (it_client != client_map.end())
{
auto it_ser_top = std::find_if(service_topic_list.begin(),
service_topic_list.end(),
[service](const EntryData &td){
return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
void RosbridgeWsClient::advertiseService(const std::string &client_name,
const std::string &service,
const std::string &type,
const InMessage &callback) {
std::lock_guard<std::mutex> lk(mutex);
std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator
it_client = client_map.find(client_name);
if (it_client != client_map.end()) {
auto it_ser_top = std::find_if(
service_topic_list.begin(), service_topic_list.end(),
[service](const EntryData &td) {
return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top != service_topic_list.end()){
if (it_ser_top != service_topic_list.end()) {
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "service: " << service << " already advertised" << std::endl;
std::cerr << "service: " << service << " already advertised" << std::endl;
#endif
return;
}
auto client = it_client->second;
std::weak_ptr<WsClient> wpClient = client;
service_topic_list.push_back(std::make_tuple(EntryType::AdvertisedService, service, client_name, wpClient));
return;
}
auto client = it_client->second;
std::weak_ptr<WsClient> wpClient = client;
service_topic_list.push_back(std::make_tuple(
EntryType::AdvertisedService, service, client_name, wpClient));
std::string message = "{\"op\":\"advertise_service\", \"service\":\"" + service + "\", \"type\":\"" + type + "\"}";
std::string message = "{\"op\":\"advertise_service\", \"service\":\"" +
service + "\", \"type\":\"" + type + "\"}";
it_client->second->on_message = callback;
start(client_name, it_client->second, message);
}
it_client->second->on_message = callback;
start(client_name, it_client->second, message);
}
#ifdef ROS_BRIDGE_DEBUG
else
{
std::cerr << client_name << "has not been created" << std::endl;
}
else {
std::cerr << client_name << "has not been created" << std::endl;
}
#endif
}
void RosbridgeWsClient::unadvertiseService(const std::string &service){
std::lock_guard<std::mutex> lk(mutex);
auto it_ser_top = std::find_if(service_topic_list.begin(),
service_topic_list.end(),
[service](const EntryData &td){
void RosbridgeWsClient::unadvertiseService(const std::string &service) {
std::lock_guard<std::mutex> lk(mutex);
auto it_ser_top = std::find_if(
service_topic_list.begin(), service_topic_list.end(),
[service](const EntryData &td) {
return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
});
if ( it_ser_top == service_topic_list.end()){
});
if (it_ser_top == service_topic_list.end()) {
#ifdef ROS_BRIDGE_DEBUG
std::cerr << "service: " << service << " not advertised" << std::endl;
std::cerr << "service: " << service << " not advertised" << std::endl;
#endif
return;
}
return;
}
std::string message = "\"op\":\"unadvertise_service\"";
message += ", \"service\":\"" + service + "\"";
message = "{" + message + "}";
std::string message = "\"op\":\"unadvertise_service\"";
message += ", \"service\":\"" + service + "\"";
message = "{" + message + "}";
std::string client_name = "service_unadvertiser" + service;
auto client = std::make_shared<WsClient>(server_port_path);
std::string client_name = "service_unadvertiser" + service;
auto client = std::make_shared<WsClient>(server_port_path);
#ifdef ROS_BRIDGE_DEBUG
client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open = [client_name, message](
std::shared_ptr<WsClient::Connection> connection) {
#else
client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
client->on_open =
[message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
connection->send(message); // unadvertise
connection->send_close(1000);
};
connection->send(message); // unadvertise
connection->send_close(1000);
};
start(client_name, client, message);
service_topic_list.erase(it_ser_top);
start(client_name, client, message);
service_topic_list.erase(it_ser_top);
}
void RosbridgeWsClient::unadvertiseAllServices(){
for (auto entry : service_topic_list){
if( std::get<integral(EntryEnum::EntryType)>(entry) == EntryType::AdvertisedService ) {
unadvertiseService(std::get<integral(EntryEnum::ServiceTopicName)>(entry));
}
void RosbridgeWsClient::unadvertiseAllServices() {
for (auto entry : service_topic_list) {
if (std::get<integral(EntryEnum::EntryType)>(entry) ==
EntryType::AdvertisedService) {
unadvertiseService(
std::get<integral(EntryEnum::ServiceTopicName)>(entry));
}
}
}
void RosbridgeWsClient::serviceResponse(const std::string &service, const std::string &id, bool result, const rapidjson::Document &values)
{
std::string message = "\"op\":\"service_response\", \"service\":\"" + service + "\", \"result\":" + ((result)? "true" : "false");
void RosbridgeWsClient::serviceResponse(const std::string &service,
const std::string &id, bool result,
const rapidjson::Document &values) {
std::string message = "\"op\":\"service_response\", \"service\":\"" +
service +
"\", \"result\":" + ((result) ? "true" : "false");
// Rosbridge somehow does not allow service_response to be sent without id and values
// , so we cannot omit them even though the documentation says they are optional.
message += ", \"id\":\"" + id + "\"";
// Rosbridge somehow does not allow service_response to be sent without id and
// values , so we cannot omit them even though the documentation says they are
// optional.
message += ", \"id\":\"" + id + "\"";
// Convert JSON document to string
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
values.Accept(writer);
// Convert JSON document to string
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
values.Accept(writer);
message += ", \"values\":" + std::string(strbuf.GetString());
message = "{" + message + "}";
message += ", \"values\":" + std::string(strbuf.GetString());
message = "{" + message + "}";
std::string client_name = "service_response_client" + service;
std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);
std::string client_name = "service_response_client" + service;
std::shared_ptr<WsClient> service_response_client =
std::make_shared<WsClient>(server_port_path);
#ifdef ROS_BRIDGE_DEBUG
service_response_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
service_response_client->on_open =
[message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#else
service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
service_response_client->on_open =
[message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Opened connection" << std::endl;
std::cout << client_name << ": Sending message: " << message << std::endl;
std::cout << client_name << ": Sending message: " << message
<< std::endl;
#endif
connection->send(message);
connection->send_close(1000);
};
};
start(client_name, service_response_client, message);
start(client_name, service_response_client, message);
}
void RosbridgeWsClient::callService(const std::string &service, const InMessage &callback, const rapidjson::Document &args, const std::string &id, int fragment_size, const std::string &compression)
{
std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\"";
if (!args.IsNull())
{
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
args.Accept(writer);
void RosbridgeWsClient::callService(const std::string &service,
const InMessage &callback,
const rapidjson::Document &args,
const std::string &id, int fragment_size,
const std::string &compression) {
std::string message =
"\"op\":\"call_service\", \"service\":\"" + service + "\"";
message += ", \"args\":" + std::string(strbuf.GetString());
}
if (id.compare("") != 0)
{
message += ", \"id\":\"" + id + "\"";
}
if (fragment_size > -1)
{
message += ", \"fragment_size\":" + std::to_string(fragment_size);
}
if (compression.compare("none") == 0 || compression.compare("png") == 0)
{
message += ", \"compression\":\"" + compression + "\"";
}
message = "{" + message + "}";
std::string client_name = "call_service_client" + service;
std::shared_ptr<WsClient> call_service_client = std::make_shared<WsClient>(server_port_path);
if (!args.IsNull()) {
rapidjson::StringBuffer strbuf;
rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
args.Accept(writer);
if (callback)
{
call_service_client->on_message = callback;
}
else
{
call_service_client->on_message = [client_name](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
std::cout << client_name << ": Sending close connection" << std::endl;
message += ", \"args\":" + std::string(strbuf.GetString());
}
if (id.compare("") != 0) {
message += ", \"id\":\"" + id + "\"";
}
if (fragment_size > -1) {
message += ", \"fragment_size\":" + std::to_string(fragment_size);
}
if (compression.compare("none") == 0 || compression.compare("png") == 0) {
message += ", \"compression\":\"" + compression + "\"";
}
message = "{" + message + "}";
std::string client_name = "call_service_client" + service;
std::shared_ptr<WsClient> call_service_client =
std::make_shared<WsClient>(server_port_path);
if (callback) {
call_service_client->on_message = callback;
} else {
call_service_client->on_message =
[client_name](std::shared_ptr<WsClient::Connection> connection,
std::shared_ptr<WsClient::InMessage> in_message) {
#ifdef ROS_BRIDGE_DEBUG
std::cout << client_name
<< ": Message received: " << in_message->string()
<< std::endl;
std::cout << client_name << ": Sending close connection" << std::endl;
#else
(void)in_message;
(void)in_message;
#endif
connection->send_close(1000);
connection->send_close(1000);
};
}
}
start(client_name, call_service_client, message);
start(client_name, call_service_client, message);
}
bool RosbridgeWsClient::serviceAvailable(const std::string &service)
{
bool RosbridgeWsClient::serviceAvailable(const std::string &service) {
#ifdef ROS_BRIDGE_DEBUG
std::cout << "checking if service " << service << " is available" << std::endl;
std::cout << "checking if service " << service << " is available"
<< std::endl;
#endif
size_t pos;
{
std::lock_guard<std::mutex> lk(mutex);
pos = available_services.find(service);
}
bool ret = pos != std::string::npos ? true : false;
size_t pos;
{
std::lock_guard<std::mutex> lk(mutex);
pos = available_services.find(service);
}
bool ret = pos != std::string::npos ? true : false;
#ifdef ROS_BRIDGE_DEBUG
if ( ret ){
std::cout << "service " << service << " is available" << std::endl;
} else {
std::cout << "service " << service << " is not available" << std::endl;
}
if (ret) {
std::cout << "service " << service << " is available" << std::endl;
} else {
std::cout << "service " << service << " is not available" << std::endl;
}
#endif
return ret;
return ret;
}
void RosbridgeWsClient::waitForService(const std::string &service)
{
waitForService(service, []{
return false; // never stop
});
void RosbridgeWsClient::waitForService(const std::string &service) {
waitForService(service, [] {
return false; // never stop
});
}
void RosbridgeWsClient::waitForService(const std::string &service, const std::function<bool(void)> stop)
{
void RosbridgeWsClient::waitForService(const std::string &service,
const std::function<bool(void)> stop) {
#ifdef ROS_BRIDGE_DEBUG
auto s = std::chrono::high_resolution_clock::now();
long counter = 0;
auto s = std::chrono::high_resolution_clock::now();
long counter = 0;
#endif
const auto poll_interval = std::chrono::milliseconds(1000);
auto poll_time_point = std::chrono::high_resolution_clock::now();
while( !stop() )
{
if ( std::chrono::high_resolution_clock::now() > poll_time_point ){
const auto poll_interval = std::chrono::milliseconds(1000);
auto poll_time_point = std::chrono::high_resolution_clock::now();
while (!stop()) {
if (std::chrono::high_resolution_clock::now() > poll_time_point) {
#ifdef ROS_BRIDGE_DEBUG
++counter;
++counter;
#endif
if ( serviceAvailable(service) ){
break;
}
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
};
if (serviceAvailable(service)) {
break;
}
poll_time_point =
std::chrono::high_resolution_clock::now() + poll_interval;
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
};
#ifdef ROS_BRIDGE_DEBUG
auto e = std::chrono::high_resolution_clock::now();
std::cout << "waitForService() " << service << " time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
<< " ms." << std::endl;
std::cout << "waitForTopic() " << service << ": number of calls to topicAvailable: "
<< counter << std::endl;
auto e = std::chrono::high_resolution_clock::now();
std::cout
<< "waitForService() " << service << " time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(e - s).count()
<< " ms." << std::endl;
std::cout << "waitForTopic() " << service
<< ": number of calls to topicAvailable: " << counter << std::endl;
#endif
}
void RosbridgeWsClient::waitForTopic(const std::string &topic)
{
waitForTopic(topic, []{
return false; // never stop
});
void RosbridgeWsClient::waitForTopic(const std::string &topic) {
waitForTopic(topic, [] {
return false; // never stop
});
}
void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::function<bool(void)> stop)
{
void RosbridgeWsClient::waitForTopic(const std::string &topic,
const std::function<bool(void)> stop) {
#ifdef ROS_BRIDGE_DEBUG
auto s = std::chrono::high_resolution_clock::now();
long counter = 0;
auto s = std::chrono::high_resolution_clock::now();
long counter = 0;
#endif
const auto poll_interval = std::chrono::milliseconds(1000);
auto poll_time_point = std::chrono::high_resolution_clock::now();
while( !stop() )
{
if ( std::chrono::high_resolution_clock::now() > poll_time_point ){
const auto poll_interval = std::chrono::milliseconds(1000);
auto poll_time_point = std::chrono::high_resolution_clock::now();
while (!stop()) {
if (std::chrono::high_resolution_clock::now() > poll_time_point) {
#ifdef ROS_BRIDGE_DEBUG
++counter;
++counter;
#endif
if ( topicAvailable(topic) ){
break;
}
poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
};
if (topicAvailable(topic)) {
break;
}
poll_time_point =
std::chrono::high_resolution_clock::now() + poll_interval;
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
};
#ifdef ROS_BRIDGE_DEBUG
auto e = std::chrono::high_resolution_clock::now();
std::cout << "waitForTopic() " << topic << " time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
<< " ms." << std::endl;
std::cout << "waitForTopic() " << topic << ": number of calls to topicAvailable: "
<< counter << std::endl;
auto e = std::chrono::high_resolution_clock::now();
std::cout
<< "waitForTopic() " << topic << " time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(e - s).count()
<< " ms." << std::endl;
std::cout << "waitForTopic() " << topic
<< ": number of calls to topicAvailable: " << counter << std::endl;
#endif
}
bool is_valid_port_path(std::string server_port_path)
{
std::regex url_regex("^(((([a-z]|[A-z])+([0-9]|_)*\\.*([a-z]|[A-z])+([0-9]|_)*))"
"|(((1?[0-9]{1,2}|2[0-4][0-9]|25[0-5])\\.){3}(1?[0-9]{1,2}|2[0-4][0-9]|25[0-5]){1}))"
":[0-9]+$");
return std::regex_match(server_port_path, url_regex);
bool is_valid_port_path(std::string server_port_path) {
std::regex url_regex(
"^(((([a-z]|[A-z])+([0-9]|_)*\\.*([a-z]|[A-z])+([0-9]|_)*))"
"|(((1?[0-9]{1,2}|2[0-4][0-9]|25[0-5])\\.){3}(1?[0-9]{1,2}|2[0-4][0-9]|"
"25[0-5]){1}))"
":[0-9]+$");
return std::regex_match(server_port_path, url_regex);
}
src/comm/ros_bridge/include/server.cpp
View file @ 0510d157
......@@ -10,6 +10,7 @@ ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc)
void ros_bridge::com_private::Server::advertiseService(
const std::string &service, const std::string &type,
const Server::CallbackType &userCallback) {
std::string clientName = serviceAdvertiserKey + service;
auto it = _serviceMap.find(clientName);
if (it != _serviceMap.end())
......
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