diff --git a/Paths/KlingenbachGross.wima b/Paths/KlingenbachGross.wima new file mode 100644 index 0000000000000000000000000000000000000000..0782009cc7604ab4afb3ef80bb87f27bb219f307 --- /dev/null +++ b/Paths/KlingenbachGross.wima @@ -0,0 +1,1253 @@ +{ + "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 + } +} diff --git a/Paths/KlingenbachKlein.wima b/Paths/KlingenbachKlein.wima new file mode 100644 index 0000000000000000000000000000000000000000..d39ef2eae09326e5b2df44635396dd61a59ed224 --- /dev/null +++ b/Paths/KlingenbachKlein.wima @@ -0,0 +1,233 @@ +{ + "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 + } +} diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 004538b50557049e638787c47d15f7f4de0a720d..b6c47293e6471a3b0452d605fb23ab6dec822211 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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 \ diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 3a08cab9fbd6d87c0a12795b91a5235844324951..98a1e3d9a03787e796db55993f3a372c5498244c 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -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{ diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 0cd0a83d6801ad369060b70eeb6cd3f5e84fc87b..e00ed6ce68460f78decea84e4128b2ea9ba89768 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -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") diff --git a/src/FlightMap/MapItems/WimaPlanMapItems.qml b/src/FlightMap/MapItems/WimaPlanMapItems.qml index ceb5a31f99d42f8e13c463a737daa8399667ecd7..44e0b2e4dcc8cdb8c977e369ac4c01a5894d300b 100644 --- a/src/FlightMap/MapItems/WimaPlanMapItems.qml +++ b/src/FlightMap/MapItems/WimaPlanMapItems.qml @@ -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() } - - } diff --git a/src/Snake/snake_typedefs.h b/src/Snake/snake_typedefs.h deleted file mode 100644 index 13abb1b1041267a1c7147ad6d21b1fb582fd2b40..0000000000000000000000000000000000000000 --- a/src/Snake/snake_typedefs.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include -#include -#include - -#include -#include -#include -#include -#include - - -namespace bg = boost::geometry; -namespace bu = boost::units; -namespace snake { - -typedef bg::model::point BoostPoint; -//typedef std::vector BoostPointList; -typedef bg::model::polygon BoostPolygon; -typedef bg::model::linestring BoostLineString; -typedef std::vector> Int64Matrix; -typedef bg::model::box BoostBox; - -typedef bu::quantity Length; -typedef bu::quantity Area; -typedef bu::quantity 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 diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc index f797e16a22428085f7d84b093a399f04f6e33609..a442503a5580e2a39266d814225855cee3df8b3d 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.cc +++ b/src/Wima/Geometry/WimaMeasurementArea.cc @@ -1,31 +1,45 @@ #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(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(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 */ - - diff --git a/src/Wima/Geometry/WimaMeasurementArea.h b/src/Wima/Geometry/WimaMeasurementArea.h index 16ac625358ae037183a0a4843fe74f3d7987e396..02d78966b103da8c2c6f40ff068bbf3e0710d230 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.h +++ b/src/Wima/Geometry/WimaMeasurementArea.h @@ -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 _metaDataMap; + // Members + QMap _metaDataMap; - SettingsFact _bottomLayerAltitude; - SettingsFact _numberOfLayers; - SettingsFact _layerDistance; + SettingsFact _tileWidth; + SettingsFact _tileHeight; + SettingsFact _minTileArea; + SettingsFact _transectDistance; + SettingsFact _minTransectLength; }; - - diff --git a/src/Wima/Snake/SnakeDataManager_old.cc b/src/Wima/Snake/SnakeDataManager_old.cc deleted file mode 100644 index aab26b00a5472dd0907e27ae1ecff08f09ce5968..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeDataManager_old.cc +++ /dev/null @@ -1,425 +0,0 @@ -#include "SnakeDataManager.h" - -#include -#include - -#include "QGCApplication.h" -#include "QGCToolbox.h" -#include "SettingsFact.h" -#include "SettingsManager.h" -#include "WimaSettings.h" - -#include -#include -#include - -#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; -using ROSBridgePtr = std::unique_ptr; - -using UniqueLock = std::unique_lock; -using SharedLock = std::shared_lock; -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 mArea; - QList sArea; - QList corridor; - QGeoCoordinate ENUOrigin; - SnakeTiles tiles; - QmlObjectListModel tilesQml; - QVariantList tileCenterPoints; - SnakeTilesLocal tilesENU; - QVector tileCenterPointsENU; - - // Waypoints - QVector waypoints; - QVector arrivalPath; - QVector returnPath; - QVector waypointsENU; - QVector arrivalPathENU; - QVector returnPathENU; - QString errorMessage; - - // Other - std::atomic_bool calcInProgress; - QNemoProgress qProgress; - std::vector 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 CommandRAII { -public: - CommandRAII(Callable &fun) : _fun(fun) {} - - ~CommandRAII() { _fun(); } - -private: - Callable _fun; -}; - -SnakeDataManager::SnakeDataManager(QObject *parent) - : QThread(parent), pImpl(std::make_unique(this)) - -{} - -SnakeDataManager::~SnakeDataManager() {} - -void SnakeDataManager::setMeasurementArea( - const QList &measurementArea) { - UniqueLock lk(this->pImpl->mutex); - this->pImpl->mArea = measurementArea; - for (auto &vertex : this->pImpl->mArea) { - vertex.setAltitude(0); - } -} - -void SnakeDataManager::setServiceArea( - const QList &serviceArea) { - UniqueLock lk(this->pImpl->mutex); - this->pImpl->sArea = serviceArea; - for (auto &vertex : this->pImpl->sArea) { - vertex.setAltitude(0); - } -} - -void SnakeDataManager::setCorridor(const QList &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 SnakeDataManager::waypoints() const { - SharedLock lk(this->pImpl->mutex); - return this->pImpl->waypoints; -} - -QVector SnakeDataManager::arrivalPath() const { - SharedLock lk(this->pImpl->mutex); - return this->pImpl->arrivalPath; -} - -QVector 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::high_resolution_clock::now() - startTime) - .count() - << " ms"; -#endif - this->pImpl->calcInProgress.store(false); - emit calcInProgressChanged(this->pImpl->calcInProgress.load()); - }; - CommandRAII 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 ¢erPoints = 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{ - 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); - } -} diff --git a/src/Wima/Snake/SnakeThread.cc b/src/Wima/Snake/SnakeThread.cc index c156e775edb31ae0a7752b9fd9ccf885ba1ea7b1..d0d61efa2d81dd6c9005617cab87533950641bd1 100644 --- a/src/Wima/Snake/SnakeThread.cc +++ b/src/Wima/Snake/SnakeThread.cc @@ -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; } } diff --git a/src/Snake/clipper/clipper.cpp b/src/Wima/Snake/clipper/clipper.cpp similarity index 100% rename from src/Snake/clipper/clipper.cpp rename to src/Wima/Snake/clipper/clipper.cpp diff --git a/src/Snake/clipper/clipper.hpp b/src/Wima/Snake/clipper/clipper.hpp similarity index 100% rename from src/Snake/clipper/clipper.hpp rename to src/Wima/Snake/clipper/clipper.hpp diff --git a/src/Snake/mapbox/feature.hpp b/src/Wima/Snake/mapbox/feature.hpp similarity index 100% rename from src/Snake/mapbox/feature.hpp rename to src/Wima/Snake/mapbox/feature.hpp diff --git a/src/Snake/mapbox/geometry.hpp b/src/Wima/Snake/mapbox/geometry.hpp similarity index 100% rename from src/Snake/mapbox/geometry.hpp rename to src/Wima/Snake/mapbox/geometry.hpp diff --git a/src/Snake/mapbox/geometry/box.hpp b/src/Wima/Snake/mapbox/geometry/box.hpp similarity index 100% rename from src/Snake/mapbox/geometry/box.hpp rename to src/Wima/Snake/mapbox/geometry/box.hpp diff --git a/src/Snake/mapbox/geometry/empty.hpp b/src/Wima/Snake/mapbox/geometry/empty.hpp similarity index 100% rename from src/Snake/mapbox/geometry/empty.hpp rename to src/Wima/Snake/mapbox/geometry/empty.hpp diff --git a/src/Snake/mapbox/geometry/envelope.hpp b/src/Wima/Snake/mapbox/geometry/envelope.hpp similarity index 100% rename from src/Snake/mapbox/geometry/envelope.hpp rename to src/Wima/Snake/mapbox/geometry/envelope.hpp diff --git a/src/Snake/mapbox/geometry/for_each_point.hpp b/src/Wima/Snake/mapbox/geometry/for_each_point.hpp similarity index 100% rename from src/Snake/mapbox/geometry/for_each_point.hpp rename to src/Wima/Snake/mapbox/geometry/for_each_point.hpp diff --git a/src/Snake/mapbox/geometry/geometry.hpp b/src/Wima/Snake/mapbox/geometry/geometry.hpp similarity index 100% rename from src/Snake/mapbox/geometry/geometry.hpp rename to src/Wima/Snake/mapbox/geometry/geometry.hpp diff --git a/src/Snake/mapbox/geometry/line_string.hpp b/src/Wima/Snake/mapbox/geometry/line_string.hpp similarity index 100% rename from src/Snake/mapbox/geometry/line_string.hpp rename to src/Wima/Snake/mapbox/geometry/line_string.hpp diff --git a/src/Snake/mapbox/geometry/multi_line_string.hpp b/src/Wima/Snake/mapbox/geometry/multi_line_string.hpp similarity index 100% rename from src/Snake/mapbox/geometry/multi_line_string.hpp rename to src/Wima/Snake/mapbox/geometry/multi_line_string.hpp diff --git a/src/Snake/mapbox/geometry/multi_point.hpp b/src/Wima/Snake/mapbox/geometry/multi_point.hpp similarity index 100% rename from src/Snake/mapbox/geometry/multi_point.hpp rename to src/Wima/Snake/mapbox/geometry/multi_point.hpp diff --git a/src/Snake/mapbox/geometry/multi_polygon.hpp b/src/Wima/Snake/mapbox/geometry/multi_polygon.hpp similarity index 100% rename from src/Snake/mapbox/geometry/multi_polygon.hpp rename to src/Wima/Snake/mapbox/geometry/multi_polygon.hpp diff --git a/src/Snake/mapbox/geometry/point.hpp b/src/Wima/Snake/mapbox/geometry/point.hpp similarity index 100% rename from src/Snake/mapbox/geometry/point.hpp rename to src/Wima/Snake/mapbox/geometry/point.hpp diff --git a/src/Snake/mapbox/geometry/point_arithmetic.hpp b/src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp similarity index 100% rename from src/Snake/mapbox/geometry/point_arithmetic.hpp rename to src/Wima/Snake/mapbox/geometry/point_arithmetic.hpp diff --git a/src/Snake/mapbox/geometry/polygon.hpp b/src/Wima/Snake/mapbox/geometry/polygon.hpp similarity index 100% rename from src/Snake/mapbox/geometry/polygon.hpp rename to src/Wima/Snake/mapbox/geometry/polygon.hpp diff --git a/src/Snake/mapbox/geometry_io.hpp b/src/Wima/Snake/mapbox/geometry_io.hpp similarity index 100% rename from src/Snake/mapbox/geometry_io.hpp rename to src/Wima/Snake/mapbox/geometry_io.hpp diff --git a/src/Snake/mapbox/optional.hpp b/src/Wima/Snake/mapbox/optional.hpp similarity index 100% rename from src/Snake/mapbox/optional.hpp rename to src/Wima/Snake/mapbox/optional.hpp diff --git a/src/Snake/mapbox/polylabel.hpp b/src/Wima/Snake/mapbox/polylabel.hpp similarity index 100% rename from src/Snake/mapbox/polylabel.hpp rename to src/Wima/Snake/mapbox/polylabel.hpp diff --git a/src/Snake/mapbox/recursive_wrapper.hpp b/src/Wima/Snake/mapbox/recursive_wrapper.hpp similarity index 100% rename from src/Snake/mapbox/recursive_wrapper.hpp rename to src/Wima/Snake/mapbox/recursive_wrapper.hpp diff --git a/src/Snake/mapbox/variant.hpp b/src/Wima/Snake/mapbox/variant.hpp similarity index 100% rename from src/Snake/mapbox/variant.hpp rename to src/Wima/Snake/mapbox/variant.hpp diff --git a/src/Snake/mapbox/variant_io.hpp b/src/Wima/Snake/mapbox/variant_io.hpp similarity index 100% rename from src/Snake/mapbox/variant_io.hpp rename to src/Wima/Snake/mapbox/variant_io.hpp diff --git a/src/Wima/Snake/route.cpp b/src/Wima/Snake/route.cpp new file mode 100644 index 0000000000000000000000000000000000000000..59b62814e5c908d698d91b1250310608f5e9c4cd --- /dev/null +++ b/src/Wima/Snake/route.cpp @@ -0,0 +1,183 @@ +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(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 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 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::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::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 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 &idxArray, + std::vector &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 idxList; + shortestPathFromGraph(connectionGraph, idx0, idx1, idxList); + if (i != route_idx.size() - 2) { + idxList.pop_back(); + } + idx2Vertex(idxList, route); + } + } + return true; +} diff --git a/src/Snake/snake.cpp b/src/Wima/Snake/snake.cpp similarity index 98% rename from src/Snake/snake.cpp rename to src/Wima/Snake/snake.cpp index 37280bac500039e370b5ac4a46922f156e717eb0..a8753b2c39fc1e2a8973fe40cb62e58b06846a23 100644 --- a/src/Snake/snake.cpp +++ b/src/Wima/Snake/snake.cpp @@ -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 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; diff --git a/src/Snake/snake.h b/src/Wima/Snake/snake.h similarity index 98% rename from src/Snake/snake.h rename to src/Wima/Snake/snake.h index fab52f5f5b2bd252a675c3984c852dbec51da5b9..26d900c926713ae941de6ccb87e628bf01cd7056 100644 --- a/src/Snake/snake.h +++ b/src/Wima/Snake/snake.h @@ -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 diff --git a/src/Wima/Snake/snake_old.cpp b/src/Wima/Snake/snake_old.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ffa2a152a96115e788a269b6a64b0f10960bf55b --- /dev/null +++ b/src/Wima/Snake/snake_old.cpp @@ -0,0 +1,348 @@ +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::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 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::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; iIsEqual(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 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::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 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 pathIdx; + long arrivalPathLength = 0; + for (long i=0; 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 processedTiles; + { + const auto &tiles = _scenario.getTilesENU(); + for (size_t i=0; i(vertex.get<0>()*CLIPPER_SCALE), + static_cast(vertex.get<1>()*CLIPPER_SCALE)}); + } + vector processedTilesClipper; + for (auto t : processedTiles){ + ClipperLib::Path path; + for (auto vertex : t.outer()){ + path.push_back(ClipperLib::IntPoint{static_cast(vertex.get<0>()*CLIPPER_SCALE), + static_cast(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 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 rotate_back(-alpha*180/M_PI); + trans::translate_transformer translate_back(x0, y0); + vector 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(transect[0].get<0>()*CLIPPER_SCALE), + static_cast(transect[0].get<1>()*CLIPPER_SCALE)}; + ClipperLib::IntPoint c2{static_cast(transect[1].get<0>()*CLIPPER_SCALE), + static_cast(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(clipperTransect[0].X)/CLIPPER_SCALE, + static_cast(clipperTransect[0].Y)/CLIPPER_SCALE}; + BoostPoint v2{static_cast(clipperTransect[1].X)/CLIPPER_SCALE, + static_cast(clipperTransect[1].Y)/CLIPPER_SCALE}; + + 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 &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::high_resolution_clock::now()-start); + cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl; +#endif +// cout << endl; +// for (size_t i=0; i &row = graph[i]; +// for (size_t j=0; j 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::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_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(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(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(end - + start) + .count() + << " ms" << std::endl; +#endif } void WimaController::_switchToSnakeWaypointManager(QVariant variant) { diff --git a/src/WimaView/WimaMeasurementAreaEditor.qml b/src/WimaView/WimaMeasurementAreaEditor.qml index 4aa61633e4625291bfb44c8123b931ed0ee225ca..e47dce3fc0d3ae85db3a1dddcabe8a0c54028ca3 100644 --- a/src/WimaView/WimaMeasurementAreaEditor.qml +++ b/src/WimaView/WimaMeasurementAreaEditor.qml @@ -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 diff --git a/src/WimaView/WimaServiceAreaEditor.qml b/src/WimaView/WimaServiceAreaEditor.qml index 8f642f82306dd4ef3b79cc2bc101a3d3721c1c84..7569fed97ca1d6d1e7e38d141c12307bf6b1f2f8 100644 --- a/src/WimaView/WimaServiceAreaEditor.qml +++ b/src/WimaView/WimaServiceAreaEditor.qml @@ -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 } diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp index 3a44e7766a88c6420ff8e98b7f1b315db8557426..5b6d22c09cd9baae8563688159cbea52f7f3a552 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.cpp +++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp @@ -1,970 +1,995 @@ #include "RosBridgeClient.h" - #include #include -#include #include #include +#include - -struct Task{ - std::function ready; // Condition under which command should be executed. - std::function execute; // Command to execute. - std::function expired; // Returns true if the command is expired. - std::function clear_up; // operation to perform if task expired. - std::string name; +struct Task { + std::function + ready; // Condition under which command should be executed. + std::function execute; // Command to execute. + std::function expired; // Returns true if the command is expired. + std::function clear_up; // operation to perform if task expired. + std::string name; }; -void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr client, const std::__cxx11::string &message) -{ +void RosbridgeWsClient::start(const std::__cxx11::string &client_name, + std::shared_ptr 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 connection) { + client->on_open = [client_name, message]( + std::shared_ptr connection) { #else - client->on_open = [message](std::shared_ptr connection) { + client->on_open = + [message](std::shared_ptr 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 /*connection*/, std::shared_ptr 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 /*connection*/, + std::shared_ptr 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 /*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 /*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 /*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 /*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(false)) - , stopped(std::make_shared(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(false)), + stopped(std::make_shared(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 ([this] { - std::list 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(false); - auto status_client = std::make_shared(this->server_port_path); - status_client->on_open = [status_set, this](std::shared_ptr) { - 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(false); - this->callService("/rosapi/topics", [topics_set, this]( - std::shared_ptr connection, - std::shared_ptr in_message){ - std::unique_lock 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 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(false); - this->callService("/rosapi/services", [this, services_set]( - std::shared_ptr connection, - std::shared_ptr in_message){ - std::unique_lock 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 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 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([this] { + std::list 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(false); + auto status_client = + std::make_shared(this->server_port_path); + status_client->on_open = + [status_set, this](std::shared_ptr) { + 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(false); + this->callService( + "/rosapi/topics", + [topics_set, + this](std::shared_ptr connection, + std::shared_ptr in_message) { + std::unique_lock 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 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(false); + this->callService( + "/rosapi/services", + [this, services_set]( + std::shared_ptr connection, + std::shared_ptr in_message) { + std::unique_lock 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 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 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 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 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 lk(mutex); - std::unordered_map>::iterator it = client_map.find(client_name); - if (it == client_map.end()) - { - client_map[client_name] = std::make_shared(server_port_path); - } +void RosbridgeWsClient::addClient(const std::string &client_name) { + std::lock_guard lk(mutex); + std::unordered_map>::iterator it = + client_map.find(client_name); + if (it == client_map.end()) { + client_map[client_name] = std::make_shared(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 RosbridgeWsClient::getClient(const std::string &client_name) -{ - std::lock_guard lk(mutex); - std::unordered_map>::iterator it = client_map.find(client_name); - if (it != client_map.end()) - { - return it->second; - } - return NULL; +std::shared_ptr +RosbridgeWsClient::getClient(const std::string &client_name) { + std::lock_guard lk(mutex); + std::unordered_map>::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 lk(mutex); - std::unordered_map>::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 client = it->second; -#ifdef ROS_BRIDGE_DEBUG - std::thread t([client, client_name](){ +void RosbridgeWsClient::stopClient(const std::string &client_name) { + std::lock_guard lk(mutex); + std::unordered_map>::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 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 lk(mutex); - std::unordered_map>::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 lk(mutex); + std::unordered_map>::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 lk(mutex); - return available_topics; +std::string RosbridgeWsClient::getAdvertisedTopics() { + std::lock_guard lk(mutex); + return available_topics; } -std::string RosbridgeWsClient::getAdvertisedServices(){ - std::lock_guard lk(mutex); - return available_services; +std::string RosbridgeWsClient::getAdvertisedServices() { + std::lock_guard lk(mutex); + return available_services; } -bool RosbridgeWsClient::topicAvailable(const std::string &topic){ - std::lock_guard lk(mutex); +bool RosbridgeWsClient::topicAvailable(const std::string &topic) { + std::lock_guard 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 lk(mutex); - std::unordered_map>::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(td); +void RosbridgeWsClient::advertise(const std::string &client_name, + const std::string &topic, + const std::string &type, + const std::string &id) { + std::lock_guard lk(mutex); + std::unordered_map>::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(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 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 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 connection) { + client->on_open = [this, topic, message, client_name]( + std::shared_ptr connection) { #else - client->on_open = [this, topic, message](std::shared_ptr connection) { + client->on_open = [this, topic, message]( + std::shared_ptr 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 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 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(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(server_port_path); + std::string client_name = "topic_unadvertiser" + topic; + auto client = std::make_shared(server_port_path); #ifdef ROS_BRIDGE_DEBUG - client->on_open = [client_name, message](std::shared_ptr connection) { + client->on_open = [client_name, message]( + std::shared_ptr connection) { #else - client->on_open = [message](std::shared_ptr connection) { + client->on_open = + [message](std::shared_ptr 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(entry) == EntryType::AdvertisedTopic ){ - unadvertise(std::get(entry)); - } +void RosbridgeWsClient::unadvertiseAll() { + for (auto entry : service_topic_list) { + if (std::get(entry) == + EntryType::AdvertisedTopic) { + unadvertise(std::get(entry)); } + } } -void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Document &msg, const std::string &id) -{ - std::lock_guard 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 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(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 writer(strbuf); - msg.Accept(writer); + return; + } + rapidjson::StringBuffer strbuf; + rapidjson::Writer 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 publish_client = std::make_shared(server_port_path); + std::shared_ptr publish_client = + std::make_shared(server_port_path); #ifdef ROS_BRIDGE_DEBUG - publish_client->on_open = [message, client_name](std::shared_ptr connection) { + publish_client->on_open = + [message, client_name](std::shared_ptr connection) { #else - publish_client->on_open = [message, client_name](std::shared_ptr connection) { + publish_client->on_open = + [message, client_name](std::shared_ptr 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 lk(mutex); - std::unordered_map>::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(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 lk(mutex); + std::unordered_map>::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(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 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 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 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 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(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(server_port_path); + std::string client_name = "topic_unsubscriber" + topic; + auto client = std::make_shared(server_port_path); #ifdef ROS_BRIDGE_DEBUG - client->on_open = [client_name, message](std::shared_ptr connection) { + client->on_open = [client_name, message]( + std::shared_ptr connection) { #else - client->on_open = [message](std::shared_ptr connection) { + client->on_open = + [message](std::shared_ptr 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(entry) == EntryType::SubscribedTopic ) { - unsubscribe(std::get(entry)); - } +void RosbridgeWsClient::unsubscribeAll() { + for (auto entry : service_topic_list) { + if (std::get(entry) == + EntryType::SubscribedTopic) { + unsubscribe(std::get(entry)); } + } } -void RosbridgeWsClient::advertiseService(const std::string &client_name, const std::string &service, const std::string &type, const InMessage &callback) -{ - std::lock_guard lk(mutex); - std::unordered_map>::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(td); +void RosbridgeWsClient::advertiseService(const std::string &client_name, + const std::string &service, + const std::string &type, + const InMessage &callback) { + std::lock_guard lk(mutex); + std::unordered_map>::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(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 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 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 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 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(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(server_port_path); + std::string client_name = "service_unadvertiser" + service; + auto client = std::make_shared(server_port_path); #ifdef ROS_BRIDGE_DEBUG - client->on_open = [client_name, message](std::shared_ptr connection) { + client->on_open = [client_name, message]( + std::shared_ptr connection) { #else - client->on_open = [message](std::shared_ptr connection) { + client->on_open = + [message](std::shared_ptr 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(entry) == EntryType::AdvertisedService ) { - unadvertiseService(std::get(entry)); - } +void RosbridgeWsClient::unadvertiseAllServices() { + for (auto entry : service_topic_list) { + if (std::get(entry) == + EntryType::AdvertisedService) { + unadvertiseService( + std::get(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 writer(strbuf); - values.Accept(writer); + // Convert JSON document to string + rapidjson::StringBuffer strbuf; + rapidjson::Writer 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 service_response_client = std::make_shared(server_port_path); + std::string client_name = "service_response_client" + service; + std::shared_ptr service_response_client = + std::make_shared(server_port_path); #ifdef ROS_BRIDGE_DEBUG - service_response_client->on_open = [message, client_name](std::shared_ptr connection) { + service_response_client->on_open = + [message, client_name](std::shared_ptr connection) { #else - service_response_client->on_open = [message](std::shared_ptr connection) { + service_response_client->on_open = + [message](std::shared_ptr 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 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 call_service_client = std::make_shared(server_port_path); + if (!args.IsNull()) { + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + args.Accept(writer); - if (callback) - { - call_service_client->on_message = callback; - } - else - { - call_service_client->on_message = [client_name](std::shared_ptr connection, std::shared_ptr 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 call_service_client = + std::make_shared(server_port_path); + + if (callback) { + call_service_client->on_message = callback; + } else { + call_service_client->on_message = + [client_name](std::shared_ptr connection, + std::shared_ptr 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 lk(mutex); - pos = available_services.find(service); - } - bool ret = pos != std::string::npos ? true : false; + size_t pos; + { + std::lock_guard 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 stop) -{ +void RosbridgeWsClient::waitForService(const std::string &service, + const std::function 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(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(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 stop) -{ +void RosbridgeWsClient::waitForTopic(const std::string &topic, + const std::function 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(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(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); } diff --git a/src/comm/ros_bridge/include/server.cpp b/src/comm/ros_bridge/include/server.cpp index 548f2026f6bdb54e94be6af9296946695810254d..845a9d6fb7dbe525ff2cc7bfd36bd8a30f04b08e 100644 --- a/src/comm/ros_bridge/include/server.cpp +++ b/src/comm/ros_bridge/include/server.cpp @@ -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())